O12重构一版提交
This commit is contained in:
@@ -261,7 +261,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
|||||||
"叁": [0, 39, 255, 255, 255, 0],
|
"叁": [0, 39, 255, 255, 255, 0],
|
||||||
"肆": [0, 0, 255, 255, 255, 255],
|
"肆": [0, 0, 255, 255, 255, 255],
|
||||||
"伍": [255, 255, 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],
|
"点赞": [255, 255, 0, 0, 0, 0],
|
||||||
"握拳": [79, 11, 0, 0, 0, 0],
|
"握拳": [79, 11, 0, 0, 0, 0],
|
||||||
"序列动作1": [250, 250, 250, 250, 250, 250],
|
"序列动作1": [250, 250, 250, 250, 250, 250],
|
||||||
@@ -277,7 +277,12 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
|||||||
"拇指压感准备1": [139, 18, 130, 0, 0, 0],
|
"拇指压感准备1": [139, 18, 130, 0, 0, 0],
|
||||||
"拇指压感测试": [39, 30, 122, 250, 250, 250],
|
"拇指压感测试": [39, 30, 122, 250, 250, 250],
|
||||||
"拇指压感准备2": [139, 18, 130, 0, 0, 0]
|
"拇指压感准备2": [139, 18, 130, 0, 0, 0]
|
||||||
}
|
},
|
||||||
|
preset_action_overrides={
|
||||||
|
"right": {
|
||||||
|
"OK": [58, 6, 153, 255, 255, 255],
|
||||||
|
},
|
||||||
|
},
|
||||||
),
|
),
|
||||||
"O12": HandConfig(
|
"O12": HandConfig(
|
||||||
# O12 active-angle API order. Keep this independent from the
|
# 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",
|
"ring_mcp_pitch",
|
||||||
"pinky_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 专业标定包
|
# 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 右手 16-Tag 完整标定
|
||||||
|
|
||||||
O12 使用 vendor `omnihand_pro_2025_node` 的标准弧度接口,固定订阅
|
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
|
runner 会自动加载仓库内 vendor Jazzy overlay,启动 HCAN device 0/channel 0
|
||||||
节点、三相机、AprilTag 与标定节点;READY 后自动开始。启动前会确认 12 路
|
节点、三相机、AprilTag 与标定节点;READY 后自动开始。启动前会确认 12 路
|
||||||
POSITION 模式、错误码、温度和反馈,并以不超过 3° 的低速点动执行固定通道预检。
|
POSITION 模式、错误码和实时反馈,并以不超过 3° 的低速点动执行固定通道预检。
|
||||||
正式扫描以 20 Hz 发布端点速度为零的弧度余弦轨迹;中止、堵转或质量失败时保持
|
这版 O12 固件的温度查询会阻塞后返回空,因此默认不发送该无效请求,而以独立错误
|
||||||
当前位置。结果发布到 `calibration_output/<O12串号>/latest_passed`,其中 JSON
|
查询中的 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 表。
|
为 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 配置哈希而不运动:
|
只验证配置、16 张 16 mm Tag、相机/外参、源 URDF 和 SDK 配置哈希而不运动:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
@@ -262,23 +394,23 @@ URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll
|
|||||||
失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记;
|
失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记;
|
||||||
恢复时只读取该标记之后的替代采集,不能把同一尝试编号下重扫前后的稳态点合并。
|
恢复时只读取该标记之后的替代采集,不能把同一尝试编号下重扫前后的稳态点合并。
|
||||||
因此已经在线硬门限验收的任务保持已完成,暂停中的任务从任务开头重采,不会因
|
因此已经在线硬门限验收的任务保持已完成,暂停中的任务从任务开头重采,不会因
|
||||||
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量
|
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面
|
||||||
独立保留;单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
|
主测量和侧面校验测量独立保留。侧面校验视角的任务级有效率只记录为诊断;逐帧
|
||||||
轴线位置若也能明确定位为单轮异常,同样只补扫该轮;补扫会保留任务预检和前次
|
识别率低于标称值,
|
||||||
采集确定的PnP分支参考,不会因重新初始化切换到另一组平面Tag镜像解。侧面
|
|
||||||
校验视角的任务级有效率只记录为诊断;G20右手预检若逐帧识别率低于标称值,
|
|
||||||
但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞,也按完整
|
但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞,也按完整
|
||||||
轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查,轴线、曲线和模型质量
|
轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查,轴线、曲线和模型质量
|
||||||
门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一
|
门限保持不变。`unified_engine_v1`取消每任务低速全行程预检;相邻方向共享已验证
|
||||||
采集事务:相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用
|
端点和任务级PnP参考。采样不足只按原速度重扫当前方向一次,拟合或第四轮留出
|
||||||
|
失败立即锁定发布并保持当前位置,不再通过反复运动碰门限。G20右手正式扫描固定使用
|
||||||
产品审定速度,不再根据单次识别密度自动提速,确保不同会话测量的是同一动态
|
产品审定速度,不再根据单次识别密度自动提速,确保不同会话测量的是同一动态
|
||||||
过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查:前三轮
|
过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查:前三轮
|
||||||
极差默认不得超过0.5°,95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的
|
极差默认不得超过0.5°,95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的
|
||||||
紧密簇、仅另一轮越界,自动补采该轮两个方向;无法明确定位时只重采完整yaw任务,
|
紧密簇、仅另一轮越界,也只输出诊断并停止发布,不自动补采。与`latest_passed`
|
||||||
不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入
|
中上一正式结果相差超过0.75°时另写入
|
||||||
`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装,但该历史差值
|
`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/DIP联合任务,共16个物理运动任务。同步roll只驱动电机一次,但两台相机仍分别拟合并通过
|
||||||
各自的观测质量门限。每个PIP任务只驱动一次对应电机,同时用“手掌→中节Tag”实测PIP、
|
各自的观测质量门限。每个PIP任务只驱动一次对应电机,同时用“手掌→中节Tag”实测PIP、
|
||||||
用“中节Tag→末节Tag”实测被动DIP;四个DIP不再由URDF mimic系数生成,并参加完整视觉
|
用“中节Tag→末节Tag”实测被动DIP;四个DIP不再由URDF mimic系数生成,并参加完整视觉
|
||||||
拟合和质量门限。每项正式四轮之前自动低速往返预检
|
拟合和质量门限。任务直接执行四轮双向正式扫描;
|
||||||
0/127/255可见性;
|
|
||||||
|
|
||||||
基准形态恢复完成后,程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
|
基准形态恢复完成后,程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
|
||||||
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0,因此四指正面+侧面同步roll中
|
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0,因此四指正面+侧面同步roll中
|
||||||
@@ -355,23 +486,22 @@ ros2 launch linkerhand_calibration three_camera_calibration.launch.py \
|
|||||||
运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
运动,被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
||||||
等已评审不变量保持不变,过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
|
等已评审不变量保持不变,过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
|
||||||
`parallel_pose_transitions`(默认true)置false可回退旧的逐电机顺序。
|
`parallel_pose_transitions`(默认true)置false可回退旧的逐电机顺序。
|
||||||
同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行,只在任务切换时退出,
|
同一任务的四轮正式扫描会保持完整避障姿态连续执行,只在任务切换时退出,
|
||||||
不再每轮重复展开/弯曲辅助手指。跨手指组切换时,下一组避障姿态仍然需要、且
|
不再每轮重复展开/弯曲辅助手指。跨手指组切换时,下一组避障姿态仍然需要、且
|
||||||
当前已经在位(含反馈容差)的辅助电机保持原位,只有下一组不再使用的避障电机
|
当前已经在位(含反馈容差)的辅助电机保持原位,只有下一组不再使用的避障电机
|
||||||
退回基准,避免"先展开回基准、马上又折回"的多余动作;已评审的
|
退回基准,避免"先展开回基准、马上又折回"的多余动作;已评审的
|
||||||
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
|
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。G20右手四轮正式速度始终使用
|
||||||
不超过8,只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值,
|
产品配置的固定值,不会因本次帧率或识别密度而改变。
|
||||||
不会因本次预检帧率或识别密度而改变;旧11-Tag布局仍保留自适应速度兼容逻辑。
|
|
||||||
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态:以`255→127`为标准物理
|
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态:以`255→127`为标准物理
|
||||||
零位,反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
零位,反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
||||||
四轮间隙极差上限0.3°;其他关节仍使用严格的0.5°baseline回差门限。
|
四轮间隙极差上限0.3°;其他关节仍使用严格的0.5°baseline回差门限。
|
||||||
预检、正式四轮和拟合重扫始终使用速度5。
|
正式四轮使用产品审定速度;唯一一次采样重扫保持相同速度。
|
||||||
正式roll的每个方向会在经过127时先到位保持0.5秒,再独立保存至少10帧静止Tag/反馈;
|
正式roll的每个方向会在经过127时先到位保持0.5秒,再独立保存至少10帧静止Tag/反馈;
|
||||||
方向分支检查和动态曲线的127相位都使用这两组双向静止数据,运动中经过127的帧不再
|
方向分支检查和动态曲线的127相位都使用这两组双向静止数据,运动中经过127的帧不再
|
||||||
替代静态保持姿态。
|
替代静态保持姿态。
|
||||||
前三轮只用于训练,第四轮完全留出;留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
前三轮只用于训练,第四轮完全留出;留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
||||||
每个任务只在低速递减预检起点执行一次8帧PnP静态初始化;预检往返和四轮正式
|
每个任务在首个正式方向起点执行PnP静态初始化;四轮正式扫描连续复用同一帧间
|
||||||
扫描连续复用同一帧间分支与任务参考,不再让每一轮独立选择平面Tag解。同一任务第1轮
|
分支与任务参考,不再让每一轮独立选择平面Tag解。同一任务第1轮
|
||||||
已确立的端点相对姿态作为后3轮的分支锚点,防止独立初始化选到相反的
|
已确立的端点相对姿态作为后3轮的分支锚点,防止独立初始化选到相反的
|
||||||
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
||||||
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`,只在逐帧IPPE双解中
|
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`,只在逐帧IPPE双解中
|
||||||
|
|||||||
@@ -26,7 +26,7 @@ artifacts:
|
|||||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
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: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||||
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
||||||
|
|
||||||
|
|||||||
@@ -28,7 +28,7 @@ artifacts:
|
|||||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||||
calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
|
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: package://linkerhand_calibration/config/l6_right_8_tags.yaml
|
||||||
tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
|
tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
|
||||||
|
|
||||||
|
|||||||
@@ -61,7 +61,7 @@ l6_calibration:
|
|||||||
motor_stall_timeout_seconds: 2.0
|
motor_stall_timeout_seconds: 2.0
|
||||||
position_timeout_seconds: 30.0
|
position_timeout_seconds: 30.0
|
||||||
sweep_timeout_seconds: 90.0
|
sweep_timeout_seconds: 90.0
|
||||||
automatic_sweep_retry_limit: 2
|
automatic_sweep_retry_limit: 1
|
||||||
non_target_motion_tolerance_u8: 3.0
|
non_target_motion_tolerance_u8: 3.0
|
||||||
fixed_base_maximum_corner_drift_px: 2.0
|
fixed_base_maximum_corner_drift_px: 5.0
|
||||||
fixed_base_movement_confirmation_frames: 5
|
fixed_base_movement_confirmation_frames: 10
|
||||||
|
|||||||
@@ -12,7 +12,7 @@ sdk:
|
|||||||
transport: hcan
|
transport: hcan
|
||||||
setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
|
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: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml
|
||||||
config_sha256: ca1791c822bf1e99f25db8806397c3f877100c9252fcce6c53a7a6b8cef7694a
|
config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c
|
||||||
|
|
||||||
cameras:
|
cameras:
|
||||||
front:
|
front:
|
||||||
@@ -31,10 +31,10 @@ cameras:
|
|||||||
artifacts:
|
artifacts:
|
||||||
source_urdf: package://linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703.urdf
|
source_urdf: package://linkerhand_calibration/urdf/o12_right/linkerhand_o12_t3_right-0703.urdf
|
||||||
source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb
|
source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb
|
||||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
camera_extrinsics: config/o12_three_camera_extrinsics.yaml
|
||||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
camera_extrinsics_sha256: 5a515d0706f4e67d5e26bfddb348b519817bd72e885ea9e43997e41016176e53
|
||||||
calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml
|
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: package://linkerhand_calibration/config/o12_right_16_tags.yaml
|
||||||
tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab
|
tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab
|
||||||
|
|
||||||
|
|||||||
@@ -9,39 +9,43 @@ o12_calibration:
|
|||||||
top_camera_info_topic: /o12_calibration/top/camera/camera_info
|
top_camera_info_topic: /o12_calibration/top/camera/camera_info
|
||||||
top_detections_topic: /o12_calibration/top/apriltag/detections
|
top_detections_topic: /o12_calibration/top/apriltag/detections
|
||||||
|
|
||||||
# O12 standard interface is radians. Raw 0..2000 mixed control is forbidden.
|
# O12 standard interface is radians. Scan the complete reviewed SDK range;
|
||||||
command_rate_hz: 20.0
|
# 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
|
repetitions: 4
|
||||||
tag_size_m: 0.016
|
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_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]
|
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_detection_rate: 0.95
|
||||||
minimum_joint_frame_rate: 0.85
|
minimum_joint_frame_rate: 0.85
|
||||||
minimum_feedback_hz: 15.0
|
minimum_feedback_hz: 35.0
|
||||||
maximum_state_image_skew_ms: 50.0
|
maximum_state_image_skew_ms: 50.0
|
||||||
minimum_sweep_frames: 40
|
minimum_sweep_frames: 40
|
||||||
minimum_state_span_fraction: 0.90
|
minimum_state_span_fraction: 0.90
|
||||||
minimum_sweep_bins: 32
|
minimum_sweep_bins: 32
|
||||||
maximum_bin_gap: 2
|
maximum_bin_gap: 2
|
||||||
endpoint_tolerance_rad: 0.01
|
endpoint_tolerance_rad: 0.01
|
||||||
endpoint_hold_seconds: 1.0
|
endpoint_hold_seconds: 0.5
|
||||||
motor_stall_timeout_seconds: 2.0
|
motor_stall_timeout_seconds: 2.0
|
||||||
position_timeout_seconds: 60.0
|
position_timeout_seconds: 60.0
|
||||||
sweep_timeout_seconds: 180.0
|
sweep_timeout_seconds: 180.0
|
||||||
automatic_sweep_retry_limit: 2
|
automatic_sweep_retry_limit: 1
|
||||||
non_target_motion_tolerance_rad: 0.015
|
non_target_motion_tolerance_rad: 0.015
|
||||||
maximum_temperature_c: 70
|
maximum_temperature_c: 70
|
||||||
# 当前 O12 固件返回空温度报告。仍发起查询;5秒无结果后依赖已验证的
|
# 当前 O12 固件返回空温度报告。仍发起查询;5秒无结果后依赖已验证的
|
||||||
# joint_error_states bit1 过热保护,并在会话记录中明确标注降级。
|
# joint_error_states bit1 过热保护,并在会话记录中明确标注降级。
|
||||||
temperature_report_required: false
|
temperature_report_required: false
|
||||||
temperature_fallback_after_seconds: 5.0
|
temperature_fallback_after_seconds: 5.0
|
||||||
# 现场确认初始保守速度过慢;正式/点动速度提高到2倍,避让另有硬上限。
|
# 快速标定档:各任务按4倍请求,但拇指pitch、侧摆、屈曲及避让均有独立硬上限。
|
||||||
motion_speed_scale: 2.0
|
motion_speed_scale: 4.0
|
||||||
|
# 正弦速度加减速时间;中段保持关节限速,避免位置余弦全程低速。
|
||||||
|
trajectory_ramp_seconds: 0.4
|
||||||
maximum_hamming: 0
|
maximum_hamming: 0
|
||||||
minimum_decision_margin: 30.0
|
minimum_decision_margin: 30.0
|
||||||
minimum_edge_pixels: 30.0
|
minimum_edge_pixels: 30.0
|
||||||
fixed_base_maximum_corner_drift_px: 2.0
|
fixed_base_maximum_corner_drift_px: 5.0
|
||||||
fixed_base_movement_confirmation_frames: 5
|
fixed_base_movement_confirmation_frames: 10
|
||||||
pnp_maximum_reprojection_error_px: 1.5
|
pnp_maximum_reprojection_error_px: 1.5
|
||||||
pnp_reprojection_tie_px: 1.5
|
pnp_reprojection_tie_px: 1.5
|
||||||
pnp_maximum_pose_jump_deg: 35.0
|
pnp_maximum_pose_jump_deg: 35.0
|
||||||
|
|||||||
@@ -28,7 +28,7 @@ artifacts:
|
|||||||
camera_extrinsics: config/o6_three_camera_extrinsics.yaml
|
camera_extrinsics: config/o6_three_camera_extrinsics.yaml
|
||||||
camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
|
camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
|
||||||
calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
|
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: package://linkerhand_calibration/config/o6_right_8_tags.yaml
|
||||||
tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
|
tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
|
||||||
|
|
||||||
|
|||||||
@@ -58,7 +58,7 @@ o6_calibration:
|
|||||||
motor_stall_timeout_seconds: 2.0
|
motor_stall_timeout_seconds: 2.0
|
||||||
position_timeout_seconds: 30.0
|
position_timeout_seconds: 30.0
|
||||||
sweep_timeout_seconds: 90.0
|
sweep_timeout_seconds: 90.0
|
||||||
automatic_sweep_retry_limit: 2
|
automatic_sweep_retry_limit: 1
|
||||||
non_target_motion_tolerance_u8: 3.0
|
non_target_motion_tolerance_u8: 3.0
|
||||||
fixed_base_maximum_corner_drift_px: 2.0
|
fixed_base_maximum_corner_drift_px: 5.0
|
||||||
fixed_base_movement_confirmation_frames: 5
|
fixed_base_movement_confirmation_frames: 10
|
||||||
|
|||||||
@@ -57,8 +57,8 @@ g20_calibration:
|
|||||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||||
# 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见;
|
# 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见;
|
||||||
# 任一角点相对会话基准漂移超过2 px并连续5帧时,判定标定中基准被移动。
|
# 任一角点相对会话基准漂移超过2 px并连续5帧时,判定标定中基准被移动。
|
||||||
fixed_base_maximum_corner_drift_px: 2.0
|
fixed_base_maximum_corner_drift_px: 5.0
|
||||||
fixed_base_movement_confirmation_frames: 5
|
fixed_base_movement_confirmation_frames: 10
|
||||||
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时,用源URDF mimic
|
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时,用源URDF mimic
|
||||||
# 关系辅助选择IPPE分支;若全部候选超限则退回纯视觉,绝不丢帧,也不生成、
|
# 关系辅助选择IPPE分支;若全部候选超限则退回纯视觉,绝不丢帧,也不生成、
|
||||||
# 缩放或替代被动IP的自身Tag实测曲线。
|
# 缩放或替代被动IP的自身Tag实测曲线。
|
||||||
@@ -110,13 +110,12 @@ g20_calibration:
|
|||||||
# roll零位127必须从两个方向到位并静止采集,禁止用运动中经过127的帧判回差。
|
# roll零位127必须从两个方向到位并静止采集,禁止用运动中经过127的帧判回差。
|
||||||
baseline_hold_seconds: 0.5
|
baseline_hold_seconds: 0.5
|
||||||
minimum_baseline_hold_frames: 10
|
minimum_baseline_hold_frames: 10
|
||||||
# 19-Tag产品每项正式四轮前先做一次低速往返,端点Tag稳定至少2秒。
|
# unified_engine_v1 不执行每任务全行程预检;保留参数仅兼容旧配置读取。
|
||||||
task_precheck_hold_seconds: 2.0
|
task_precheck_hold_seconds: 2.0
|
||||||
position_timeout_seconds: 30.0
|
position_timeout_seconds: 30.0
|
||||||
sweep_timeout_seconds: 90.0
|
sweep_timeout_seconds: 90.0
|
||||||
# 启动宽限1秒后,反馈连续2秒没有至少1个u8的进展,按机械卡滞立即暂停;
|
# 启动宽限1秒后,反馈连续2秒没有至少1个u8的进展,按机械卡滞立即暂停;
|
||||||
# 这类故障不进入遮挡/超时的三次自动重扫。
|
# 这类硬故障不自动重试。
|
||||||
# 低速5也应持续产生反馈进展;5秒无进展即停,减少机构持续顶死时间。
|
|
||||||
motor_stall_timeout_seconds: 2.0
|
motor_stall_timeout_seconds: 2.0
|
||||||
motor_stall_startup_grace_seconds: 1.0
|
motor_stall_startup_grace_seconds: 1.0
|
||||||
motor_stall_minimum_progress_u8: 1.0
|
motor_stall_minimum_progress_u8: 1.0
|
||||||
@@ -126,18 +125,18 @@ g20_calibration:
|
|||||||
minimum_sweep_bins: 32
|
minimum_sweep_bins: 32
|
||||||
maximum_bin_gap: 16
|
maximum_bin_gap: 16
|
||||||
# 可恢复的采样失败自动重扫当前方向;超过次数才暂停等待人工处理。
|
# 可恢复的采样失败自动重扫当前方向;超过次数才暂停等待人工处理。
|
||||||
automatic_sweep_retry_limit: 2
|
automatic_sweep_retry_limit: 1
|
||||||
# 轨迹拟合失败优先只重扫失败轮次;零位/URDF模型失败不重复运动。
|
# 轨迹拟合失败优先只重扫失败轮次;零位/URDF模型失败不重复运动。
|
||||||
automatic_fit_retry_limit: 2
|
automatic_fit_retry_limit: 0
|
||||||
automatic_motion_retry_limit: 2
|
automatic_motion_retry_limit: 0
|
||||||
# 留空为正式标定;设为pinky/ring/middle/index时只采该指正面+侧面roll,
|
# 留空为正式标定;设为pinky/ring/middle/index时只采该指正面+侧面roll,
|
||||||
# 即使正面baseline回差失败也继续完成侧面对照,并永久锁定本会话URDF发布。
|
# 即使正面baseline回差失败也继续完成侧面对照,并永久锁定本会话URDF发布。
|
||||||
cross_view_roll_diagnostic_finger: ""
|
cross_view_roll_diagnostic_finger: ""
|
||||||
# 过程检查允许25%的黄色预警带,最终验收仍使用下面的严格门限。
|
# 过程检查允许25%的黄色预警带,最终验收仍使用下面的严格门限。
|
||||||
provisional_warning_ratio: 1.25
|
provisional_warning_ratio: 1.25
|
||||||
retry_minimum_speed: 3
|
retry_minimum_speed: 3
|
||||||
retry_speed_scales: [0.8, 0.6]
|
retry_speed_scales: [1.0]
|
||||||
retry_endpoint_hold_seconds: [0.75, 1.0]
|
retry_endpoint_hold_seconds: [0.5]
|
||||||
|
|
||||||
trajectory_maximum_plane_rms_m: 0.004
|
trajectory_maximum_plane_rms_m: 0.004
|
||||||
trajectory_maximum_radial_rms_m: 0.004
|
trajectory_maximum_radial_rms_m: 0.004
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
"""Hardware- and model-independent calibration kernel."""
|
"""Hardware- and model-independent calibration kernel."""
|
||||||
|
|
||||||
from .domain import (
|
from .domain import (
|
||||||
|
AcquisitionPolicy,
|
||||||
ArtifactPolicy,
|
ArtifactPolicy,
|
||||||
CalibrationProfile,
|
CalibrationProfile,
|
||||||
CommandLayout,
|
CommandLayout,
|
||||||
@@ -40,6 +41,7 @@ from .geometry import (
|
|||||||
)
|
)
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
|
"AcquisitionPolicy",
|
||||||
"ArtifactPolicy",
|
"ArtifactPolicy",
|
||||||
"CalibrationProfile",
|
"CalibrationProfile",
|
||||||
"CommandLayout",
|
"CommandLayout",
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
"""Calibration domain types."""
|
"""Calibration domain types."""
|
||||||
|
|
||||||
from .profile import (
|
from .profile import (
|
||||||
|
AcquisitionPolicy,
|
||||||
ArtifactPolicy,
|
ArtifactPolicy,
|
||||||
CalibrationProfile,
|
CalibrationProfile,
|
||||||
CommandLayout,
|
CommandLayout,
|
||||||
@@ -21,6 +22,7 @@ from .profile import (
|
|||||||
from .sample import SampleRecord
|
from .sample import SampleRecord
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
|
"AcquisitionPolicy",
|
||||||
"ArtifactPolicy",
|
"ArtifactPolicy",
|
||||||
"CalibrationProfile",
|
"CalibrationProfile",
|
||||||
"CommandLayout",
|
"CommandLayout",
|
||||||
|
|||||||
@@ -66,6 +66,13 @@ class CommandLayout:
|
|||||||
baseline: tuple[float, ...] = ()
|
baseline: tuple[float, ...] = ()
|
||||||
lower_bounds: tuple[float, ...] = ()
|
lower_bounds: tuple[float, ...] = ()
|
||||||
upper_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
|
feedback_by_index: bool = False
|
||||||
|
|
||||||
@property
|
@property
|
||||||
@@ -93,6 +100,22 @@ class CommandLayout:
|
|||||||
else (255.0,) * self.command_count
|
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:
|
def normalize(self, index: int, value: float) -> float:
|
||||||
lower = self.minimum_values[int(index)]
|
lower = self.minimum_values[int(index)]
|
||||||
upper = self.maximum_values[int(index)]
|
upper = self.maximum_values[int(index)]
|
||||||
@@ -237,6 +260,24 @@ class QualityPolicy:
|
|||||||
isolated_holdout: bool = False
|
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)
|
@dataclass(frozen=True)
|
||||||
class ScopePolicy:
|
class ScopePolicy:
|
||||||
calibrate_joints: Mapping[str, frozenset[str]]
|
calibrate_joints: Mapping[str, frozenset[str]]
|
||||||
@@ -273,6 +314,7 @@ class CalibrationProfile:
|
|||||||
quality: QualityPolicy
|
quality: QualityPolicy
|
||||||
scope: ScopePolicy
|
scope: ScopePolicy
|
||||||
artifacts: ArtifactPolicy
|
artifacts: ArtifactPolicy
|
||||||
|
acquisition: AcquisitionPolicy = field(default_factory=AcquisitionPolicy)
|
||||||
# Per-URDF-joint provenance used by partial calibration artifacts.
|
# Per-URDF-joint provenance used by partial calibration artifacts.
|
||||||
# Known values are: measured_static_dynamic, measured_dynamic_cad_static,
|
# Known values are: measured_static_dynamic, measured_dynamic_cad_static,
|
||||||
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
|
# 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")
|
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))
|
indices = set(range(command.command_count))
|
||||||
if not set(command.disabled_indices).issubset(indices):
|
if not set(command.disabled_indices).issubset(indices):
|
||||||
errors.append("disabled command index is out of range")
|
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")
|
errors.append("coupling model target has no source mapping")
|
||||||
if not set(zero.coupling_model_by_joint.values()).issubset(
|
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")
|
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")
|
errors.append(f"{label} filename must not contain a directory")
|
||||||
if not profile.namespace.startswith("/"):
|
if not profile.namespace.startswith("/"):
|
||||||
errors.append("runtime namespace must be absolute")
|
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:
|
if profile.joint_coverage:
|
||||||
valid_coverage = {
|
valid_coverage = {
|
||||||
"measured_static_dynamic",
|
"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
|
projections = matrix @ axis
|
||||||
low = projections[command_values <= 16]
|
low = projections[command_values <= 16]
|
||||||
high = projections[command_values >= 239]
|
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)):
|
if low.size and high.size and float(np.median(low)) < float(np.median(high)):
|
||||||
axis = -axis
|
axis = -axis
|
||||||
return axis / np.linalg.norm(axis)
|
return axis / np.linalg.norm(axis)
|
||||||
|
|||||||
@@ -7,6 +7,7 @@ from .patch import (
|
|||||||
UrdfPatchSet,
|
UrdfPatchSet,
|
||||||
apply_urdf_patch_text,
|
apply_urdf_patch_text,
|
||||||
materialize_relative_mesh_assets,
|
materialize_relative_mesh_assets,
|
||||||
|
validate_urdf_mimic_ranges,
|
||||||
write_urdf_patches,
|
write_urdf_patches,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -18,5 +19,6 @@ __all__ = [
|
|||||||
"apply_urdf_patch_text",
|
"apply_urdf_patch_text",
|
||||||
"build_correction_plan",
|
"build_correction_plan",
|
||||||
"materialize_relative_mesh_assets",
|
"materialize_relative_mesh_assets",
|
||||||
|
"validate_urdf_mimic_ranges",
|
||||||
"write_urdf_patches",
|
"write_urdf_patches",
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -9,6 +9,7 @@ publishing a new file.
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
|
import math
|
||||||
import os
|
import os
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
import re
|
import re
|
||||||
@@ -191,6 +192,99 @@ def _files_have_identical_contents(left: Path, right: Path) -> bool:
|
|||||||
return True
|
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(
|
def materialize_relative_mesh_assets(
|
||||||
*, source: Path, output: Path, urdf_root: ET.Element
|
*, source: Path, output: Path, urdf_root: ET.Element
|
||||||
) -> tuple[Path, ...]:
|
) -> tuple[Path, ...]:
|
||||||
@@ -311,5 +405,6 @@ __all__ = [
|
|||||||
"UrdfPatchSet",
|
"UrdfPatchSet",
|
||||||
"apply_urdf_patch_text",
|
"apply_urdf_patch_text",
|
||||||
"materialize_relative_mesh_assets",
|
"materialize_relative_mesh_assets",
|
||||||
|
"validate_urdf_mimic_ranges",
|
||||||
"write_urdf_patches",
|
"write_urdf_patches",
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -33,7 +33,6 @@ _HARD_THRESHOLD_KEYS = frozenset(
|
|||||||
"maximum_reprojection_error_px",
|
"maximum_reprojection_error_px",
|
||||||
"maximum_axis_cycle_difference_rad",
|
"maximum_axis_cycle_difference_rad",
|
||||||
"maximum_pose_line_rms_m",
|
"maximum_pose_line_rms_m",
|
||||||
"maximum_hysteresis_rad",
|
|
||||||
"maximum_validation_error_rad",
|
"maximum_validation_error_rad",
|
||||||
}
|
}
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -40,6 +40,8 @@ from ...core import (
|
|||||||
robust_rotation_summary,
|
robust_rotation_summary,
|
||||||
)
|
)
|
||||||
from ...core.urdf import build_correction_plan
|
from ...core.urdf import build_correction_plan
|
||||||
|
from ...runtime import ACQUISITION_POLICY_VERSION, CalibrationEngine
|
||||||
|
from ...runtime.adapters import ProfileSdkAdapter
|
||||||
from ...extrinsics import (
|
from ...extrinsics import (
|
||||||
ThreeCameraExtrinsics,
|
ThreeCameraExtrinsics,
|
||||||
camera_info_fingerprint,
|
camera_info_fingerprint,
|
||||||
@@ -2628,8 +2630,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.declare_parameter("pnp_group_initialization_frames", 8)
|
self.declare_parameter("pnp_group_initialization_frames", 8)
|
||||||
self.declare_parameter("pnp_group_normal_alignment_scale_deg", 5.0)
|
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("pnp_group_maximum_normal_alignment_deg", 15.0)
|
||||||
self.declare_parameter("fixed_base_maximum_corner_drift_px", 2.0)
|
self.declare_parameter("fixed_base_maximum_corner_drift_px", 5.0)
|
||||||
self.declare_parameter("fixed_base_movement_confirmation_frames", 5)
|
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_multiplier", 1.03)
|
||||||
self.declare_parameter("thumb_ip_pnp_coupling_scale_deg", 3.0)
|
self.declare_parameter("thumb_ip_pnp_coupling_scale_deg", 3.0)
|
||||||
self.declare_parameter(
|
self.declare_parameter(
|
||||||
@@ -2686,7 +2688,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.declare_parameter("task_precheck_hold_seconds", 2.0)
|
self.declare_parameter("task_precheck_hold_seconds", 2.0)
|
||||||
self.declare_parameter("position_timeout_seconds", 30.0)
|
self.declare_parameter("position_timeout_seconds", 30.0)
|
||||||
self.declare_parameter("sweep_timeout_seconds", 90.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_startup_grace_seconds", 1.0)
|
||||||
self.declare_parameter("motor_stall_minimum_progress_u8", 1.0)
|
self.declare_parameter("motor_stall_minimum_progress_u8", 1.0)
|
||||||
self.declare_parameter("invalid_timeout_seconds", 3.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_state_span_u8", 240.0)
|
||||||
self.declare_parameter("minimum_sweep_bins", 32)
|
self.declare_parameter("minimum_sweep_bins", 32)
|
||||||
self.declare_parameter("maximum_bin_gap", 16)
|
self.declare_parameter("maximum_bin_gap", 16)
|
||||||
self.declare_parameter("automatic_sweep_retry_limit", 3)
|
self.declare_parameter("automatic_sweep_retry_limit", 1)
|
||||||
self.declare_parameter("automatic_fit_retry_limit", 2)
|
self.declare_parameter("automatic_fit_retry_limit", 0)
|
||||||
self.declare_parameter("automatic_motion_retry_limit", 2)
|
self.declare_parameter("automatic_motion_retry_limit", 0)
|
||||||
self.declare_parameter("cross_view_roll_diagnostic_finger", "")
|
self.declare_parameter("cross_view_roll_diagnostic_finger", "")
|
||||||
self.declare_parameter("provisional_warning_ratio", 1.25)
|
self.declare_parameter("provisional_warning_ratio", 1.25)
|
||||||
self.declare_parameter("retry_minimum_speed", 3)
|
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(
|
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_plane_rms_m", 0.004)
|
||||||
self.declare_parameter("trajectory_maximum_radial_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.profile = product_contract.profile
|
||||||
self.zero_profile = product_contract.zero_profile
|
self.zero_profile = product_contract.zero_profile
|
||||||
self.calibration_profile = product_contract.typed_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"))
|
self.serial_number = str(value("serial_number"))
|
||||||
if self.serial_number == "UNSET":
|
if self.serial_number == "UNSET":
|
||||||
raise ValueError("serial_number is required")
|
raise ValueError("serial_number is required")
|
||||||
@@ -4066,16 +4070,34 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
|
|
||||||
def _state_callback(self, message: JointState) -> None:
|
def _state_callback(self, message: JointState) -> None:
|
||||||
command_names = _command_names(self)
|
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
|
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)
|
stamp = _stamp_ns(message.header.stamp)
|
||||||
if stamp <= 0:
|
if stamp <= 0:
|
||||||
stamp = int(self.get_clock().now().nanoseconds)
|
stamp = int(self.get_clock().now().nanoseconds)
|
||||||
@@ -5291,6 +5313,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
if len(starts) != 1:
|
if len(starts) != 1:
|
||||||
raise RuntimeError("resume raw must contain exactly one session_start")
|
raise RuntimeError("resume raw must contain exactly one session_start")
|
||||||
start = starts[0]
|
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 = {
|
previous_tag_sizes = {
|
||||||
int(tag_id): float(size)
|
int(tag_id): float(size)
|
||||||
for tag_id, size in dict(
|
for tag_id, size in dict(
|
||||||
@@ -5800,6 +5827,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.raw_path,
|
self.raw_path,
|
||||||
{
|
{
|
||||||
"kind": "session_start",
|
"kind": "session_start",
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
"model": self.model,
|
"model": self.model,
|
||||||
"hand_type": self.hand_type,
|
"hand_type": self.hand_type,
|
||||||
"tag_layout": self.profile.layout_id,
|
"tag_layout": self.profile.layout_id,
|
||||||
@@ -6149,7 +6177,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
retry = self.sweep_retry_counts.get(key, 0)
|
retry = self.sweep_retry_counts.get(key, 0)
|
||||||
if retry:
|
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 = [
|
speeds = [
|
||||||
max(self.retry_minimum_speed, int(round(speed * scale)))
|
max(self.retry_minimum_speed, int(round(speed * scale)))
|
||||||
for speed in speeds
|
for speed in speeds
|
||||||
@@ -7002,10 +7030,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
retry = self.sweep_retry_counts.get(key, 0)
|
retry = self.sweep_retry_counts.get(key, 0)
|
||||||
if retry <= 0:
|
if retry <= 0:
|
||||||
return float(self.sweep_timeout_seconds)
|
return float(self.sweep_timeout_seconds)
|
||||||
scale = self.retry_speed_scales[
|
# unified_engine_v1 retries at the original speed.
|
||||||
min(retry, len(self.retry_speed_scales)) - 1
|
return float(self.sweep_timeout_seconds)
|
||||||
]
|
|
||||||
return float(self.sweep_timeout_seconds) / float(scale)
|
|
||||||
|
|
||||||
def _endpoint_tolerance_for_spec(
|
def _endpoint_tolerance_for_spec(
|
||||||
self, spec: SweepSpec, endpoint_u8: int
|
self, spec: SweepSpec, endpoint_u8: int
|
||||||
@@ -9238,9 +9264,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
band_attempt = int(
|
band_attempt = int(
|
||||||
self.sweep_attempts.get(_sweep_storage_key(spec), 1)
|
self.sweep_attempts.get(_sweep_storage_key(spec), 1)
|
||||||
)
|
)
|
||||||
retry_limit = int(
|
retry_limit = 0
|
||||||
getattr(self, "automatic_fit_retry_limit", 0)
|
|
||||||
)
|
|
||||||
# The final fit re-applies the unmodified hard thresholds to the
|
# The final fit re-applies the unmodified hard thresholds to the
|
||||||
# same records. Letting even a tiny overrun continue would make
|
# same records. Letting even a tiny overrun continue would make
|
||||||
# the final fit recall this task after every later joint has been
|
# 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)
|
_fit_failure_is_systematic(failures, self.repetitions)
|
||||||
or repeated_branch_clusters
|
or repeated_branch_clusters
|
||||||
)
|
)
|
||||||
fit_retry_limit = int(
|
fit_retry_limit = 0
|
||||||
getattr(self, "automatic_fit_retry_limit", 0)
|
|
||||||
)
|
|
||||||
self.fit_failure = {
|
self.fit_failure = {
|
||||||
"kind": "fit_failure",
|
"kind": "fit_failure",
|
||||||
"view": spec.view,
|
"view": spec.view,
|
||||||
@@ -9559,8 +9581,8 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
_sweep_storage_key(item.spec), item.cycle, item.direction
|
_sweep_storage_key(item.spec), item.cycle, item.direction
|
||||||
)
|
)
|
||||||
retries = self.sweep_retry_counts.get(key, 0)
|
retries = self.sweep_retry_counts.get(key, 0)
|
||||||
retry_limit = getattr(self, "automatic_sweep_retry_limit", 0)
|
retry_limit = 1
|
||||||
if retries >= retry_limit:
|
if not CalibrationEngine.permits_retry("sweep_acquisition", retries):
|
||||||
self._pause(reason)
|
self._pause(reason)
|
||||||
return
|
return
|
||||||
retries += 1
|
retries += 1
|
||||||
@@ -9583,10 +9605,10 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self._reset_view_trackers(runtime)
|
self._reset_view_trackers(runtime)
|
||||||
runtime.pnp_reset_count += 1
|
runtime.pnp_reset_count += 1
|
||||||
speed_scales = getattr(
|
speed_scales = getattr(
|
||||||
self, "retry_speed_scales", (0.8, 0.6, 0.5)
|
self, "retry_speed_scales", (1.0,)
|
||||||
)
|
)
|
||||||
endpoint_holds = getattr(
|
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
|
# Write the invalidation before mutating the authoritative in-memory
|
||||||
# stores. Restart replays this event as a tombstone, so a crash can
|
# stores. Restart replays this event as a tombstone, so a crash can
|
||||||
@@ -10000,7 +10022,73 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
return
|
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():
|
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)
|
commands = sorted(bins)
|
||||||
if len(commands) < self.minimum_sweep_bins:
|
if len(commands) < self.minimum_sweep_bins:
|
||||||
self._retry_active_sweep_or_pause(
|
self._retry_active_sweep_or_pause(
|
||||||
@@ -11579,6 +11667,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
def _retry_combination_validation_or_pause(
|
def _retry_combination_validation_or_pause(
|
||||||
self, reason: str, now: float
|
self, reason: str, now: float
|
||||||
) -> None:
|
) -> None:
|
||||||
|
if not CalibrationEngine.permits_retry("holdout", 0):
|
||||||
|
self._pause(reason)
|
||||||
|
return
|
||||||
item = self.active_combination_validation
|
item = self.active_combination_validation
|
||||||
if item is None:
|
if item is None:
|
||||||
self._pause(reason)
|
self._pause(reason)
|
||||||
@@ -11771,6 +11862,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
name: round(float(value), 8)
|
name: round(float(value), 8)
|
||||||
for name, value in self.zero_result.all_active_offsets_rad.items()
|
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
|
urdf_offsets = published_zero_offsets
|
||||||
if self.profile.layout_id == G20_RIGHT_19_LAYOUT:
|
if self.profile.layout_id == G20_RIGHT_19_LAYOUT:
|
||||||
urdf_offsets = {
|
urdf_offsets = {
|
||||||
@@ -11962,7 +12066,30 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.reason = str(reason)
|
self.reason = str(reason)
|
||||||
self.position_hold_since = None
|
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:
|
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)
|
retries = self.motion_retry_counts.get(reason, 0)
|
||||||
if retries >= self.automatic_motion_retry_limit:
|
if retries >= self.automatic_motion_retry_limit:
|
||||||
self._pause(reason)
|
self._pause(reason)
|
||||||
@@ -12064,6 +12191,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.reason = f"automatic_retry_{reason}"
|
self.reason = f"automatic_retry_{reason}"
|
||||||
|
|
||||||
def _retry_validation_or_pause(self, reason: str, now: float) -> None:
|
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:
|
if self.active_validation is None:
|
||||||
self._pause(reason)
|
self._pause(reason)
|
||||||
return
|
return
|
||||||
@@ -12170,9 +12300,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
):
|
):
|
||||||
return
|
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:
|
if baseline_reached:
|
||||||
return_waypoints = getattr(
|
return_waypoints = getattr(
|
||||||
self, "return_waypoints", deque()
|
self, "return_waypoints", deque()
|
||||||
@@ -12249,15 +12376,12 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
),
|
),
|
||||||
)
|
)
|
||||||
reached = self._command_vector_reached(preparation_command)
|
reached = self._command_vector_reached(preparation_command)
|
||||||
if now - self.motion_stage_started_at > self.position_timeout_seconds:
|
if (
|
||||||
self._retry_motion_or_pause(
|
reached
|
||||||
(
|
and now - self.motion_stage_started_at
|
||||||
"sweep_start_tag_timeout"
|
> self.position_timeout_seconds
|
||||||
if reached
|
):
|
||||||
else "sweep_start_position_timeout"
|
self._handle_sweep_start_timeout(now, reached)
|
||||||
),
|
|
||||||
now,
|
|
||||||
)
|
|
||||||
return
|
return
|
||||||
prepare_context = (
|
prepare_context = (
|
||||||
f"prepare_motor_{self.active_sweep.spec.motor_index}"
|
f"prepare_motor_{self.active_sweep.spec.motor_index}"
|
||||||
@@ -12350,26 +12474,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
if self.state == STATE_SWEEP:
|
if self.state == STATE_SWEEP:
|
||||||
assert self.active_sweep is not None
|
assert self.active_sweep is not None
|
||||||
motor = self.active_sweep.spec.motor_index
|
motor = self.active_sweep.spec.motor_index
|
||||||
if now - self.sweep_started_at > self._active_sweep_timeout_seconds():
|
# Temporary vision/synchronisation loss only drops those frames.
|
||||||
self._retry_active_sweep_or_pause("sweep_timeout")
|
# Complete the motion and let retained-data quality request the
|
||||||
return
|
# single same-speed rescan when necessary.
|
||||||
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
|
|
||||||
checkpoint_target_value = getattr(
|
checkpoint_target_value = getattr(
|
||||||
self, "sweep_checkpoint_target_u8", None
|
self, "sweep_checkpoint_target_u8", None
|
||||||
)
|
)
|
||||||
@@ -12532,13 +12639,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.sweep_endpoint_since is not None
|
self.sweep_endpoint_since is not None
|
||||||
and now - self.sweep_endpoint_since
|
and now - self.sweep_endpoint_since
|
||||||
>= self._active_endpoint_hold_seconds()
|
>= 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()
|
self._finish_active_sweep()
|
||||||
return
|
return
|
||||||
@@ -12575,10 +12675,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
else:
|
else:
|
||||||
self.position_hold_since = None
|
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
|
return
|
||||||
assert self.active_validation is not None
|
assert self.active_validation is not None
|
||||||
reached = self._motion_command_reached(
|
reached = self._motion_command_reached(
|
||||||
@@ -12619,8 +12715,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.reason = "capturing_random_validation_pose"
|
self.reason = "capturing_random_validation_pose"
|
||||||
else:
|
else:
|
||||||
self.position_hold_since = None
|
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
|
return
|
||||||
if self.state == STATE_VALIDATION_CAPTURE:
|
if self.state == STATE_VALIDATION_CAPTURE:
|
||||||
if self.active_combination_validation is not None:
|
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,
|
palm_orientation_maximum_command_distance_u8=64,
|
||||||
capabilities=frozenset(
|
capabilities=frozenset(
|
||||||
{
|
{
|
||||||
"precheck_sweeps",
|
|
||||||
"steady_command_checkpoints",
|
"steady_command_checkpoints",
|
||||||
"directional_zero",
|
"directional_zero",
|
||||||
"isolated_holdout",
|
"isolated_holdout",
|
||||||
@@ -721,7 +720,7 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
|||||||
"palm_axis_relative_motion_v3",
|
"palm_axis_relative_motion_v3",
|
||||||
}
|
}
|
||||||
),
|
),
|
||||||
precheck_sweeps=True,
|
precheck_sweeps=False,
|
||||||
steady_command_checkpoints=True,
|
steady_command_checkpoints=True,
|
||||||
directional_zero=True,
|
directional_zero=True,
|
||||||
isolated_holdout=True,
|
isolated_holdout=True,
|
||||||
|
|||||||
@@ -29,6 +29,7 @@ from ...operator_report import (
|
|||||||
from ...product import ProductConfig, load_product_config, sha256_file
|
from ...product import ProductConfig, load_product_config, sha256_file
|
||||||
from .publication import atomic_session_pointer, finalize_session_artifacts
|
from .publication import atomic_session_pointer, finalize_session_artifacts
|
||||||
from ...storage import atomic_write_json
|
from ...storage import atomic_write_json
|
||||||
|
from ...runtime import ACQUISITION_POLICY_VERSION
|
||||||
|
|
||||||
|
|
||||||
EXIT_PASS = 0
|
EXIT_PASS = 0
|
||||||
@@ -131,7 +132,9 @@ class ProgressConsole:
|
|||||||
"\n".join(
|
"\n".join(
|
||||||
[
|
[
|
||||||
f"⚠ 当前任务出现问题:{reason.removeprefix('automatic_retry_')}",
|
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,
|
flush=True,
|
||||||
@@ -476,6 +479,8 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
|||||||
continue
|
continue
|
||||||
if (
|
if (
|
||||||
start is None
|
start is None
|
||||||
|
or start.get("acquisition_policy_version")
|
||||||
|
!= ACQUISITION_POLICY_VERSION
|
||||||
or start.get("hand_type") != config.side
|
or start.get("hand_type") != config.side
|
||||||
or start.get("tag_layout") != config.tag_layout
|
or start.get("tag_layout") != config.tag_layout
|
||||||
or start.get("source_urdf_sha256")
|
or start.get("source_urdf_sha256")
|
||||||
|
|||||||
@@ -80,6 +80,14 @@ class ZeroCalibrationProfile:
|
|||||||
# signed rotation axes to disambiguate the otherwise mirrored palm-frame
|
# signed rotation axes to disambiguate the otherwise mirrored palm-frame
|
||||||
# branches. The default remains undirected for legacy G20/L6 profiles.
|
# branches. The default remains undirected for legacy G20/L6 profiles.
|
||||||
directed_base_axis_joints: frozenset[str] = frozenset()
|
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
|
@property
|
||||||
def reference_finger(self) -> str:
|
def reference_finger(self) -> str:
|
||||||
@@ -815,7 +823,9 @@ def _canonical_reference_records(
|
|||||||
|
|
||||||
|
|
||||||
def _interpolate_reference_rotation(
|
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:
|
) -> Rotation | None:
|
||||||
by_command: dict[int, list[np.ndarray]] = {}
|
by_command: dict[int, list[np.ndarray]] = {}
|
||||||
for record in records:
|
for record in records:
|
||||||
@@ -839,7 +849,9 @@ def _interpolate_reference_rotation(
|
|||||||
if lower_command is not None and upper_command is not None:
|
if lower_command is not None and upper_command is not None:
|
||||||
lower_distance = zero - lower_command
|
lower_distance = zero - lower_command
|
||||||
upper_distance = upper_command - zero
|
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]
|
lower_rotation = rotations[lower_command]
|
||||||
upper_rotation = rotations[upper_command]
|
upper_rotation = rotations[upper_command]
|
||||||
fraction = lower_distance / (upper_command - lower_command)
|
fraction = lower_distance / (upper_command - lower_command)
|
||||||
@@ -847,7 +859,9 @@ def _interpolate_reference_rotation(
|
|||||||
return lower_rotation * Rotation.from_rotvec(delta * fraction)
|
return lower_rotation * Rotation.from_rotvec(delta * fraction)
|
||||||
|
|
||||||
nearest_command = min(rotations, key=lambda command: abs(command - zero))
|
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 rotations[nearest_command]
|
||||||
return None
|
return None
|
||||||
|
|
||||||
@@ -873,7 +887,9 @@ def _near_zero_records(
|
|||||||
|
|
||||||
|
|
||||||
def _baseline_reference(
|
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]:
|
) -> tuple[float, float, float, float]:
|
||||||
groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {}
|
groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {}
|
||||||
for record in records:
|
for record in records:
|
||||||
@@ -883,17 +899,20 @@ def _baseline_reference(
|
|||||||
for group in groups.values()
|
for group in groups.values()
|
||||||
if (
|
if (
|
||||||
rotation := _interpolate_reference_rotation(
|
rotation := _interpolate_reference_rotation(
|
||||||
group, zero_command_u8
|
group,
|
||||||
|
zero_command_u8,
|
||||||
|
maximum_distance_u8,
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
is not None
|
is not None
|
||||||
]
|
]
|
||||||
if not values:
|
if not values:
|
||||||
raise ValueError(
|
distance = (
|
||||||
"joint records have no samples within "
|
"the observed physical stroke"
|
||||||
f"{ZERO_REFERENCE_MAXIMUM_DISTANCE_U8} commands of zero "
|
if maximum_distance_u8 is None
|
||||||
f"{zero_command_u8}"
|
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]
|
return robust_rotation_summary(values)[0]
|
||||||
|
|
||||||
|
|
||||||
@@ -957,12 +976,23 @@ def fit_rotation_joint_curve(
|
|||||||
*,
|
*,
|
||||||
zero_command_u8: int,
|
zero_command_u8: int,
|
||||||
canonical_zero_direction: str | None = None,
|
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:
|
) -> JointCurveFit:
|
||||||
"""Fit a direction-aware curve from parent-to-child Tag orientations.
|
"""Fit a direction-aware curve from parent-to-child Tag orientations.
|
||||||
|
|
||||||
With ``canonical_zero_direction`` both branches share one physical
|
With ``canonical_zero_direction`` both branches share one physical
|
||||||
reference. Only the canonical branch is zero at ``zero_command_u8``;
|
reference. Only the canonical branch is zero at ``zero_command_u8``;
|
||||||
the other branch retains its measured backlash/compliance offset.
|
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]
|
samples = [dict(record) for record in records]
|
||||||
if len(samples) < 12:
|
if len(samples) < 12:
|
||||||
@@ -981,6 +1011,7 @@ def fit_rotation_joint_curve(
|
|||||||
cycle_records, canonical_zero_direction
|
cycle_records, canonical_zero_direction
|
||||||
),
|
),
|
||||||
zero_command_u8,
|
zero_command_u8,
|
||||||
|
zero_reference_maximum_distance_u8,
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
for record in samples:
|
for record in samples:
|
||||||
@@ -996,6 +1027,7 @@ def fit_rotation_joint_curve(
|
|||||||
samples,
|
samples,
|
||||||
values_by_record,
|
values_by_record,
|
||||||
preserve_direction_offset=canonical_zero_direction is not None,
|
preserve_direction_offset=canonical_zero_direction is not None,
|
||||||
|
require_observed_domain_endpoints=require_observed_domain_endpoints,
|
||||||
)
|
)
|
||||||
if canonical_zero_direction is None:
|
if canonical_zero_direction is None:
|
||||||
for key in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
for key in ("angle_rad", "decreasing_rad", "increasing_rad"):
|
||||||
@@ -1031,6 +1063,7 @@ def fit_rotation_joint_curve(
|
|||||||
samples, canonical_zero_direction
|
samples, canonical_zero_direction
|
||||||
),
|
),
|
||||||
zero_command_u8,
|
zero_command_u8,
|
||||||
|
zero_reference_maximum_distance_u8,
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
"canonical_zero_direction": canonical_zero_direction,
|
"canonical_zero_direction": canonical_zero_direction,
|
||||||
@@ -1105,6 +1138,9 @@ def rotation_curve_holdout_errors(
|
|||||||
records: Sequence[Mapping[str, Any]],
|
records: Sequence[Mapping[str, Any]],
|
||||||
*,
|
*,
|
||||||
zero_command_u8: int,
|
zero_command_u8: int,
|
||||||
|
zero_reference_maximum_distance_u8: int | None = (
|
||||||
|
ZERO_REFERENCE_MAXIMUM_DISTANCE_U8
|
||||||
|
),
|
||||||
) -> tuple[float, ...]:
|
) -> tuple[float, ...]:
|
||||||
"""Validate a fitted curve on an untouched scan cycle."""
|
"""Validate a fitted curve on an untouched scan cycle."""
|
||||||
samples = [dict(record) for record in records]
|
samples = [dict(record) for record in records]
|
||||||
@@ -1115,6 +1151,7 @@ def rotation_curve_holdout_errors(
|
|||||||
_baseline_reference(
|
_baseline_reference(
|
||||||
_canonical_reference_records(samples, canonical_zero_direction),
|
_canonical_reference_records(samples, canonical_zero_direction),
|
||||||
zero_command_u8,
|
zero_command_u8,
|
||||||
|
zero_reference_maximum_distance_u8,
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis")
|
axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis")
|
||||||
@@ -1190,6 +1227,12 @@ class JointAxisMeasurement:
|
|||||||
circle_axis_observability: float = 0.0
|
circle_axis_observability: float = 0.0
|
||||||
axis_point_source: str = "circle_center"
|
axis_point_source: str = "circle_center"
|
||||||
pose_axis_line_rms_m: float = 0.0
|
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
|
# 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
|
# 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
|
# 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,
|
view_normal_common_xyz: Sequence[float] | None,
|
||||||
canonical_zero_direction: str | None = None,
|
canonical_zero_direction: str | None = None,
|
||||||
allow_axial_translation: bool = False,
|
allow_axial_translation: bool = False,
|
||||||
|
residual_diagnostics: dict[str, float] | None = None,
|
||||||
) -> tuple[np.ndarray, float, str]:
|
) -> tuple[np.ndarray, float, str]:
|
||||||
"""Fit the closest point on a revolute axis from full relative poses.
|
"""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] = []
|
matrices: list[np.ndarray] = []
|
||||||
translations: list[np.ndarray] = []
|
translations: list[np.ndarray] = []
|
||||||
|
raw_matrices: list[np.ndarray] = []
|
||||||
|
raw_translations: list[np.ndarray] = []
|
||||||
used_image_plane_projection = False
|
used_image_plane_projection = False
|
||||||
raw_angles: list[float] = []
|
raw_angles: list[float] = []
|
||||||
translation_angles: list[float] = []
|
translation_angles: list[float] = []
|
||||||
@@ -2100,6 +2146,8 @@ def _fit_axis_point_from_pose_trajectory(
|
|||||||
)
|
)
|
||||||
projection = axis_projection
|
projection = axis_projection
|
||||||
axis_point_matrix = (np.eye(3) - delta_rotation) @ basis
|
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:
|
if use_image_plane_projection:
|
||||||
assert view_normal_common is not None
|
assert view_normal_common is not None
|
||||||
parent_rotation = Rotation.from_matrix(
|
parent_rotation = Rotation.from_matrix(
|
||||||
@@ -2151,6 +2199,17 @@ def _fit_axis_point_from_pose_trajectory(
|
|||||||
)
|
)
|
||||||
residual = matrix @ solution.x - translation
|
residual = matrix @ solution.x - translation
|
||||||
rms = float(np.sqrt(np.mean(np.square(residual))))
|
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 = (
|
source = (
|
||||||
"pose_trajectory_image_plane"
|
"pose_trajectory_image_plane"
|
||||||
if used_image_plane_projection
|
if used_image_plane_projection
|
||||||
@@ -2169,6 +2228,7 @@ def fit_joint_axis_measurement(
|
|||||||
constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS,
|
constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS,
|
||||||
view_normal_common_xyz: Sequence[float] | None = None,
|
view_normal_common_xyz: Sequence[float] | None = None,
|
||||||
canonical_zero_direction: str | None = None,
|
canonical_zero_direction: str | None = None,
|
||||||
|
separate_axial_residual: bool = False,
|
||||||
) -> JointAxisMeasurement:
|
) -> JointAxisMeasurement:
|
||||||
"""Fit one physical screw axis from one complete scan cycle."""
|
"""Fit one physical screw axis from one complete scan cycle."""
|
||||||
samples = [
|
samples = [
|
||||||
@@ -2267,6 +2327,8 @@ def fit_joint_axis_measurement(
|
|||||||
)
|
)
|
||||||
axis_direction_source = "rotation_circle_fusion"
|
axis_direction_source = "rotation_circle_fusion"
|
||||||
axis_common = parent_rotation.apply(fitted_axis)
|
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 = (
|
point_parent, pose_axis_line_rms_m, axis_point_source = (
|
||||||
_fit_axis_point_from_pose_trajectory(
|
_fit_axis_point_from_pose_trajectory(
|
||||||
samples,
|
samples,
|
||||||
@@ -2276,7 +2338,8 @@ def fit_joint_axis_measurement(
|
|||||||
phase_reference_point_parent_xyz=circle["center_xyz_m"],
|
phase_reference_point_parent_xyz=circle["center_xyz_m"],
|
||||||
view_normal_common_xyz=view_normal_common_xyz,
|
view_normal_common_xyz=view_normal_common_xyz,
|
||||||
canonical_zero_direction=canonical_zero_direction,
|
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
|
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),
|
circle_axis_observability=float(circle_axis_observability),
|
||||||
axis_point_source=axis_point_source,
|
axis_point_source=axis_point_source,
|
||||||
pose_axis_line_rms_m=pose_axis_line_rms_m,
|
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),),
|
pose_axis_line_source_joints=(str(joint),),
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -2493,6 +2560,9 @@ class ZeroSolveResult:
|
|||||||
axis_cone_mismatch_by_joint_rad: Mapping[str, float]
|
axis_cone_mismatch_by_joint_rad: Mapping[str, float]
|
||||||
axis_cone_bias_classification_by_joint: Mapping[str, str]
|
axis_cone_bias_classification_by_joint: Mapping[str, str]
|
||||||
failure_reasons: 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(
|
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(
|
if abs(float(parent_axis @ view_normal)) >= math.cos(
|
||||||
math.radians(45.0)
|
math.radians(45.0)
|
||||||
):
|
):
|
||||||
# Orthographic image-plane displacement is invariant to
|
# Use an image-plane phase for an end-on observation.
|
||||||
# independent optical-depth bias on the two planar Tags.
|
# Projection alone removes optical depth, but does NOT
|
||||||
# With an end-on axis, along-axis placement also projects
|
# remove an arbitrary along-axis point coordinate when
|
||||||
# to zero (or a small component for a mildly oblique view).
|
# the camera is even slightly oblique to the axis.
|
||||||
predicted_radial = predicted_delta - view_normal * float(
|
# A closest point is defined relative to its parent Tag
|
||||||
predicted_delta @ view_normal
|
# 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_image_input = (
|
||||||
observed_delta @ view_normal
|
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
|
angle_axis = view_normal
|
||||||
if float(angle_axis @ parent_axis) < 0.0:
|
if float(angle_axis @ parent_axis) < 0.0:
|
||||||
@@ -4008,7 +4091,13 @@ def solve_urdf_zero_offsets(
|
|||||||
<= max(significance_sigma * uncertainty, confidence_half_width)
|
<= max(significance_sigma * uncertainty, confidence_half_width)
|
||||||
):
|
):
|
||||||
applied_training[name] = 0.0
|
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)
|
insignificant_large.append(name)
|
||||||
else:
|
else:
|
||||||
applied_training[name] = value
|
applied_training[name] = value
|
||||||
|
|||||||
@@ -393,8 +393,43 @@ def fit_coupling_model(
|
|||||||
maximum_residual_p95_rad: float = math.radians(2.0),
|
maximum_residual_p95_rad: float = math.radians(2.0),
|
||||||
maximum_residual_rad: float = math.radians(3.0),
|
maximum_residual_rad: float = math.radians(3.0),
|
||||||
) -> MimicFit:
|
) -> 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}")
|
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
|
degree = 1 if model == "linear_mimic" else 2
|
||||||
active = np.concatenate(
|
active = np.concatenate(
|
||||||
(
|
(
|
||||||
@@ -556,8 +591,6 @@ def fit_l6_session(
|
|||||||
)
|
)
|
||||||
if fit.maximum_monotonic_correction_rad > correction_limit:
|
if fit.maximum_monotonic_correction_rad > correction_limit:
|
||||||
raise ValueError(f"{name} monotonic correction exceeds 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
|
curves[name] = fit
|
||||||
holdout[name] = errors
|
holdout[name] = errors
|
||||||
cycle_curves[name] = {
|
cycle_curves[name] = {
|
||||||
|
|||||||
@@ -7,6 +7,7 @@ from dataclasses import dataclass
|
|||||||
import json
|
import json
|
||||||
import math
|
import math
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
import threading
|
||||||
import time
|
import time
|
||||||
import traceback
|
import traceback
|
||||||
from typing import Any
|
from typing import Any
|
||||||
@@ -14,6 +15,7 @@ from typing import Any
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
import rclpy
|
import rclpy
|
||||||
from apriltag_msgs.msg import AprilTagDetectionArray
|
from apriltag_msgs.msg import AprilTagDetectionArray
|
||||||
|
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
from rclpy.qos import qos_profile_sensor_data
|
from rclpy.qos import qos_profile_sensor_data
|
||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
@@ -29,6 +31,8 @@ from ...extrinsics import (
|
|||||||
)
|
)
|
||||||
from ...pnp import SquareTagPose, SquareTagPoseTracker
|
from ...pnp import SquareTagPose, SquareTagPoseTracker
|
||||||
from ...storage import append_jsonl, atomic_write_json
|
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 .motion import cosine_position_trajectory_u8
|
||||||
from .pipeline import finalize_l6_session
|
from .pipeline import finalize_l6_session
|
||||||
from .profile import build_typed_profile
|
from .profile import build_typed_profile
|
||||||
@@ -58,6 +62,11 @@ def _stamp_ns(stamp: Any) -> int:
|
|||||||
class L6ThreeCameraCalibrationNode(Node):
|
class L6ThreeCameraCalibrationNode(Node):
|
||||||
"""Own one reviewed six-channel partial-calibration session."""
|
"""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__(
|
def __init__(
|
||||||
self,
|
self,
|
||||||
*,
|
*,
|
||||||
@@ -73,10 +82,47 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
self.baseline_command = tuple(self.profile.command.baseline_values)
|
self.baseline_command = tuple(self.profile.command.baseline_values)
|
||||||
self.command_lower = tuple(self.profile.command.minimum_values)
|
self.command_lower = tuple(self.profile.command.minimum_values)
|
||||||
self.command_upper = tuple(self.profile.command.maximum_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.sample_kind = str(sample_kind)
|
||||||
self.sweep_quality_kind = f"{self.model_name.lower()}_sweep_observation_quality"
|
self.sweep_quality_kind = f"{self.model_name.lower()}_sweep_observation_quality"
|
||||||
self.finalize_session = finalizer or finalize_l6_session
|
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")
|
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._declare_parameters()
|
||||||
self._load_parameters()
|
self._load_parameters()
|
||||||
self.session_dir.mkdir(parents=True, exist_ok=True)
|
self.session_dir.mkdir(parents=True, exist_ok=True)
|
||||||
@@ -89,6 +135,11 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
"profile_id": self.profile.key.profile_id,
|
"profile_id": self.profile.key.profile_id,
|
||||||
"serial_number": self.serial_number,
|
"serial_number": self.serial_number,
|
||||||
"curve_input_domain": f"feedback_{self.command_unit}",
|
"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
|
String, f"{self.profile.namespace}/status", 10
|
||||||
)
|
)
|
||||||
self.create_subscription(
|
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.camera_matrices: dict[str, np.ndarray] = {}
|
||||||
self.image_sizes: dict[str, tuple[int, int]] = {}
|
self.image_sizes: dict[str, tuple[int, int]] = {}
|
||||||
@@ -132,21 +184,34 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
selected, message
|
selected, message
|
||||||
),
|
),
|
||||||
qos_profile_sensor_data,
|
qos_profile_sensor_data,
|
||||||
|
callback_group=self.vision_callback_groups.get(view),
|
||||||
)
|
)
|
||||||
self.create_subscription(
|
self.create_subscription(
|
||||||
AprilTagDetectionArray,
|
AprilTagDetectionArray,
|
||||||
self.detection_topics[view],
|
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,
|
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(
|
||||||
self.create_service(Trigger, f"{self.profile.namespace}/abort", self._abort)
|
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.latest_state_u8: tuple[float, ...] = ()
|
||||||
self.state_history: deque[StateSample] = deque(maxlen=2000)
|
self.state_history: deque[StateSample] = deque(maxlen=2000)
|
||||||
self.state_receive_times: deque[float] = deque(maxlen=300)
|
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.raw_records: list[dict[str, Any]] = []
|
||||||
self.steps: list[MotionStep] = []
|
self.steps: list[MotionStep] = []
|
||||||
self.step_index = -1
|
self.step_index = -1
|
||||||
@@ -161,8 +226,11 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
self.step_speed_ready_at = 0.0
|
self.step_speed_ready_at = 0.0
|
||||||
self.step_requested_u8 = float("nan")
|
self.step_requested_u8 = float("nan")
|
||||||
self.step_start_state_u8: tuple[float, ...] = ()
|
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_phase = 0.0
|
||||||
|
self.step_trajectory_blend = 0.0
|
||||||
self.step_trajectory_duration_seconds = 0.0
|
self.step_trajectory_duration_seconds = 0.0
|
||||||
self.step_moving_indices: frozenset[int] = frozenset()
|
self.step_moving_indices: frozenset[int] = frozenset()
|
||||||
self.step_valid_frames = 0
|
self.step_valid_frames = 0
|
||||||
@@ -185,6 +253,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
1.0 / float(self.command_rate_hz)
|
1.0 / float(self.command_rate_hz)
|
||||||
if self.command_unit == "rad" else 0.01,
|
if self.command_unit == "rad" else 0.01,
|
||||||
self._tick,
|
self._tick,
|
||||||
|
callback_group=self.motion_callback_group,
|
||||||
)
|
)
|
||||||
self.create_timer(0.5, self._publish_status)
|
self.create_timer(0.5, self._publish_status)
|
||||||
|
|
||||||
@@ -202,6 +271,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
"calibration_config_expected_sha256": "",
|
"calibration_config_expected_sha256": "",
|
||||||
"tag_config_expected_sha256": "",
|
"tag_config_expected_sha256": "",
|
||||||
"sdk_config_expected_sha256": "",
|
"sdk_config_expected_sha256": "",
|
||||||
|
"resume_raw_samples_path": "",
|
||||||
"command_topic": f"/{model}/cb_right_hand_control_cmd",
|
"command_topic": f"/{model}/cb_right_hand_control_cmd",
|
||||||
"state_topic": f"/{model}/cb_right_hand_state",
|
"state_topic": f"/{model}/cb_right_hand_state",
|
||||||
"setting_topic": f"/{model}/cb_hand_setting_cmd",
|
"setting_topic": f"/{model}/cb_hand_setting_cmd",
|
||||||
@@ -231,7 +301,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
"motor_stall_timeout_seconds": 2.0,
|
"motor_stall_timeout_seconds": 2.0,
|
||||||
"position_timeout_seconds": 30.0,
|
"position_timeout_seconds": 30.0,
|
||||||
"sweep_timeout_seconds": 90.0,
|
"sweep_timeout_seconds": 90.0,
|
||||||
"automatic_sweep_retry_limit": 2,
|
"automatic_sweep_retry_limit": 1,
|
||||||
"non_target_motion_tolerance_u8": 3.0,
|
"non_target_motion_tolerance_u8": 3.0,
|
||||||
"minimum_sweep_frames": 40,
|
"minimum_sweep_frames": 40,
|
||||||
"minimum_state_span_u8": 240.0,
|
"minimum_state_span_u8": 240.0,
|
||||||
@@ -247,8 +317,8 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
"maximum_hamming": 0,
|
"maximum_hamming": 0,
|
||||||
"minimum_decision_margin": 30.0,
|
"minimum_decision_margin": 30.0,
|
||||||
"minimum_edge_pixels": 30.0,
|
"minimum_edge_pixels": 30.0,
|
||||||
"fixed_base_maximum_corner_drift_px": 2.0,
|
"fixed_base_maximum_corner_drift_px": 5.0,
|
||||||
"fixed_base_movement_confirmation_frames": 5,
|
"fixed_base_movement_confirmation_frames": 10,
|
||||||
"tag_size_m": 0.016,
|
"tag_size_m": 0.016,
|
||||||
"pnp_maximum_reprojection_error_px": 1.5,
|
"pnp_maximum_reprojection_error_px": 1.5,
|
||||||
"pnp_reprojection_tie_px": 1.5,
|
"pnp_reprojection_tie_px": 1.5,
|
||||||
@@ -299,6 +369,12 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
self.protected_inputs["sdk_config_sha256"] = str(
|
self.protected_inputs["sdk_config_sha256"] = str(
|
||||||
value("sdk_config_expected_sha256")
|
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 (
|
if (
|
||||||
set(self.protected_inputs)
|
set(self.protected_inputs)
|
||||||
!= self.profile.artifacts.protected_input_fields
|
!= self.profile.artifacts.protected_input_fields
|
||||||
@@ -371,31 +447,17 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
raise ValueError("minimum_joint_frame_rate must be in (0, 1]")
|
raise ValueError("minimum_joint_frame_rate must be in (0, 1]")
|
||||||
|
|
||||||
def _build_steps(self) -> list[MotionStep]:
|
def _build_steps(self) -> list[MotionStep]:
|
||||||
command_unit = getattr(self, "command_unit", "u8")
|
|
||||||
steps = [
|
steps = [
|
||||||
MotionStep(
|
MotionStep(
|
||||||
"baseline", None, None, 255, int(self.baseline_speed_u8)
|
"baseline", None, None, 255, int(self.baseline_speed_u8)
|
||||||
)
|
)
|
||||||
]
|
]
|
||||||
for task in self.profile.motion.tasks:
|
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 = (
|
formal_speed = (
|
||||||
task.formal_speed
|
task.formal_speed
|
||||||
if command_unit == "rad"
|
if getattr(self, "command_unit", self.profile.command.unit) == "rad"
|
||||||
else self.formal_speed_u8
|
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):
|
for cycle in (0, 1, 2, 3):
|
||||||
steps.extend(
|
steps.extend(
|
||||||
[
|
[
|
||||||
@@ -428,8 +490,9 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
|
|
||||||
def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
|
def _abort(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
|
||||||
hold = (
|
hold = (
|
||||||
list(self.latest_state_u8)
|
list(self.last_published_command_u8)
|
||||||
if self.command_unit == "rad" and len(self.latest_state_u8) == self.command_count
|
if self.command_unit == "rad"
|
||||||
|
and self.last_published_command_u8 is not None
|
||||||
else list(self.baseline_command)
|
else list(self.baseline_command)
|
||||||
)
|
)
|
||||||
self._publish_command(hold)
|
self._publish_command(hold)
|
||||||
@@ -444,32 +507,60 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
return response
|
return response
|
||||||
|
|
||||||
def _camera_info_callback(self, view: str, message: CameraInfo) -> None:
|
def _camera_info_callback(self, view: str, message: CameraInfo) -> None:
|
||||||
matrix = np.asarray(message.k, dtype=float).reshape(3, 3)
|
from ...core.geometry.camera import rectified_camera_matrix
|
||||||
if np.all(np.isfinite(matrix)) and matrix[0, 0] > 0 and matrix[1, 1] > 0:
|
try:
|
||||||
self.camera_matrices[view] = matrix
|
matrix = rectified_camera_matrix(message.p)
|
||||||
self.image_sizes[view] = (int(message.width), int(message.height))
|
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:
|
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
|
return
|
||||||
if message.name and not self.profile.command.feedback_by_index:
|
violation = next((
|
||||||
by_name = dict(zip((str(name) for name in message.name), message.position))
|
(index, value)
|
||||||
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
|
|
||||||
for index, value in enumerate(state)
|
for index, value in enumerate(state)
|
||||||
):
|
if value < self.feedback_lower[index]
|
||||||
self._pause("feedback_outside_registered_command_domain")
|
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
|
return
|
||||||
stamp = _stamp_ns(message.header.stamp)
|
stamp = _stamp_ns(message.header.stamp)
|
||||||
if stamp <= 0:
|
if stamp <= 0:
|
||||||
@@ -478,22 +569,9 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
self.state_history.append(StateSample(stamp, state))
|
self.state_history.append(StateSample(stamp, state))
|
||||||
self.latest_state_u8 = state
|
self.latest_state_u8 = state
|
||||||
self.state_receive_times.append(time.monotonic())
|
self.state_receive_times.append(time.monotonic())
|
||||||
step = self._current_step()
|
# Non-target motion is retained in every sample for diagnostics and
|
||||||
if step is not None and step.task_key is not None:
|
# coupling fitting. It is not a runtime stop condition: tendon hands
|
||||||
for index, actual in enumerate(state):
|
# legitimately back-drive neighbouring SDK coordinates.
|
||||||
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
|
|
||||||
|
|
||||||
def _task(self, key: str):
|
def _task(self, key: str):
|
||||||
return next(task for task in self.profile.motion.tasks if task.key == key)
|
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()
|
step = self._current_step()
|
||||||
recording_this_view = bool(
|
recording_this_view = bool(
|
||||||
step is not None
|
step is not None
|
||||||
|
and self.step_command_sent
|
||||||
and step.recording
|
and step.recording
|
||||||
and self._task(str(step.task_key)).view == view
|
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}"
|
f"fixed_base_tag_moved:{view}:drift_px={drift:.3f}"
|
||||||
)
|
)
|
||||||
return
|
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
|
return
|
||||||
stamp = _stamp_ns(message.header.stamp)
|
stamp = _stamp_ns(message.header.stamp)
|
||||||
selected: dict[str, SquareTagPose] = {}
|
selected: dict[str, SquareTagPose] = dict(locked_reference_poses)
|
||||||
for role in roles:
|
# 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(
|
pose, pnp_reason = self.trackers[view].estimate(
|
||||||
role,
|
role,
|
||||||
corners[role],
|
corners[role],
|
||||||
@@ -734,6 +850,10 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
return
|
return
|
||||||
selected[role] = pose
|
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:
|
if recording_this_view:
|
||||||
self.step_pnp_valid_frames += 1
|
self.step_pnp_valid_frames += 1
|
||||||
self.last_view_valid_at[view] = time.monotonic()
|
self.last_view_valid_at[view] = time.monotonic()
|
||||||
@@ -754,12 +874,16 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
self.step_state_sync_frames += 1
|
self.step_state_sync_frames += 1
|
||||||
task = self._task(step.task_key)
|
task = self._task(step.task_key)
|
||||||
feedback = float(state_u8[task.command_index])
|
feedback = float(state_u8[task.command_index])
|
||||||
lower = self.command_lower[task.command_index]
|
lower = self.feedback_lower[task.command_index]
|
||||||
upper = self.command_upper[task.command_index]
|
upper = self.feedback_upper[task.command_index]
|
||||||
if not lower <= feedback <= upper:
|
if not lower <= feedback <= upper:
|
||||||
self._count_step_rejection("feedback:outside_registered_domain")
|
self._count_step_rejection("feedback:outside_registered_feedback_domain")
|
||||||
return
|
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:
|
for joint in task.joints:
|
||||||
measurement = self.profile.measurement.measurements[joint]
|
measurement = self.profile.measurement.measurements[joint]
|
||||||
parent = selected[str(measurement.parent_role)]
|
parent = selected[str(measurement.parent_role)]
|
||||||
@@ -840,14 +964,34 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
})
|
})
|
||||||
self.raw_records.append(record)
|
self.raw_records.append(record)
|
||||||
append_jsonl(self.raw_path, 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
|
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:
|
def _feedback_hz(self) -> float:
|
||||||
if len(self.state_receive_times) < 2:
|
if len(self.state_receive_times) < 2:
|
||||||
return 0.0
|
return 0.0
|
||||||
elapsed = self.state_receive_times[-1] - self.state_receive_times[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
|
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:
|
def _publish_torque(self) -> None:
|
||||||
if not self.commands_enabled or self.command_unit != "u8":
|
if not self.commands_enabled or self.command_unit != "u8":
|
||||||
return
|
return
|
||||||
@@ -892,6 +1036,8 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
message.position = bounded
|
message.position = bounded
|
||||||
self.command_publisher.publish(message)
|
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, ...]:
|
def _target_command(self, step: MotionStep) -> tuple[float, ...]:
|
||||||
if step.target_command is not None:
|
if step.target_command is not None:
|
||||||
@@ -907,6 +1053,15 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
return tuple(target)
|
return tuple(target)
|
||||||
|
|
||||||
def _begin_step(self, step: MotionStep) -> None:
|
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()
|
now = time.monotonic()
|
||||||
if self.commanded_speed != step.speed_u8:
|
if self.commanded_speed != step.speed_u8:
|
||||||
self._publish_speed(step.speed_u8)
|
self._publish_speed(step.speed_u8)
|
||||||
@@ -917,7 +1072,18 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
return
|
return
|
||||||
if len(self.latest_state_u8) != self.command_count:
|
if len(self.latest_state_u8) != self.command_count:
|
||||||
return
|
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_started_at = now
|
||||||
self.step_last_progress_at = now
|
self.step_last_progress_at = now
|
||||||
self.step_last_feedback = (
|
self.step_last_feedback = (
|
||||||
@@ -940,17 +1106,21 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
index for index, error in enumerate(errors)
|
index for index, error in enumerate(errors)
|
||||||
if error > float(self.non_target_motion_tolerance_u8)
|
if error > float(self.non_target_motion_tolerance_u8)
|
||||||
)
|
)
|
||||||
self.step_last_distance_u8 = (
|
command_distance = (
|
||||||
float(sum(errors))
|
float(sum(errors))
|
||||||
if step.command_index is None
|
if step.command_index is None
|
||||||
else float(errors[step.command_index])
|
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)
|
maximum_distance = max(errors)
|
||||||
if self.command_unit == "rad":
|
if self.command_unit == "rad":
|
||||||
self.step_trajectory_duration_seconds = (
|
_, _, self.step_trajectory_duration_seconds = (
|
||||||
0.0 if maximum_distance <= 0.0 else
|
self._radian_trajectory_fraction(
|
||||||
math.pi * maximum_distance / (2.0 * float(step.speed_u8))
|
maximum_distance, 0.0, float(step.speed_u8)
|
||||||
|
)
|
||||||
)
|
)
|
||||||
else:
|
else:
|
||||||
_, _, self.step_trajectory_duration_seconds = cosine_position_trajectory_u8(
|
_, _, self.step_trajectory_duration_seconds = cosine_position_trajectory_u8(
|
||||||
@@ -958,6 +1128,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
float(self.command_trajectory_full_range_seconds),
|
float(self.command_trajectory_full_range_seconds),
|
||||||
)
|
)
|
||||||
self.step_trajectory_phase = 0.0
|
self.step_trajectory_phase = 0.0
|
||||||
|
self.step_trajectory_blend = 0.0
|
||||||
self.step_last_command_u8 = None
|
self.step_last_command_u8 = None
|
||||||
self.step_hold_since = None
|
self.step_hold_since = None
|
||||||
roles: tuple[str, ...] = ()
|
roles: tuple[str, ...] = ()
|
||||||
@@ -971,6 +1142,16 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
f"direction={step.direction}:target={step.target_u8}:attempt={step.attempt}"
|
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:
|
def _advance_step_trajectory(self, step: MotionStep, now: float) -> None:
|
||||||
if hasattr(self, "_target_command"):
|
if hasattr(self, "_target_command"):
|
||||||
target_values = self._target_command(step)
|
target_values = self._target_command(step)
|
||||||
@@ -981,15 +1162,24 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
target[step.command_index] = float(step.target_u8)
|
target[step.command_index] = float(step.target_u8)
|
||||||
target_values = tuple(target)
|
target_values = tuple(target)
|
||||||
elapsed = max(0.0, float(now) - self.step_started_at)
|
elapsed = max(0.0, float(now) - self.step_started_at)
|
||||||
duration = float(
|
if getattr(self, "command_unit", "u8") == "rad":
|
||||||
getattr(
|
maximum_distance = max(
|
||||||
self,
|
abs(target - start)
|
||||||
"step_trajectory_duration_seconds",
|
for start, target in zip(self.step_start_state_u8, target_values)
|
||||||
self.command_trajectory_full_range_seconds,
|
|
||||||
)
|
)
|
||||||
)
|
blend, phase, duration = self._radian_trajectory_fraction(
|
||||||
phase = 1.0 if duration <= 0.0 else min(1.0, elapsed / duration)
|
maximum_distance, elapsed, float(step.speed_u8)
|
||||||
blend = 0.5 - 0.5 * math.cos(math.pi * phase)
|
)
|
||||||
|
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 = [
|
values = [
|
||||||
start + (target - start) * blend
|
start + (target - start) * blend
|
||||||
for start, target in zip(self.step_start_state_u8, target_values)
|
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)
|
for index, value in enumerate(values)
|
||||||
)
|
)
|
||||||
self.step_trajectory_phase = phase
|
self.step_trajectory_phase = phase
|
||||||
|
self.step_trajectory_blend = blend
|
||||||
self.step_requested_u8 = (
|
self.step_requested_u8 = (
|
||||||
float(np.mean(values))
|
float(np.mean(values))
|
||||||
if step.command_index is None
|
if step.command_index is None
|
||||||
else float(values[step.command_index])
|
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._publish_command(list(command))
|
||||||
self.step_last_command_u8 = 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)
|
gap = max((right - left for left, right in zip(bins, bins[1:])), default=256)
|
||||||
maximum_gap = float(self.maximum_bin_gap)
|
maximum_gap = float(self.maximum_bin_gap)
|
||||||
else:
|
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(
|
normalized = sorted(
|
||||||
set(
|
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
|
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]))
|
float(np.ptp([self.profile.command.normalize(task.command_index, value) for value in feedback]))
|
||||||
if feedback.size else 0.0
|
if feedback.size else 0.0
|
||||||
)
|
)
|
||||||
required_span = float(self.minimum_state_span_u8)
|
span_policy = getattr(
|
||||||
gap = max((right - left for left, right in zip(bins, bins[1:])), default=32)
|
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)
|
maximum_gap = float(self.maximum_bin_gap)
|
||||||
observation = self._step_observation_metrics()
|
observation = self._step_observation_metrics()
|
||||||
detection_rate = float(observation["tag_detection_rate"])
|
detection_rate = float(observation["tag_detection_rate"])
|
||||||
joint_frame_rate = float(observation["joint_frame_rate"])
|
joint_frame_rate = float(observation["joint_frame_rate"])
|
||||||
failures = []
|
progress = (
|
||||||
if len(rows) < int(self.minimum_sweep_frames):
|
[self.profile.command.normalize(task.command_index, value) for value in feedback]
|
||||||
failures.append(f"frames={len(rows)}")
|
if command_unit == "rad"
|
||||||
if feedback.size == 0 or span < required_span:
|
else [float(value) / 255.0 for value in feedback]
|
||||||
failures.append("feedback_span")
|
)
|
||||||
if len(bins) < int(self.minimum_sweep_bins):
|
engine = getattr(self, "calibration_engine", CalibrationEngine(self.profile))
|
||||||
failures.append(f"bins={len(bins)}")
|
decision = engine.evaluate_sweep(
|
||||||
if gap > maximum_gap:
|
progress,
|
||||||
failures.append(f"maximum_gap={gap}")
|
minimum_span=(required_span if command_unit == "rad" else required_span / 255.0),
|
||||||
if detection_rate < float(self.minimum_detection_rate):
|
total_frames=self.step_total_frames,
|
||||||
role_rates = observation["tag_detection_rate_by_role"]
|
joint_frame_rate=joint_frame_rate,
|
||||||
worst_role = min(role_rates, key=role_rates.get, default="unknown")
|
feedback_hz=self._feedback_hz(),
|
||||||
tag_id = next(
|
detection_rate=detection_rate,
|
||||||
(
|
bin_count=(
|
||||||
tag.tag_id
|
int(getattr(self, "normalized_sweep_bin_count", 256))
|
||||||
for view in self.profile.vision.views
|
if command_unit == "rad" else 256
|
||||||
for tag in view.tags
|
),
|
||||||
if tag.role == worst_role
|
)
|
||||||
),
|
failures = list(decision.failures)
|
||||||
"?",
|
|
||||||
)
|
|
||||||
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}")
|
|
||||||
append_jsonl(
|
append_jsonl(
|
||||||
self.raw_path,
|
self.raw_path,
|
||||||
{
|
{
|
||||||
@@ -1088,8 +1303,12 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
"valid_frames": len(rows),
|
"valid_frames": len(rows),
|
||||||
"total_frames": self.step_total_frames,
|
"total_frames": self.step_total_frames,
|
||||||
"feedback_bins": len(bins),
|
"feedback_bins": len(bins),
|
||||||
|
"feedback_span": round(span, 9),
|
||||||
|
"required_feedback_span": round(required_span, 9),
|
||||||
"maximum_bin_gap": gap,
|
"maximum_bin_gap": gap,
|
||||||
**observation,
|
**observation,
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
|
"warnings": list(decision.warnings),
|
||||||
"failures": failures,
|
"failures": failures,
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
@@ -1099,16 +1318,22 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
def _retry_step(self, step: MotionStep, reason: str) -> bool:
|
def _retry_step(self, step: MotionStep, reason: str) -> bool:
|
||||||
key = (str(step.task_key), int(step.cycle), str(step.direction))
|
key = (str(step.task_key), int(step.cycle), str(step.direction))
|
||||||
retries = self.retry_counts.get(key, 0)
|
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
|
return False
|
||||||
attempt = retries + 2
|
attempt = retries + 2
|
||||||
self.retry_counts[key] = retries + 1
|
self.retry_counts[key] = retries + 1
|
||||||
task = self._task(str(step.task_key))
|
task = self._task(str(step.task_key))
|
||||||
start = task.start_value if step.direction == "decreasing" else task.end_value
|
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 = [
|
replacement = [
|
||||||
MotionStep("retry_prepare", step.task_key, step.command_index, start, retry_speed, step.cycle, attempt=attempt),
|
step_type("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("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
|
self.steps[self.step_index + 1:self.step_index + 1] = replacement
|
||||||
append_jsonl(
|
append_jsonl(
|
||||||
@@ -1126,6 +1351,15 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
return True
|
return True
|
||||||
|
|
||||||
def _finish_step(self, step: MotionStep) -> None:
|
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:
|
if step.recording:
|
||||||
try:
|
try:
|
||||||
self._qualify_recording_step(step)
|
self._qualify_recording_step(step)
|
||||||
@@ -1138,6 +1372,77 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
if self.step_index >= len(self.steps):
|
if self.step_index >= len(self.steps):
|
||||||
self._finalize()
|
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:
|
def _tick(self) -> None:
|
||||||
if self.state in {"PASSED", "PAUSED", "ABORTED"}:
|
if self.state in {"PASSED", "PAUSED", "ABORTED"}:
|
||||||
return
|
return
|
||||||
@@ -1168,12 +1473,14 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
return
|
return
|
||||||
now = time.monotonic()
|
now = time.monotonic()
|
||||||
self._advance_step_trajectory(step, now)
|
self._advance_step_trajectory(step, now)
|
||||||
timeout = self.sweep_timeout_seconds if step.recording else self.position_timeout_seconds
|
# Absolute duration is diagnostic only. A slow but continuously
|
||||||
if now - self.step_started_at > float(timeout):
|
# progressing joint is valid; the two-second no-progress watchdog is
|
||||||
self._pause(f"motion_timeout:{self.reason}")
|
# the motion safety gate.
|
||||||
return
|
|
||||||
if len(self.latest_state_u8) != self.command_count:
|
if len(self.latest_state_u8) != self.command_count:
|
||||||
return
|
return
|
||||||
|
if self.command_unit == "rad":
|
||||||
|
self._tick_radian_motion(step, now)
|
||||||
|
return
|
||||||
target_state = list(self._target_command(step))
|
target_state = list(self._target_command(step))
|
||||||
endpoint_errors = [
|
endpoint_errors = [
|
||||||
abs(value - target_state[index])
|
abs(value - target_state[index])
|
||||||
@@ -1220,10 +1527,6 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
if self.step_trajectory_phase < 1.0:
|
if self.step_trajectory_phase < 1.0:
|
||||||
self.step_hold_since = None
|
self.step_hold_since = None
|
||||||
return
|
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:
|
if self.step_hold_since is None:
|
||||||
self.step_hold_since = now
|
self.step_hold_since = now
|
||||||
return
|
return
|
||||||
@@ -1273,9 +1576,14 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
return
|
return
|
||||||
self.state = "PAUSED"
|
self.state = "PAUSED"
|
||||||
self.reason = str(reason)
|
self.reason = str(reason)
|
||||||
# Holding the latest position is safer than issuing an automatic move
|
# In radian mode, hold the last command rather than feeding an
|
||||||
# after a stall, unexpected motor motion, or moved palm reference.
|
# uncalibrated feedback value back into the command domain.
|
||||||
if len(self.latest_state_u8) == self.command_count:
|
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(
|
self._publish_command(
|
||||||
[
|
[
|
||||||
int(np.clip(round(value), 0, 255))
|
int(np.clip(round(value), 0, 255))
|
||||||
@@ -1283,7 +1591,32 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
for value in self.latest_state_u8
|
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]:
|
def _status(self) -> dict[str, Any]:
|
||||||
step = self._current_step()
|
step = self._current_step()
|
||||||
@@ -1305,16 +1638,19 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
]
|
]
|
||||||
step_fraction = 0.0
|
step_fraction = 0.0
|
||||||
if step is not None and math.isfinite(feedback):
|
if step is not None and math.isfinite(feedback):
|
||||||
current_distance = (
|
if self.command_unit == "rad":
|
||||||
float(sum(channel_errors))
|
step_fraction = self.step_trajectory_blend
|
||||||
if step.command_index is None
|
else:
|
||||||
else channel_errors[step.command_index]
|
current_distance = (
|
||||||
)
|
float(sum(channel_errors))
|
||||||
initial_distance = self.step_initial_distance_u8
|
if step.command_index is None
|
||||||
if math.isfinite(initial_distance) and initial_distance > 0.0:
|
else channel_errors[step.command_index]
|
||||||
step_fraction = 1.0 - current_distance / initial_distance
|
)
|
||||||
elif current_distance <= float(self.endpoint_tolerance_u8):
|
initial_distance = self.step_initial_distance_u8
|
||||||
step_fraction = 1.0
|
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))
|
step_fraction = float(np.clip(step_fraction, 0.0, 1.0))
|
||||||
elif self.state == "PASSED":
|
elif self.state == "PASSED":
|
||||||
step_fraction = 1.0
|
step_fraction = 1.0
|
||||||
@@ -1393,6 +1729,7 @@ class L6ThreeCameraCalibrationNode(Node):
|
|||||||
self.fixed_base_maximum_corner_drift_px
|
self.fixed_base_maximum_corner_drift_px
|
||||||
),
|
),
|
||||||
"feedback_hz": round(self._feedback_hz(), 3),
|
"feedback_hz": round(self._feedback_hz(), 3),
|
||||||
|
"command_publish_hz": round(self._command_hz(), 3),
|
||||||
"camera_info_views": sorted(self.camera_matrices),
|
"camera_info_views": sorted(self.camera_matrices),
|
||||||
"state_publisher_count": self.count_publishers(self.state_topic),
|
"state_publisher_count": self.count_publishers(self.state_topic),
|
||||||
"command_publisher_count": self.count_publishers(self.command_topic),
|
"command_publisher_count": self.count_publishers(self.command_topic),
|
||||||
|
|||||||
@@ -8,6 +8,8 @@ import math
|
|||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Any, Mapping, Sequence
|
from typing import Any, Mapping, Sequence
|
||||||
|
|
||||||
|
from ...runtime.engine import CalibrationEngine
|
||||||
|
|
||||||
from .artifacts import (
|
from .artifacts import (
|
||||||
artifact_hashes,
|
artifact_hashes,
|
||||||
atomic_write_json,
|
atomic_write_json,
|
||||||
@@ -23,6 +25,7 @@ from .profile import (
|
|||||||
MEASURED_PASSIVE_JOINTS,
|
MEASURED_PASSIVE_JOINTS,
|
||||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||||
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
||||||
|
build_typed_profile,
|
||||||
)
|
)
|
||||||
from .urdf import L6UrdfCorrection, write_l6_corrected_urdf
|
from .urdf import L6UrdfCorrection, write_l6_corrected_urdf
|
||||||
|
|
||||||
@@ -148,6 +151,13 @@ def finalize_l6_session(
|
|||||||
accepted_records_by_joint(records),
|
accepted_records_by_joint(records),
|
||||||
require_thumb_axis_zero=True,
|
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
|
# Validate the complete runtime schema before materializing any corrected
|
||||||
# URDF. A fit/schema rejection therefore leaves only the node's failure
|
# URDF. A fit/schema rejection therefore leaves only the node's failure
|
||||||
# diagnostic and the immutable raw samples.
|
# diagnostic and the immutable raw samples.
|
||||||
|
|||||||
@@ -272,8 +272,8 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
),
|
),
|
||||||
motion=MotionPolicy(
|
motion=MotionPolicy(
|
||||||
tasks=tasks,
|
tasks=tasks,
|
||||||
precheck_sweeps=True,
|
precheck_sweeps=False,
|
||||||
steady_command_checkpoints=True,
|
steady_command_checkpoints=False,
|
||||||
speed_parameters={
|
speed_parameters={
|
||||||
"preflight_u8": 1,
|
"preflight_u8": 1,
|
||||||
"formal_u8": 1,
|
"formal_u8": 1,
|
||||||
@@ -313,7 +313,6 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
{
|
{
|
||||||
"minimum_detection_rate",
|
"minimum_detection_rate",
|
||||||
"maximum_state_image_skew_ms",
|
"maximum_state_image_skew_ms",
|
||||||
"maximum_hysteresis_rad",
|
|
||||||
"maximum_validation_error_rad",
|
"maximum_validation_error_rad",
|
||||||
"maximum_mimic_residual_rad",
|
"maximum_mimic_residual_rad",
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -33,6 +33,7 @@ _STATE_LABELS = {
|
|||||||
"WAIT_DEVICES": "等待六通道反馈和三相机内参",
|
"WAIT_DEVICES": "等待六通道反馈和三相机内参",
|
||||||
"READY": "设备就绪",
|
"READY": "设备就绪",
|
||||||
"RUNNING": "标定中",
|
"RUNNING": "标定中",
|
||||||
|
"FINALIZING": "拟合、验证并生成 URDF",
|
||||||
"PASSED": "通过",
|
"PASSED": "通过",
|
||||||
"PAUSED": "已暂停",
|
"PAUSED": "已暂停",
|
||||||
"ABORTED": "已中止",
|
"ABORTED": "已中止",
|
||||||
@@ -47,6 +48,7 @@ _PHASE_LABELS = {
|
|||||||
"preflight": "任务运动预检",
|
"preflight": "任务运动预检",
|
||||||
"prepare": "扫描起点准备",
|
"prepare": "扫描起点准备",
|
||||||
"retry_prepare": "自动重扫起点准备",
|
"retry_prepare": "自动重扫起点准备",
|
||||||
|
"resume_prepare": "断点恢复起点准备",
|
||||||
"sweep": "正式扫描",
|
"sweep": "正式扫描",
|
||||||
"clearance": "手指避让",
|
"clearance": "手指避让",
|
||||||
"clearance_outer": "小指/无名指避让",
|
"clearance_outer": "小指/无名指避让",
|
||||||
@@ -111,12 +113,6 @@ def _l6_reason_zh(
|
|||||||
"查看会话诊断中的具体 frames/bins/maximum_gap/tag_rate;先处理遮挡或"
|
"查看会话诊断中的具体 frames/bins/maximum_gap/tag_rate;先处理遮挡或"
|
||||||
"反馈采样问题,再重新开始。程序已禁止发布本次结果。",
|
"反馈采样问题,再重新开始。程序已禁止发布本次结果。",
|
||||||
)
|
)
|
||||||
if reason.startswith("non_target_motor_moved:"):
|
|
||||||
return (
|
|
||||||
"MOTION-NONTARGET-304",
|
|
||||||
"扫描期间检测到非目标电机离开保持位置。",
|
|
||||||
"停止其他控制节点并检查机械耦合或反馈通道顺序,确认后重新开始。",
|
|
||||||
)
|
|
||||||
if reason.startswith("multiple_state_publishers:"):
|
if reason.startswith("multiple_state_publishers:"):
|
||||||
count = status.get("state_publisher_count", "?")
|
count = status.get("state_publisher_count", "?")
|
||||||
return (
|
return (
|
||||||
@@ -275,28 +271,38 @@ def render_six_channel_progress_zh(
|
|||||||
)
|
)
|
||||||
lines.append(
|
lines.append(
|
||||||
f"运动:峰值 {float(speed_rad_s):.3f} rad/s;"
|
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", [])
|
command_names = status.get("command_names", [])
|
||||||
if (
|
if (
|
||||||
isinstance(latest_state, (list, tuple))
|
isinstance(latest_state, (list, tuple))
|
||||||
and isinstance(command_names, (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"})
|
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(
|
feedback_text = ", ".join(
|
||||||
f"{name}={float(value):.1f}"
|
f"{name}={float(value):.{digits}f}{suffix}"
|
||||||
for name, value in zip(command_names, latest_state)
|
for name, value in zip(command_names, latest_state)
|
||||||
)
|
)
|
||||||
maximum_error_channel = status.get("maximum_error_channel")
|
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 = (
|
error_text = (
|
||||||
"未知"
|
"未知"
|
||||||
if maximum_error_channel is None or maximum_error is None
|
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"}:
|
if state in {"PAUSED", "ABORTED"}:
|
||||||
_code, problem, suggestion = reason_renderer(status)
|
_code, problem, suggestion = reason_renderer(status)
|
||||||
lines.extend((f"原因:{problem}", f"建议:{suggestion}"))
|
lines.extend((f"原因:{problem}", f"建议:{suggestion}"))
|
||||||
@@ -367,6 +373,7 @@ def _launch_command(
|
|||||||
record_bag: bool,
|
record_bag: bool,
|
||||||
commands_enabled: bool,
|
commands_enabled: bool,
|
||||||
sdk_startup_speed_u8: int = 1,
|
sdk_startup_speed_u8: int = 1,
|
||||||
|
resume_from: Path | None = None,
|
||||||
) -> list[str]:
|
) -> list[str]:
|
||||||
arguments = {
|
arguments = {
|
||||||
"model": config.model,
|
"model": config.model,
|
||||||
@@ -393,6 +400,10 @@ def _launch_command(
|
|||||||
"commands_enabled": str(commands_enabled).lower(),
|
"commands_enabled": str(commands_enabled).lower(),
|
||||||
"record_bag": str(record_bag).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.
|
# HCAN/ZLG vendor products do not have a Linux SocketCAN interface.
|
||||||
# Omitting the launch override also avoids the invalid token
|
# Omitting the launch override also avoids the invalid token
|
||||||
# ``can_interface:=`` when the reviewed product value is intentionally empty.
|
# ``can_interface:=`` when the reviewed product value is intentionally empty.
|
||||||
|
|||||||
@@ -2,14 +2,21 @@
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import asdict
|
||||||
import math
|
import math
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Any, Mapping, Sequence
|
from typing import Any, Mapping, Sequence
|
||||||
import xml.etree.ElementTree as ET
|
import xml.etree.ElementTree as ET
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
from .fitting import O12FitResult, curve_input_knots_rad, curve_values_at_rad
|
from .fitting import O12FitResult, curve_input_knots_rad, curve_values_at_rad
|
||||||
|
from .kinematics import (
|
||||||
|
PASSIVE_POLYNOMIAL_BY_JOINT,
|
||||||
|
PASSIVE_SDK_SOURCE_BY_JOINT,
|
||||||
|
vendor_passive_curve,
|
||||||
|
)
|
||||||
from .profile import (
|
from .profile import (
|
||||||
ACTIVE_JOINTS,
|
ACTIVE_JOINTS,
|
||||||
COMMAND_INDEX_BY_JOINT,
|
COMMAND_INDEX_BY_JOINT,
|
||||||
@@ -22,10 +29,14 @@ from .profile import (
|
|||||||
SAFE_UPPER_RAD,
|
SAFE_UPPER_RAD,
|
||||||
SDK_LOWER_RAD,
|
SDK_LOWER_RAD,
|
||||||
SDK_TO_URDF_JOINT,
|
SDK_TO_URDF_JOINT,
|
||||||
|
SDK_TO_URDF_SIGN,
|
||||||
SDK_UPPER_RAD,
|
SDK_UPPER_RAD,
|
||||||
|
STATIC_ZERO_EXCLUDED_JOINTS,
|
||||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||||
build_typed_profile,
|
build_typed_profile,
|
||||||
)
|
)
|
||||||
|
from .urdf import o12_endpoint_mimic_contract
|
||||||
|
from .zero import SPATIAL_ZERO_POLICY
|
||||||
|
|
||||||
|
|
||||||
ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS)
|
ALL_REVOLUTE_JOINTS = frozenset(ACTIVE_JOINTS + PASSIVE_JOINTS)
|
||||||
@@ -102,48 +113,107 @@ def build_o12_runtime_payload(
|
|||||||
passed: bool,
|
passed: bool,
|
||||||
) -> dict[str, Any]:
|
) -> dict[str, Any]:
|
||||||
profile = build_typed_profile()
|
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)
|
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]]] = {}
|
curves: dict[str, tuple[np.ndarray, np.ndarray, np.ndarray, list[float]]] = {}
|
||||||
joints: dict[str, dict[str, Any]] = {}
|
joints: dict[str, dict[str, Any]] = {}
|
||||||
|
|
||||||
for name in ACTIVE_JOINTS:
|
for name in ACTIVE_JOINTS:
|
||||||
donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
|
donor = TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.get(name, name)
|
||||||
fit = result.curves[donor]
|
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(
|
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(
|
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)
|
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)
|
curves[name] = (angle, decreasing, increasing, inputs)
|
||||||
motor = COMMAND_INDEX_BY_JOINT[name]
|
|
||||||
joint: dict[str, Any] = {
|
joint: dict[str, Any] = {
|
||||||
"urdf_joint": name,
|
"urdf_joint": name,
|
||||||
"sdk_channel": COMMAND_NAMES[motor],
|
"sdk_channel": COMMAND_NAMES[motor],
|
||||||
"motor_index": motor,
|
"motor_index": motor,
|
||||||
"passive": False,
|
"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),
|
"curve_input_knots_rad": _round(inputs),
|
||||||
"angle_rad": _round(angle),
|
"angle_rad": _round(angle),
|
||||||
"decreasing_rad": _round(decreasing),
|
"decreasing_rad": _round(decreasing),
|
||||||
"increasing_rad": _round(increasing),
|
"increasing_rad": _round(increasing),
|
||||||
"zero_feedback_rad": 0.0,
|
"zero_feedback_rad": 0.0,
|
||||||
"urdf_sign_adapter": (
|
"measured_feedback_domain_rad": [
|
||||||
"sdk_negative_to_urdf_positive"
|
round(float(value), 10) for value in feedback_domain
|
||||||
if SDK_UPPER_RAD[motor] <= 0.0 and SDK_LOWER_RAD[motor] < 0.0
|
],
|
||||||
else "identity"
|
"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": (
|
"raw_increasing_curve_branch": (
|
||||||
"decreasing"
|
"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"
|
else "increasing"
|
||||||
),
|
),
|
||||||
}
|
}
|
||||||
if donor != name:
|
if donor != name:
|
||||||
joint["transferred_from_joint"] = donor
|
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
|
joints[name] = joint
|
||||||
|
|
||||||
for name in PASSIVE_JOINTS:
|
for name in PASSIVE_JOINTS:
|
||||||
@@ -152,20 +222,33 @@ def build_o12_runtime_payload(
|
|||||||
motor = _motor_index(source_name)
|
motor = _motor_index(source_name)
|
||||||
if name in MEASURED_PASSIVE_JOINTS:
|
if name in MEASURED_PASSIVE_JOINTS:
|
||||||
fit = result.curves[name]
|
fit = result.curves[name]
|
||||||
inputs = list(curve_input_knots_rad(name))
|
feedback_domain = result.feedback_domains_rad[name]
|
||||||
decreasing = _zeroed(
|
inputs = list(curve_input_knots_rad(
|
||||||
curve_values_at_rad(name, fit, inputs, "decreasing_rad"), inputs
|
name, feedback_domain_rad=feedback_domain
|
||||||
) + float(metadata["offset"])
|
))
|
||||||
increasing = _zeroed(
|
sdk_source = PASSIVE_SDK_SOURCE_BY_JOINT[name]
|
||||||
curve_values_at_rad(name, fit, inputs, "increasing_rad"), inputs
|
sdk_motor = _motor_index(sdk_source)
|
||||||
) + float(metadata["offset"])
|
expected_direction = float(SDK_TO_URDF_SIGN[sdk_motor])
|
||||||
angle = 0.5 * (decreasing + increasing)
|
vendor_values = np.asarray(
|
||||||
coupling = result.mimic_fits[name]
|
vendor_passive_curve(
|
||||||
coefficients = [float(metadata["offset"]), *coupling.coefficients]
|
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)))
|
coefficients.extend([0.0] * (6 - len(coefficients)))
|
||||||
coupling_model = coupling.model
|
coupling_model = "vendor_o12_polynomial"
|
||||||
multiplier = coupling.urdf_mimic_multiplier
|
multiplier = endpoint_mimics[name]
|
||||||
policy = coupling.urdf_mimic_policy
|
policy = "source_cad_closed_endpoint"
|
||||||
status = profile.joint_coverage[name]
|
status = profile.joint_coverage[name]
|
||||||
else:
|
else:
|
||||||
source_curve, source_dec, source_inc, inputs = curves[source_name]
|
source_curve, source_dec, source_inc, inputs = curves[source_name]
|
||||||
@@ -179,14 +262,14 @@ def build_o12_runtime_payload(
|
|||||||
policy = "cad_nominal_preserved"
|
policy = "cad_nominal_preserved"
|
||||||
status = profile.joint_coverage[name]
|
status = profile.joint_coverage[name]
|
||||||
curves[name] = (angle, decreasing, increasing, list(inputs))
|
curves[name] = (angle, decreasing, increasing, list(inputs))
|
||||||
joints[name] = {
|
joint = {
|
||||||
"urdf_joint": name,
|
"urdf_joint": name,
|
||||||
"sdk_channel": COMMAND_NAMES[motor],
|
"sdk_channel": COMMAND_NAMES[motor],
|
||||||
"motor_index": motor,
|
"motor_index": motor,
|
||||||
"passive": True,
|
"passive": True,
|
||||||
"source_joint": source_name,
|
"source_joint": source_name,
|
||||||
"calibration_status": status,
|
"calibration_status": status,
|
||||||
"static_zero_policy": "cad_preserved_tag_mount_ambiguous",
|
"static_zero_policy": "cad_mechanical_endpoint",
|
||||||
"curve_input_knots_rad": _round(inputs),
|
"curve_input_knots_rad": _round(inputs),
|
||||||
"angle_rad": _round(angle),
|
"angle_rad": _round(angle),
|
||||||
"decreasing_rad": _round(decreasing),
|
"decreasing_rad": _round(decreasing),
|
||||||
@@ -195,12 +278,37 @@ def build_o12_runtime_payload(
|
|||||||
"coupling_coefficients": [round(float(v), 10) for v in coefficients],
|
"coupling_coefficients": [round(float(v), 10) for v in coefficients],
|
||||||
"mimic_multiplier": round(float(multiplier), 10),
|
"mimic_multiplier": round(float(multiplier), 10),
|
||||||
"urdf_mimic_policy": policy,
|
"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": (
|
"raw_increasing_curve_branch": (
|
||||||
"decreasing"
|
"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"
|
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([
|
errors = np.abs(np.concatenate([
|
||||||
np.asarray(values, dtype=float)
|
np.asarray(values, dtype=float)
|
||||||
@@ -213,7 +321,7 @@ def build_o12_runtime_payload(
|
|||||||
"model": "O12",
|
"model": "O12",
|
||||||
"side": "right",
|
"side": "right",
|
||||||
"serial_number": str(serial_number),
|
"serial_number": str(serial_number),
|
||||||
"calibration_scope": "full",
|
"calibration_scope": "full_dynamic_except_thumb_mcp_static_zero",
|
||||||
"publication_pointer": "latest_passed",
|
"publication_pointer": "latest_passed",
|
||||||
"angle_unit": "rad",
|
"angle_unit": "rad",
|
||||||
"curve_input_domain": "feedback_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_mae_rad": round(float(np.mean(errors)), 10),
|
||||||
"validation_p95_rad": round(float(np.percentile(errors, 95.0)), 10),
|
"validation_p95_rad": round(float(np.percentile(errors, 95.0)), 10),
|
||||||
"validation_max_rad": round(float(np.max(errors)), 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",
|
"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,
|
"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)
|
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")
|
raise ValueError(f"{name} has invalid O12 motor binding")
|
||||||
if joint.get("raw_increasing_curve_branch") not in {"decreasing", "increasing"}:
|
if joint.get("raw_increasing_curve_branch") not in {"decreasing", "increasing"}:
|
||||||
raise ValueError(f"{name} has invalid O12 direction binding")
|
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")):
|
if not bool(payload["quality"].get("passed")):
|
||||||
raise ValueError("failed O12 calibration cannot be published")
|
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 __future__ import annotations
|
||||||
|
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass, replace
|
||||||
import math
|
import math
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Any, Mapping, Sequence
|
from typing import Any, Mapping, Sequence
|
||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
from ..g20.profile import JointCurveFit
|
from ..g20.profile import (
|
||||||
from ..g20.zero_solver import fit_rotation_joint_curve, rotation_curve_holdout_errors
|
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 ..l6.fitting import MimicFit, fit_coupling_model
|
||||||
from .profile import (
|
from .profile import (
|
||||||
CALIBRATED_ACTIVE_JOINTS,
|
CALIBRATED_ACTIVE_JOINTS,
|
||||||
COMMAND_INDEX_BY_JOINT,
|
COMMAND_INDEX_BY_JOINT,
|
||||||
|
COMMAND_NAMES,
|
||||||
|
GEOMETRIC_ZERO_JOINTS,
|
||||||
|
ROOT_GEOMETRIC_ZERO_JOINTS,
|
||||||
MEASURED_PASSIVE_JOINTS,
|
MEASURED_PASSIVE_JOINTS,
|
||||||
MIMIC_SOURCE_BY_JOINT,
|
MIMIC_SOURCE_BY_JOINT,
|
||||||
|
SDK_TO_URDF_JOINT,
|
||||||
|
SDK_TO_URDF_SIGN,
|
||||||
|
STATIC_ZERO_EXCLUDED_JOINTS,
|
||||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||||
build_typed_profile,
|
build_typed_profile,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class O12FitResult:
|
class O12FitResult:
|
||||||
curves: Mapping[str, JointCurveFit]
|
curves: Mapping[str, JointCurveFit]
|
||||||
@@ -29,6 +47,86 @@ class O12FitResult:
|
|||||||
travels_rad: Mapping[str, float]
|
travels_rad: Mapping[str, float]
|
||||||
mimic_fits: Mapping[str, MimicFit]
|
mimic_fits: Mapping[str, MimicFit]
|
||||||
holdout_errors_rad: Mapping[str, tuple[float, ...]]
|
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]:
|
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:
|
def feedback_rad_to_curve_index(
|
||||||
"""Map a physical feedback angle to the fitter's normalized 255..0 axis."""
|
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)]
|
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:
|
if abs(denominator) <= 1.0e-12:
|
||||||
raise ValueError(f"O12 task {task.key} has a degenerate range")
|
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)))
|
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)]
|
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(
|
return tuple(
|
||||||
float(value)
|
float(value)
|
||||||
for value in np.linspace(
|
for value in np.linspace(
|
||||||
min(task.start_value, task.end_value),
|
lower,
|
||||||
max(task.start_value, task.end_value),
|
upper,
|
||||||
int(count),
|
int(count),
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def curve_values_at_rad(
|
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, ...]:
|
) -> tuple[float, ...]:
|
||||||
values = np.asarray(getattr(fit, branch), dtype=float)
|
values = np.asarray(getattr(fit, branch), dtype=float)
|
||||||
indices = np.arange(256, dtype=float)
|
indices = np.arange(256, dtype=float)
|
||||||
return tuple(
|
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
|
for value in inputs_rad
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
def _virtual_records(
|
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]]:
|
) -> list[dict[str, Any]]:
|
||||||
result = []
|
result = []
|
||||||
for source in rows:
|
for source in rows:
|
||||||
row = dict(source)
|
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))
|
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)
|
result.append(row)
|
||||||
return result
|
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:
|
def _travel(fit: JointCurveFit) -> float:
|
||||||
return 0.5 * (
|
return 0.5 * (
|
||||||
float(fit.decreasing_rad[0] - fit.decreasing_rad[255])
|
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(
|
def fit_o12_session(
|
||||||
source_urdf: str | Path,
|
source_urdf: str | Path,
|
||||||
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
|
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:
|
) -> 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
|
expected = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
|
||||||
if set(records_by_joint) != expected:
|
if set(records_by_joint) != expected:
|
||||||
missing = sorted(expected - set(records_by_joint))
|
missing = sorted(expected - set(records_by_joint))
|
||||||
@@ -104,16 +442,40 @@ def fit_o12_session(
|
|||||||
curves: dict[str, JointCurveFit] = {}
|
curves: dict[str, JointCurveFit] = {}
|
||||||
holdout: dict[str, tuple[float, ...]] = {}
|
holdout: dict[str, tuple[float, ...]] = {}
|
||||||
cycle_curves: dict[str, dict[int, JointCurveFit]] = {}
|
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):
|
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}]
|
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]
|
validation = [row for row in rows if int(row["cycle"]) == 3]
|
||||||
if not training or not validation:
|
if not training or not validation:
|
||||||
raise ValueError(f"{joint} is missing training or holdout records")
|
raise ValueError(f"{joint} is missing training or holdout records")
|
||||||
fit = fit_rotation_joint_curve(
|
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))
|
absolute = np.abs(np.asarray(errors, dtype=float))
|
||||||
if (
|
if (
|
||||||
float(np.mean(absolute)) > math.radians(1.0)
|
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)
|
or float(np.max(absolute)) > math.radians(3.0)
|
||||||
):
|
):
|
||||||
raise ValueError(f"{joint} isolated holdout failed")
|
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
|
curves[joint] = fit
|
||||||
holdout[joint] = tuple(float(value) for value in errors)
|
holdout[joint] = tuple(float(value) for value in errors)
|
||||||
cycle_curves[joint] = {
|
cycle_curves[joint] = {
|
||||||
@@ -130,6 +490,8 @@ def fit_o12_session(
|
|||||||
[row for row in training if int(row["cycle"]) == cycle],
|
[row for row in training if int(row["cycle"]) == cycle],
|
||||||
zero_command_u8=255,
|
zero_command_u8=255,
|
||||||
canonical_zero_direction="decreasing",
|
canonical_zero_direction="decreasing",
|
||||||
|
require_observed_domain_endpoints=False,
|
||||||
|
zero_reference_maximum_distance_u8=None,
|
||||||
)
|
)
|
||||||
for cycle in (0, 1, 2)
|
for cycle in (0, 1, 2)
|
||||||
}
|
}
|
||||||
@@ -151,31 +513,176 @@ def fit_o12_session(
|
|||||||
target,
|
target,
|
||||||
curves[source],
|
curves[source],
|
||||||
curves[target],
|
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,
|
cycle_curve_pairs=cycle_pairs,
|
||||||
minimum_multiplier=0.5,
|
minimum_multiplier=0.5,
|
||||||
maximum_multiplier=2.2,
|
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 = {
|
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():
|
for target, donor in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT.items():
|
||||||
travels[target] = travels[donor]
|
travels[target] = travels[donor]
|
||||||
# O12 active feedback is already a physical angle with open/centred zero.
|
offsets = {name: 0.0 for name in SDK_TO_URDF_JOINT}
|
||||||
# Calibration publishes the visual transfer curves and measured travel;
|
zero_methods = {name: "source_cad_zero_not_measured" for name in offsets}
|
||||||
# it must not invent a static offset from an arbitrary Tag mounting angle.
|
for name in STATIC_ZERO_EXCLUDED_JOINTS:
|
||||||
offsets = {joint: 0.0 for joint in travels}
|
zero_methods[name] = "source_cad_zero_profile_excluded"
|
||||||
return O12FitResult(
|
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,
|
curves=curves,
|
||||||
zero_offsets_rad=offsets,
|
zero_offsets_rad=offsets,
|
||||||
travels_rad=travels,
|
travels_rad=travels,
|
||||||
mimic_fits=mimic_fits,
|
mimic_fits=mimic_fits,
|
||||||
holdout_errors_rad=holdout,
|
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__ = [
|
__all__ = [
|
||||||
"O12FitResult", "curve_input_knots_rad", "curve_values_at_rad",
|
"O12FitResult", "O12_THUMB_ROOT_AXIS_JOINTS",
|
||||||
"feedback_rad_to_curve_index", "fit_o12_session",
|
"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(
|
def build_calibration_motion_command(
|
||||||
task: TaskSpec,
|
task: TaskSpec,
|
||||||
command_rad: float,
|
command_rad: float,
|
||||||
@@ -73,4 +129,5 @@ def build_calibration_return_waypoints(
|
|||||||
__all__ = [
|
__all__ = [
|
||||||
"build_calibration_motion_command", "build_calibration_preparation_waypoints",
|
"build_calibration_motion_command", "build_calibration_preparation_waypoints",
|
||||||
"build_calibration_return_waypoints", "cosine_position_trajectory_rad",
|
"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 __future__ import annotations
|
||||||
|
|
||||||
from datetime import datetime
|
from datetime import datetime
|
||||||
|
from dataclasses import asdict, replace
|
||||||
import json
|
import json
|
||||||
|
import math
|
||||||
import os
|
import os
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Any, Mapping, Sequence
|
from typing import Any, Mapping, Sequence
|
||||||
|
|
||||||
from ...product import sha256_file
|
from ...product import sha256_file
|
||||||
|
from ...runtime.engine import CalibrationEngine
|
||||||
from ...storage import atomic_write_json
|
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 .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 .urdf import O12UrdfCorrection, write_o12_corrected_urdf
|
||||||
|
from .zero import O12SpatialZeroError, SPATIAL_ZERO_POLICY
|
||||||
|
|
||||||
|
|
||||||
MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
|
MEASURED_JOINTS = CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS
|
||||||
@@ -54,6 +67,36 @@ def accepted_records_by_joint(
|
|||||||
return result
|
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]]:
|
def load_o12_raw_samples(path: str | Path) -> list[dict[str, Any]]:
|
||||||
source = Path(path).expanduser().resolve()
|
source = Path(path).expanduser().resolve()
|
||||||
if not source.is_file():
|
if not source.is_file():
|
||||||
@@ -97,7 +140,92 @@ def finalize_o12_session(
|
|||||||
) -> tuple[dict[str, Any], O12FitResult, O12UrdfCorrection]:
|
) -> tuple[dict[str, Any], O12FitResult, O12UrdfCorrection]:
|
||||||
directory = Path(session_dir).expanduser().resolve()
|
directory = Path(session_dir).expanduser().resolve()
|
||||||
directory.mkdir(parents=True, exist_ok=True)
|
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(
|
payload = build_o12_runtime_payload(
|
||||||
serial_number=serial_number,
|
serial_number=serial_number,
|
||||||
source_urdf=source_urdf,
|
source_urdf=source_urdf,
|
||||||
@@ -105,6 +233,26 @@ def finalize_o12_session(
|
|||||||
protected_inputs=protected_inputs,
|
protected_inputs=protected_inputs,
|
||||||
passed=True,
|
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(
|
correction = write_o12_corrected_urdf(
|
||||||
source_urdf=source_urdf,
|
source_urdf=source_urdf,
|
||||||
output_directory=directory,
|
output_directory=directory,
|
||||||
@@ -112,6 +260,9 @@ def finalize_o12_session(
|
|||||||
result=result,
|
result=result,
|
||||||
timestamp=timestamp or datetime.now().strftime("%Y%m%d_%H%M%S"),
|
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"
|
json_path = directory / f"o12_right_{serial_number}_calibration.json"
|
||||||
atomic_write_json(json_path, payload)
|
atomic_write_json(json_path, payload)
|
||||||
summary = {
|
summary = {
|
||||||
@@ -119,13 +270,52 @@ def finalize_o12_session(
|
|||||||
"profile_id": "O12/right/o12_right_16/v1",
|
"profile_id": "O12/right/o12_right_16/v1",
|
||||||
"serial_number": str(serial_number),
|
"serial_number": str(serial_number),
|
||||||
"result": "PASS",
|
"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_active_joints": sorted(CALIBRATED_ACTIVE_JOINTS),
|
||||||
"measured_passive_joints": sorted(MEASURED_PASSIVE_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": {
|
"ring_transfer": {
|
||||||
"source": "pinky_mcp_pitch",
|
"source": "pinky_mcp_pitch",
|
||||||
"target": "ring_mcp_pitch",
|
"target": "ring_mcp_pitch",
|
||||||
"preserved_fields": list(correction.preserved_ring_fields),
|
"preserved_fields": list(correction.preserved_ring_fields),
|
||||||
},
|
},
|
||||||
|
"resume": resume,
|
||||||
"artifacts": {
|
"artifacts": {
|
||||||
"json": json_path.name,
|
"json": json_path.name,
|
||||||
"urdf": correction.path.name,
|
"urdf": correction.path.name,
|
||||||
@@ -140,6 +330,7 @@ def finalize_o12_session(
|
|||||||
|
|
||||||
|
|
||||||
__all__ = [
|
__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",
|
"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 __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
import math
|
import math
|
||||||
|
|
||||||
from ...core import (
|
from ...core import (
|
||||||
|
AcquisitionPolicy,
|
||||||
ArtifactPolicy,
|
ArtifactPolicy,
|
||||||
CalibrationProfile,
|
CalibrationProfile,
|
||||||
CommandLayout,
|
CommandLayout,
|
||||||
@@ -58,15 +60,24 @@ SDK_UPPER_RAD = (
|
|||||||
1.53588974175501, 1.53588974175501,
|
1.53588974175501, 1.53588974175501,
|
||||||
)
|
)
|
||||||
|
|
||||||
# Source-URDF-safe intersections. The SDK exposes a larger range on several
|
# The calibration domain is the vendor's physical O12 range. Restricting a
|
||||||
# axes, but calibration must never command outside the immutable CAD model.
|
# scan to the source URDF would be circular: that URDF is precisely the model
|
||||||
SAFE_LOWER_RAD = (
|
# being corrected, and its smaller thumb/outer-finger limits previously hid
|
||||||
0.0, -0.94, -0.8272860654453121, -1.29,
|
# the real hardware endpoints. "SAFE" is retained as the public profile name
|
||||||
-0.26, 0.0, 0.0, -0.26, 0.0, 0.0, 0.0, 0.0,
|
# 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 = (
|
FEEDBACK_UPPER_RAD = tuple(
|
||||||
0.73, 0.0, 0.0, 0.0,
|
value + FEEDBACK_DOMAIN_MARGIN_RAD for value in SAFE_UPPER_RAD
|
||||||
0.26, 1.33, 1.48, 0.26, 1.33, 1.71, 1.38, 1.38,
|
|
||||||
)
|
)
|
||||||
|
|
||||||
SDK_TO_URDF_JOINT: tuple[str, ...] = (
|
SDK_TO_URDF_JOINT: tuple[str, ...] = (
|
||||||
@@ -84,6 +95,16 @@ SDK_TO_URDF_JOINT: tuple[str, ...] = (
|
|||||||
"pinky_mcp_pitch",
|
"pinky_mcp_pitch",
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# Fixed direction contract between vendor channels and the target CAD axes.
|
||||||
|
# The magnitude remains visually calibrated because O12's 12 active SDK
|
||||||
|
# coordinates include tendon/solver coordinates that are not one-to-one with
|
||||||
|
# all 19 physical URDF joints.
|
||||||
|
SDK_TO_URDF_SIGN: tuple[float, ...] = (
|
||||||
|
1.0, -1.0, -1.0, -1.0,
|
||||||
|
-1.0, 1.0, 1.0, -1.0,
|
||||||
|
1.0, 1.0, 1.0, 1.0,
|
||||||
|
)
|
||||||
|
|
||||||
ACTIVE_JOINTS = SDK_TO_URDF_JOINT
|
ACTIVE_JOINTS = SDK_TO_URDF_JOINT
|
||||||
PASSIVE_JOINTS: tuple[str, ...] = (
|
PASSIVE_JOINTS: tuple[str, ...] = (
|
||||||
"thumb_dip",
|
"thumb_dip",
|
||||||
@@ -110,14 +131,125 @@ MIMIC_SOURCE_BY_JOINT = {
|
|||||||
"pinky_pip": "pinky_mcp_pitch",
|
"pinky_pip": "pinky_mcp_pitch",
|
||||||
"pinky_dip": "pinky_pip",
|
"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 = {
|
COMMAND_INDEX_BY_JOINT = {
|
||||||
joint: index for index, joint in enumerate(SDK_TO_URDF_JOINT)
|
joint: index for index, joint in enumerate(SDK_TO_URDF_JOINT)
|
||||||
}
|
}
|
||||||
|
|
||||||
PARK_FINGER_RAD = 0.65 * 1.38
|
# The outer pair is used only as collision clearance after the pinky's own
|
||||||
PARK_MIDDLE_MCP_RAD = 0.65 * 1.33
|
# full-range scan has already exercised the same safe endpoint. O12 has only
|
||||||
PARK_MIDDLE_PIP_RAD = 0.65 * 1.71
|
# 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)
|
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(
|
def _task(
|
||||||
@@ -147,15 +279,24 @@ def _task(
|
|||||||
def build_typed_profile() -> CalibrationProfile:
|
def build_typed_profile() -> CalibrationProfile:
|
||||||
middle_clearance = (
|
middle_clearance = (
|
||||||
(4, INDEX_CLEARANCE_RAD),
|
(4, INDEX_CLEARANCE_RAD),
|
||||||
(10, PARK_FINGER_RAD),
|
(5, INDEX_CLEARANCE_MCP_RAD),
|
||||||
(11, PARK_FINGER_RAD),
|
(10, PARK_RING_MCP_RAD),
|
||||||
|
(11, PARK_PINKY_MCP_RAD),
|
||||||
|
)
|
||||||
|
middle_roll_clearance = (
|
||||||
|
*middle_clearance,
|
||||||
|
(8, ROLL_CLEARANCE_MCP_RAD),
|
||||||
)
|
)
|
||||||
index_clearance = (
|
index_clearance = (
|
||||||
(7, 0.0),
|
(7, 0.0),
|
||||||
(8, PARK_MIDDLE_MCP_RAD),
|
(8, PARK_MIDDLE_MCP_RAD),
|
||||||
(9, PARK_MIDDLE_PIP_RAD),
|
(9, PARK_MIDDLE_PIP_RAD),
|
||||||
(10, PARK_FINGER_RAD),
|
(10, PARK_RING_MCP_RAD),
|
||||||
(11, PARK_FINGER_RAD),
|
(11, PARK_PINKY_MCP_RAD),
|
||||||
|
)
|
||||||
|
index_roll_clearance = (
|
||||||
|
*index_clearance,
|
||||||
|
(5, ROLL_CLEARANCE_MCP_RAD),
|
||||||
)
|
)
|
||||||
tasks = (
|
tasks = (
|
||||||
_task("thumb_pitch_front", "front", 2, ("thumb_cmc_pitch",), start=0.0, end=SAFE_LOWER_RAD[2], speed=0.03),
|
_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_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("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("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_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("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_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),
|
_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_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),
|
"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_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_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),
|
"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"
|
"transferred_static_dynamic"
|
||||||
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
|
if name in TRANSFERRED_ACTIVE_SOURCE_BY_JOINT
|
||||||
else "measured_static_dynamic"
|
else "measured_static_dynamic"
|
||||||
|
if name in GEOMETRIC_ZERO_JOINTS
|
||||||
|
else "measured_dynamic_cad_static"
|
||||||
)
|
)
|
||||||
for name in active
|
for name in active
|
||||||
},
|
},
|
||||||
@@ -218,6 +361,8 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
# The public command domain is the reviewed SDK/CAD intersection.
|
# The public command domain is the reviewed SDK/CAD intersection.
|
||||||
lower_bounds=SAFE_LOWER_RAD,
|
lower_bounds=SAFE_LOWER_RAD,
|
||||||
upper_bounds=SAFE_UPPER_RAD,
|
upper_bounds=SAFE_UPPER_RAD,
|
||||||
|
feedback_lower_bounds=FEEDBACK_LOWER_RAD,
|
||||||
|
feedback_upper_bounds=FEEDBACK_UPPER_RAD,
|
||||||
unit="rad",
|
unit="rad",
|
||||||
feedback_by_index=True,
|
feedback_by_index=True,
|
||||||
command_index_by_joint=COMMAND_INDEX_BY_JOINT,
|
command_index_by_joint=COMMAND_INDEX_BY_JOINT,
|
||||||
@@ -243,7 +388,7 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
common_frame="calibration_common",
|
common_frame="calibration_common",
|
||||||
extrinsic_reference_view="front",
|
extrinsic_reference_view="front",
|
||||||
extrinsics_quality_limits={
|
extrinsics_quality_limits={
|
||||||
"reprojection_rms_px": 1.2,
|
"reprojection_rms_px": MAXIMUM_EXTRINSICS_REPROJECTION_RMS_PX,
|
||||||
"maximum_rotation_repeatability_deg": 0.3,
|
"maximum_rotation_repeatability_deg": 0.3,
|
||||||
"maximum_translation_repeatability_m": 0.0015,
|
"maximum_translation_repeatability_m": 0.0015,
|
||||||
},
|
},
|
||||||
@@ -251,10 +396,12 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
),
|
),
|
||||||
motion=MotionPolicy(
|
motion=MotionPolicy(
|
||||||
tasks=tasks,
|
tasks=tasks,
|
||||||
precheck_sweeps=True,
|
# O12 uses AcquisitionPolicy.mapping_probe_maximum_rad for one
|
||||||
steady_command_checkpoints=True,
|
# channel-mapping jog; it does not inherit legacy sweep prechecks.
|
||||||
|
precheck_sweeps=False,
|
||||||
|
steady_command_checkpoints=False,
|
||||||
speed_parameters={
|
speed_parameters={
|
||||||
"command_rate_hz": 20.0,
|
"command_rate_hz": 50.0,
|
||||||
"clearance_flex_rad_s": 0.10,
|
"clearance_flex_rad_s": 0.10,
|
||||||
"clearance_splay_rad_s": 0.04,
|
"clearance_splay_rad_s": 0.04,
|
||||||
"probe_travel_rad": math.radians(3.0),
|
"probe_travel_rad": math.radians(3.0),
|
||||||
@@ -267,22 +414,31 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
zero=ZeroSolvePolicy(
|
zero=ZeroSolvePolicy(
|
||||||
active_joints=active,
|
active_joints=active,
|
||||||
passive_joints=passive,
|
passive_joints=passive,
|
||||||
direct_zero_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS)),
|
direct_zero_joints=tuple(sorted(GEOMETRIC_ZERO_JOINTS)),
|
||||||
axis_joints=tuple(sorted(CALIBRATED_ACTIVE_JOINTS | MEASURED_PASSIVE_JOINTS)),
|
axis_joints=tuple(sorted(
|
||||||
mechanical_endpoint_joints=frozenset(CALIBRATED_ACTIVE_JOINTS),
|
CALIBRATED_ACTIVE_JOINTS
|
||||||
|
| (MEASURED_PASSIVE_JOINTS - {"thumb_dip"})
|
||||||
|
)),
|
||||||
|
mechanical_endpoint_joints=frozenset(ENDPOINT_ANCHOR_BY_JOINT),
|
||||||
post_solve_endpoint_joints=frozenset(),
|
post_solve_endpoint_joints=frozenset(),
|
||||||
mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT,
|
mimic_source_by_joint=MIMIC_SOURCE_BY_JOINT,
|
||||||
cad_frozen_joints=passive,
|
cad_frozen_joints=passive | (active - GEOMETRIC_ZERO_JOINTS - set(TRANSFERRED_ACTIVE_SOURCE_BY_JOINT)),
|
||||||
endpoint_anchor_by_joint={name: "lower_at_start" for name in CALIBRATED_ACTIVE_JOINTS},
|
endpoint_anchor_by_joint=ENDPOINT_ANCHOR_BY_JOINT,
|
||||||
fitted_mimic_joints=MEASURED_PASSIVE_JOINTS,
|
# AprilTags validate passive motion, but the deployed passive
|
||||||
coupling_model_by_joint={name: "quadratic_runtime" for name in MEASURED_PASSIVE_JOINTS},
|
# 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(
|
quality=QualityPolicy(
|
||||||
training_cycles=(0, 1, 2),
|
training_cycles=(0, 1, 2),
|
||||||
holdout_cycle=3,
|
holdout_cycle=3,
|
||||||
hard_threshold_keys=frozenset({
|
hard_threshold_keys=frozenset({
|
||||||
"minimum_detection_rate", "maximum_state_image_skew_ms",
|
"minimum_detection_rate", "maximum_state_image_skew_ms",
|
||||||
"maximum_hysteresis_rad", "maximum_validation_error_rad",
|
"maximum_validation_error_rad",
|
||||||
"maximum_mimic_residual_rad",
|
"maximum_mimic_residual_rad",
|
||||||
}),
|
}),
|
||||||
isolated_holdout=True,
|
isolated_holdout=True,
|
||||||
@@ -301,9 +457,16 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
"calibration_config_sha256", "tag_config_sha256", "sdk_config_sha256",
|
"calibration_config_sha256", "tag_config_sha256", "sdk_config_sha256",
|
||||||
}),
|
}),
|
||||||
publication_pointer="latest_passed",
|
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,
|
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,
|
joint_coverage=coverage,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -336,9 +499,19 @@ def build_profile() -> RegisteredProfile:
|
|||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
"ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_INDEX_BY_JOINT",
|
"ACTIVE_JOINTS", "CALIBRATED_ACTIVE_JOINTS", "COMMAND_INDEX_BY_JOINT",
|
||||||
"COMMAND_NAMES", "INDEX_CLEARANCE_RAD", "KEY", "MEASURED_PASSIVE_JOINTS",
|
"CLEARANCE_FLEX_ENDPOINT_TOLERANCE_RAD",
|
||||||
"MIMIC_SOURCE_BY_JOINT", "PARK_FINGER_RAD", "PARK_MIDDLE_MCP_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",
|
"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",
|
"SDK_LOWER_RAD", "SDK_TO_URDF_JOINT", "SDK_UPPER_RAD",
|
||||||
|
"STATIC_ZERO_EXCLUDED_JOINTS",
|
||||||
"TRANSFERRED_ACTIVE_SOURCE_BY_JOINT", "build_profile", "build_typed_profile",
|
"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 json
|
||||||
import os
|
import os
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
import re
|
||||||
import subprocess
|
import subprocess
|
||||||
import time
|
import time
|
||||||
from typing import Any
|
from typing import Any
|
||||||
@@ -22,10 +23,10 @@ from ..l6.runner import (
|
|||||||
_l6_reason_zh,
|
_l6_reason_zh,
|
||||||
_launch_command,
|
_launch_command,
|
||||||
_stop_stack,
|
_stop_stack,
|
||||||
_wait_until,
|
|
||||||
render_six_channel_progress_zh,
|
render_six_channel_progress_zh,
|
||||||
)
|
)
|
||||||
from .pipeline import finalize_o12_session, load_o12_raw_samples
|
from .pipeline import finalize_o12_session, load_o12_raw_samples
|
||||||
|
from .resume import automatic_resume_candidate, ordered_resume_units
|
||||||
|
|
||||||
|
|
||||||
_TASK_LABELS = {
|
_TASK_LABELS = {
|
||||||
@@ -34,17 +35,30 @@ _TASK_LABELS = {
|
|||||||
"thumb_mcp_dip_front": "拇指 MCP / 被动 DIP(正面 ID1→ID2→ID3)",
|
"thumb_mcp_dip_front": "拇指 MCP / 被动 DIP(正面 ID1→ID2→ID3)",
|
||||||
"thumb_yaw_top": "拇指 CMC yaw(顶部 ID14→ID15)",
|
"thumb_yaw_top": "拇指 CMC yaw(顶部 ID14→ID15)",
|
||||||
"pinky_chain_side": "小指 MCP / 被动 PIP/DIP(侧面 ID4→ID5→ID6→ID7)",
|
"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_mcp_side": "中指 MCP pitch(侧面 ID4→ID8)",
|
||||||
"middle_pip_dip_side": "中指 PIP / 被动 DIP(侧面 ID8→ID9)",
|
"middle_pip_dip_side": "中指 PIP / 被动 DIP(侧面 ID8→ID9)",
|
||||||
"index_roll_front": "食指 MCP roll(正面 ID0→ID13)",
|
"index_roll_front": "食指 MCP roll(正面 ID0→ID13 + 侧面 ID4→ID10)",
|
||||||
"index_mcp_side": "食指 MCP pitch(侧面 ID4→ID10)",
|
"index_mcp_side": "食指 MCP pitch(侧面 ID4→ID11)",
|
||||||
"index_pip_dip_side": "食指 PIP / 被动 DIP(侧面 ID10→ID11)",
|
"index_pip_dip_side": "食指 PIP / 被动 DIP(侧面 ID10→ID11)",
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
def _o12_reason_zh(status):
|
def _o12_reason_zh(status):
|
||||||
reason = str(status.get("reason", ""))
|
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:"):
|
if reason.startswith("mapping_preflight_no_target_motion:"):
|
||||||
task = reason.split(":", 1)[1]
|
task = reason.split(":", 1)[1]
|
||||||
return (
|
return (
|
||||||
@@ -52,11 +66,42 @@ def _o12_reason_zh(status):
|
|||||||
f"固定 SDK 映射点动时没有观察到目标关节运动:{task}。",
|
f"固定 SDK 映射点动时没有观察到目标关节运动:{task}。",
|
||||||
"检查对应 Tag、通道映射和机械连接;不要交换通道后强行继续。",
|
"检查对应 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 (
|
return (
|
||||||
"O12-HEALTH-201",
|
"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")
|
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")
|
if status.get("temperature_fallback_active")
|
||||||
else "等待温度能力确认"
|
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="
|
"\nO12安全:POSITION="
|
||||||
f"{bool(status.get('position_mode_verified'))};"
|
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"温度策略={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:
|
def __init__(self, progress: _ProgressConsole) -> None:
|
||||||
super().__init__("o12_calibration_runner")
|
super().__init__("o12_calibration_runner")
|
||||||
self.status: dict[str, Any] = {}
|
self.status: dict[str, Any] = {}
|
||||||
|
self.last_status_at = 0.0
|
||||||
self.progress = progress
|
self.progress = progress
|
||||||
self.create_subscription(String, "/o12_calibration/status", self._status, 10)
|
self.create_subscription(String, "/o12_calibration/status", self._status, 10)
|
||||||
self.start_client = self.create_client(Trigger, "/o12_calibration/start")
|
self.start_client = self.create_client(Trigger, "/o12_calibration/start")
|
||||||
@@ -101,9 +242,76 @@ class _Monitor(Node):
|
|||||||
return
|
return
|
||||||
if isinstance(value, dict):
|
if isinstance(value, dict):
|
||||||
self.status = value
|
self.status = value
|
||||||
|
self.last_status_at = time.monotonic()
|
||||||
self.progress.update(value)
|
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]:
|
def _overlay_environment(setup: Path) -> dict[str, str]:
|
||||||
completed = subprocess.run(
|
completed = subprocess.run(
|
||||||
["bash", "-c", 'source "$1" >/dev/null 2>&1; env -0', "bash", str(setup)],
|
["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
|
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(
|
def _run_online(
|
||||||
config: ProductConfig, *, record_bag: bool, commands_enabled: bool
|
config: ProductConfig, *, record_bag: bool, commands_enabled: bool,
|
||||||
|
allow_resume: bool,
|
||||||
) -> int:
|
) -> int:
|
||||||
if config.sdk_setup is None or config.sdk_config is None:
|
if config.sdk_setup is None or config.sdk_config is None:
|
||||||
raise ValueError("O12 vendor SDK overlay/config are required")
|
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")
|
session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||||
while session.exists():
|
while session.exists():
|
||||||
time.sleep(1.0)
|
time.sleep(1.0)
|
||||||
session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S")
|
session = config.session_root / datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||||
session.mkdir(parents=True)
|
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_path = session / "calibration.log"
|
||||||
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
|
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()
|
rclpy.init()
|
||||||
monitor = _Monitor(_ProgressConsole(renderer=render_o12_progress_zh))
|
monitor = _Monitor(_ProgressConsole(renderer=render_o12_progress_zh))
|
||||||
|
process: subprocess.Popen[str] | None = None
|
||||||
try:
|
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,
|
monitor, process,
|
||||||
lambda status: status.get("state") in {"READY", "PAUSED", "ABORTED"},
|
lambda status: status.get("state") in {"READY", "PAUSED", "ABORTED"},
|
||||||
timeout=120.0,
|
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":
|
if not ready or monitor.status.get("state") != "READY":
|
||||||
print(
|
print(
|
||||||
"O12 启动预检失败:请检查 HCAN、POSITION 模式、错误/温度回读、"
|
"O12 启动预检失败:请检查 HCAN、POSITION 模式、错误/温度回读、"
|
||||||
@@ -167,21 +476,29 @@ def _run_online(
|
|||||||
if response is None or not response.success:
|
if response is None or not response.success:
|
||||||
print(f"O12 标定未启动:{getattr(response, 'message', '')}", flush=True)
|
print(f"O12 标定未启动:{getattr(response, 'message', '')}", flush=True)
|
||||||
return 2
|
return 2
|
||||||
print("O12 标定已自动开始:POSITION,20 Hz,弧度余弦轨迹。", flush=True)
|
print(
|
||||||
finished = _wait_until(
|
"O12 标定已自动开始:POSITION,50 Hz,平滑限速轨迹。",
|
||||||
|
flush=True,
|
||||||
|
)
|
||||||
|
finished = _wait_until_o12(
|
||||||
monitor, process,
|
monitor, process,
|
||||||
lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"},
|
lambda status: status.get("state") in {"PASSED", "PAUSED", "ABORTED"},
|
||||||
timeout=None,
|
timeout=None,
|
||||||
)
|
)
|
||||||
if not finished or monitor.status.get("state") != "PASSED":
|
if not finished or monitor.status.get("state") != "PASSED":
|
||||||
|
exception = _log_exception_summary(log_path)
|
||||||
print(
|
print(
|
||||||
"O12 标定已安全停止并保持当前位置:"
|
"O12 标定已安全停止并保持当前位置:"
|
||||||
+ str(monitor.status.get("reason", "process_exit")),
|
+ str(monitor.status.get("reason", "process_exit")),
|
||||||
flush=True,
|
flush=True,
|
||||||
)
|
)
|
||||||
|
if exception:
|
||||||
|
print(f"标定节点异常:{exception}", flush=True)
|
||||||
|
print(f"诊断日志:{log_path}", flush=True)
|
||||||
return 3
|
return 3
|
||||||
print("\n".join((
|
print("\n".join((
|
||||||
"PASS:O12 右手 11 个实测任务和独立 holdout 已通过。",
|
"PASS:O12 右手 11 个实测任务和独立 holdout 已通过;"
|
||||||
|
"thumb_mcp 静态零位保留原始 CAD。",
|
||||||
f"发布结果:{config.session_root / 'latest_passed'}",
|
f"发布结果:{config.session_root / 'latest_passed'}",
|
||||||
f"JSON:{monitor.status.get('final_json')}",
|
f"JSON:{monitor.status.get('final_json')}",
|
||||||
f"URDF:{monitor.status.get('final_urdf')}",
|
f"URDF:{monitor.status.get('final_urdf')}",
|
||||||
@@ -196,7 +513,8 @@ def _run_online(
|
|||||||
monitor.destroy_node()
|
monitor.destroy_node()
|
||||||
if rclpy.ok():
|
if rclpy.ok():
|
||||||
rclpy.shutdown()
|
rclpy.shutdown()
|
||||||
_stop_stack(process)
|
if process is not None:
|
||||||
|
_stop_stack(process)
|
||||||
log_stream.flush()
|
log_stream.flush()
|
||||||
os.fsync(log_stream.fileno())
|
os.fsync(log_stream.fileno())
|
||||||
log_stream.close()
|
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-raw", default="")
|
||||||
parser.add_argument("--offline-output", default="")
|
parser.add_argument("--offline-output", default="")
|
||||||
parser.add_argument("--publish-offline", action="store_true")
|
parser.add_argument("--publish-offline", action="store_true")
|
||||||
|
parser.add_argument(
|
||||||
|
"--no-resume", action="store_true",
|
||||||
|
help="忽略兼容的失败会话,从头开始采集",
|
||||||
|
)
|
||||||
selected = parser.parse_args(args)
|
selected = parser.parse_args(args)
|
||||||
config = load_product_config(
|
config = load_product_config(
|
||||||
selected.config,
|
selected.config,
|
||||||
@@ -234,22 +556,20 @@ def main(args: list[str] | None = None) -> None:
|
|||||||
session_dir=output,
|
session_dir=output,
|
||||||
serial_number=config.serial_number,
|
serial_number=config.serial_number,
|
||||||
source_urdf=config.source_urdf,
|
source_urdf=config.source_urdf,
|
||||||
protected_inputs={
|
protected_inputs=_protected_inputs(config),
|
||||||
"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,
|
|
||||||
},
|
|
||||||
records=load_o12_raw_samples(selected.offline_raw),
|
records=load_o12_raw_samples(selected.offline_raw),
|
||||||
publish=selected.publish_offline,
|
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
|
return
|
||||||
raise SystemExit(_run_online(
|
raise SystemExit(_run_online(
|
||||||
config,
|
config,
|
||||||
record_bag=selected.record_bag,
|
record_bag=selected.record_bag,
|
||||||
commands_enabled=not selected.commands_disabled,
|
commands_enabled=not selected.commands_disabled,
|
||||||
|
allow_resume=not selected.no_resume,
|
||||||
))
|
))
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -10,23 +10,144 @@ import re
|
|||||||
from typing import Mapping
|
from typing import Mapping
|
||||||
import xml.etree.ElementTree as ET
|
import xml.etree.ElementTree as ET
|
||||||
|
|
||||||
from ...core.urdf import UrdfJointPatch, UrdfPatchSet, write_urdf_patches
|
import numpy as np
|
||||||
from .fitting import O12FitResult
|
|
||||||
|
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 (
|
from .profile import (
|
||||||
|
ACTIVE_JOINTS,
|
||||||
CALIBRATED_ACTIVE_JOINTS,
|
CALIBRATED_ACTIVE_JOINTS,
|
||||||
|
COMMAND_INDEX_BY_JOINT,
|
||||||
MEASURED_PASSIVE_JOINTS,
|
MEASURED_PASSIVE_JOINTS,
|
||||||
|
MIMIC_SOURCE_BY_JOINT,
|
||||||
|
SDK_TO_URDF_SIGN,
|
||||||
|
STATIC_ZERO_EXCLUDED_JOINTS,
|
||||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
SPLAY_JOINTS = frozenset(
|
SPLAY_JOINTS = frozenset({"index_mcp_roll", "middle_mcp_roll"})
|
||||||
{"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)
|
@dataclass(frozen=True)
|
||||||
class O12UrdfCorrection:
|
class O12UrdfCorrection:
|
||||||
path: Path
|
path: Path
|
||||||
|
origin_offsets_rad: Mapping[str, float]
|
||||||
corrected_limits_rad: Mapping[str, tuple[float, float]]
|
corrected_limits_rad: Mapping[str, tuple[float, float]]
|
||||||
mimic_multipliers: Mapping[str, float]
|
mimic_multipliers: Mapping[str, float]
|
||||||
preserved_ring_fields: tuple[str, ...]
|
preserved_ring_fields: tuple[str, ...]
|
||||||
@@ -40,11 +161,13 @@ def write_o12_corrected_urdf(
|
|||||||
result: O12FitResult,
|
result: O12FitResult,
|
||||||
timestamp: str | None = None,
|
timestamp: str | None = None,
|
||||||
) -> O12UrdfCorrection:
|
) -> 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,
|
O12 SDK feedback is the continuous input coordinate, while the 16-Tag
|
||||||
so origins and passive limits stay byte-for-byte CAD. The unobserved ring
|
trajectories identify the corresponding 19-joint URDF motion. The source
|
||||||
keeps its own origin, limits and both CAD mimic ratios.
|
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()
|
source = Path(source_urdf).expanduser().resolve()
|
||||||
if not source.is_file():
|
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")
|
raise ValueError("O12 travel result has the wrong active joint set")
|
||||||
if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS:
|
if set(result.mimic_fits) != MEASURED_PASSIVE_JOINTS:
|
||||||
raise ValueError("O12 mimic result has the wrong passive joint set")
|
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()
|
root = ET.parse(source).getroot()
|
||||||
joints = {
|
joints = {
|
||||||
@@ -72,40 +212,95 @@ def write_o12_corrected_urdf(
|
|||||||
|
|
||||||
corrected_limits: dict[str, tuple[float, float]] = {}
|
corrected_limits: dict[str, tuple[float, float]] = {}
|
||||||
patches: dict[str, UrdfJointPatch] = {}
|
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")
|
limit = joints[name].find("limit")
|
||||||
if limit is None:
|
if limit is None:
|
||||||
raise ValueError(f"O12 source active joint has no limit: {name}")
|
raise ValueError(f"O12 source active joint has no limit: {name}")
|
||||||
cad_lower = float(limit.get("lower", "nan"))
|
lower_text = limit.get("lower")
|
||||||
cad_upper = float(limit.get("upper", "nan"))
|
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]))
|
travel = abs(float(result.travels_rad[name]))
|
||||||
if not math.isfinite(travel) or travel <= math.radians(2.0):
|
if not math.isfinite(travel) or travel <= math.radians(2.0):
|
||||||
raise ValueError(f"invalid measured O12 travel: {name}")
|
raise ValueError(f"invalid measured O12 travel: {name}")
|
||||||
if name in SPLAY_JOINTS:
|
if not cad_lower < cad_upper:
|
||||||
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:
|
|
||||||
raise ValueError(f"invalid corrected O12 limits: {name}")
|
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)
|
corrected_limits[name] = (lower, upper)
|
||||||
|
active_ranges[name] = (lower, upper)
|
||||||
|
origin_offset = float(result.zero_offsets_rad.get(name, 0.0))
|
||||||
patches[name] = UrdfJointPatch(
|
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}"
|
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):
|
for name in sorted(MEASURED_PASSIVE_JOINTS):
|
||||||
mimic = joints[name].find("mimic")
|
mimic = joints[name].find("mimic")
|
||||||
if mimic is None:
|
if mimic is None:
|
||||||
raise ValueError(f"O12 passive joint has no mimic element: {name}")
|
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:
|
if not math.isfinite(multiplier) or not 0.5 <= multiplier <= 2.2:
|
||||||
raise ValueError(f"invalid O12 mimic multiplier: {name}")
|
raise ValueError(f"invalid O12 mimic multiplier: {name}")
|
||||||
mimic_multipliers[name] = multiplier
|
|
||||||
patches[name] = UrdfJointPatch(
|
patches[name] = UrdfJointPatch(
|
||||||
mimic_multiplier=f"{multiplier:.15g}"
|
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")
|
stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||||
if re.fullmatch(r"\d{8}_\d{6}", stamp) is None:
|
if re.fullmatch(r"\d{8}_\d{6}", stamp) is None:
|
||||||
raise ValueError("O12 URDF timestamp must use YYYYMMDD_HHMMSS")
|
raise ValueError("O12 URDF timestamp must use YYYYMMDD_HHMMSS")
|
||||||
@@ -125,16 +320,32 @@ def write_o12_corrected_urdf(
|
|||||||
forbidden_source_stem_patterns=(r"calibrated",),
|
forbidden_source_stem_patterns=(r"calibrated",),
|
||||||
copy_complete_mesh_directory=True,
|
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(
|
return O12UrdfCorrection(
|
||||||
path=destination,
|
path=destination,
|
||||||
|
origin_offsets_rad={
|
||||||
|
name: float(result.zero_offsets_rad[name])
|
||||||
|
for name in ACTIVE_JOINTS
|
||||||
|
},
|
||||||
corrected_limits_rad=corrected_limits,
|
corrected_limits_rad=corrected_limits,
|
||||||
mimic_multipliers=mimic_multipliers,
|
mimic_multipliers=mimic_multipliers,
|
||||||
preserved_ring_fields=(
|
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_pip.origin", "ring_pip.limit", "ring_pip.mimic",
|
||||||
"ring_dip.origin", "ring_dip.limit", "ring_dip.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:
|
if fit.maximum_monotonic_correction_rad > correction_limit:
|
||||||
raise ValueError(f"{name} monotonic correction exceeds 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
|
curves[name] = fit
|
||||||
holdout[name] = errors
|
holdout[name] = errors
|
||||||
cycle_curves[name] = {
|
cycle_curves[name] = {
|
||||||
|
|||||||
@@ -8,6 +8,8 @@ import math
|
|||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
from typing import Any, Mapping, Sequence
|
from typing import Any, Mapping, Sequence
|
||||||
|
|
||||||
|
from ...runtime.engine import CalibrationEngine
|
||||||
|
|
||||||
from .artifacts import (
|
from .artifacts import (
|
||||||
artifact_hashes,
|
artifact_hashes,
|
||||||
atomic_write_json,
|
atomic_write_json,
|
||||||
@@ -24,6 +26,7 @@ from .profile import (
|
|||||||
MEASURED_PASSIVE_JOINTS,
|
MEASURED_PASSIVE_JOINTS,
|
||||||
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
TRANSFERRED_ACTIVE_SOURCE_BY_JOINT,
|
||||||
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
TRANSFERRED_PASSIVE_SOURCE_BY_JOINT,
|
||||||
|
build_typed_profile,
|
||||||
)
|
)
|
||||||
from .urdf import O6UrdfCorrection, write_o6_corrected_urdf
|
from .urdf import O6UrdfCorrection, write_o6_corrected_urdf
|
||||||
|
|
||||||
@@ -111,6 +114,13 @@ def finalize_o6_session(
|
|||||||
result = fit_o6_session(
|
result = fit_o6_session(
|
||||||
source_urdf, accepted_records_by_joint(records), require_thumb_axis_zero=True
|
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(
|
payload = build_o6_runtime_payload(
|
||||||
serial_number=serial_number,
|
serial_number=serial_number,
|
||||||
source_urdf=source_urdf,
|
source_urdf=source_urdf,
|
||||||
|
|||||||
@@ -217,8 +217,8 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
),
|
),
|
||||||
motion=MotionPolicy(
|
motion=MotionPolicy(
|
||||||
tasks=tasks,
|
tasks=tasks,
|
||||||
precheck_sweeps=True,
|
precheck_sweeps=False,
|
||||||
steady_command_checkpoints=True,
|
steady_command_checkpoints=False,
|
||||||
speed_parameters={
|
speed_parameters={
|
||||||
"baseline_u8": BASELINE_SPEED_U8,
|
"baseline_u8": BASELINE_SPEED_U8,
|
||||||
"preflight_u8": PREFLIGHT_SPEED_U8,
|
"preflight_u8": PREFLIGHT_SPEED_U8,
|
||||||
@@ -253,7 +253,6 @@ def build_typed_profile() -> CalibrationProfile:
|
|||||||
hard_threshold_keys=frozenset({
|
hard_threshold_keys=frozenset({
|
||||||
"minimum_detection_rate",
|
"minimum_detection_rate",
|
||||||
"maximum_state_image_skew_ms",
|
"maximum_state_image_skew_ms",
|
||||||
"maximum_hysteresis_rad",
|
|
||||||
"maximum_validation_error_rad",
|
"maximum_validation_error_rad",
|
||||||
"maximum_mimic_residual_rad",
|
"maximum_mimic_residual_rad",
|
||||||
}),
|
}),
|
||||||
|
|||||||
@@ -6,11 +6,13 @@ from dataclasses import dataclass
|
|||||||
from typing import Any, Callable, Iterator
|
from typing import Any, Callable, Iterator
|
||||||
|
|
||||||
from ..core import CalibrationProfile, ProfileKey, validate_profile
|
from ..core import CalibrationProfile, ProfileKey, validate_profile
|
||||||
|
from ..runtime.engine import CalibrationEngine
|
||||||
|
from ..runtime.adapters import ProfileSdkAdapter
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class EngineBindings:
|
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
|
hand_profile: Any
|
||||||
zero_profile: Any
|
zero_profile: Any
|
||||||
@@ -19,6 +21,7 @@ class EngineBindings:
|
|||||||
return_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
|
return_waypoints: Callable[..., tuple[tuple[int, ...], ...]]
|
||||||
cli_main: Callable[[list[str] | None], None]
|
cli_main: Callable[[list[str] | None], None]
|
||||||
node_main: Callable[[list[str] | None], None]
|
node_main: Callable[[list[str] | None], None]
|
||||||
|
sdk_adapter_factory: Callable[[Any], ProfileSdkAdapter] = ProfileSdkAdapter
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -26,6 +29,12 @@ class RegisteredProfile:
|
|||||||
profile: CalibrationProfile
|
profile: CalibrationProfile
|
||||||
engine: EngineBindings
|
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:
|
class ProfileRegistry:
|
||||||
"""An in-package registry; no discovery plugins or string evaluation."""
|
"""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 .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__ = [
|
__all__ = [
|
||||||
"ControllerSnapshot",
|
"ACQUISITION_POLICY_VERSION", "CalibrationEngine", "CalibrationResult",
|
||||||
"SessionController",
|
"ControllerSnapshot", "ScanUnit", "SessionController", "SessionState",
|
||||||
"SessionState",
|
"SweepQuality", "build_session_controller",
|
||||||
"build_session_controller",
|
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -1 +1,5 @@
|
|||||||
"""Camera, detector, and hand-SDK runtime adapters."""
|
"""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)
|
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:
|
def main(args: list[str] | None = None) -> None:
|
||||||
"""Select the profile first, then delegate to its reviewed CLI strategy."""
|
"""Select the profile first, then delegate to its reviewed CLI strategy."""
|
||||||
selector = argparse.ArgumentParser(add_help=False)
|
selector = argparse.ArgumentParser(add_help=False)
|
||||||
|
|||||||
@@ -635,6 +635,7 @@ def _fit_joint_curve(
|
|||||||
*,
|
*,
|
||||||
endpoint_reference: Mapping[str, Sequence[float]] | None = None,
|
endpoint_reference: Mapping[str, Sequence[float]] | None = None,
|
||||||
preserve_direction_offset: bool = False,
|
preserve_direction_offset: bool = False,
|
||||||
|
require_observed_domain_endpoints: bool = True,
|
||||||
) -> tuple[dict[str, Any], float, float]:
|
) -> tuple[dict[str, Any], float, float]:
|
||||||
by_direction: dict[str, list[list[float]]] = {
|
by_direction: dict[str, list[list[float]]] = {
|
||||||
direction: [[] for _ in range(256)] for direction in DIRECTIONS
|
direction: [[] for _ in range(256)] for direction in DIRECTIONS
|
||||||
@@ -655,10 +656,12 @@ def _fit_joint_curve(
|
|||||||
],
|
],
|
||||||
dtype=int,
|
dtype=int,
|
||||||
)
|
)
|
||||||
if (
|
if commands.size < 3:
|
||||||
commands.size < 3
|
raise ValueError(
|
||||||
or int(commands[0]) != 0
|
f"{direction} centre trajectory requires at least 3 commands"
|
||||||
or int(commands[-1]) != 255
|
)
|
||||||
|
if require_observed_domain_endpoints and (
|
||||||
|
int(commands[0]) != 0 or int(commands[-1]) != 255
|
||||||
):
|
):
|
||||||
raise ValueError(
|
raise ValueError(
|
||||||
f"{direction} centre trajectory requires commands 0 and 255"
|
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 = []
|
result = []
|
||||||
for directory in directories:
|
for directory in directories:
|
||||||
files = sorted(
|
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:
|
if files:
|
||||||
result.append(
|
result.append(
|
||||||
@@ -51,6 +56,7 @@ setup(
|
|||||||
license="MIT",
|
license="MIT",
|
||||||
entry_points={
|
entry_points={
|
||||||
"console_scripts": [
|
"console_scripts": [
|
||||||
|
"compare_calibration_urdfs = linkerhand_calibration.urdf_comparison:main",
|
||||||
(
|
(
|
||||||
"hikrobot_camera_node = "
|
"hikrobot_camera_node = "
|
||||||
"linkerhand_calibration.hikrobot_camera:main"
|
"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["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
|
||||||
assert parameters["minimum_sweep_bins"] >= 32
|
assert parameters["minimum_sweep_bins"] >= 32
|
||||||
assert parameters["maximum_bin_gap"] <= 16
|
assert parameters["maximum_bin_gap"] <= 16
|
||||||
assert parameters["automatic_sweep_retry_limit"] == 2
|
assert parameters["automatic_sweep_retry_limit"] == 1
|
||||||
assert parameters["automatic_fit_retry_limit"] == 2
|
assert parameters["automatic_fit_retry_limit"] == 0
|
||||||
assert parameters["motor_stall_timeout_seconds"] == 2.0
|
assert parameters["motor_stall_timeout_seconds"] == 2.0
|
||||||
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
|
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
|
||||||
assert parameters["motor_stall_minimum_progress_u8"] == 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["provisional_warning_ratio"] == 1.25
|
||||||
assert parameters["retry_speed_scales"] == [0.8, 0.6]
|
assert parameters["retry_speed_scales"] == [1.0]
|
||||||
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0]
|
assert parameters["retry_endpoint_hold_seconds"] == [0.5]
|
||||||
assert parameters["position_timeout_seconds"] >= 20.0
|
assert parameters["position_timeout_seconds"] >= 20.0
|
||||||
assert parameters["maximum_state_image_skew_ms"] <= 50.0
|
assert parameters["maximum_state_image_skew_ms"] <= 50.0
|
||||||
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.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,
|
get_zero_calibration_profile,
|
||||||
write_zero_corrected_urdf,
|
write_zero_corrected_urdf,
|
||||||
)
|
)
|
||||||
|
from linkerhand_calibration.runtime import ACQUISITION_POLICY_VERSION
|
||||||
|
|
||||||
|
|
||||||
REPO = Path(__file__).resolve().parents[3]
|
REPO = Path(__file__).resolve().parents[3]
|
||||||
@@ -963,6 +964,7 @@ def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> N
|
|||||||
json.dumps(
|
json.dumps(
|
||||||
{
|
{
|
||||||
"kind": "session_start",
|
"kind": "session_start",
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
"hand_type": "right",
|
"hand_type": "right",
|
||||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||||
"source_urdf_sha256": config.source_urdf_sha256,
|
"source_urdf_sha256": config.source_urdf_sha256,
|
||||||
@@ -1180,6 +1182,7 @@ def test_automatic_resume_accepts_ctrl_c_checkpoint_without_summary(
|
|||||||
json.dumps(
|
json.dumps(
|
||||||
{
|
{
|
||||||
"kind": "session_start",
|
"kind": "session_start",
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
"hand_type": "right",
|
"hand_type": "right",
|
||||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||||
"source_urdf_sha256": config.source_urdf_sha256,
|
"source_urdf_sha256": config.source_urdf_sha256,
|
||||||
@@ -1205,6 +1208,7 @@ def test_automatic_resume_skips_newer_attempt_without_start_checkpoint(
|
|||||||
json.dumps(
|
json.dumps(
|
||||||
{
|
{
|
||||||
"kind": "session_start",
|
"kind": "session_start",
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
"hand_type": "right",
|
"hand_type": "right",
|
||||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||||
"source_urdf_sha256": config.source_urdf_sha256,
|
"source_urdf_sha256": config.source_urdf_sha256,
|
||||||
@@ -1251,6 +1255,7 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
|||||||
rows = [
|
rows = [
|
||||||
{
|
{
|
||||||
"kind": "session_start",
|
"kind": "session_start",
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
"hand_type": "right",
|
"hand_type": "right",
|
||||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||||
"view_tags": {
|
"view_tags": {
|
||||||
@@ -1344,6 +1349,7 @@ def test_full_resume_discards_all_old_tasks_after_start_position_change(
|
|||||||
rows = [
|
rows = [
|
||||||
{
|
{
|
||||||
"kind": "session_start",
|
"kind": "session_start",
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
"model": "G20",
|
"model": "G20",
|
||||||
"hand_type": "right",
|
"hand_type": "right",
|
||||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||||
@@ -1448,6 +1454,7 @@ def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks(
|
|||||||
rows = [
|
rows = [
|
||||||
{
|
{
|
||||||
"kind": "session_start",
|
"kind": "session_start",
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
"model": "G20",
|
"model": "G20",
|
||||||
"hand_type": "right",
|
"hand_type": "right",
|
||||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||||
@@ -2725,6 +2732,7 @@ def test_node_drops_imported_task_failing_hard_gates(
|
|||||||
rows = [
|
rows = [
|
||||||
{
|
{
|
||||||
"kind": "session_start",
|
"kind": "session_start",
|
||||||
|
"acquisition_policy_version": ACQUISITION_POLICY_VERSION,
|
||||||
"hand_type": "right",
|
"hand_type": "right",
|
||||||
"tag_layout": G20_RIGHT_19_LAYOUT,
|
"tag_layout": G20_RIGHT_19_LAYOUT,
|
||||||
"view_tags": {
|
"view_tags": {
|
||||||
|
|||||||
@@ -1,6 +1,7 @@
|
|||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
import json
|
||||||
import re
|
import re
|
||||||
from types import SimpleNamespace
|
from types import SimpleNamespace
|
||||||
import xml.etree.ElementTree as ET
|
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)
|
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 = _l6_sweep_quality_fake(tmp_path)
|
||||||
fake.step_tag_quality_frames["pinky_dip"] = 190
|
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:
|
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)
|
steps = O6ThreeCameraCalibrationNode._build_steps(fake)
|
||||||
assert steps[0].phase == "baseline"
|
assert steps[0].phase == "baseline"
|
||||||
assert steps[0].speed_u8 == 80
|
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 {
|
assert {
|
||||||
step.speed_u8
|
step.speed_u8
|
||||||
for step in steps
|
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:
|
def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None:
|
||||||
plan = _build_sweep_plan(RIGHT_19_HAND_PROFILE, repetitions=4)
|
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):
|
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 all(item.spec == spec for item in task)
|
||||||
assert [
|
assert [
|
||||||
(item.precheck, item.cycle, item.direction)
|
(item.precheck, item.cycle, item.direction)
|
||||||
for item in task
|
for item in task
|
||||||
] == [
|
] == [
|
||||||
(True, -1, DIRECTION_DECREASING),
|
|
||||||
(True, -1, DIRECTION_INCREASING),
|
|
||||||
*[
|
*[
|
||||||
(False, cycle, direction)
|
(False, cycle, direction)
|
||||||
for cycle in range(4)
|
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:])
|
for left, right in zip(task, task[1:])
|
||||||
]
|
]
|
||||||
assert transitions == [
|
assert transitions == [
|
||||||
"immediate_reverse",
|
|
||||||
"immediate_reverse",
|
|
||||||
"immediate_reverse",
|
"immediate_reverse",
|
||||||
"cycle_reset",
|
"cycle_reset",
|
||||||
"immediate_reverse",
|
"immediate_reverse",
|
||||||
@@ -271,7 +267,7 @@ def test_right_19_plan_is_one_deterministic_transaction_per_task() -> None:
|
|||||||
]
|
]
|
||||||
task_boundaries = [
|
task_boundaries = [
|
||||||
(plan[index], plan[index + 1])
|
(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(
|
assert all(
|
||||||
_sweep_plan_transition(RIGHT_19_HAND_PROFILE, left, right)
|
_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,
|
minimum_per_joint=1,
|
||||||
)
|
)
|
||||||
|
|
||||||
# 16 tasks × (precheck out/back, precheck->formal, four formal reversals).
|
# Four formal decreasing/increasing pairs per task; no full-range precheck.
|
||||||
assert immediate_boundaries == 16 * 6
|
assert immediate_boundaries == 16 * 4
|
||||||
assert cycle_resets == 16 * 3
|
assert cycle_resets == 16 * 3
|
||||||
assert task_changes == 15
|
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 len(reset_items) == len(RIGHT_19_HAND_PROFILE.sweep_specs)
|
||||||
assert all(item.precheck for item in reset_items)
|
assert all(not item.precheck and item.cycle == 0 for item in reset_items)
|
||||||
assert all(item.cycle == -1 for item in reset_items)
|
|
||||||
assert all(
|
assert all(
|
||||||
item.direction == DIRECTION_DECREASING for item in reset_items
|
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]
|
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)
|
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
||||||
item = SweepItem(spec, 0, DIRECTION_INCREASING)
|
item = SweepItem(spec, 0, DIRECTION_INCREASING)
|
||||||
transitions: list[str] = []
|
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),
|
_publish_hold_current=lambda: holds.append(True),
|
||||||
_begin_return_baseline=lambda after: transitions.append(after),
|
_begin_return_baseline=lambda after: transitions.append(after),
|
||||||
_pause=lambda reason: pauses.append(reason),
|
_pause=lambda reason: pauses.append(reason),
|
||||||
retry_speed_scales=(0.8, 0.6, 0.5),
|
retry_speed_scales=(1.0,),
|
||||||
retry_endpoint_hold_seconds=(0.75, 1.0, 1.25),
|
retry_endpoint_hold_seconds=(0.5,),
|
||||||
)
|
)
|
||||||
|
|
||||||
G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
|
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(
|
G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
|
||||||
node, "sweep_bin_gap_too_large"
|
node, "sweep_bin_gap_too_large"
|
||||||
)
|
)
|
||||||
G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause(
|
assert transitions == ["retry_sweep"]
|
||||||
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 pauses == ["sweep_bin_gap_too_large"]
|
assert pauses == ["sweep_bin_gap_too_large"]
|
||||||
assert holds == []
|
assert holds == []
|
||||||
assert node.sweep_retry_counts[(6, 0, DIRECTION_INCREASING)] == 3
|
assert node.sweep_retry_counts[(6, 0, DIRECTION_INCREASING)] == 1
|
||||||
events = [
|
events = [
|
||||||
json.loads(line)
|
json.loads(line)
|
||||||
for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines()
|
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:
|
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"]
|
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)
|
spec = next(item for item in SWEEP_SPECS if item.motor_index == 15)
|
||||||
transitions: list[str] = []
|
transitions: list[str] = []
|
||||||
pauses: 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"}],
|
[{"joint": "thumb_ip", "metric": "rotation_orthogonal_rms_deg"}],
|
||||||
)
|
)
|
||||||
|
|
||||||
assert prepared == [True]
|
assert prepared == []
|
||||||
assert transitions == ["resume_sweep"]
|
assert transitions == []
|
||||||
assert pauses == []
|
assert pauses == ["joint_fit_check_failed"]
|
||||||
assert node.paused_reason == "joint_fit_check_failed"
|
|
||||||
|
|
||||||
|
|
||||||
def test_repeatable_all_cycle_model_conflict_does_not_waste_full_retry(
|
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
|
assert node.fit_failure["directions_to_rescan"] == 0
|
||||||
|
|
||||||
|
|
||||||
def test_near_threshold_provisional_failure_rescans_in_place(tmp_path) -> None:
|
def test_near_threshold_provisional_failure_stops_without_rescan(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.
|
|
||||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
||||||
node = SimpleNamespace(
|
node = SimpleNamespace(
|
||||||
raw_path=tmp_path / "raw_samples.jsonl",
|
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()
|
for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines()
|
||||||
]
|
]
|
||||||
kinds = [event["kind"] for event in events]
|
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
|
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]] = []
|
commands: list[list[int]] = []
|
||||||
pauses: list[str] = []
|
pauses: list[str] = []
|
||||||
holds: list[bool] = []
|
holds: list[bool] = []
|
||||||
@@ -3363,12 +3347,12 @@ def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None:
|
|||||||
node, "return_baseline_timeout", now
|
node, "return_baseline_timeout", now
|
||||||
)
|
)
|
||||||
|
|
||||||
assert len(commands) == 2
|
assert commands == []
|
||||||
assert holds == []
|
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(
|
spec = next(
|
||||||
item
|
item
|
||||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
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]] = []
|
resets: list[tuple[object, bool]] = []
|
||||||
diagnostic_resets: list[object] = []
|
diagnostic_resets: list[object] = []
|
||||||
commands: list[list[int]] = []
|
commands: list[list[int]] = []
|
||||||
|
starts: list[float] = []
|
||||||
|
pauses: list[str] = []
|
||||||
start_frames = [object()]
|
start_frames = [object()]
|
||||||
node = SimpleNamespace(
|
node = SimpleNamespace(
|
||||||
profile=RIGHT_19_HAND_PROFILE,
|
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_speed_profile=lambda profile: None,
|
||||||
_publish_command=lambda command: commands.append(command),
|
_publish_command=lambda command: commands.append(command),
|
||||||
_reset_motion_progress=lambda now, error: None,
|
_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(
|
G20ThreeCameraCalibrationNode._handle_sweep_start_timeout(
|
||||||
node, "sweep_start_tag_timeout", 20.0
|
node, 20.0, True
|
||||||
)
|
)
|
||||||
|
|
||||||
assert resets == [(runtime, True)]
|
assert starts == [20.0]
|
||||||
assert diagnostic_resets == [runtime]
|
assert pauses == []
|
||||||
assert runtime.pnp_invalid_since is None
|
|
||||||
assert runtime.pnp_reset_count == 1
|
|
||||||
assert start_frames == []
|
|
||||||
assert len(commands) == 1
|
|
||||||
event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
|
event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
|
||||||
assert event["pnp_trackers_reset"] == ["side"]
|
assert event["kind"] == "sweep_start_vision_timeout_warning"
|
||||||
assert event["task_reference_preserved"] is True
|
|
||||||
|
|
||||||
|
|
||||||
def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None:
|
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"
|
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)
|
spec = next(item for item in SWEEP_SPECS if item.motor_index == 0)
|
||||||
item = SweepItem(spec, 0, DIRECTION_DECREASING)
|
item = SweepItem(spec, 0, DIRECTION_DECREASING)
|
||||||
key = (spec.motor_index, 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(
|
assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds(
|
||||||
node
|
node
|
||||||
) == 112.5
|
) == 90.0
|
||||||
node.sweep_retry_counts[key] = 3
|
node.sweep_retry_counts[key] = 3
|
||||||
assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds(
|
assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds(
|
||||||
node
|
node
|
||||||
) == 180.0
|
) == 90.0
|
||||||
|
|
||||||
|
|
||||||
def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None:
|
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_pitch_front",
|
||||||
"thumb_cmc_roll_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(
|
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
|
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={})
|
node = _warning_band_node(tmp_path, attempts={})
|
||||||
failures = [
|
failures = [
|
||||||
{
|
{
|
||||||
@@ -5343,22 +5325,19 @@ def test_warning_band_failure_rescans_once_in_place(tmp_path) -> None:
|
|||||||
node, node._spec, failures, allow_warning=True
|
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 paused is True
|
||||||
assert "prepare_retry" in node._calls
|
assert node._calls == ["pause_joint_fit_check_failed"]
|
||||||
rows = [
|
rows = [
|
||||||
json.loads(line)
|
json.loads(line)
|
||||||
for line in node.raw_path.read_text().splitlines()
|
for line in node.raw_path.read_text().splitlines()
|
||||||
]
|
]
|
||||||
kinds = {row["kind"] for row in rows}
|
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 "provisional_fit_warning" not in kinds
|
||||||
assert "fit_failure" 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(
|
spec = next(
|
||||||
item
|
item
|
||||||
for item in RIGHT_19_HAND_PROFILE.sweep_specs
|
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 paused is True
|
||||||
assert node._calls == ["prepare_retry", "return_resume_sweep"]
|
assert node._calls == ["pause_joint_fit_check_failed"]
|
||||||
rows = [
|
rows = [
|
||||||
json.loads(line)
|
json.loads(line)
|
||||||
for line in node.raw_path.read_text().splitlines()
|
for line in node.raw_path.read_text().splitlines()
|
||||||
]
|
]
|
||||||
assert [row["kind"] for row in rows] == [
|
assert [row["kind"] for row in rows] == [
|
||||||
"provisional_fit_warning_rescan",
|
"provisional_fit_warning_retry_exhausted",
|
||||||
"fit_failure",
|
"fit_failure",
|
||||||
]
|
]
|
||||||
assert rows[0]["attempt"] == 2
|
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:
|
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",
|
"fit_failure",
|
||||||
]
|
]
|
||||||
assert rows[0]["attempt"] == 3
|
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:
|
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,
|
allow_warning=True,
|
||||||
)
|
)
|
||||||
assert paused is 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._calls.clear()
|
||||||
node.sweep_attempts[node._spec.key] = 2
|
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,
|
UrdfJointPatch,
|
||||||
UrdfPatchSet,
|
UrdfPatchSet,
|
||||||
apply_urdf_patch_text,
|
apply_urdf_patch_text,
|
||||||
|
validate_urdf_mimic_ranges,
|
||||||
write_urdf_patches,
|
write_urdf_patches,
|
||||||
)
|
)
|
||||||
|
|
||||||
@@ -162,3 +163,37 @@ def test_patch_engine_copies_referenced_and_complete_mesh_bundle(
|
|||||||
assert (
|
assert (
|
||||||
destination.parent / "meshes/auxiliary.stl"
|
destination.parent / "meshes/auxiliary.stl"
|
||||||
).read_bytes() == b"auxiliary"
|
).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