admin/linkerhand_retarget_ros2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

merge into: admin/linkerhand_retarget_ros2:o30
Branches Tags
admin/linkerhand_retarget_ros2:master admin/linkerhand_retarget_ros2:calibration admin/linkerhand_retarget_ros2:bd3 admin/linkerhand_retarget_ros2:o30 admin/linkerhand_retarget_ros2:bd
..
pull from: admin/linkerhand_retarget_ros2:bd3
Branches Tags
admin/linkerhand_retarget_ros2:calibration admin/linkerhand_retarget_ros2:bd3 admin/linkerhand_retarget_ros2:o30 admin/linkerhand_retarget_ros2:bd admin/linkerhand_retarget_ros2:master

5 Commits
o30 .. bd3

Author SHA1 Message Date
admin 286581bcba 大拇指单独标定，yaw正确和稳定性修改 2026-08-31 17:17:39 +08:00
admin 4dadfb954b 大拇指零位正确性稳定性修改 2026-08-30 17:35:23 +08:00
admin 83c69b48c2 标定稳定性 2026-08-27 16:24:01 +08:00
admin 4e594ddb09 G20右手四指独立标定（少thumb_mcp） 2026-08-24 10:12:23 +08:00
admin ef65681230 G20四指单独标定（少末端tag） 2026-08-21 12:21:39 +08:00
38 changed files with 29088 additions and 3233 deletions
Expand all files Collapse all files
.gitignore
+2
View File
@@ -92,3 +92,5 @@ candump-*
# Local Codex/agent workspace metadata
/.agents/
/.codex/
/.codebuddy/
/.zcode/
docs/G20三相机零位标定与URDF修正操作说明.md
-669
View File
@@ -1,669 +0,0 @@
# G20 三相机零位标定与 URDF 修正操作说明
> 适用工程：`linkerhand_retarget_ros2`
> 适用标定包：`g20_thumb_apriltag_calibration`
> 文档基线：2026-08-11 当前 V3 轴坐标系逻辑
> 适用对象：G20 左手和右手
## 1. 文档目的
本文说明当前代码中 G20 三相机、11 个 AprilTag 的完整标定逻辑及现场操作流程，包含：
- 20 通道 u8 命令到 21 个 URDF 关节动态角度曲线的标定；
- 拇指 CMC roll/yaw/pitch 的静态 URDF 零位求解；
- 四指动态曲线继承和静态 CAD 零位保护；
- 第三轮留出验证、自动重采、暂停和恢复策略；
- 从原始 CAD URDF 生成修正 URDF；
- 使用已有 `raw_samples.jsonl` 进行无运动离线重放；
- 修正 URDF 和配套 JSON 的仿真使用方法。
本文中的右手 `G20_RIGHT_001/20260811_120146` 数值仅用于说明当前算法的实际结果，**不是代码中写死的标定角度，也不是其他机械手的目标值**。
## 2. 标定的最终产物
一次完整标定同时生成两类互相配套的产物：
1. 精简标定 JSON：保存每个关节按命令 `0～255` 索引的 256 点 `angle_rad` 动态曲线，以及 URDF 静态零偏和质量指标。
2. 修正 URDF：只把通过验证的静态零偏写入原始 CAD URDF 的主动关节 `origin.rpy`。
运行时必须遵守以下规则：
- 修正 URDF 已经包含静态 `urdf_zero_offset_rad`，运行时不能再加一次；
- 动态关节位置必须使用同一机械手、同一侧、同一次标定 JSON 中的 `angle_rad`；
- 左手曲线不能用于右手，右手曲线不能用于左手；
- 不能用已经修正过的 URDF 作为下一次标定的源文件。
## 3. 硬件和坐标配置
### 3.1 三台相机
默认机位和序列号如下：
| 机位 | 默认序列号 | 默认作用 |
|---|---|---|
| 正面 `front` | `DB2163742` | 拇指 CMC pitch/roll、MCP/IP、参考指 MCP roll |
| 侧面 `side` | `DB2163749` | 参考指 MCP pitch、PIP/DIP |
| 上面 `top` | `DB2163739` | 拇指 CMC yaw |
默认采集参数：
- 分辨率和像素格式由海康节点配置；
- 帧率：`30 Hz`；
- 曝光：`5000 us`；
- 增益：`0 dB`；
- 自动曝光：关闭；
- AprilTag 检测降采样：`decimate=1.5`；
- 启动文件固定使用 `rmw_fastrtps_cpp` 和 64 MB Fast DDS 共享内存配置。
每台相机必须有对应当前镜头、焦距、分辨率的独立内参文件：
```text
~/.ros/camera_info/hikrobot_DB2163742.yaml
~/.ros/camera_info/hikrobot_DB2163749.yaml
~/.ros/camera_info/hikrobot_DB2163739.yaml
```
三相机外参默认文件：
```text
/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml
```
移动相机、改变镜头焦距/对焦、改变分辨率或重新标定内参后，必须重新标定外参。
### 3.2 11 个 AprilTag
使用 `tag36h11`，有效黑框边长配置为 `16 mm`，不包含外围白边。
| Tag ID | 机位 | 固定位置或运动件 |
|---:|---|---|
| 0 | 正面 | 掌壳固定基准 |
| 1 | 正面 | 拇指 CMC 后连杆 |
| 2 | 正面 | 拇指 MCP 后连杆 |
| 3 | 正面 | 拇指 IP 后末节 |
| 4 | 侧面 | 掌壳侧面固定基准 |
| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
| 8 | 上面 | 掌壳或底座固定基准 |
| 9 | 上面 | 拇指 CMC yaw 运动件 |
| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
Tag 必须固定在刚性件上，不能跨关节、贴在软胶上、在运动中翘起或移动。Tag 的平面内旋转不要求贴正，但整个标定会话中安装姿态必须保持不变。
## 4. 左右手配置差异
左右手使用同一套拟合、留出验证和 URDF 写入算法，但使用独立的参考指、电机和镜像避挡姿态。
| 项目 | 左手 | 右手 |
|---|---|---|
| 四指参考源 | 食指 `index` | 小指 `pinky` |
| 扫描电机顺序 | `0/5/15/6/1/16/10` | `0/5/15/9/4/19/10` |
| 参考指 MCP roll | 电机 6 | 电机 9 |
| 参考指 MCP pitch | 电机 1 | 电机 4 |
| 参考指 PIP | 电机 16 | 电机 19 |
| 参考指 roll 避挡 | 其他侧摆电机置 0 | 其他侧摆电机置 255 |
| 电机 0 辅助姿态 | 使用基准姿态 | 电机 5、10 固定 255 |
两侧的非零拇指静态零偏都必须由各自当前会话的数据计算，不共享任何标定角度。
## 5. 扫描任务和运动策略
### 5.1 固定基准命令
调用 `/g20_calibration/start` 后，程序先下发并确认以下 20 通道姿态：
```text
[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
```
普通关节以命令 `255` 为动态曲线零点；四指 MCP 侧摆以命令 `127` 为动态曲线零点。
### 5.2 七个扫描任务
每个任务执行 3 轮 `255→0→255`，即每个任务 6 个方向。总计：
```text
7 个任务 × 3 轮 × 2 个方向 = 42 个扫描方向
```
| 顺序 | 任务 | 电机 | 机位 | 同时拟合 |
|---:|---|---:|---|---|
| 1 | 拇指 CMC pitch | 0 | 正面 | `thumb_cmc_pitch` |
| 2 | 拇指 CMC roll | 5 | 正面 | `thumb_cmc_roll` |
| 3 | 拇指 MCP | 15 | 正面 | `thumb_mcp`、被动 `thumb_ip` |
| 4 | 参考指 MCP roll | 左6/右9 | 正面 | 参考指 `mcp_roll` |
| 5 | 参考指 MCP pitch | 左1/右4 | 侧面 | 参考指 `mcp_pitch` |
| 6 | 参考指 PIP | 左16/右19 | 侧面 | 参考指 `pip`、被动 `dip` |
| 7 | 拇指 CMC yaw | 10 | 上面 | `thumb_cmc_yaw` |
特殊辅助姿态：
- 扫描拇指 yaw 时，电机 5 固定为 `145`，避免 Tag 9 姿态过斜；
- 右手扫描电机 0 时，电机 5 和 10 固定为 `255`；
- 扫描参考指 MCP roll 时，其余三指侧摆移到对应左右手避挡端；
- 当前方向重试和人工恢复时，辅助姿态保持一致。
### 5.3 标定速度
G20 SDK 使用五指速度数组：
- 常规速度：`15`；
- 参考指 MCP roll：参考指速度 `5`；
- 参考指 MCP pitch/PIP：参考指速度 `10`；
- 自动重试速度比例：`80% / 60% / 50%`，最低速度不低于 `3`。
## 6. 轨迹和关节轴拟合
### 6.1 时间戳配对
每帧 Tag 图像与 20 通道机械手状态按时间戳配对，默认最大允许偏差为 `50 ms`。标定曲线按实际命令分箱，但已确认的固件端点饱和反馈可以归入对应的命令端点分箱。
每个扫描方向至少需要：
- 40 帧同步有效数据；
- 覆盖至少 240 个 u8；
- 至少 32 个有效整数分箱；
- 相邻有效分箱最大间隔不超过 16；
- 同时包含命令 0 和 255 端点。
### 6.2 动态角度曲线
程序使用父/子 Tag 的完整相对旋转轨迹拟合关节转角，分别拟合下降和上升方向，检查单调修正、正反程回差和三轮行程一致性，再生成按命令索引的 256 点 `angle_rad`。
四指策略：
- 左手实测食指动态曲线，继承给中指、无名指、小指；
- 右手实测小指动态曲线，继承给食指、中指、无名指；
- 继承仅用于动态命令—角度关系；
- 不把参考指的静态装配偏差复制给其他独立电机。
### 6.3 三维轴方向和轴线位置
当前算法不直接用单帧平面 Tag PnP 姿态作为关节角：
- 轴方向主要来自整段相对旋转的螺旋轴；
- 斜视且三维运动平面可观的关节，保留旋转轴和中心圆轴的交叉检查；
- 接近端视的关节使用相对姿态轴约束圆轨迹方向；
- 轴线上一点由整段相对 SE(3) 的 `(I-R)p=t` 方程拟合；
- 接近沿轴观察时，丢弃单目无法稳定确定的光轴深度，只使用图像平面内可观分量。
### 6.4 IPPE 双分支处理
每轮运动前在静止端点联合 8 帧选择整组最稳定的平面 Tag PnP 分支。侧面 Tag 4/5/6/7 还检查贴面法向一致性，避免选择低重投影误差但几何镜像的分支。
## 7. 当前 V3 静态零位求解逻辑
### 7.1 为什么不能用两条根轴线间距确定掌部旋转
拇指 CMC roll 根轴和四指 MCP roll 根轴在 CAD 中近似平行。旧逻辑使用两条三维轴线的空间间距确定掌坐标系绕根轴的旋转，但单目 PnP 的固定深度偏差会改变这条间距方向，并被误算成稳定的拇指 roll 静态零偏。
这种误差可以三轮高度重复，因此“重复性好”并不能证明绝对零位正确。
### 7.2 V3 掌坐标系锚定
当前 V3 使用：
1. 行程更充分的根轴实测方向；
2. 保持原始 CAD 直立的参考指 MCP pitch 实测轴方向；
3. 两条根轴线位置只用于平移，不参与绕根轴旋转。
左手使用食指 MCP pitch，右手使用小指 MCP pitch。该逻辑全部由当前会话轨迹计算，不包含按左右手或序列号写死的拇指角度。
### 7.3 拇指零位依赖链
拇指静态零位按可观测链逐级求解：
- `thumb_cmc_yaw` 实测轴方向观测 `thumb_cmc_roll` 零位；
- `thumb_cmc_pitch` 实测轴方向观测 `thumb_cmc_yaw` 零位；
- `thumb_mcp` 实测轴线相位观测 `thumb_cmc_pitch` 零位；
- 被动 `thumb_ip` 的浅圆弧只作为诊断，不能覆盖 `thumb_mcp` 的原始 CAD 静态零位。
逐关节一维鲁棒求解可防止远端异常把已经确定的上游零位一起拖到边界。
### 7.4 四指静态零位保护
当前 11-Tag 布局只能直接观测一根参考指，不能证明四根独立电机具有相同绝对装配相位。因此：
- 四指 MCP roll 静态修正固定为原始 CAD 0；
- 四指 MCP pitch 静态修正固定为原始 CAD 0；
- 四指 PIP 静态修正固定为原始 CAD 0；
- `thumb_mcp` 静态修正固定为原始 CAD 0；
- 这些关节的动态 256 点曲线仍然实测或继承。
这里的“0”表示不修改原始 CAD `origin.rpy`，不是额外写入某台机械手的人工标定角度。
## 8. 质量门限和留出验证
### 8.1 预检门限
| 指标 | 默认要求 |
|---|---:|
| 预检窗口 | 60 帧 |
| 所需 Tag 同时有效率 | ≥95% |
| 检测频率 | ≥15 Hz，正常应接近30 Hz |
| Hamming | 0 |
| Decision margin | ≥30 |
| Tag 最小边长 | ≥30 px |
| PnP 重投影 RMS | ≤1.5 px |
| 图像—状态时间差 | ≤50 ms |
### 8.2 轨迹和轴门限
| 指标 | 默认要求 |
|---|---:|
| 主动关节旋转轴外 RMS | ≤2.5° |
| 被动关节旋转轴外 RMS | ≤7.5° |
| 三轮轴方向极差 | ≤0.75° |
| 轴线径向 RMS | ≤3 mm |
| SE(3) 轴线拟合 RMS | ≤1 mm |
| 可观三维圆的姿态轴/圆轴夹角 | ≤1° |
| 父子轴锥角几何不一致 | ≤5° |
| 主动曲线三轮行程差 | ≤3° |
| 被动曲线三轮行程差 | ≤10° |
| 主动最大单调修正 | ≤2° |
| 主动最大回差 | ≤5° |
| 被动最大单调修正 | ≤3° |
| 被动最大回差 | ≤7.5° |
拇指可求解静态偏移默认安全范围为 `±20°`。四指静态偏移不由相机相位覆盖，保持原始 CAD。
### 8.3 第三轮强制留出
前两轮用于训练，第三轮必须作为独立留出验证：
- 轨迹角度 MAE ≤1°；
- 轨迹角度 P95 ≤2°；
- 非零静态修正必须在第三轮优于原始 URDF；
- 改善必须通过按三轮分组的 95% bootstrap 置信检查；
- 最终再使用三轮全部数据重拟合正式结果。
`validation_enabled:=false` 只关闭额外随机机械动作，不能关闭第三轮留出验证。
## 9. 运动安全和端点处理
默认端点容差为 `±2 u8`，只有经过实机确认的固件饱和端点使用专用容差：
| 端点 | 专用容差 |
|---|---:|
| 拇指 yaw 电机10，命令0 | ±4 u8 |
| 右手拇指 yaw 电机10，命令255 | ±5 u8，实机可能反馈250 |
| 右手小指 PIP 电机19，命令0 | ±5 u8，实机可能反馈5 |
运动保护：
- 单方向扫描超时：90 秒；
- 连续 8 秒没有至少 1 u8 的目标方向进展：立即暂停并保持当前位置；
- 机械停滞不消耗遮挡/采样自动重试预算；
- 发现摩擦、碰撞或仍在变化的反馈时，不要反复调用 `resume` 强推；
- 只有确认是稳定固件端点时，才允许为该电机、该端点配置专用容差。
## 10. 标定前准备
### 10.1 构建和加载环境
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install \
--packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
source install/setup.bash
```
每次修改代码并重新构建后，必须关闭旧标定进程，在新终端重新 `source install/setup.bash` 后启动。
### 10.2 现场检查
开始前确认：
- `can0` 已启动；
- MVS 客户端没有占用三台相机；
- 三个内参文件和外参文件对应当前相机安装；
- 11 个 Tag 固定、平整、全行程可见；
- 机械手全行程没有碰撞；
- 没有其他节点向同一只手发布位置命令；
- 准备好随时断开电机电源；
- 源 URDF 是原始 CAD 文件。
## 11. 禁止运动预检
### 11.1 右手预检
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch g20_thumb_apriltag_calibration \
three_camera_calibration.launch.py \
hand_type:=right \
serial_number:=G20_RIGHT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf \
can_interface:=can0 \
commands_enabled:=false
```
### 11.2 左手预检
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch g20_thumb_apriltag_calibration \
three_camera_calibration.launch.py \
hand_type:=left \
serial_number:=G20_LEFT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
can_interface:=can0 \
commands_enabled:=false
```
`commands_enabled:=false` 时不允许标定位置运动。完成检查后应关闭预检进程，再启动正式流程，避免相机和 SDK 被两个进程同时占用。
## 12. 正式标定操作
### 12.1 右手正式启动
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch g20_thumb_apriltag_calibration \
three_camera_calibration.launch.py \
hand_type:=right \
serial_number:=G20_RIGHT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf \
can_interface:=can0
```
### 12.2 左手正式启动
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch g20_thumb_apriltag_calibration \
three_camera_calibration.launch.py \
hand_type:=left \
serial_number:=G20_LEFT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
can_interface:=can0
```
### 12.3 查看状态
另开一个终端：
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 topic echo --once \
/g20_calibration/status_text \
--field data
```
需要连续监控时去掉 `--once`。
查看三个校正画面：
```bash
ros2 run image_view image_view --ros-args \
--remap image:=/g20_calibration/front/camera/image_rect
ros2 run image_view image_view --ros-args \
--remap image:=/g20_calibration/side/camera/image_rect
ros2 run image_view image_view --ros-args \
--remap image:=/g20_calibration/top/camera/image_rect
```
### 12.4 开始运动
只有状态进入“等待开始”，三个机位均显示“就绪”、外参匹配、所需 Tag 无缺失、SDK 正常后，才调用一次：
```bash
ros2 service call \
/g20_calibration/start \
std_srvs/srv/Trigger {}
```
不要重复调用 `start`。程序会自动完成基准姿态、42 个扫描方向、拟合、第三轮留出验证、正式 JSON 和 URDF 写入。
## 13. 暂停、恢复和终止
### 13.1 手工暂停
```bash
ros2 service call \
/g20_calibration/pause \
std_srvs/srv/Trigger {}
```
暂停会保持当前实际位置，不会自动返回基准。
### 13.2 恢复可恢复故障
```bash
ros2 service call \
/g20_calibration/resume \
std_srvs/srv/Trigger {}
```
恢复规则：
- 只要求当前活动机位恢复就绪；
- 当前失败方向会从起点完整重扫；
- 关节拟合失败会清除当前失败关节数据并重扫该关节的 6 个方向；
- 已经通过的其他关节数据保留；
- 不要用 `start` 代替 `resume`。
### 13.3 终止
```bash
ros2 service call \
/g20_calibration/abort \
std_srvs/srv/Trigger {}
```
终止会停止任务并保持当前位置，不主动移动机械手。
## 14. 自动重试和失败分类
| 失败类型 | 程序行为 | 操作建议 |
|---|---|---|
| 短时 Tag 丢失、同步中断、端点/分箱不足 | 自动重扫当前方向，最多3次 | 修正遮挡后必要时 `resume` |
| 单轮/单关节轨迹拟合失败 | 优先重扫失败轮次；最多自动重采2轮 | 修正可见性或机械行程后 `resume` |
| 电机8秒无进展 | 立即保持并暂停，不消耗采样重试 | 先排查机械问题或确认固件端点 |
| 三轮轴方向或零位离散过大 | 当前关节结束后暂停 | 修正问题后 `resume` 重扫该关节 |
| 零位触边、父子轴几何不一致、留出无改善 | 稳定模型失败，不自动重扫 | `resume` 被拒绝；修正根因后启动新会话 |
| 操作员暂停 | 保持当前位置 | 确认安全后 `resume` |
自动重试只改变采集速度和保持时间，不放宽最终质量门限。
## 15. 常见状态问题
### 15.1 预检只有约 3 Hz
优先检查：
- 是否通过正式 launch 启动，从而加载 Fast DDS 大图共享内存配置；
- 是否还有旧相机或 MVS 客户端占用设备；
- `camera_info` 和 `image_raw/image_rect` 是否都接近 30 Hz；
- 是否重新 `source install/setup.bash`；
- 是否存在多个图像查看或录制进程造成额外负载。
不要通过降低 Tag 有效率门限绕过帧率问题。
### 15.2 显示“缺失Tag=无”，但同时有效率不足
“当前缺失”只表示最新帧；同时有效率是预检滑动窗口内所有必需 Tag 同帧有效的比例。等待窗口更新，或排查间歇性遮挡、反光、角点质量和 PnP 分支失败。
### 15.3 电机目标255、反馈稳定250
当前代码只对右手电机10的255端配置 `±5 u8`。其他电机不能因为一次卡滞而放宽。先确认反馈确实稳定在固件端点，且不存在摩擦或碰撞。
### 15.4 被动 DIP 轴外残差过大
被动关节允许更宽的单轴残差，但仍必须满足跨轮轴方向和第三轮留出。若 Tag、外参均正常，需检查耦合机构是否存在非理想运动、松动或采样过程中 PnP 分支变化；不要直接放宽最终门限。
### 15.5 零位/URDF模型验证失败
该失败表示当前稳定观测无法由“原始 CAD + 纯关节零位旋转”解释。重复运动通常不会修复，程序会拒绝 `resume`。应检查原始 URDF、Tag所在刚性件、相机外参身份和标定模型后启动新会话。
## 16. 输出目录和文件
默认会话目录：
```text
calibration_output/<序列号>/<YYYYMMDD_HHMMSS>/
```
主要文件：
```text
raw_samples.jsonl
g20_<left|right>_<序列号>_calibration.json
```
修正 URDF 默认写入原始 URDF 所在目录：
```text
linkerhand_g20_<left|right>_zero_calibrated_<序列号>_<时间戳>.urdf
```
写入约束：
- 每次都从原始 CAD URDF 生成；
- 采用 `T_original × Rot(axis, offset)`；
- 只修改通过验收的主动关节 `origin.rpy`；
- 不修改 `origin.xyz`、`axis.xyz`、mesh、mimic、连杆长度或机械限位；
- 不覆盖原始 URDF；
- 失败时不生成正式 JSON 和正式修正 URDF。
程序会拒绝名称包含 `zero_calibrated` 或已有校准时间戳特征的源 URDF，防止重复修正。
## 17. 使用已有采样离线重放
当完整 `raw_samples.jsonl` 已存在时，可以使用当前代码重新拟合和生成新产物，不连接相机、不发送机械手命令。
先只读验证：
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
python3 -m g20_thumb_apriltag_calibration.offline_replay \
calibration_output/G20_RIGHT_001/20260811_120146 \
--output-tag AXIS_FRAME_V3
```
确认报告 `passed: true` 后写入新产物：
```bash
python3 -m g20_thumb_apriltag_calibration.offline_replay \
calibration_output/G20_RIGHT_001/20260811_120146 \
--output-tag AXIS_FRAME_V3 \
--write
```
`--output-tag` 只允许安全文件名字符。离线重放拒绝覆盖已有 JSON、URDF 和报告。
离线流程会额外验证：
- 原始 URDF 哈希在处理前后不变；
- 写出的 URDF 等价于求解的运动学修正；
- URDF 未修改关节平移和机械限位；
- 将修正 URDF 作为候选模型重新求解后，残余零偏不超过允许值；
- 正式文件哈希与临时候选文件一致。
## 18. 仿真运行
加载修正 URDF 后，使用同次标定 JSON 将 20 通道 u8 命令映射为 21 个 URDF 关节角：
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
source install/setup.bash
ros2 launch g20_thumb_apriltag_calibration \
calibrated_joint_state_bridge.launch.py \
hand_type:=right \
calibration_file:=/home/lxp/projects/linkerhand_retarget_ros2/calibration_output/G20_RIGHT_001/<时间戳>/g20_right_G20_RIGHT_001_calibration.json
```
默认：
- 订阅 `/cb_right_hand_control_cmd`；
- 发布 `/sim/mujoco/g20/right/joint_state`；
- 发布值只包含动态 `angle_rad`；
- 不会再次叠加 URDF 静态零偏。
启动前必须停止其他向同一仿真关节话题发布的桥接节点，避免多个发布者同时驱动模型。
## 19. 当前右手 V3 样例结果
会话：
```text
G20_RIGHT_001/20260811_120146
```
使用当前 V3 掌坐标系锚定逻辑离线重放后：
| 关节 | 静态 URDF 修正 |
|---|---:|
| `thumb_cmc_roll` | `+3.833362°` |
| `thumb_cmc_yaw` | `+1.310573°` |
| `thumb_cmc_pitch` | `+1.896384°` |
| `thumb_mcp` | `0°`，保留CAD |
| 四指 MCP roll/pitch、PIP | `0°`，保留CAD |
该次 roll 三轮估计为：
```text
3.825454° / 3.866482° / 3.907776°
```
这些值说明当前会话的重复性，不是新标定的固定目标。重新标定应使用新采样独立计算；如果结果明显偏离且无法通过跨轮/留出门限，程序应拒绝生成正式产物。
对应 V3 文件：
```text
src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_20260811_120146_AXIS_FRAME_V3.urdf
calibration_output/G20_RIGHT_001/20260811_120146/
g20_right_G20_RIGHT_001_calibration_AXIS_FRAME_V3.json
g20_right_G20_RIGHT_001_offline_validation_AXIS_FRAME_V3.json
```
## 20. 正式验收检查表
正式使用新产物前逐项确认：
- [ ] 标定使用原始 CAD URDF，而不是旧修正 URDF；
- [ ] 三个相机序列号、内参身份和外参身份匹配；
- [ ] 三个机位接近 30 Hz；
- [ ] 所需 Tag 同时有效率达到 95%；
- [ ] 42 个扫描方向全部完成；
- [ ] 三轮轴方向极差、轨迹残差和回差通过；
- [ ] 第三轮轨迹 MAE/P95 通过；
- [ ] 非零零位在第三轮显著改善原始 URDF；
- [ ] 状态为 `COMPLETE`；
- [ ] 正式 JSON 和修正 URDF 均已生成；
- [ ] 仿真加载的是新 URDF 和同次 JSON；
- [ ] 仿真关节话题只有一个发布者；
- [ ] 使用典型张开、握拳和拇指—食指捏合姿态与实机复核。
docs/O30右手三相机标定与URDF修正操作说明.md
-157
View File
@@ -1,157 +0,0 @@
# O30 右手三相机标定与 URDF 修正操作说明
> 支持范围：O30 右手、20 个有效电机、20 个主动 URDF 关节。
## 1. 固定输入
O30 SDK 工程：
```text
/home/lxp/projects/linkerhand-o30-ros2
```
原始 CAD URDF：
```text
/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf
```
标定基准命令固定为：
```text
[0, 0, 255, 205, 165, 20,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
```
20 个通道均按完整 `255→0→255` 行程扫描。Tag 0～10 的机位、ID 和
父子连杆角色与 G20 右手标定保持一致。
## 2. SDK 与 URDF 映射
| SDK 下标 | SDK 名称 | URDF 关节 |
|---:|---|---|
| 0 | `thumb_roll` | `thumb_cmc_roll` |
| 1 | `thumb_yaw` | `thumb_cmc_yaw` |
| 2～5 | `index/middle/ring/little_yaw` | 四指 `mcp_roll` |
| 6 | `thumb_root1` | `thumb_mcp` |
| 7～10 | 四指 `root1` | 四指 `mcp_pitch` |
| 11～14 | 四指 `root2` | 四指 `pip` |
| 15 | `thumb_tip` | `thumb_ip` |
| 16～19 | 四指 `tip` | 四指 `dip` |
O30 没有 `thumb_cmc_pitch`，也没有 G20 的被动 IP/DIP；O30 JSON 中的
20 个 URDF 关节全部标记为主动关节。
## 3. 扫描任务
右手以小指为四指参考源，共执行 8 个任务、每项 3 轮：
| 顺序 | 关节 | 电机 | 机位 |
|---:|---|---:|---|
| 1 | `thumb_cmc_roll` | 0 | front |
| 2 | `thumb_mcp` | 6 | front |
| 3 | `thumb_ip` | 15 | front |
| 4 | `pinky_mcp_roll` | 5 | front |
| 5 | `pinky_mcp_pitch` | 10 | side |
| 6 | `pinky_pip` | 14 | side |
| 7 | `pinky_dip` | 19 | side |
| 8 | `thumb_cmc_yaw` | 1 | top |
总计 `8 × 3 × 2 = 48` 个唯一扫描方向。O30 每一轮固定按
`0→255`、`255→0` 完成一次往返，因此单关节三轮均为 `0→255→0`，并在
每轮第一个方向前重新初始化活动机位的PnP跟踪。状态中的计划进度只统计48个
唯一方向，另行显示包含自动重扫在内的实际执行方向次数。扫描小指 `mcp_roll` 时，电机
2/3/4 固定为 255，使未贴 Tag 的三指避开正面机位。
O30 命令增大时 URDF 动态角度增大；这与 G20 的命令—角度方向相反，标定
JSON 会保存单调非递减曲线，并在各电机自己的基准命令处严格归零。
O30 的固件反馈端点允许使用独立于 G20 的到位死区：一般通道为 `±4 u8`；
拇指 CMC 侧摆电机 0 在低速命令 `255` 端为 `±9 u8`；
食指 MCP 侧摆电机 2 在命令 `255` 端为 `±8 u8`；
O30 固件的速度 `0` 仍然很快，且实机位置-时间模式无法可靠连续往返，因此标定
强制使用普通位置模式和最低内部速度 `o30_internal_speed_u8:=0`。标定节点只发送
最终目标，O30 SDK 在独立的约 125 Hz 控制线程中生成单字节位置斜坡，避免相机/PnP
计算延迟导致 3～5 u8 的补偿跳步。默认 `o30_command_full_range_seconds:=6.0`，
完整走过 `0～255` 约需 6 秒，两个方向采用相同限速。暂停、终止或异常时立即把
斜坡目标替换为当前反馈并恢复直控模式。该策略不改变安全扫描范围，也不改变四指
MCP 侧摆静态零位固定为 CAD 0 的规则。拇指 CMC 侧摆使用 `1 u8` 细步，其余
通道累计 `3 u8` 后再向固件更新目标；后者为每个目标留出保持时间，避免电机6等
关节因持续刷新单格目标而一直不启动，总体斜率和正反向用时保持不变。
拇指 MCP（电机 6）在命令 0 端实测稳定反馈为 7，因此该端使用 `±8 u8`。
这些门限只判断固件是否已经稳定到位，不改变实际下发的 `0～255` 扫描范围。
## 4. 静态 URDF 零位策略
- 四指 `mcp_roll`：测量小指动态曲线，但四个关节的静态 URDF 零偏全部固定为 0。
- 小指 `mcp_pitch`：可由下游 PIP 轴观测并通过留出验证后修正。
- 小指 `pip`：可由下游 DIP 轴线相位观测并通过留出验证后修正。
- 小指 `dip`、拇指 `ip`：没有下游观察关节，静态零偏固定为 0。
- 食指、中指、无名指：动态曲线从小指曲线按各自基准命令重新归零；静态偏差不继承。
- 拇指 `cmc_roll/cmc_yaw/mcp`：按相邻下游轴逐级求解并执行第三轮留出验证。
修正 URDF 只改通过验证的关节 `origin.rpy`，不改 `origin.xyz`、`axis`、
mesh、连杆长度和 CAD 限位。生成后的 URDF 不能作为下一次标定的源文件。
## 5. 启动
先保证 O30 SDK 已编译，并依次加载 ROS、O30 SDK 和本工程：
```bash
source /opt/ros/jazzy/setup.bash
source /home/lxp/projects/linkerhand-o30-ros2/install/setup.bash
source /home/lxp/projects/linkerhand_retarget_ros2/install/setup.bash
```
预检（不运动）：
```bash
ros2 launch g20_thumb_apriltag_calibration \
three_camera_calibration.launch.py \
hand_model:=O30 hand_type:=right \
serial_number:=O30_RIGHT_001 \
source_urdf_path:=/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/o30_three_camera_extrinsics.yaml \
start_sdk:=false commands_enabled:=false
```
正式启动，蓝色/黑色厂商 CANFD 盒：
```bash
ros2 launch g20_thumb_apriltag_calibration \
three_camera_calibration.launch.py \
hand_model:=O30 hand_type:=right \
serial_number:=O30_RIGHT_001 \
source_urdf_path:=/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/o30_three_camera_extrinsics.yaml \
o30_comm_type:=libcanbus canfd_device:=0 o30_auto_setup:=false \
o30_internal_speed_u8:=0 o30_command_full_range_seconds:=6.0
```
三个机位预检通过后查看状态并启动：
```bash
ros2 topic echo /o30_calibration/status_text
ros2 service call /o30_calibration/start std_srvs/srv/Trigger {}
```
暂停、继续和终止服务分别为：
```text
/o30_calibration/pause
/o30_calibration/resume
/o30_calibration/abort
```
## 6. 产物
会话目录中生成：
```text
raw_samples.jsonl
o30_right_<serial>_calibration.json
```
修正 URDF 默认生成在原始 URDF 同目录，文件名包含
`zero_calibrated_<serial>_<timestamp>`。JSON 的动态角度必须和这次生成的修正
URDF 配套使用；运行时不能再次叠加 `urdf_zero_offset_rad`。
src/g20_thumb_apriltag_calibration/README.md
+250 -20
View File
@@ -1,8 +1,228 @@
# G20 左右手与 O30 右手 AprilTag 标定
# G20 左右手 AprilTag 标定
O30 右手使用相同的三相机/11-Tag 几何采集框架，但采用独立的 20 电机
profile、8 项扫描任务和主动关节零位策略。完整映射、固定基准命令和启动方法见
[O30 右手操作说明](../../docs/O30右手三相机标定与URDF修正操作说明.md)。
## G20右手正式一键标定
固定三相机和19张Tag安装完成后，用户只运行：
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
```
完全独立地只标定大拇指4项任务时，使用：
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
--scope thumb
```
该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机
方向观测以及拇指自己的机械端点；输出URDF从原始CAD生成，只修改4个拇指主动
关节，12个四指关节保持CAD零位。独立结果发布到`latest_thumb_passed`，其JSON是
拇指标定/诊断产物，不冒充可直接运行的完整整手曲线JSON。
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定，才额外使用：
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
--scope thumb \
--base-session calibration_output/G20_RIGHT_001/latest_passed
```
合并模式会冻结基础会话中的12个四指主动零位；发布前再次读取基础会话JSON核对，
任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。
拇指专项结果重复性通过后，不必再做原来的16项整手扫描。以该拇指会话为基础，
只重新采集12项四指任务并合成完整整手URDF：
```bash
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
--scope fingers \
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
```
`fingers`模式严格冻结基础会话中的4个拇指主动零位；最终完整整手URDF中的拇指
零位与专项会话schema-v4数值完全一致。默认`full`也先调用与`thumb`完全相同的
独立拇指内核，再冻结这4个结果求解12个四指零位，因此四指数据不能反向改写
拇指结果。原来的默认`full`仍保留，用于需要16项全部重新采集的情况。
采样文件中的运动域是显式且不可混用的：
`requested_command_u8` 表示下发命令，`feedback_u8` 表示电机反馈。
在线拟合和离线重放通过同一个数据契约投影到曲线索引；新会话不会把含糊的
`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为
`IMPORTING_BASE` 和 `REVALIDATING_INHERITED`，完成复核后才允许机械手运动。
提供`--base-session`时，它必须解析到同一序列号目录下的完整PASS会话；启动前会校验源CAD
URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF，不在旧校准
URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
`thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务，其余任务的原始记录导入后仍按
数据契约和产物哈希检查，但不会以历史四指拟合结果否决本次拇指专项标定。
开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经
完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的
独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与
最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据
当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收
失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记；
恢复时只读取该标记之后的替代采集，不能把同一尝试编号下重扫前后的稳态点合并。
因此已经在线硬门限验收的任务保持已完成，暂停中的任务从任务开头重采，不会因
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量
独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次
采集确定的PnP分支参考，不会因重新初始化切换到另一组平面Tag镜像解。侧面
校验视角的任务级有效率只记录为诊断；G20右手预检若逐帧识别率低于标称值，
但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞，也按完整
轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查，轴线、曲线和模型质量
门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一
采集事务：相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用
产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态
过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查：前三轮
极差默认不得超过0.5°，95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的
紧密簇、仅另一轮越界，自动补采该轮两个方向；无法明确定位时只重采完整yaw任务，
不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入
`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装，但该历史差值
不直接否决当前会话，也不会用旧结果约束新零位。需要强制从第一个关节
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
一个完整同步任务断点；只有两边数据都完整时才复用。
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
16个会话数据求解主动关节URDF零位修正、
21条视觉实测命令曲线发布；四指PIP/DIP的动态曲线均实测，四指DIP静态零位保留CAD。
终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。
四指末端的16 mm Tag允许使用刚性延长杆避挡；软件不假设末端Tag平面与中节Tag
平面平行。延长杆和Tag在一次标定期间必须完全刚性，不能晃动、扭转或重新调整。
侧面掌部基准Tag（ID 4）与各活动指节Tag也不要求安装面平行：首次联合PnP使用
静态多帧刚性、重投影误差和跨轮任务参考选择分支，不再用固定15°安装角门限阻断扫描。
单Tag独立位姿仍保留75°倾角保护；对包含锁定掌部基准和完整父子链的任务，倾角保护只
限制独立选择，不会在联合选择前删除正深度、低重投影的IPPE候选。联合跟踪继续用相邻帧
绝对/相对位姿连续性约束这些斜视候选，最终轴线残差、四轮重复性和隔离留出门限不放宽。
终端中的Tag计数表示“可见”；等待扫描起点时会另列PnP初始化进度和累计拒绝原因。
若联合候选仍然失败，`raw_samples.jsonl` 会按8个反馈计数的区间保存
`group_pnp_candidate_event`，其中包含缺失角色、逐Tag候选数/倾角/重投影、角点和相机内参
哈希，可直接定位运动到哪个机械位置后开始失效，而不需要再次盲扫整条流程。
正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
全部通过后更新。
## G20右手19-Tag底层调试入口
以下内容仅保留给旧会话回放和开发调试；正式一键命令只发布上面的精简
schema v4 JSON，不再生成schema v5运行文件。
新布局用独立参数启用，原有左右手11-Tag流程仍默认使用
`tag_layout:=legacy_11`，两套配置和结果schema互不覆盖：
```bash
ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
hand_type:=right \
tag_layout:=g20_right_19 \
serial_number:=G20_RIGHT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_expected_sha256:=<CAD负责人确认的G20右手源URDF_SHA256>
```
当前产品布局共19张`tag36h11`，所有Tag的黑色码区边长均为16 mm；四指末节ID为
`7,14,16,18`。正面为`0,1,2,3,10,11,12,13`，侧面为
`4,5,6,7,14,15,16,17,18`，上面为`8,9`。详细角色和逐ID尺寸以
`config/three_camera_tags_g20_right_19.yaml`为唯一软件配置源。程序执行4项拇指任务，
以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、
侧面PIP/DIP联合任务，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过
各自的观测质量门限。每个PIP任务只驱动一次对应电机，同时用“手掌→中节Tag”实测PIP、
用“中节Tag→末节Tag”实测被动DIP；四个DIP不再由URDF mimic系数生成，并参加完整视觉
拟合和质量门限。每项正式四轮之前自动低速往返预检
0/127/255可见性；
基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中
允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不
放宽。终端用`锁`表示该固定参考有效，例如`正面[0锁,12✓]`，`✗`才表示需要处理的
实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手，否则缓存参考
失效，必须重新启动标定；两次独立会话之间轻微调整整只手的位置不会改变机械端点
零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计；
任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把
采集质量良好的任务误判为可见性失败。
恢复末端Tag后，程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次
相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差；实体手在反馈0端
能够触掌是独立的机械端点约束。三个thumb CMC主动轴、`thumb_mcp`、四指MCP pitch
和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出，不再把跨相机的
平面PnP绝对相位直接当作编码器零位，也不写死为0。生成URDF时同步修正这些关节的
坐标上限以及相关被动关节的mimic坐标偏置，
保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和
第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、`thumb_mcp`、
四指MCP roll、MCP pitch及PIP，共16个。`thumb_mcp`与四指屈伸关节一样使用
实测全屈曲行程和CAD机械端点联合求解，不直接采用MCP/IP耦合运动的单目PnP相位。
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出；
同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时
运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
等已评审不变量保持不变，过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
`parallel_pose_transitions`（默认true）置false可回退旧的逐电机顺序。
同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出，
不再每轮重复展开/弯曲辅助手指。跨手指组切换时，下一组避障姿态仍然需要、且
当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机
退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
不超过8，只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值，
不会因本次预检帧率或识别密度而改变；旧11-Tag布局仍保留自适应速度兼容逻辑。
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理
零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。
预检、正式四轮和拟合重扫始终使用速度5。
正式roll的每个方向会在经过127时先到位保持0.5秒，再独立保存至少10帧静止Tag/反馈；
方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再
替代静态保持姿态。
前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。
每个任务只在低速递减预检起点执行一次8帧PnP静态初始化；预检往返和四轮正式
扫描连续复用同一帧间分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮
已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中
排除与MCP同步运动明显矛盾（残差超过7.5°）的ID3镜像候选。该先验不生成或缩放
`thumb_ip`曲线；通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。
当前19-Tag产品流程发布精简schema v4：21条运行时曲线全部来自当前会话的视觉实测。
URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`，共16个，
其中三个thumb CMC轴、`thumb_mcp`及四指`mcp_pitch/pip`共12个字段由实测旋转行程
与机械端点联合求解；
写出非零`thumb_mcp`零偏时同步平移其关节坐标上限，并更新被动`thumb_ip`的
`mimic offset`，因此不会改变CAD定义的最大屈曲实体姿态；
`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容，
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入
数度的系统性"绕视线"偏差（亚像素重投影无法发现，会话20260820_105535实测
前后轴向稳定相差11.4°），因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收，
正侧姿态差只写入`cross_view_roll_axis_diagnostic`。四根MCP侧摆轴在产品URDF中
严格平行：小指作为先采集的参考轴，其余三指复用该公共方向并各自独立拟合轴线位置，
避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响，其纯旋转拟合得到的是
随手指结构变化的伪轴线，不能与侧视PIP连杆的物理轴线使用统一距离门限；
两者线距仅记录在诊断中。侧视校验通道
（`*_mcp_roll_side`）的
姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断，不触发重复采集；这两个量来自
近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变，真正发布的正面主轴
仍使用原跨轮严格门限。侧视逐帧`axis_pose_line_rms`同样只作诊断，组合轴线改用四轮
位置RMS验收；径向、平面、圆一致性及可见性门限全部保留。
侧面端视roll的圆轨迹方向已经受
姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限；
径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的
二维投影圆角度，侧面姿态曲线仅保留为诊断；任一正式视角自身四轮不重复或第四轮
留出失败仍会拒绝发布。任一静态目标、第四轮留出、
遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。
8个组合姿态仅保留为开发诊断，正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔
位置验收的手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
`a609d521`验证过的完整四轮相对旋转曲线；全部实测关节均由隔离第四轮逐关节验收。
现场需要区分某根手指的roll机构回差与单机位误差时，可设置
`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次
正面+侧面同步roll，共10个预检/正式方向；任一机位数据不足会重扫同一物理任务，
双机位数据齐全后即使存在轴质量失败也不再自动重采，而是把失败项随双机位结果
一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久
锁定URDF发布，不能用`resume`转换成正式标定。
schema v5明确声明曲线输入域为真实反馈u8；运行桥默认订阅
`/g20/cb_right_hand_state`，并按反馈增减方向选择正程/反程曲线，停止时锁存最后运动
方向。尚未观察到运动方向时使用`255→127`标准分支，不使用两个机械分支的平均值。
schema v4继续兼容旧
命令域。两者都只发布动态角度，不重复叠加已写入URDF的静态偏移。
## 三机位三维关节轴零位标定（schema v4）
@@ -181,7 +401,7 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
@@ -198,13 +418,14 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何
按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
但拟合出的共同掌坐标相位不写入四指 `origin.rpy`；只发布各指相对四指中值的实测
装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械
端点，视觉轴链继续用于轴线、PnP和留出诊断；代码和配置中不保存任何按左右手或
序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。
7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。
视觉依赖链仍为：yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指
pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再决定四个具有机械端点
的拇指主动关节绝对零位；四指PIP/DIP轴线相位也继续用于机构诊断。
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
@@ -213,18 +434,22 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。
JSON；任一曲线点越过CAD限位都会阻止正式发布，程序不会自动扩大URDF限位。
坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
完整关节，每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色
预警，但只要仍超过硬门限，就在当前关节立即使用剩余重试预算
（`provisional_fit_warning_rescan`）；第三次仍超限则当场暂停，不允许预警数据继续到
后续关节。最终拟合仍按原硬门限验收，因此不会在全部任务采完后才回头重采靠前
关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
仍保存在 `raw_samples.jsonl`。
仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的
PnP双簇行程，程序将它判为系统性分支失败并当场停止，不再浪费第3次全关节重扫。
每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
@@ -277,20 +502,24 @@ ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试
```text
calibration_output/G20_LEFT_001/<时间戳>/
g20_left_G20_LEFT_001_calibration.json
src/.../g20_left/
linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
meshes/*.STL
calibration_output/G20_RIGHT_001/<时间戳>/
g20_right_G20_RIGHT_001_calibration.json
src/.../g20_right/
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
meshes/*.STL
```
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
改变；当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解；四指DIP和
`thumb_ip`为被动关节，发布实测动态曲线并保留CAD静态零位，其mimic坐标偏置只随
上游主动关节坐标系变换作等价调整。
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
@@ -319,7 +548,8 @@ ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
```
默认订阅 `/cb_right_hand_control_cmd`，发布
schema v5默认订阅 `/g20/cb_right_hand_state`；schema v4默认订阅
`/g20/cb_right_hand_control_cmd`。两者均发布
`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
因此不会静默退回旧标定。
@@ -640,7 +870,7 @@ for name, joint in data["joints"].items():
不影响 AprilTag 的 ROI 输入。
静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
当前末节16 mm Tag如果中心位置 RMS 持续不合格，应优先增加照明、缩短
相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
src/g20_thumb_apriltag_calibration/config/calibration.yaml
+1 -1
View File
@@ -44,7 +44,7 @@ g20_thumb_calibration:
minimum_detection_hz: 15.0
maximum_hamming: 0
minimum_decision_margin: 30.0
# Trial threshold for the current 10 mm tags (observed at 32-38 px).
# Trial threshold for small/far tags (historically observed at 32-38 px).
# Final acceptance is still guarded by static RMS and random validation.
minimum_edge_pixels: 30.0
# Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.
src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml
+37
View File
@@ -0,0 +1,37 @@
schema_version: 1
model: G20
side: right
tag_layout: g20_right_19
serial_number: G20_RIGHT_001
can_interface: can0
output_root: calibration_output
cameras:
front:
serial_number: DB2163742
camera_name: hikrobot_front_DB2163742
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
side:
serial_number: DB2163749
camera_name: hikrobot_side_DB2163749
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
top:
serial_number: DB2163739
camera_name: hikrobot_top_DB2163739
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
artifacts:
source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
release:
# Each task already contains three training cycles plus an isolated fourth
# holdout, so a second complete hardware session duplicates hours of motion.
required_independent_passes: 1
static_repeatability_deg: 1.0
src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
+86 -25
View File
@@ -1,4 +1,4 @@
/**:
g20_calibration:
ros__parameters:
command_topic: /g20/cb_left_hand_control_cmd
state_topic: /g20/cb_left_hand_state
@@ -16,18 +16,30 @@
normal_calibration_speed: 15
index_roll_calibration_speed: 5
index_flex_calibration_speed: 10
# O30固件速度0仍很快，标定时使用最低内部速度，并由SDK位置斜坡控制平均速度。
o30_internal_speed_u8: 0
# O30 SDK位置斜坡完整走过0～255所需时间；仅O30使用，G20不受影响。
o30_command_full_range_seconds: 6.0
# 19-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量，
# 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束，
# 始终保持速度5；任一方向采样余量不足也保持原速度。
adaptive_formal_speed_enabled: true
adaptive_formal_speed_max_scale: 1.5
adaptive_formal_speed_minimum_bins: 64
adaptive_formal_speed_maximum_bin_gap: 8
speed_setting_settle_seconds: 0.25
# tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。
# tag36h11尺寸是检测角点围成的黑色正方形边长，不包含外围白边。
# 19张Tag的黑色码区外边长均为16 mm。自定义PnP必须与
# apriltag_ros逐ID尺寸一致，禁止用纸张/白边尺寸代替码区尺寸。
tag_size_m: 0.016
# ROS 2无法从YAML空数组推断整数/浮点数组类型。这四个
# 末端Tag仍显式写16 mm，防止节点启动时得到未初始化参数。
tag_size_override_ids: [7, 14, 16, 18]
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016]
repetitions: 3
# 19-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。
g20_right_19_repetitions: 4
preflight_frames: 60
minimum_detection_rate: 0.95
minimum_detection_hz: 15.0
minimum_feedback_hz: 25.0
maximum_hamming: 0
minimum_decision_margin: 30.0
minimum_edge_pixels: 30.0
@@ -40,9 +52,19 @@
pnp_tracker_reset_seconds: 5.0
# 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
pnp_group_initialization_frames: 8
# 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
# 侧面当前任务所需Tag在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
pnp_group_normal_alignment_scale_deg: 5.0
pnp_group_maximum_normal_alignment_deg: 15.0
# 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见；
# 任一角点相对会话基准漂移超过2 px并连续5帧时，判定标定中基准被移动。
fixed_base_maximum_corner_drift_px: 2.0
fixed_base_movement_confirmation_frames: 5
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic
# 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、
# 缩放或替代被动IP的自身Tag实测曲线。
thumb_ip_pnp_coupling_multiplier: 1.03
thumb_ip_pnp_coupling_scale_deg: 3.0
thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
top_pnp_invalid_reset_seconds: 1.0
# 三维位姿必须与实测20通道状态严格按时间戳配对。
maximum_state_image_skew_ms: 50.0
@@ -58,25 +80,26 @@
# 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
axis_maximum_rotation_circle_difference_deg: 1.0
# 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
# 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及第三轮留出。
# 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及最终轮留出。
active_maximum_rotation_orthogonal_rms_deg: 2.5
passive_maximum_rotation_orthogonal_rms_deg: 7.5
zero_maximum_axis_cycle_difference_deg: 0.75
# 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
zero_maximum_axis_cone_mismatch_deg: 5.0
zero_maximum_observability_condition_number: 10000000000.0
zero_maximum_offset_deg: 20.0
# 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。
# 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何
# 求解且不假定电气端点等于CAD上限；thumb_mcp及四指MCP pitch/PIP
# 静态零位由实测全行程与CAD机械端点联合求解，不写死为0。
zero_finger_maximum_offset_deg: 3.0
# 只对实物已确认等同CAD端点的关节使用该限制；CMC电气端点不作此假设。
mechanical_endpoint_maximum_offset_deg: 5.0
endpoint_tolerance_u8: 2.0
# O30实机在多数目标处会稳定相差最多4；仅O30使用，不改变G20门限。
o30_endpoint_tolerance_u8: 4.0
# O30拇指CMC侧摆（电机0）低速命令255端实测稳定反馈为247；该端使用±9。
o30_thumb_cmc_roll_255_endpoint_tolerance_u8: 9.0
# O30食指MCP侧摆（电机2）命令255端实测稳定反馈为248；该端使用±8。
o30_index_mcp_roll_255_endpoint_tolerance_u8: 8.0
# O30拇指MCP（电机6）命令0端实测稳定反馈为7；该端使用±8。
o30_thumb_mcp_zero_endpoint_tolerance_u8: 8.0
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈
# 必须已经稳定；大残差仍由机械卡滞保护处理。
steady_checkpoint_command_feedback_tolerance_u8: 8.0
steady_checkpoint_maximum_feedback_range_u8: 2.0
# 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
# 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
@@ -84,12 +107,18 @@
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
pinky_pip_zero_endpoint_tolerance_u8: 5.0
endpoint_hold_seconds: 0.5
# roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。
baseline_hold_seconds: 0.5
minimum_baseline_hold_frames: 10
# 19-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。
task_precheck_hold_seconds: 2.0
position_timeout_seconds: 30.0
sweep_timeout_seconds: 90.0
# 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停；
# 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
# 这类故障不进入遮挡/超时的三次自动重扫。
motor_stall_timeout_seconds: 8.0
# 低速5也应持续产生反馈进展；5秒无进展即停，减少机构持续顶死时间。
motor_stall_timeout_seconds: 2.0
motor_stall_startup_grace_seconds: 1.0
motor_stall_minimum_progress_u8: 1.0
invalid_timeout_seconds: 3.0
minimum_sweep_frames: 40
@@ -97,15 +126,18 @@
minimum_sweep_bins: 32
maximum_bin_gap: 16
# 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
automatic_sweep_retry_limit: 3
automatic_sweep_retry_limit: 2
# 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
automatic_fit_retry_limit: 2
automatic_motion_retry_limit: 2
# 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll，
# 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。
cross_view_roll_diagnostic_finger: ""
# 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
provisional_warning_ratio: 1.25
retry_minimum_speed: 3
retry_speed_scales: [0.8, 0.6, 0.5]
retry_endpoint_hold_seconds: [0.75, 1.0, 1.25]
retry_speed_scales: [0.8, 0.6]
retry_endpoint_hold_seconds: [0.75, 1.0]
trajectory_maximum_plane_rms_m: 0.004
trajectory_maximum_radial_rms_m: 0.004
@@ -118,15 +150,44 @@
trajectory_maximum_cycle_travel_difference_deg: 3.0
passive_maximum_cycle_travel_difference_deg: 10.0
maximum_monotonic_correction_deg: 2.0
maximum_hysteresis_deg: 5.0
# 旧布局仍用连续扫描正反程差门限；19-Tag产品的连续运动包含速度相关滞后，
# 由方向曲线和最终留出验证建模，不再重复硬判。其绝对正反程门禁使用下面
# 的九点稳态command_maximum_direction_gap_deg。
maximum_hysteresis_deg: 2.0
# 19-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
# 不超过0.5°；四指roll例外：127以255→127为唯一物理零位，反向分支
# 保留实测偏差，并改为检查分支间隙上限及跨轮稳定性。
baseline_maximum_hysteresis_deg: 0.5
directional_zero_maximum_branch_gap_deg: 2.0
directional_zero_maximum_branch_gap_range_deg: 0.3
cross_view_roll_maximum_branch_gap_difference_deg: 0.3
# 正面roll是Tag中心的二维投影角，侧面roll是三维姿态角。允许一个有界的
# 固定比例吸收Tag安装倾角/偏置带来的投影缩放，再严格比较两条曲线形状；
# 比例过大、方向相反、形状RMS及两视角各自的四轮重复性仍会失败。
cross_view_roll_maximum_shape_rms_deg: 1.25
cross_view_roll_maximum_projection_scale_ratio: 1.5
passive_maximum_monotonic_correction_deg: 3.0
passive_maximum_hysteresis_deg: 7.5
passive_maximum_hysteresis_deg: 2.0
# 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域
# 的两条运行曲线仍原样保留固件方向死区，不能把command/feedback差算成回差。
command_maximum_direction_gap_deg: 2.0
# 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。
# 默认无额外随机动作；19-Tag产品最终一轮始终作为不可关闭的留出验证。
validation_enabled: false
# 第四轮留出求解后必须再走8个固定安全组合姿态；三机位规定Tag全部可见
# 且实测20通道到位才允许发布。只保存Tag位姿，不保存原始图像。
# Developer diagnostic only. The formal fourth sweep cycle already gives
# every isolated PIP/DIP pair an independent holdout.
combination_validation_enabled: false
combination_validation_frames: 10
combination_maximum_position_p95_m: 0.003
combination_maximum_orientation_p95_deg: 2.0
validation_command_count: 3
validation_frames: 10
validation_seed: 20260804
validation_timeout_seconds: 20.0
maximum_validation_mae_deg: 1.0
maximum_validation_p95_deg: 2.0
# 19-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
maximum_validation_error_deg: 3.0
zero_maximum_confidence_half_width_deg: 1.5
src/g20_thumb_apriltag_calibration/config/three_camera_tags_o30.yaml → src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml
+25 -25
View File
@@ -1,4 +1,4 @@
/o30_calibration/front/apriltag/apriltag:
/g20_calibration/front/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
@@ -15,32 +15,32 @@
debug: false
pose_estimation_method: pnp
tag:
ids: [0, 1, 2, 3, 10]
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll]
ids: [0, 1, 2, 3, 10, 11, 12, 13]
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
/g20_calibration/side/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
profile: false
max_hamming: 0
detector:
threads: 4
decimate: 1.5
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [4, 5, 6, 15, 17]
frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
/o30_calibration/side/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
profile: false
max_hamming: 0
detector:
threads: 4
decimate: 1.5
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [4, 5, 6, 7]
frames: [side_base, pinky_mcp, pinky_pip, pinky_dip]
sizes: [0.016, 0.016, 0.016, 0.016]
/o30_calibration/top/apriltag/apriltag:
/g20_calibration/top/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
+63
View File
@@ -0,0 +1,63 @@
/g20_calibration/front/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
profile: false
max_hamming: 0
detector:
threads: 4
decimate: 1.5
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [0, 1, 2, 3, 10, 11, 12, 13]
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
/g20_calibration/side/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
profile: false
max_hamming: 0
detector:
threads: 4
# Distal Tags use the same measured 16 mm black-code edge as all others.
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [4, 5, 6, 7, 14, 15, 16, 17, 18]
frames: [side_base, ring_pip, pinky_pip, pinky_dip, ring_dip, middle_pip, middle_dip, index_pip, index_dip]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
/g20_calibration/top/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
profile: false
max_hamming: 0
detector:
threads: 4
decimate: 1.5
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [8, 9]
frames: [top_base, thumb_yaw]
sizes: [0.016, 0.016]
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py
+102 -93
View File
@@ -1,8 +1,13 @@
"""Map supported-hand u8 commands to URDF angles using calibration JSON.
"""Map G20 u8 feedback to URDF joint angles using one calibration JSON.
The static encoder-zero corrections in ``zero_angles`` are already baked into
the corrected URDF joint origins. This bridge therefore publishes only the
dynamic ``angle_rad`` values and never adds the static offsets a second time.
Schema-v5 trajectories are fitted against timestamp-synchronised hardware
feedback, not controller set-points. They must therefore be queried with the
SDK ``hand_state`` topic. The retained schema-v4 path is command-indexed for
backwards compatibility only.
"""
from __future__ import annotations
@@ -16,7 +21,11 @@ import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from .full_hand import get_hand_calibration_profile, validate_compact_payload
from .full_hand import (
get_hand_calibration_profile,
infer_compact_payload_layout,
validate_compact_payload,
)
G20_COMMAND_NAMES: tuple[str, ...] = (
@@ -69,64 +78,9 @@ G20_URDF_JOINT_NAMES: tuple[str, ...] = (
"thumb_mcp",
)
O30_COMMAND_NAMES: tuple[str, ...] = (
"thumb_roll",
"thumb_yaw",
"index_yaw",
"middle_yaw",
"ring_yaw",
"little_yaw",
"thumb_root1",
"index_root1",
"middle_root1",
"ring_root1",
"little_root1",
"index_root2",
"middle_root2",
"ring_root2",
"little_root2",
"thumb_tip",
"index_tip",
"middle_tip",
"ring_tip",
"little_tip",
)
O30_URDF_JOINT_NAMES: tuple[str, ...] = (
"thumb_cmc_roll",
"thumb_cmc_yaw",
"thumb_mcp",
"thumb_ip",
"index_mcp_roll",
"index_mcp_pitch",
"index_pip",
"index_dip",
"middle_mcp_roll",
"middle_mcp_pitch",
"middle_pip",
"middle_dip",
"ring_mcp_roll",
"ring_mcp_pitch",
"ring_pip",
"ring_dip",
"pinky_mcp_roll",
"pinky_mcp_pitch",
"pinky_pip",
"pinky_dip",
)
COMMAND_NAMES_BY_MODEL = {
"G20": G20_COMMAND_NAMES,
"O30": O30_COMMAND_NAMES,
}
URDF_JOINT_NAMES_BY_MODEL = {
"G20": G20_URDF_JOINT_NAMES,
"O30": O30_URDF_JOINT_NAMES,
}
class CalibratedCommandMapper:
"""Validated, model/side-specific lookup from commands to URDF radians."""
"""Validated, side-specific lookup from G20 u8 values to URDF radians."""
def __init__(
self, payload: Mapping[str, Any], *, expected_side: str | None = None
@@ -141,31 +95,58 @@ class CalibratedCommandMapper:
quality = payload["quality"]
if quality.get("passed") is not True:
raise ValueError("calibration quality.passed must be true")
model = str(payload["model"]).upper()
profile = get_hand_calibration_profile(side, model)
urdf_joint_names = URDF_JOINT_NAMES_BY_MODEL[model]
self.model = model
layout_id = infer_compact_payload_layout(payload)
profile = get_hand_calibration_profile(side, layout_id)
self.side = side
self.layout_id = layout_id
self.serial_number = str(payload["serial_number"])
self.command_names = COMMAND_NAMES_BY_MODEL[model]
self.urdf_joint_names = urdf_joint_names
schema_version = int(payload["schema_version"])
self.input_domain = str(
payload.get(
"curve_input_domain",
"command_u8" if schema_version == 4 else "",
)
)
if self.input_domain not in {"command_u8", "feedback_u8"}:
raise ValueError("calibration curve_input_domain is invalid")
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in urdf_joint_names
for name in G20_URDF_JOINT_NAMES
}
self._curves = {
name: tuple(
float(value)
for value in payload["joints"][name]["angle_rad"]
)
for name in urdf_joint_names
for name in G20_URDF_JOINT_NAMES
}
self._decreasing_curves = {
name: tuple(
float(value)
for value in payload["joints"][name].get(
"decreasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in G20_URDF_JOINT_NAMES
}
self._increasing_curves = {
name: tuple(
float(value)
for value in payload["joints"][name].get(
"increasing_rad", payload["joints"][name]["angle_rad"]
)
)
for name in G20_URDF_JOINT_NAMES
}
self._previous_by_motor: dict[int, float] = {}
self._direction_by_motor: dict[int, str] = {}
self.direction_deadband_u8 = 0.5
@staticmethod
def _command_index(value: float) -> int:
command = float(value)
if not math.isfinite(command):
raise ValueError("command positions must be finite")
raise ValueError("G20 command positions must be finite")
return max(0, min(255, int(math.floor(command + 0.5))))
def map_positions(
@@ -180,25 +161,47 @@ class CalibratedCommandMapper:
if len(set(names)) != len(names):
raise ValueError("JointState names must be unique")
by_name = dict(zip((str(name) for name in names), values))
missing = [name for name in self.command_names if name not in by_name]
missing = [name for name in G20_COMMAND_NAMES if name not in by_name]
if missing:
raise ValueError(
f"{self.model} command is missing named channels: "
+ ",".join(missing)
"G20 command is missing named channels: " + ",".join(missing)
)
command = tuple(by_name[name] for name in self.command_names)
command = tuple(by_name[name] for name in G20_COMMAND_NAMES)
else:
if len(values) != len(self.command_names):
if len(values) != len(G20_COMMAND_NAMES):
raise ValueError(
f"unnamed {self.model} command must contain exactly "
f"{len(self.command_names)} positions"
"unnamed G20 command must contain exactly 20 positions"
)
command = values
indices = tuple(self._command_index(value) for value in command)
return tuple(
self._curves[name][indices[self._motor_by_joint[name]]]
for name in self.urdf_joint_names
)
direction_by_motor: dict[int, str | None] = {}
for motor, value in enumerate(command):
previous = self._previous_by_motor.get(motor)
direction = self._direction_by_motor.get(motor)
if previous is not None:
if value > previous + self.direction_deadband_u8:
direction = "increasing"
elif value < previous - self.direction_deadband_u8:
direction = "decreasing"
direction_by_motor[motor] = direction
result: list[float] = []
for name in G20_URDF_JOINT_NAMES:
motor = self._motor_by_joint[name]
direction = direction_by_motor[motor]
curves = (
self._increasing_curves
if direction == "increasing"
else self._decreasing_curves
if direction == "decreasing"
else self._curves
)
result.append(curves[name][indices[motor]])
for motor, value in enumerate(command):
self._previous_by_motor[motor] = value
direction = direction_by_motor[motor]
if direction is not None:
self._direction_by_motor[motor] = direction
return tuple(result)
def load_calibrated_command_mapper(
@@ -211,35 +214,41 @@ def load_calibrated_command_mapper(
return CalibratedCommandMapper(payload, expected_side=expected_side)
def default_input_topic(hand_type: str, input_domain: str) -> str:
side = str(hand_type).lower()
if side not in {"left", "right"}:
raise ValueError("hand_type must be left or right")
if input_domain == "feedback_u8":
return f"/g20/cb_{side}_hand_state"
if input_domain == "command_u8":
return f"/g20/cb_{side}_hand_control_cmd"
raise ValueError("calibration curve_input_domain is invalid")
class CalibratedJointStateBridge(Node):
def __init__(self) -> None:
super().__init__("g20_calibrated_joint_state_bridge")
self.declare_parameter("hand_model", "G20")
self.declare_parameter("hand_type", "right")
self.declare_parameter("calibration_file", "")
self.declare_parameter("input_topic", "")
self.declare_parameter("output_topic", "")
hand_model = str(self.get_parameter("hand_model").value).upper()
hand_type = str(self.get_parameter("hand_type").value).lower()
get_hand_calibration_profile(hand_type, hand_model)
if hand_type not in {"left", "right"}:
raise ValueError("hand_type must be left or right")
calibration_file = str(self.get_parameter("calibration_file").value)
if not calibration_file:
raise ValueError("calibration_file is required")
self.mapper = load_calibrated_command_mapper(
calibration_file, expected_side=hand_type
)
if self.mapper.model != hand_model:
raise ValueError(
f"calibration model {self.mapper.model!r} does not match "
f"requested model {hand_model!r}"
)
input_topic = str(self.get_parameter("input_topic").value).strip()
output_topic = str(self.get_parameter("output_topic").value).strip()
self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd"
self.input_topic = input_topic or default_input_topic(
hand_type, self.mapper.input_domain
)
self.output_topic = (
output_topic
or f"/sim/mujoco/{hand_model.lower()}/{hand_type}/joint_state"
output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
)
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
self.subscription = self.create_subscription(
@@ -247,9 +256,9 @@ class CalibratedJointStateBridge(Node):
)
self._last_error = ""
self.get_logger().info(
f"loaded {hand_type} {hand_model} calibration for "
f"{self.mapper.serial_number}: "
f"{self.input_topic} -> {self.output_topic}"
f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
f"{self.input_topic} ({self.mapper.input_domain}) -> "
f"{self.output_topic}"
)
def _command_callback(self, command: JointState) -> None:
@@ -264,7 +273,7 @@ class CalibratedJointStateBridge(Node):
self._last_error = ""
result = JointState()
result.header = command.header
result.name = list(self.mapper.urdf_joint_names)
result.name = list(G20_URDF_JOINT_NAMES)
result.position = list(positions)
self.publisher.publish(result)
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py
+1349 -258
View File
File diff suppressed because it is too large Load Diff
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py
+1206 -125
View File
File diff suppressed because it is too large Load Diff
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py
+727
View File
@@ -0,0 +1,727 @@
"""One-command runner for a registered hand-calibration product."""
from __future__ import annotations
import argparse
from datetime import datetime
import json
import os
from pathlib import Path
import signal
import subprocess
import sys
import time
import traceback
from typing import Any, Mapping
from ament_index_python.packages import get_package_share_directory
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from std_srvs.srv import Trigger
from .hikrobot_camera import configure_fastdds_large_image_transport
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
from .product import ProductConfig, load_product_config, sha256_file
from .publication import atomic_session_pointer, finalize_session_artifacts
from .storage import atomic_write_json
EXIT_PASS = 0
EXIT_QUALITY = 2
EXIT_SAFETY = 3
STATUS_TIMEOUT_SECONDS = 90.0
FITTING_STATUS_TIMEOUT_SECONDS = 600.0
def _status_timeout_seconds(status: Mapping[str, Any]) -> float:
"""Return the watchdog deadline for the node's current phase.
Motion and acquisition are expected to publish twice a second and retain
the strict transport watchdog. The final 3-D fit is intentionally a
synchronous, CPU-bound operation, so its executor cannot service the
status timer until the fit returns. The node publishes an explicit
FITTING status immediately before entering that operation; allow that
known phase enough time without weakening motion safety checks.
"""
if str(status.get("state", "")) == "FITTING":
return FITTING_STATUS_TIMEOUT_SECONDS
return STATUS_TIMEOUT_SECONDS
def _calibration_node_exited_before_status(log_path: Path) -> bool:
"""Detect a launch child crash while the parent launch is still alive."""
try:
with log_path.open("rb") as stream:
stream.seek(0, os.SEEK_END)
size = stream.tell()
stream.seek(max(0, size - 65536), os.SEEK_SET)
tail = stream.read().decode("utf-8", errors="replace")
except OSError:
return False
return (
"[three_camera_calibration_node-" in tail
and "]: process has died" in tail
)
class CalibrationMonitor(Node):
def __init__(self) -> None:
super().__init__("g20_calibration_product_runner")
self.latest_status: dict[str, Any] = {}
self.last_status_at = time.monotonic()
self.start_requested = False
self.start_future: Any = None
self.abort_future: Any = None
self.create_subscription(String, "/g20_calibration/status", self._status, 10)
self.start_client = self.create_client(Trigger, "/g20_calibration/start")
self.abort_client = self.create_client(Trigger, "/g20_calibration/abort")
def _status(self, message: String) -> None:
try:
payload = json.loads(message.data)
except (TypeError, json.JSONDecodeError):
return
if isinstance(payload, dict):
self.latest_status = payload
self.last_status_at = time.monotonic()
def maybe_start(self) -> None:
if self.start_requested or self.latest_status.get("state") != "WAIT_START":
return
if not self.start_client.service_is_ready():
self.start_client.wait_for_service(timeout_sec=0.05)
return
self.start_requested = True
self.start_future = self.start_client.call_async(Trigger.Request())
def abort(self) -> None:
if not self.abort_client.service_is_ready():
self.abort_client.wait_for_service(timeout_sec=1.0)
if self.abort_client.service_is_ready():
self.abort_future = self.abort_client.call_async(Trigger.Request())
class ProgressConsole:
def __init__(self, serial_number: str) -> None:
self.serial_number = serial_number
self.estimator = ProgressEstimator.start()
self.last_text = ""
self.last_issue = ""
def update(self, status: Mapping[str, Any]) -> None:
text = render_progress_zh(self.serial_number, status, self.estimator)
if text == self.last_text:
return
self.last_text = text
if sys.stdout.isatty():
sys.stdout.write("\x1b[2J\x1b[H" + text + "\n")
sys.stdout.flush()
else:
print(text, flush=True)
reason = str(status.get("reason", ""))
if reason.startswith("automatic_retry_") and reason != self.last_issue:
self.last_issue = reason
active = status.get("active", {})
print(
"\n".join(
[
f"⚠ 当前任务出现问题：{reason.removeprefix('automatic_retry_')}",
f"系统处理：只重扫当前任务（第 {active.get('automatic_retry_count', 1)}/2 次）",
]
),
flush=True,
)
def _default_product_config() -> Path:
try:
installed = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
) / "config" / "g20_right_product.yaml"
if installed.is_file():
return installed
except Exception:
pass
return (
Path.cwd()
/ "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml"
).resolve()
def _launch_command(
config: ProductConfig,
session: Path,
*,
resume_from: Path | None = None,
recalibration_scope: str = "full",
) -> list[str]:
values = {
"model": config.model,
"hand_type": config.side,
"tag_layout": config.tag_layout,
"serial_number": config.serial_number,
"can_interface": config.can_interface,
"session_dir": str(session),
"output_root": str(config.output_root),
"camera_extrinsics_file": str(config.camera_extrinsics),
"source_urdf_path": str(config.source_urdf),
"source_urdf_expected_sha256": config.source_urdf_sha256,
"corrected_urdf_output_dir": str(session),
"calibration_config": str(config.calibration_config),
"tag_config": str(config.tag_config),
"commands_enabled": "true",
"start_cameras": "true",
"start_sdk": "true",
"record_bag": "false",
"validation_enabled": "false",
"recalibration_scope": recalibration_scope,
}
if resume_from is not None:
values["resume_raw_samples_path"] = str(
resume_from / "raw_samples.jsonl"
)
for view, camera in config.cameras.items():
values[f"{view}_camera_serial"] = camera["serial_number"]
values[f"{view}_camera_name"] = camera["camera_name"]
values[f"{view}_camera_info_url"] = camera["camera_info"]
return [
"ros2",
"launch",
"g20_thumb_apriltag_calibration",
"three_camera_calibration.launch.py",
*(f"{name}:={value}" for name, value in values.items()),
]
def _stop_stack(process: subprocess.Popen[Any]) -> None:
if process.poll() is not None:
return
try:
os.killpg(process.pid, signal.SIGINT)
except ProcessLookupError:
return
try:
process.wait(timeout=15.0)
except subprocess.TimeoutExpired:
try:
os.killpg(process.pid, signal.SIGTERM)
except ProcessLookupError:
return
try:
process.wait(timeout=5.0)
except subprocess.TimeoutExpired:
try:
os.killpg(process.pid, signal.SIGKILL)
except ProcessLookupError:
return
process.wait(timeout=5.0)
def _write_trace(log_path: Path, error: BaseException) -> None:
with log_path.open("a", encoding="utf-8") as stream:
stream.write("\n[one-command exception]\n")
traceback.print_exception(type(error), error, error.__traceback__, file=stream)
def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None:
monitor.abort()
deadline = time.monotonic() + float(timeout_seconds)
while time.monotonic() < deadline and rclpy.ok():
rclpy.spin_once(monitor, timeout_sec=0.1)
if monitor.latest_status.get("state") == "ABORTED":
return
def _run_hardware_session(
config: ProductConfig,
session: Path,
*,
resume_from: Path | None = None,
recalibration_scope: str = "full",
) -> tuple[dict[str, Any], int]:
session.mkdir(parents=True, exist_ok=False)
(session / "raw_samples.jsonl").touch()
log_path = session / "calibration.log"
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
atomic_session_pointer(config.session_root, "latest_attempt", session)
monitor = CalibrationMonitor()
console = ProgressConsole(config.serial_number)
process: subprocess.Popen[Any] | None = None
latest_status: dict[str, Any] = {
"state": "PREFLIGHT",
"reason": "starting_ros_stack",
"progress": 0.0,
"views": {},
"feedback_hz": 0.0,
}
exit_code = EXIT_QUALITY
try:
process = subprocess.Popen(
_launch_command(
config,
session,
resume_from=resume_from,
recalibration_scope=recalibration_scope,
),
cwd=config.workspace,
stdout=log_stream,
stderr=subprocess.STDOUT,
text=True,
start_new_session=True,
)
launched_at = time.monotonic()
last_render = 0.0
last_startup_log_check = 0.0
while True:
rclpy.spin_once(monitor, timeout_sec=0.1)
if monitor.latest_status:
latest_status = monitor.latest_status
monitor.maybe_start()
now = time.monotonic()
if now - last_render >= 0.5:
console.update(latest_status)
last_render = now
if monitor.start_future is not None and monitor.start_future.done():
response = monitor.start_future.result()
if response is None or not response.success:
message = "start service failed" if response is None else response.message
raise RuntimeError(f"CFG-START-008:{message}")
monitor.start_future = None
state = str(latest_status.get("state", ""))
if state == "COMPLETE":
exit_code = EXIT_PASS
break
if state in {"PAUSED", "ABORTED"}:
reason = str(latest_status.get("reason", "calibration_paused"))
exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY
if state == "PAUSED" and "stall" not in reason:
# Ordinary quality failures return to the reviewed baseline
# before the process tree is stopped. Mechanical stalls
# deliberately skip this path and keep the current pose.
failure_status = dict(latest_status)
_request_safe_abort(monitor)
latest_status = failure_status
break
if process.poll() is not None:
raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}")
if (
not monitor.latest_status
and now - last_startup_log_check >= 0.5
):
last_startup_log_check = now
log_stream.flush()
if _calibration_node_exited_before_status(log_path):
raise RuntimeError(
"CAM-STATUS-202:calibration node exited before status"
)
if (
not monitor.latest_status
and now - launched_at > STATUS_TIMEOUT_SECONDS
):
raise RuntimeError("CAM-STATUS-202:no calibration status received")
if (
monitor.latest_status
and now - monitor.last_status_at
> _status_timeout_seconds(monitor.latest_status)
):
raise RuntimeError("MOTION-COMM-303:calibration status stopped")
except KeyboardInterrupt as error:
latest_status["state"] = "ABORTED"
latest_status["reason"] = "operator_abort"
_request_safe_abort(monitor)
_write_trace(log_path, error)
exit_code = EXIT_SAFETY
except BaseException as error:
latest_status["state"] = "PAUSED"
latest_status["reason"] = str(error)
_write_trace(log_path, error)
exit_code = EXIT_QUALITY
finally:
if process is not None:
_stop_stack(process)
monitor.destroy_node()
log_stream.flush()
os.fsync(log_stream.fileno())
log_stream.close()
if exit_code != EXIT_PASS:
_, block = build_failure_report(
config,
session,
latest_status,
reason=str(latest_status.get("reason", "unknown_failure")),
)
print(block, flush=True)
return latest_status, exit_code
def _startup_failure_block(path: Path, error: BaseException) -> str:
return "\n".join(
[
"========== 请复制以下内容给开发者 ==========",
"结果：FAIL",
"错误代码：CFG-PRODUCT-001",
"失败阶段：启动静态预检",
f"问题：{error}",
f"产品配置：{path}",
"自动处理：未启动相机、SDK或机械手运动",
"建议：复制本诊断块给开发者，不要手工修改哈希绕过检查。",
"========== 复制结束 ==========",
]
)
def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
"""Return the newest compatible failed attempt, never a passed session.
Do not trust only ``latest_attempt``. A process interrupted during the
device-only startup gate may have already moved that pointer while still
containing no ``session_start`` checkpoint. In that case walk backwards
to the preceding usable failed session instead of throwing away hours of
completed tasks.
"""
root = config.session_root
try:
resolved_root = root.resolve(strict=True)
except OSError:
return None
candidates: list[Path] = []
pointer = config.session_root / "latest_attempt"
if pointer.exists():
try:
candidates.append(pointer.resolve(strict=True))
except OSError:
pass
try:
candidates.extend(
sorted(
(
path
for path in root.iterdir()
if path.is_dir() and not path.name.startswith("latest_")
),
key=lambda path: path.name,
reverse=True,
)
)
except OSError:
return None
passed_pointer = config.session_root / "latest_passed"
passed: Path | None = None
if passed_pointer.exists():
try:
passed = passed_pointer.resolve(strict=True)
except OSError:
pass
seen: set[Path] = set()
for unresolved in candidates:
try:
candidate = unresolved.resolve(strict=True)
except OSError:
continue
if candidate in seen:
continue
seen.add(candidate)
if candidate.parent != resolved_root or not candidate.is_dir():
continue
# A failed attempt older than the current formal release is stale and
# must not seed a new independent calibration.
if passed is not None and candidate.name <= passed.name:
continue
raw_path = candidate / "raw_samples.jsonl"
if not raw_path.is_file():
continue
summary_path = candidate / "calibration_summary_zh.json"
summary: dict[str, Any] | None = None
if summary_path.is_file():
try:
loaded = json.loads(summary_path.read_text(encoding="utf-8"))
except (OSError, json.JSONDecodeError):
continue
if not isinstance(loaded, dict) or loaded.get("result") != "FAIL":
continue
summary = loaded
hashes = summary.get("hashes", {})
if not isinstance(hashes, Mapping):
continue
if (
str(hashes.get("source_urdf_sha256", ""))
!= config.source_urdf_sha256
or str(hashes.get("camera_extrinsics_sha256", ""))
!= config.camera_extrinsics_sha256
):
continue
start: dict[str, Any] | None = None
try:
with raw_path.open("r", encoding="utf-8") as stream:
for line in stream:
if not line.strip():
continue
value = json.loads(line)
if (
isinstance(value, dict)
and value.get("kind") == "session_start"
):
start = value
break
except (OSError, json.JSONDecodeError):
continue
if (
start is None
or start.get("hand_type") != config.side
or start.get("tag_layout") != config.tag_layout
or start.get("source_urdf_sha256")
!= config.source_urdf_sha256
):
continue
if summary is None:
# Ctrl+C can terminate the ROS launch tree before the wrapper gets
# a chance to create calibration_summary_zh.json. The immutable
# checkpoint itself is enough to resume only after independently
# proving that its external geometry still matches the product.
try:
checkpoint_extrinsics = Path(
str(start["camera_extrinsics_file"])
).expanduser().resolve(strict=True)
if sha256_file(checkpoint_extrinsics) != (
config.camera_extrinsics_sha256
):
continue
except (KeyError, OSError, ValueError):
continue
return candidate
return None
def _resolve_partial_base_session(
config: ProductConfig, base_session: str | Path | None
) -> Path:
"""Validate the complete passed session that donates non-target tasks."""
if base_session is None or not str(base_session).strip():
raise ValueError(
"partial scope requires --base-session pointing to a passed "
"complete G20 right session"
)
candidate = Path(base_session).expanduser().resolve(strict=True)
root = config.session_root.resolve()
if candidate.parent != root or not candidate.is_dir():
raise ValueError(
"base session must resolve to a direct session directory under "
f"{root}"
)
raw_path = candidate / "raw_samples.jsonl"
summary_path = candidate / "calibration_summary_zh.json"
payload_path = (
candidate
/ f"g20_right_{config.serial_number}_calibration.json"
)
for required in (raw_path, summary_path, payload_path):
if not required.is_file():
raise ValueError(f"base session is missing required artifact: {required}")
try:
summary = json.loads(summary_path.read_text(encoding="utf-8"))
except json.JSONDecodeError as error:
raise ValueError("base session summary is invalid JSON") from error
if (
not isinstance(summary, dict)
or summary.get("result") != "PASS"
or not bool(summary.get("quality", {}).get("passed"))
):
raise ValueError("base session is not a formally passed session")
hashes = summary.get("hashes", {})
expected_hashes = {
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
}
if not isinstance(hashes, Mapping) or any(
str(hashes.get(name, "")) != expected
for name, expected in expected_hashes.items()
):
raise ValueError(
"base session source URDF, camera extrinsics or calibration "
"configuration differs from the current product"
)
return candidate
def run(
config_path: str | Path,
*,
workspace: str | Path | None = None,
preflight_only: bool = False,
allow_resume: bool = True,
scope: str = "full",
base_session: str | Path | None = None,
) -> int:
path = Path(config_path).expanduser().resolve()
try:
# Resolve every file and camera identity before allowing a hardware
# process to start. A second load enables the real CAN existence gate.
config = load_product_config(path, workspace=workspace, check_can=False)
load_product_config(path, workspace=workspace, check_can=True)
selected_scope = str(scope).strip().lower()
if selected_scope not in {"full", "thumb", "fingers"}:
raise ValueError("scope must be one of: full, thumb, fingers")
if selected_scope == "full" and base_session is not None:
raise ValueError(
"--base-session is valid only with --scope thumb/fingers"
)
partial_base = None
if selected_scope == "fingers" or base_session is not None:
partial_base = _resolve_partial_base_session(config, base_session)
except BaseException as error:
print(_startup_failure_block(path, error), flush=True)
return EXIT_QUALITY
if preflight_only:
print("PASS：产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。")
return EXIT_PASS
config.session_root.mkdir(parents=True, exist_ok=True)
resume_candidate = (
partial_base
if selected_scope != "full"
else (_automatic_resume_candidate(config) if allow_resume else None)
)
if resume_candidate is not None:
if selected_scope == "thumb":
print(
"拇指专项标定：四指任务继承自已通过会话 "
f"{resume_candidate.name}；4项拇指任务将全部重新采集，"
"四指零位保持不变。",
flush=True,
)
elif selected_scope == "fingers":
print(
"四指专项标定：拇指任务和4个拇指零位继承自已通过会话 "
f"{resume_candidate.name}；12项四指任务将全部重新采集。",
flush=True,
)
else:
print(
"检测到兼容的失败会话，将恢复已完整通过的关节任务："
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
flush=True,
)
elif selected_scope == "thumb":
print(
"独立拇指标定：不导入四指会话；仅采集4项拇指任务，"
"四指URDF零位保持原始CAD值。",
flush=True,
)
maximum_sessions = config.required_independent_passes
for pass_index in range(maximum_sessions):
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session = config.session_root / stamp
while session.exists():
time.sleep(1.0)
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session = config.session_root / stamp
if maximum_sessions > 1:
print(f"正式标定复验：第 {pass_index + 1}/{maximum_sessions} 次", flush=True)
status, code = _run_hardware_session(
config,
session,
resume_from=(
resume_candidate
if selected_scope != "full" or pass_index == 0
else None
),
recalibration_scope=selected_scope,
)
if code != EXIT_PASS:
return code
# Keep the exact node-side completion contract durable before the
# independent publication layer starts. If publication itself fails,
# developers can re-run artifact checks without repeating motion or
# inventing lost combination-validation metrics.
atomic_write_json(session / "node_status.json", status)
try:
summary, release_ready = finalize_session_artifacts(
config, session, node_status=status
)
except BaseException as error:
_write_trace(session / "calibration.log", error)
status = dict(status)
status["state"] = "PAUSED"
status["reason"] = f"PUB-ARTIFACT-601:{error}"
_, block = build_failure_report(config, session, status, reason=status["reason"])
print(block, flush=True)
return EXIT_QUALITY
if release_ready:
result_pointer = (
config.session_root / "latest_thumb_passed"
if selected_scope == "thumb" and partial_base is None
else config.session_root / "latest_passed"
)
print(
"\n".join(
[
f"PASS：{config.model} {config.side} 标定、URDF修正和复验全部通过。",
f"正式结果：{result_pointer}",
f"JSON：{session / summary['artifacts']['json']}",
f"URDF：{summary['artifacts']['urdf']}",
]
),
flush=True,
)
return EXIT_PASS
print("本次会话质量PASS；正在自动执行第二次独立完整复验。", flush=True)
return EXIT_QUALITY
def main(args: list[str] | None = None) -> None:
parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定")
parser.add_argument("--config", default=str(_default_product_config()))
parser.add_argument("--workspace", default=None)
parser.add_argument("--preflight-only", action="store_true")
parser.add_argument(
"--scope",
choices=("full", "thumb", "fingers"),
default="full",
help=(
"full重新标定全手；thumb仅重采4项拇指任务；"
"fingers复用已认证拇指并仅重采12项四指任务"
),
)
parser.add_argument(
"--base-session",
default=None,
help=(
"可选：thumb模式将结果合并到该完整会话；"
"fingers模式必须提供该基础会话"
),
)
parser.add_argument(
"--no-resume",
action="store_true",
help="忽略失败会话，从第一个关节开始全新采集",
)
arguments = parser.parse_args(args)
configure_fastdds_large_image_transport()
ros_log_dir = Path(
os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs")
)
ros_log_dir.mkdir(parents=True, exist_ok=True)
rclpy.init()
try:
code = run(
arguments.config,
workspace=arguments.workspace,
preflight_only=arguments.preflight_only,
allow_resume=not arguments.no_resume,
scope=arguments.scope,
base_session=arguments.base_session,
)
finally:
if rclpy.ok():
rclpy.shutdown()
raise SystemExit(code)
if __name__ == "__main__":
main()
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py
+590
View File
@@ -0,0 +1,590 @@
"""Stable Chinese progress and copy/paste diagnostics for non-expert users."""
from __future__ import annotations
from dataclasses import dataclass
import json
import math
from pathlib import Path
import time
from typing import Any, Mapping
from .product import ProductConfig
from .storage import atomic_write_json
from .three_camera_diagnostics import JOINT_NAMES_ZH, STATE_NAMES_ZH, three_camera_reason_zh
def _duration(seconds: float | None) -> str:
if seconds is None or not math.isfinite(seconds) or seconds < 0.0:
return "计算中"
value = int(round(seconds))
return f"{value // 60}分{value % 60:02d}秒"
def _task_tag_id_status(views: Mapping[str, Any]) -> str:
"""Render every currently required Tag ID with its live visibility."""
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
ordered_views = [
*[name for name in ("front", "side", "top") if name in views],
*sorted(name for name in views if name not in labels),
]
groups: list[str] = []
for name in ordered_views:
item = views.get(name, {})
if not isinstance(item, Mapping):
continue
required = sorted({int(value) for value in item.get("required_tag_ids", [])})
if not required:
continue
visible = {int(value) for value in item.get("detected_tag_ids", [])}
locked = {
int(value)
for value in item.get("locked_reference_tag_ids", [])
}
ids = ",".join(
f"{tag_id}{'锁' if tag_id in locked else '✓' if tag_id in visible else '✗'}"
for tag_id in required
)
groups.append(f"{labels.get(name, name)}[{ids}]")
if not groups:
return ""
return (
" 当前任务ID："
+ " ".join(groups)
+ "（✓实时可见/锁=基准锁定/✗不可用）"
)
def _pnp_wait_status(views: Mapping[str, Any]) -> str:
"""Explain why visible Tags have not produced a usable 3-D pose."""
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
details: list[str] = []
for name in ("front", "side", "top"):
item = views.get(name, {})
if not isinstance(item, Mapping) or item.get("pnp_pose_valid"):
continue
parts: list[str] = []
progress = item.get("pnp_initialization_progress")
if isinstance(progress, Mapping):
parts.append(
"初始化"
f"{int(progress.get('accepted', 0))}/"
f"{int(progress.get('required', 0))}"
)
counts: dict[str, int] = {}
for field in ("pnp_rejection_counts", "group_pnp_rejection_counts"):
values = item.get(field, {})
if not isinstance(values, Mapping):
continue
for reason, count in values.items():
counts[str(reason)] = counts.get(str(reason), 0) + int(count)
if counts:
common = sorted(
counts.items(), key=lambda pair: (-pair[1], pair[0])
)[:2]
parts.append(
"拒绝=" + ",".join(f"{reason}×{count}" for reason, count in common)
)
if parts:
details.append(f"{labels[name]} " + "；".join(parts))
if not details:
return ""
return "PnP：" + " | ".join(details) + "（Tag可见不等于三维位姿有效）"
@dataclass
class ProgressEstimator:
started_at: float
last_progress: float = 0.0
last_progress_at: float = 0.0
seconds_per_fraction: float | None = None
@classmethod
def start(cls) -> "ProgressEstimator":
now = time.monotonic()
return cls(started_at=now, last_progress_at=now)
def remaining(self, progress: float, now: float | None = None) -> float | None:
current = time.monotonic() if now is None else float(now)
value = float(max(0.0, min(1.0, progress)))
delta = value - self.last_progress
elapsed = current - self.last_progress_at
if delta >= 0.002 and elapsed > 0.0:
estimate = elapsed / delta
self.seconds_per_fraction = (
estimate
if self.seconds_per_fraction is None
else 0.8 * self.seconds_per_fraction + 0.2 * estimate
)
self.last_progress = value
self.last_progress_at = current
if value >= 1.0:
return 0.0
if self.seconds_per_fraction is not None:
return max(0.0, self.seconds_per_fraction * (1.0 - value))
if value >= 0.02:
return max(0.0, (current - self.started_at) * (1.0 - value) / value)
return None
def render_progress_zh(
serial_number: str,
status: Mapping[str, Any],
estimator: ProgressEstimator,
) -> str:
progress = float(status.get("progress", 0.0))
active = status.get("active", {})
if not isinstance(active, Mapping):
active = {}
state = str(status.get("state", "PREFLIGHT"))
stage = STATE_NAMES_ZH.get(state, state)
preflight_mode = str(status.get("preflight_mode", ""))
if preflight_mode == "device_only_before_baseline":
stage = "设备连接预检"
elif preflight_mode == "recovering_baseline":
stage = "安全恢复基准形态"
elif preflight_mode == "baseline_tags_after_recovery":
stage = "基准姿态标签预检"
elif str(status.get("reason", "")) == (
"holding_same_finger_clearance_before_next_task"
):
stage = "保持避让姿态，切换同指下一项"
elif str(status.get("reason", "")) == (
"waiting_for_task_tags_at_sweep_start"
):
stage = "已到扫描起点，等待任务Tag"
joints = active.get("joints", [])
joint = "/".join(JOINT_NAMES_ZH.get(str(name), str(name)) for name in joints)
if str(active.get("task_name", "")).endswith("_roll_multiview") and joints:
joint = (
JOINT_NAMES_ZH.get(str(joints[0]), str(joints[0]))
+ "（正面+侧面同步）"
)
if not joint:
import_progress = status.get("base_import", {})
import_fraction = (
float(import_progress.get("fraction", 0.0))
if isinstance(import_progress, Mapping)
else 0.0
)
joint = str(active.get("label_zh", "")) or (
f"导入基础会话 {100.0 * import_fraction:.1f}%"
if state in {"IMPORTING_BASE", "REVALIDATING_INHERITED"}
else (
"等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
)
)
direction = {
"decreasing": "递减",
"increasing": "递增",
}.get(str(active.get("direction", "")), "-")
cycle = active.get("cycle", "-")
repetitions = active.get("repetitions", 4)
requested = active.get(
"current_motion_target_u8",
active.get("command_u8", active.get("pose_name", "-")),
)
actual = active.get("actual_u8", "-")
views = status.get("views", {})
if not isinstance(views, Mapping):
views = {}
ready_cameras = sum(
bool(item.get("camera_info_valid") and item.get("camera_extrinsics_valid"))
for item in views.values()
if isinstance(item, Mapping)
)
required_tags = sum(
len(item.get("required_tag_ids", []))
for item in views.values()
if isinstance(item, Mapping)
)
visible_tags = sum(
len(
{
*item.get("detected_tag_ids", []),
*item.get("locked_reference_tag_ids", []),
}
)
for item in views.values()
if isinstance(item, Mapping)
)
locked_tag_count = sum(
len(item.get("locked_reference_tag_ids", []))
for item in views.values()
if isinstance(item, Mapping)
)
configured_tags = sum(
len(item.get("configured_tag_ids", item.get("required_tag_ids", [])))
for item in views.values()
if isinstance(item, Mapping)
)
visible_configured_tags = sum(
len(
item.get(
"visible_configured_tag_ids",
item.get("detected_tag_ids", []),
)
)
for item in views.values()
if isinstance(item, Mapping)
)
occlusion_allowed = bool(
state in {"PREFLIGHT", "WAIT_START"}
or active.get("pose_name")
)
if preflight_mode == "device_only_before_baseline":
tag_status = (
f"当前 {visible_configured_tags}/{configured_tags}"
"（基准恢复后检查）"
)
elif preflight_mode == "recovering_baseline":
tag_status = (
f"当前 {visible_configured_tags}/{configured_tags}"
"（恢复中不作为门槛）"
)
elif occlusion_allowed:
tag_status = (
f"当前 {visible_configured_tags}/{configured_tags}（允许遮挡） "
f"本阶段必需 {visible_tags}/{required_tags}"
)
else:
tag_status = (
f"{visible_tags}/{required_tags} 可见"
+ (f"（含锁定 {locked_tag_count}）" if locked_tag_count else "")
)
task_tag_ids = (
_task_tag_id_status(views)
if state
in {
"PREPARE_SWEEP",
"SWEEP",
"VALIDATION_MOVE",
"VALIDATION_CAPTURE",
}
else ""
)
retry = int(active.get("automatic_retry_count", 0))
fit_attempt = int(active.get("fit_attempt", 1))
fit_attempt_limit = int(active.get("fit_attempt_limit", 1))
feedback_hz = float(status.get("feedback_hz", 0.0))
eta = (
"等待Tag"
if str(status.get("reason", ""))
== "waiting_for_task_tags_at_sweep_start"
else _duration(estimator.remaining(progress))
)
lines = [
f"[{serial_number}] 标定中 {progress * 100:5.1f}% 预计剩余 {eta}",
f"阶段：{stage}（第 {cycle}/{repetitions} 轮）",
f"任务：{joint} 命令/反馈：{requested}/{actual} 方向：{direction}",
f"Tag：{tag_status}{task_tag_ids} 相机：{ready_cameras}/3 正常 反馈：{feedback_hz:.1f} Hz",
f"质量：有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry} 次",
]
if str(status.get("reason", "")) == "waiting_for_task_tags_at_sweep_start":
pnp_wait = _pnp_wait_status(views)
if pnp_wait:
lines.append(pnp_wait)
fit_retry_cycles = [
int(cycle) for cycle in active.get("fit_retry_cycles", [])
]
if fit_attempt > 1 and fit_retry_cycles:
cycle_text = "/".join(str(cycle) for cycle in fit_retry_cycles)
if len(fit_retry_cycles) >= repetitions:
lines.append(
"拟合：首次拟合未能定位单一异常轮，"
f"联合补采第 {cycle_text} 轮双向"
)
else:
lines.append(
f"拟合：首次拟合定位第 {cycle_text} 轮需复核，"
"仅补采对应轮次双向；其余轮次数据保留"
)
elif fit_attempt_limit > 1:
lines.append(
f"拟合：任务级硬门限检查（最多允许 "
f"{fit_attempt_limit - 1} 次异常轮补采）"
)
resume = status.get("resume", {})
if isinstance(resume, Mapping) and resume.get("used"):
lines.append(
"断点：已恢复 "
f"{int(resume.get('completed_task_count', 0))}/"
f"{int(resume.get('total_task_count', 16))} 个完整任务；"
"失败任务已丢弃并重新采集"
)
return "\n".join(lines)
def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, str]:
value = str(reason)
active = status.get("active", {})
if not isinstance(active, Mapping):
active = {}
views = status.get("views", {})
missing = [
tag
for item in views.values()
if isinstance(item, Mapping)
for tag in item.get("missing_tag_ids", [])
] if isinstance(views, Mapping) else []
group_pnp_reasons = [
str(item.get("group_pnp_reason"))
for item in views.values()
if isinstance(item, Mapping) and item.get("group_pnp_reason")
] if isinstance(views, Mapping) else []
pnp_diagnostics_present = bool(
group_pnp_reasons
or (
isinstance(views, Mapping)
and any(
isinstance(item, Mapping)
and any(
item.get(field)
for field in (
"pnp_rejection_counts",
"group_pnp_rejection_counts",
"pnp_initialization_progress",
"group_missing_candidate_roles",
)
)
for item in views.values()
)
)
)
if value.startswith("CFG-"):
return value, "产品配置或文件预检失败", "不要移动相机；复制本诊断块给开发者。"
if value.startswith("DATA-CONTRACT-"):
return (
value.split(":", 1)[0],
"标定样本的请求命令与反馈值数据契约不完整",
"机械手无需重新采集；保留原始会话并用离线重放验证，"
"复制契约错误给开发者。",
)
if value.startswith("CAM-STATUS-202"):
return (
"CAM-STATUS-202",
"标定节点启动后未发布状态",
"机械手尚未开始运动；查看calibration.log中首个节点异常后从断点继续。",
)
if value.startswith("MOTION-COMM-303"):
return (
"MOTION-COMM-303",
"标定节点状态心跳停止更新",
"已采集的完整任务会保留；检查calibration.log后从断点继续，不要重新采集。",
)
if value == "fixed_base_reference_moved":
return (
"CAM-REFERENCE-204",
"顶部基准Tag在本次标定开始后发生移动",
"Tag 8可在新会话预检前重新摆放；固定Tag 8和顶部相机后启动新会话。",
)
if "palm_orientation_quality_failed" in value:
return (
"VAL-QUALITY-501",
"掌部方向校正的可观测轨迹不足",
"保持Tag安装不变；调整标定前手位，使正面Tag 10–13至少三枚"
"在对应运动起始段可见。",
)
if "motor_state_stalled" in value:
return "MOTION-STALL-301", "电机反馈停止向目标推进，程序已保持当前位置", "先检查机械卡阻，未排除前不要重复强推。"
if any(
token in value
for token in (
"sweep_missing_endpoint_bin",
"sweep_bins_too_few",
"sweep_bin_gap_too_large",
"task_precheck_missing_command_127",
)
):
return "OBS-SAMPLE-104", "当前任务的有效视觉轨迹不完整", "根据诊断中的关节、机位和缺失区间处理遮挡或反光后重新运行。"
if (
("synchronised" in value or "sweep_start_tag_timeout" in value)
and not missing
and pnp_diagnostics_present
):
problem = (
"Tag可见，但三维PnP轨迹在运动中失效"
if value.startswith("synchronised")
and int(active.get("valid_frames", 0) or 0) > 0
else "Tag可见，但三维PnP位姿初始化未完成"
)
return (
"CAM-GEOMETRY-201",
problem,
"不要根据二维码可见性调整Tag；复制累计PnP拒绝原因给开发者。",
)
if missing or "tag" in value or "detection" in value or "synchronised" in value:
return "OBS-TAG-103", f"所需Tag不可用或持续丢失：{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。"
if "state" in value and ("timeout" in value or "lost" in value):
return "MOTION-COMM-302", "机械手反馈中断或频率不足", "检查CAN接口和机械手供电后重新运行。"
if "camera" in value or "pnp" in value or "extrinsics" in value:
return "CAM-GEOMETRY-201", "相机、内外参或位姿求解未通过", "确认三台相机没有移动，然后复制本诊断块给开发者。"
if "checkpoint" in value:
return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留；复制本诊断块给开发者检查采集状态机。"
if "fit" in value or "trajectory" in value or "axis" in value:
return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限；复制本诊断块给开发者分析原始样本。"
if (
"endpoint_zero_offsets' is not defined" in value
or "validated_endpoint_zero_state" in value
):
return (
"PUB-ARTIFACT-601",
"已验证零位状态未能完整传递到URDF发布阶段",
"采集与验证数据仍可保留；不要移动相机或Tag，复制诊断块给开发者。",
)
if "validation" in value or "combination" in value or "quality" in value or "zero" in value:
return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布；复制本诊断块给开发者。"
if "publish" in value or "artifact" in value or "URDF" in value:
return "PUB-ARTIFACT-601", "结果文件校验或原子发布失败", "原始URDF未被覆盖；复制本诊断块给开发者。"
return "PUB-UNEXPECTED-699", "标定程序出现未分类异常", "复制本诊断块给开发者，完整堆栈已写入calibration.log。"
def build_failure_report(
config: ProductConfig,
session: str | Path,
status: Mapping[str, Any],
*,
reason: str,
) -> tuple[dict[str, Any], str]:
directory = Path(session).resolve()
code, problem, suggestion = classify_error(reason, status)
active = status.get("active", {})
if not isinstance(active, Mapping):
active = {}
else:
active = dict(active)
views = status.get("views", {})
if isinstance(views, Mapping):
group_pnp_reasons = {
str(view): str(item.get("group_pnp_reason"))
for view, item in views.items()
if isinstance(item, Mapping) and item.get("group_pnp_reason")
}
if group_pnp_reasons:
active["group_pnp_reasons"] = group_pnp_reasons
for field in (
"pnp_rejection_counts",
"group_pnp_rejection_counts",
"pnp_initialization_progress",
"group_missing_candidate_roles",
"pnp_candidate_diagnostics",
):
values = {
str(view): item.get(field)
for view, item in views.items()
if isinstance(item, Mapping) and item.get(field)
}
if values:
active[field] = values
try:
explanation, automatic_action = three_camera_reason_zh(
str(status.get("state", "")), reason, active
)
except Exception:
explanation, automatic_action = problem, "已停止本次发布并保留全部诊断数据"
camera_state = {
view: (
"OK"
if isinstance(item, Mapping)
and item.get("camera_info_valid", item.get("ready"))
and item.get("camera_extrinsics_valid", item.get("ready"))
and item.get("stream_alive", item.get("ready"))
else "NOT_READY"
)
for view, item in (views.items() if isinstance(views, Mapping) else [])
}
session_id = f"{config.serial_number}_{directory.name}"
metrics = {
"valid_frames": active.get("valid_frames"),
"sample": active.get("sample", {}),
"detection_frames": active.get("detection_frames"),
"detection_valid_frames": active.get("detection_valid_frames"),
"detection_rate": active.get("detection_rate"),
"detection_rate_by_view": active.get("detection_rate_by_view", {}),
"automatic_retry_count": active.get("automatic_retry_count", 0),
}
pnp_diagnostics = {
str(view): {
field: item.get(field)
for field in (
"group_pnp_reason",
"group_missing_candidate_roles",
"pnp_rejections",
"pnp_rejection_counts",
"group_pnp_rejection_counts",
"pnp_initialization_progress",
"pnp_candidate_diagnostics",
)
if item.get(field)
}
for view, item in (
views.items() if isinstance(views, Mapping) else []
)
if isinstance(item, Mapping)
and any(
item.get(field)
for field in (
"group_pnp_reason",
"group_missing_candidate_roles",
"pnp_rejections",
"pnp_rejection_counts",
"group_pnp_rejection_counts",
"pnp_initialization_progress",
"pnp_candidate_diagnostics",
)
)
}
payload: dict[str, Any] = {
"schema_version": 1,
"serial_number": config.serial_number,
"session_id": session_id,
"result": "FAIL",
"error_code": code,
"stage": str(status.get("state", "startup")),
"reason": str(reason),
"problem_zh": problem,
"explanation_zh": explanation,
"automatic_action_zh": automatic_action,
"suggestion_zh": suggestion,
"metrics": metrics,
"pnp_diagnostics": pnp_diagnostics,
"camera_state": camera_state,
"feedback_hz": status.get("feedback_hz", 0.0),
"hashes": {
"product_config_sha256": __import__("hashlib").sha256(config.path.read_bytes()).hexdigest(),
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"source_urdf_sha256": config.source_urdf_sha256,
},
"session_dir": str(directory),
"quality": {"passed": False},
}
atomic_write_json(directory / "calibration_summary_zh.json", payload)
block = "\n".join(
[
"========== 请复制以下内容给开发者 ==========",
f"会话编号：{session_id}",
"结果：FAIL",
f"错误代码：{code}",
f"失败阶段：{payload['stage']}",
f"问题：{problem}",
f"详细说明：{explanation}",
f"自动处理：{automatic_action}",
f"关键指标：{json.dumps(metrics, ensure_ascii=False, separators=(',', ':'))}",
"PnP诊断："
+ json.dumps(
pnp_diagnostics,
ensure_ascii=False,
separators=(",", ":"),
),
f"相机状态：{json.dumps(camera_state, ensure_ascii=False, separators=(',', ':'))}",
f"反馈状态：{float(payload['feedback_hz'] or 0.0):.1f} Hz",
f"配置哈希：{payload['hashes']['product_config_sha256']}",
f"外参哈希：{config.camera_extrinsics_sha256}",
f"源 URDF 哈希：{config.source_urdf_sha256}",
f"会话目录：{directory}",
f"建议：{suggestion}",
"========== 复制结束 ==========",
]
)
return payload, block
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py
+427 -22
View File
@@ -477,6 +477,11 @@ def select_static_rigid_group_initialization(
relative_translation_scale_m: float,
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
normal_alignment_scale_rad: float = math.radians(5.0),
task_reference_pairs: Mapping[
tuple[str, str], tuple[Rotation, np.ndarray]
] | None = None,
task_reference_rotation_scale_rad: float = math.radians(1.0),
task_reference_translation_scale_m: float = 0.01,
) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
"""Select a static multi-Tag IPPE branch path in bounded time.
@@ -497,13 +502,18 @@ def select_static_rigid_group_initialization(
normal_pair_names = tuple(
(str(first), str(second)) for first, second in normal_alignment_pairs
)
task_references = dict(task_reference_pairs or {})
if not frames:
raise ValueError("at least one PnP frame is required")
if not role_names or len(set(role_names)) != len(role_names):
raise ValueError("roles must be non-empty and unique")
if any(
parent not in role_names or child not in role_names
for parent, child in (*pair_names, *normal_pair_names)
for parent, child in (
*pair_names,
*normal_pair_names,
*task_references,
)
):
raise ValueError("geometry pairs must reference roles")
scales = (
@@ -513,6 +523,8 @@ def select_static_rigid_group_initialization(
float(relative_rotation_scale_rad),
float(relative_translation_scale_m),
float(normal_alignment_scale_rad),
float(task_reference_rotation_scale_rad),
float(task_reference_translation_scale_m),
)
if min(scales) <= 0.0:
raise ValueError("static initialization scales must be positive")
@@ -523,6 +535,8 @@ def select_static_rigid_group_initialization(
relative_rotation_scale,
relative_translation_scale,
normal_scale,
task_reference_rotation_scale,
task_reference_translation_scale,
) = scales
combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
@@ -578,6 +592,16 @@ def select_static_rigid_group_initialization(
+ float(np.linalg.norm(translation - reference_translation))
/ relative_translation_scale
)
for pair, (task_rotation, task_translation) in task_references.items():
rotation, translation = _relative_pose(
combination[pair[0]], combination[pair[1]]
)
score += (
float((task_rotation.inv() * rotation).magnitude())
/ task_reference_rotation_scale
+ float(np.linalg.norm(translation - task_translation))
/ task_reference_translation_scale
)
return float(score)
best_total = float("inf")
@@ -628,6 +652,9 @@ def select_static_rigid_group_initialization(
quality = dict(quality)
quality["total_cost"] = float(best_total)
quality["initialization_search"] = "static_reference"
quality["task_reference_used"] = (
"true" if task_references else "false"
)
return selected_path, quality
@@ -886,11 +913,15 @@ class SquareTagPoseTracker:
self.last_candidates_by_role: dict[
str, tuple[SquareTagPose, ...]
] = {}
self.last_candidate_diagnostics_by_role: dict[
str, dict[str, float | int]
] = {}
self.branch_correction_counts: dict[str, int] = {}
def reset(self) -> None:
self._previous.clear()
self.last_candidates_by_role.clear()
self.last_candidate_diagnostics_by_role.clear()
self.branch_correction_counts.clear()
def estimate(
@@ -911,28 +942,91 @@ class SquareTagPoseTracker:
)
except (ValueError, cv2.error):
self.last_candidates_by_role[str(role)] = ()
self.last_candidate_diagnostics_by_role[str(role)] = {
"solved_candidate_count": 0,
"reprojection_candidate_count": 0,
"independent_tilt_candidate_count": 0,
"maximum_reprojection_error_px": float(
self.maximum_reprojection_error_px
),
"maximum_independent_tilt_deg": math.degrees(
self.maximum_tag_tilt_rad
),
}
return None, "pnp_solve_failed"
if not candidates:
self.last_candidates_by_role[str(role)] = ()
self.last_candidate_diagnostics_by_role[str(role)] = {
"solved_candidate_count": 0,
"reprojection_candidate_count": 0,
"independent_tilt_candidate_count": 0,
"maximum_reprojection_error_px": float(
self.maximum_reprojection_error_px
),
"maximum_independent_tilt_deg": math.degrees(
self.maximum_tag_tilt_rad
),
}
return None, "pnp_solve_failed"
usable_candidates: list[SquareTagPose] = []
for candidate in candidates:
reprojection_candidates = [
candidate
for candidate in candidates
if candidate.reprojection_error_px
<= self.maximum_reprojection_error_px
]
candidate_tilts_rad: list[float] = []
independent_candidates: list[SquareTagPose] = []
for candidate in reprojection_candidates:
normal = Rotation.from_quat(
candidate.quaternion_xyzw
).as_matrix()[:, 2]
tilt = math.acos(
float(np.clip(abs(normal[2]), 0.0, 1.0))
)
if (
candidate.reprojection_error_px
<= self.maximum_reprojection_error_px
and tilt <= self.maximum_tag_tilt_rad
):
usable_candidates.append(candidate)
candidate_tilts_rad.append(float(tilt))
if tilt <= self.maximum_tag_tilt_rad:
independent_candidates.append(candidate)
# Candidate generation and candidate selection have different
# contracts. The per-Tag tilt limit protects a pose used without any
# other geometry, but it must not erase a finite, low-reprojection
# IPPE solution before SquareTagGroupPoseTracker can evaluate it
# against the fixed palm reference, the articulated chain and the
# preceding group pose. At a strongly oblique view the planar
# ambiguity is usually smaller, and rejecting both branches at a
# fixed angle caused deterministic mid-sweep holes despite continuous
# image detections. Group tracking therefore receives every
# reprojection-valid candidate; independent tracking below retains the
# original tilt safety gate.
self.last_candidates_by_role[str(role)] = tuple(
usable_candidates
reprojection_candidates
)
if not usable_candidates:
diagnostics: dict[str, float | int] = {
"solved_candidate_count": len(candidates),
"reprojection_candidate_count": len(reprojection_candidates),
"independent_tilt_candidate_count": len(independent_candidates),
"minimum_reprojection_error_px": float(
min(
candidate.reprojection_error_px
for candidate in candidates
)
),
"maximum_reprojection_error_px": float(
self.maximum_reprojection_error_px
),
"maximum_independent_tilt_deg": math.degrees(
self.maximum_tag_tilt_rad
),
}
if candidate_tilts_rad:
diagnostics["minimum_candidate_tilt_deg"] = math.degrees(
min(candidate_tilts_rad)
)
diagnostics["maximum_candidate_tilt_deg"] = math.degrees(
max(candidate_tilts_rad)
)
self.last_candidate_diagnostics_by_role[str(role)] = diagnostics
if not independent_candidates:
return None, "no_pose_within_reprojection_or_tilt_limit"
previous_record = self._previous.get(str(role))
@@ -944,7 +1038,7 @@ class SquareTagPoseTracker:
previous = previous_pose
selected, reason = select_continuous_pose(
usable_candidates,
independent_candidates,
previous=previous,
maximum_reprojection_error_px=(
self.maximum_reprojection_error_px
@@ -1013,6 +1107,13 @@ class SquareTagGroupPoseTracker:
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
normal_alignment_scale_rad: float = math.radians(5.0),
maximum_normal_alignment_rad: float | None = None,
return_reference_rotation_scale_rad: float = math.radians(1.0),
return_reference_maximum_command_gap_u8: int = 8,
coupled_rotation_pairs: Sequence[
tuple[str, str, str, str, float]
] = (),
coupled_rotation_scale_rad: float = math.radians(3.0),
maximum_coupled_rotation_residual_rad: float | None = None,
) -> None:
self.roles = tuple(str(role) for role in roles)
self.adjacent_pairs = tuple(
@@ -1023,6 +1124,22 @@ class SquareTagGroupPoseTracker:
(str(first), str(second))
for first, second in normal_alignment_pairs
)
self.coupled_rotation_pairs = tuple(
(
str(driver_parent),
str(driver_child),
str(follower_parent),
str(follower_child),
float(multiplier),
)
for (
driver_parent,
driver_child,
follower_parent,
follower_child,
multiplier,
) in coupled_rotation_pairs
)
if not self.roles or len(set(self.roles)) != len(self.roles):
raise ValueError("roles must be non-empty and unique")
if any(
@@ -1054,6 +1171,20 @@ class SquareTagGroupPoseTracker:
if maximum_normal_alignment_rad is None
else float(maximum_normal_alignment_rad)
)
self.return_reference_rotation_scale_rad = float(
return_reference_rotation_scale_rad
)
self.return_reference_maximum_command_gap_u8 = int(
return_reference_maximum_command_gap_u8
)
self.coupled_rotation_scale_rad = float(
coupled_rotation_scale_rad
)
self.maximum_coupled_rotation_residual_rad = (
None
if maximum_coupled_rotation_residual_rad is None
else float(maximum_coupled_rotation_residual_rad)
)
reset_seconds = float(reset_after_seconds)
if min(
self.maximum_pose_jump_rad,
@@ -1062,6 +1193,8 @@ class SquareTagGroupPoseTracker:
self.relative_translation_scale_m,
self.reprojection_scale_px,
self.normal_alignment_scale_rad,
self.return_reference_rotation_scale_rad,
self.coupled_rotation_scale_rad,
reset_seconds,
) <= 0.0:
raise ValueError("group tracking scales must be positive")
@@ -1069,11 +1202,33 @@ class SquareTagGroupPoseTracker:
raise ValueError("reprojection_weight must be non-negative")
if self.initialization_frames < 1:
raise ValueError("initialization_frames must be positive")
if self.return_reference_maximum_command_gap_u8 < 0:
raise ValueError(
"return reference maximum command gap must be non-negative"
)
if (
self.maximum_normal_alignment_rad is not None
and self.maximum_normal_alignment_rad <= 0.0
):
raise ValueError("maximum normal alignment must be positive")
if any(
role not in self.roles
for coupling in self.coupled_rotation_pairs
for role in coupling[:4]
):
raise ValueError("coupled rotation pairs must reference roles")
if any(
multiplier <= 0.0
for *_, multiplier in self.coupled_rotation_pairs
):
raise ValueError("coupled rotation multipliers must be positive")
if (
self.maximum_coupled_rotation_residual_rad is not None
and self.maximum_coupled_rotation_residual_rad <= 0.0
):
raise ValueError(
"maximum coupled rotation residual must be positive"
)
self.reset_after_ns = int(reset_seconds * 1_000_000_000)
self._previous: dict[str, SquareTagPose] = {}
self._previous_stamp_ns: int | None = None
@@ -1083,28 +1238,214 @@ class SquareTagGroupPoseTracker:
self._initial_stamps_ns: list[int] = []
self.last_initialization_quality: dict[str, float | str] = {}
self.branch_correction_counts: dict[str, int] = {}
self._decreasing_relative_rotations: dict[
int, dict[tuple[str, str], Rotation]
] = {}
self._coupled_reference_rotations: dict[
tuple[str, str], Rotation
] = {}
self._task_reference_relative_poses: dict[
tuple[str, str], tuple[Rotation, np.ndarray]
] = {}
self.last_missing_roles: tuple[str, ...] = ()
def reset(self) -> None:
def reset(self, *, preserve_task_reference: bool = False) -> None:
self._previous.clear()
self._previous_stamp_ns = None
self._initial_candidates.clear()
self._initial_stamps_ns.clear()
self.last_initialization_quality.clear()
self.branch_correction_counts.clear()
self._decreasing_relative_rotations.clear()
self._coupled_reference_rotations.clear()
self.last_missing_roles = ()
if not preserve_task_reference:
self._task_reference_relative_poses.clear()
def _task_reference_cost(
self, combination: Mapping[str, SquareTagPose]
) -> float:
residual = 0.0
for pair, (expected_rotation, expected_translation) in (
self._task_reference_relative_poses.items()
):
rotation, translation = _relative_pose(
combination[pair[0]], combination[pair[1]]
)
residual += (
float((expected_rotation.inv() * rotation).magnitude())
/ self.return_reference_rotation_scale_rad
+ float(np.linalg.norm(translation - expected_translation))
/ self.relative_translation_scale_m
)
return residual
def _coupled_rotation_residuals(
self, combination: Mapping[str, SquareTagPose]
) -> tuple[float, ...]:
if not self.coupled_rotation_pairs:
return ()
residuals: list[float] = []
for (
driver_parent,
driver_child,
follower_parent,
follower_child,
multiplier,
) in self.coupled_rotation_pairs:
driver_pair = (driver_parent, driver_child)
follower_pair = (follower_parent, follower_child)
if (
driver_pair not in self._coupled_reference_rotations
or follower_pair not in self._coupled_reference_rotations
):
return ()
driver_rotation = _relative_pose(
combination[driver_parent], combination[driver_child]
)[0]
follower_rotation = _relative_pose(
combination[follower_parent], combination[follower_child]
)[0]
driver_travel = (
self._coupled_reference_rotations[driver_pair].inv()
* driver_rotation
).magnitude()
follower_travel = (
self._coupled_reference_rotations[follower_pair].inv()
* follower_rotation
).magnitude()
residuals.append(
abs(float(follower_travel) - multiplier * float(driver_travel))
)
return tuple(residuals)
def _informative_coupled_rotation_costs(
self,
combinations: Sequence[Mapping[str, SquareTagPose]],
) -> tuple[float, ...]:
"""Return branch costs only while the weak coupling prior is credible.
The URDF mimic ratio is useful for distinguishing two planar-IPPE
branches, but it is not measurement truth for a passive joint. Once
every otherwise viable combination disagrees with that ratio, using
it would bias the measured curve (and previously rejected every
frame). In that case fall back to visual continuity for this frame.
"""
residuals = tuple(
self._coupled_rotation_residuals(combination)
for combination in combinations
)
if not residuals or not any(residuals):
return tuple(0.0 for _ in combinations)
if (
self.maximum_coupled_rotation_residual_rad is not None
and not any(
values
and max(values)
<= self.maximum_coupled_rotation_residual_rad
for values in residuals
)
):
return tuple(0.0 for _ in combinations)
return tuple(
sum(values) / self.coupled_rotation_scale_rad
for values in residuals
)
def _return_reference(
self, command_u8: int | None
) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]:
if command_u8 is None or not self._decreasing_relative_rotations:
return {}
command = int(command_u8)
nearest = min(
self._decreasing_relative_rotations,
key=lambda candidate: abs(candidate - command),
)
if (
abs(nearest - command)
> self.return_reference_maximum_command_gap_u8
):
return {}
references = self._decreasing_relative_rotations[nearest]
commands = sorted(self._decreasing_relative_rotations)
axes: dict[tuple[str, str], np.ndarray | None] = {}
for pair in self.adjacent_pairs:
endpoint_delta = (
self._decreasing_relative_rotations[commands[-1]][pair].inv()
* self._decreasing_relative_rotations[commands[0]][pair]
).as_rotvec()
norm = float(np.linalg.norm(endpoint_delta))
axes[pair] = (
None
if norm < math.radians(5.0)
else endpoint_delta / norm
)
return {
pair: (rotation, axes[pair])
for pair, rotation in references.items()
}
def _return_reference_cost(
self,
combination: Mapping[str, SquareTagPose],
reference: Mapping[
tuple[str, str], tuple[Rotation, np.ndarray | None]
],
) -> float:
residual = 0.0
for pair, (expected, motion_axis) in reference.items():
vector = (
expected.inv()
* _relative_pose(
combination[pair[0]], combination[pair[1]]
)[0]
).as_rotvec()
if motion_axis is not None:
# The outbound trajectory identifies the physical one-DOF
# motion axis. Do not penalize return travel along that axis:
# it may contain real mechanical hysteresis that calibration
# must measure. A planar-IPPE mirror branch appears primarily
# as a large orthogonal tilt and is rejected by this residual.
vector = vector - motion_axis * float(vector @ motion_axis)
residual += float(np.linalg.norm(vector))
return residual / self.return_reference_rotation_scale_rad
def select(
self,
candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
*,
stamp_ns: int,
trajectory_command_u8: int | None = None,
trajectory_direction: str | None = None,
) -> tuple[dict[str, SquareTagPose] | None, str]:
"""Return one mutually consistent pose for every configured role."""
direction = (
None
if trajectory_direction is None
else str(trajectory_direction)
)
if direction not in {None, "decreasing", "increasing"}:
raise ValueError(
"trajectory_direction must be decreasing or increasing"
)
return_reference = (
self._return_reference(trajectory_command_u8)
if direction == "increasing"
else {}
)
candidate_lists = [
tuple(candidates_by_role.get(role, ()))
for role in self.roles
]
if any(not candidates for candidates in candidate_lists):
self.last_missing_roles = tuple(
role
for role, candidates in zip(self.roles, candidate_lists)
if not candidates
)
if self.last_missing_roles:
return None, "group_missing_pose_candidates"
self.last_missing_roles = ()
combinations = [
dict(zip(self.roles, combination))
@@ -1175,6 +1516,15 @@ class SquareTagGroupPoseTracker:
normal_alignment_scale_rad=(
self.normal_alignment_scale_rad
),
task_reference_pairs=(
self._task_reference_relative_poses
),
task_reference_rotation_scale_rad=(
self.return_reference_rotation_scale_rad
),
task_reference_translation_scale_m=(
self.relative_translation_scale_m
),
)
)
selected = selected_path[-1]
@@ -1195,23 +1545,31 @@ class SquareTagGroupPoseTracker:
):
return None, "group_normal_alignment"
else:
coupling_costs = self._informative_coupled_rotation_costs(
combinations
)
selected = min(
combinations,
key=lambda combination: (
zip(combinations, coupling_costs),
key=lambda item: (
sum(
pose.reprojection_error_px
for pose in combination.values()
for pose in item[0].values()
)
/ self.reprojection_scale_px
+ sum(
_normal_alignment_rad(
combination[first], combination[second]
item[0][first], item[0][second]
)
for first, second in self.normal_alignment_pairs
)
/ self.normal_alignment_scale_rad
+ self._return_reference_cost(
item[0], return_reference
)
+ item[1]
+ self._task_reference_cost(item[0])
),
)
)[0]
maximum_alignment = max(
(
_normal_alignment_rad(
@@ -1238,7 +1596,7 @@ class SquareTagGroupPoseTracker:
)
for pair in self.adjacent_pairs
}
scored: list[tuple[float, dict[str, SquareTagPose]]] = []
base_scored: list[tuple[float, dict[str, SquareTagPose]]] = []
for combination in combinations:
absolute_rotation_motion = 0.0
absolute_translation_motion = 0.0
@@ -1305,11 +1663,23 @@ class SquareTagGroupPoseTracker:
+ relative_translation_motion
/ self.relative_translation_scale_m
+ self.reprojection_weight * reprojection_penalty
+ self._return_reference_cost(
combination, return_reference
)
)
scored.append((float(score), combination))
base_scored.append((float(score), combination))
if not scored:
if not base_scored:
return None, "group_pose_jump"
coupling_costs = self._informative_coupled_rotation_costs(
[combination for _, combination in base_scored]
)
scored = [
(base_score + coupling_cost, combination)
for (base_score, combination), coupling_cost in zip(
base_scored, coupling_costs
)
]
selected = min(scored, key=lambda item: item[0])[1]
aligned: dict[str, SquareTagPose] = {}
@@ -1343,4 +1713,39 @@ class SquareTagGroupPoseTracker:
self._previous = aligned
self._previous_stamp_ns = stamp
if (
direction == "decreasing"
and trajectory_command_u8 is not None
and not self._coupled_reference_rotations
):
for (
driver_parent,
driver_child,
follower_parent,
follower_child,
_multiplier,
) in self.coupled_rotation_pairs:
for pair in (
(driver_parent, driver_child),
(follower_parent, follower_child),
):
self._coupled_reference_rotations[pair] = _relative_pose(
aligned[pair[0]], aligned[pair[1]]
)[0]
if direction == "decreasing" and trajectory_command_u8 is not None:
self._decreasing_relative_rotations[
int(trajectory_command_u8)
] = {
pair: _relative_pose(
aligned[pair[0]], aligned[pair[1]]
)[0]
for pair in self.adjacent_pairs
}
if not self._task_reference_relative_poses:
self._task_reference_relative_poses = {
pair: _relative_pose(
aligned[pair[0]], aligned[pair[1]]
)
for pair in self.adjacent_pairs
}
return dict(aligned), ""
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py
+403
View File
@@ -0,0 +1,403 @@
"""Immutable product identity and shared calibration preflight.
Model-specific kinematics live in registered calibration contracts. The
loader itself only verifies that the selected contract, Tag configuration,
cameras and immutable artifacts agree, so adding another hand does not
require another copy of the one-command runner.
"""
from __future__ import annotations
from dataclasses import dataclass
import hashlib
from pathlib import Path
import re
from typing import Any, Mapping
import yaml
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
from .full_hand import (
G20_RIGHT_19_LAYOUT,
HandCalibrationProfile,
get_hand_calibration_profile,
)
from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile
VIEWS = ("front", "side", "top")
@dataclass(frozen=True)
class ProductCalibrationContract:
"""Declarative boundary between one hand model and the shared engine."""
model: str
side: str
layout_id: str
profile: HandCalibrationProfile
zero_profile: ZeroCalibrationProfile
@property
def required_tag_ids(self) -> frozenset[int]:
return frozenset(
int(tag_id)
for tags in self.profile.view_tags.values()
for tag_id in tags.values()
)
@property
def views(self) -> tuple[str, ...]:
return tuple(self.profile.view_tags)
_PRODUCT_CONTRACTS: dict[
tuple[str, str, str], ProductCalibrationContract
] = {}
def register_product_calibration_contract(
contract: ProductCalibrationContract,
) -> None:
"""Register one model/side/layout without modifying shared workflow code."""
model = str(contract.model).strip().upper()
side = str(contract.side).strip().lower()
layout = str(contract.layout_id).strip().lower()
if not model or side not in {"left", "right"} or not layout:
raise ValueError("product calibration contract identity is invalid")
if contract.profile.side != side:
raise ValueError("product contract side differs from hand profile")
if contract.profile.layout_id.lower() != layout:
raise ValueError("product contract layout differs from hand profile")
if contract.zero_profile.hand != contract.profile:
raise ValueError("zero-calibration profile differs from hand profile")
if len(contract.profile.baseline_command) != contract.profile.command_count:
raise ValueError("profile baseline and command names differ in length")
if not contract.required_tag_ids:
raise ValueError("product contract must declare at least one Tag")
key = (model, side, layout)
existing = _PRODUCT_CONTRACTS.get(key)
if existing is not None and existing != contract:
raise ValueError(f"product calibration contract already registered: {key}")
_PRODUCT_CONTRACTS[key] = contract
def get_product_calibration_contract(
model: str, side: str, layout_id: str
) -> ProductCalibrationContract:
key = (
str(model).strip().upper(),
str(side).strip().lower(),
str(layout_id).strip().lower(),
)
try:
return _PRODUCT_CONTRACTS[key]
except KeyError as error:
supported = ", ".join("/".join(item) for item in sorted(_PRODUCT_CONTRACTS))
raise ValueError(
f"unsupported calibration product {key}; registered={supported}"
) from error
register_product_calibration_contract(
ProductCalibrationContract(
model="G20",
side="right",
layout_id=G20_RIGHT_19_LAYOUT,
profile=get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
zero_profile=get_zero_calibration_profile(
"right", G20_RIGHT_19_LAYOUT
),
)
)
for _legacy_side in ("left", "right"):
register_product_calibration_contract(
ProductCalibrationContract(
model="G20",
side=_legacy_side,
layout_id="legacy_11",
profile=get_hand_calibration_profile(_legacy_side, "legacy_11"),
zero_profile=get_zero_calibration_profile(
_legacy_side, "legacy_11"
),
)
)
def sha256_file(path: str | Path) -> str:
digest = hashlib.sha256()
with Path(path).open("rb") as stream:
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
digest.update(chunk)
return digest.hexdigest()
def _mapping(value: Any, name: str) -> Mapping[str, Any]:
if not isinstance(value, Mapping):
raise ValueError(f"{name} must be a mapping")
return value
def _resolve_path(value: Any, *, workspace: Path, name: str) -> Path:
text = str(value).strip()
if not text:
raise ValueError(f"{name} is required")
candidate = Path(text).expanduser()
if not candidate.is_absolute():
candidate = workspace / candidate
return candidate.resolve()
def _camera_info_fingerprint(path: Path) -> str:
with path.open("r", encoding="utf-8") as stream:
payload = _mapping(yaml.safe_load(stream), str(path))
def values(key: str) -> list[float]:
item = _mapping(payload.get(key, {}), key)
return [float(value) for value in item.get("data", [])]
return camera_info_fingerprint(
width=int(payload["image_width"]),
height=int(payload["image_height"]),
camera_matrix=values("camera_matrix"),
distortion=values("distortion_coefficients"),
rectification=values("rectification_matrix"),
projection=values("projection_matrix"),
)
def _tag_sizes_m_by_id(path: Path) -> dict[int, float]:
with path.open("r", encoding="utf-8") as stream:
payload = _mapping(yaml.safe_load(stream), str(path))
result: dict[int, float] = {}
for node in payload.values():
parameters = _mapping(_mapping(node, "tag node").get("ros__parameters"), "ros__parameters")
tag = _mapping(parameters.get("tag"), "tag")
ids = [int(value) for value in tag.get("ids", [])]
sizes = [float(value) for value in tag.get("sizes", [])]
if len(ids) != len(sizes):
raise ValueError("Tag IDs and sizes must have the same length")
if set(result).intersection(ids):
raise ValueError("Tag IDs must be unique across the three views")
if any(not 0.0 < size < 0.1 for size in sizes):
raise ValueError("Tag sizes must be in (0, 0.1) metres")
result.update(zip(ids, sizes))
return result
def _custom_pnp_tag_sizes_m_by_id(
path: Path, configured_ids: set[int]
) -> dict[int, float]:
with path.open("r", encoding="utf-8") as stream:
payload = _mapping(yaml.safe_load(stream), str(path))
parameters = _mapping(
_mapping(payload.get("g20_calibration"), "g20_calibration").get(
"ros__parameters"
),
"ros__parameters",
)
default = float(parameters.get("tag_size_m", 0.0))
ids = [int(value) for value in parameters.get("tag_size_override_ids", [])]
sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])]
if len(ids) != len(sizes) or len(set(ids)) != len(ids):
raise ValueError("custom PnP Tag-size overrides are invalid")
if any(tag_id not in configured_ids for tag_id in ids):
raise ValueError("custom PnP Tag-size override ID is not configured")
result = {tag_id: default for tag_id in configured_ids}
result.update(zip(ids, sizes))
return result
@dataclass(frozen=True)
class ProductConfig:
path: Path
workspace: Path
model: str
side: str
tag_layout: str
calibration_contract: ProductCalibrationContract
serial_number: str
can_interface: str
source_urdf: Path
source_urdf_sha256: str
camera_extrinsics: Path
camera_extrinsics_sha256: str
calibration_config: Path
calibration_config_sha256: str
tag_config: Path
tag_config_sha256: str
output_root: Path
cameras: Mapping[str, Mapping[str, str]]
required_independent_passes: int
static_repeatability_rad: float
@property
def session_root(self) -> Path:
return self.output_root / self.serial_number
def load_product_config(
path: str | Path,
*,
workspace: str | Path | None = None,
check_can: bool = True,
) -> ProductConfig:
"""Load and verify a product against its registered calibration contract."""
source = Path(path).expanduser().resolve()
if not source.is_file():
raise ValueError(f"product config does not exist: {source}")
with source.open("r", encoding="utf-8") as stream:
raw = _mapping(yaml.safe_load(stream), str(source))
if int(raw.get("schema_version", -1)) != 1:
raise ValueError("product config schema_version must be 1")
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
serial = str(raw.get("serial_number", ""))
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
raise ValueError("serial_number is invalid")
model = str(raw.get("model", "")).strip().upper()
side = str(raw.get("side", "")).strip().lower()
# Keep the deployed G20 schema compatible while making the layout an
# explicit product choice for all new configurations.
layout = str(
raw.get(
"tag_layout",
G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "",
)
).strip().lower()
contract = get_product_calibration_contract(model, side, layout)
can_interface = str(raw.get("can_interface", "")).strip()
if not can_interface:
raise ValueError("can_interface is required")
if check_can and not (Path("/sys/class/net") / can_interface).exists():
raise ValueError(f"CAN interface does not exist: {can_interface}")
artifacts = _mapping(raw.get("artifacts"), "artifacts")
source_urdf = _resolve_path(
artifacts.get("source_urdf"), workspace=root, name="source_urdf"
)
extrinsics = _resolve_path(
artifacts.get("camera_extrinsics"),
workspace=root,
name="camera_extrinsics",
)
calibration_config = _resolve_path(
artifacts.get("calibration_config"),
workspace=root,
name="calibration_config",
)
tag_config = _resolve_path(
artifacts.get("tag_config"), workspace=root, name="tag_config"
)
for name, candidate in (
("source URDF", source_urdf),
("camera extrinsics", extrinsics),
("calibration config", calibration_config),
("Tag config", tag_config),
):
if not candidate.is_file():
raise ValueError(f"{name} does not exist: {candidate}")
expected_source_hash = str(artifacts.get("source_urdf_sha256", "")).lower()
expected_extrinsics_hash = str(
artifacts.get("camera_extrinsics_sha256", "")
).lower()
expected_tag_hash = str(artifacts.get("tag_config_sha256", "")).lower()
expected_calibration_hash = str(
artifacts.get("calibration_config_sha256", "")
).lower()
hashes = (
("source URDF", source_urdf, expected_source_hash),
("camera extrinsics", extrinsics, expected_extrinsics_hash),
("Tag config", tag_config, expected_tag_hash),
("calibration config", calibration_config, expected_calibration_hash),
)
for name, candidate, expected in hashes:
if re.fullmatch(r"[0-9a-f]{64}", expected) is None:
raise ValueError(f"{name} expected SHA-256 is invalid")
actual = sha256_file(candidate)
if actual != expected:
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
expected_tag_ids = set(contract.required_tag_ids)
tag_sizes = _tag_sizes_m_by_id(tag_config)
if set(tag_sizes) != expected_tag_ids:
raise ValueError(
f"{model}/{side}/{layout} Tag IDs differ from the registered "
f"profile: expected={sorted(expected_tag_ids)} "
f"actual={sorted(tag_sizes)}"
)
if _custom_pnp_tag_sizes_m_by_id(
calibration_config, expected_tag_ids
) != tag_sizes:
raise ValueError(
"custom PnP Tag sizes do not match the apriltag_ros Tag config"
)
camera_raw = _mapping(raw.get("cameras"), "cameras")
required_views = tuple(contract.views)
if set(required_views) != set(VIEWS):
raise ValueError(
"the current shared engine requires front/side/top views"
)
if set(camera_raw) != set(required_views):
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
cameras: dict[str, dict[str, str]] = {}
serials: set[str] = set()
for view in required_views:
item = _mapping(camera_raw[view], f"cameras.{view}")
camera_serial = str(item.get("serial_number", ""))
info_path = _resolve_path(
item.get("camera_info"), workspace=root, name=f"{view}.camera_info"
)
if not info_path.is_file():
raise ValueError(f"{view} camera info does not exist: {info_path}")
if camera_serial in serials:
raise ValueError("the three camera serial numbers must be unique")
serials.add(camera_serial)
identity = loaded_extrinsics.cameras[view]
fingerprint = _camera_info_fingerprint(info_path)
if camera_serial != identity.serial_number:
raise ValueError(f"{view} camera serial does not match extrinsics")
if fingerprint != identity.intrinsics_sha256:
raise ValueError(f"{view} camera intrinsics do not match extrinsics")
cameras[view] = {
"serial_number": camera_serial,
"camera_info": str(info_path),
"camera_name": str(item.get("camera_name", f"hikrobot_{view}_{camera_serial}")),
}
output_root = _resolve_path(
raw.get("output_root", "calibration_output"),
workspace=root,
name="output_root",
)
release = _mapping(raw.get("release", {}), "release")
passes = int(release.get("required_independent_passes", 2))
if passes not in {1, 2}:
raise ValueError("required_independent_passes must be 1 or 2")
repeatability_deg = float(release.get("static_repeatability_deg", 1.0))
if not 0.0 < repeatability_deg <= 5.0:
raise ValueError("static_repeatability_deg must be in (0, 5]")
return ProductConfig(
path=source,
workspace=root,
model=model,
side=side,
tag_layout=layout,
calibration_contract=contract,
serial_number=serial,
can_interface=can_interface,
source_urdf=source_urdf,
source_urdf_sha256=expected_source_hash,
camera_extrinsics=extrinsics,
camera_extrinsics_sha256=expected_extrinsics_hash,
calibration_config=calibration_config,
calibration_config_sha256=expected_calibration_hash,
tag_config=tag_config,
tag_config_sha256=expected_tag_hash,
output_root=output_root,
cameras=cameras,
required_independent_passes=passes,
static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0,
)
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py
+890
View File
@@ -0,0 +1,890 @@
"""Validate and atomically publish one inseparable G20 JSON/URDF session."""
from __future__ import annotations
from datetime import datetime, timezone
import copy
import json
import math
import os
from pathlib import Path
import re
from typing import Any, Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation
from .full_hand import (
G20_COMBINATION_REQUIRED_TARGET_KEYS,
G20_RIGHT_19_LAYOUT,
get_hand_calibration_profile,
validate_compact_payload,
)
from .product import ProductConfig, sha256_file
from .storage import atomic_write_json
from .urdf_zero import (
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
get_zero_calibration_profile,
)
PASSIVE_JOINTS = frozenset(
{"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
)
VISUALLY_MEASURED_FINGER_DIPS = frozenset(
{"index_dip", "middle_dip", "ring_dip", "pinky_dip"}
)
ACTIVE_ZERO_JOINTS = frozenset(
get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
)
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
) - ACTIVE_ZERO_JOINTS
SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9
def _load_json(path: Path) -> dict[str, Any]:
with path.open("r", encoding="utf-8") as stream:
payload = json.load(stream)
if not isinstance(payload, dict):
raise ValueError(f"{path} must contain a JSON object")
return payload
def _joint_blocks(text: str) -> dict[str, str]:
pattern = re.compile(
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
re.DOTALL,
)
return {match.group("name"): match.group(0) for match in pattern.finditer(text)}
def _mask_origin_rpy(block: str) -> str:
return re.sub(
r"(<origin\b[^>]*\brpy\s*=\s*)([\"'])[^\"']*\2",
r"\1\2__CALIBRATED_RPY__\2",
block,
count=1,
)
def _mask_active_origin_rpy_fields(text: str) -> str:
pattern = re.compile(
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
re.DOTALL,
)
def replace(match: re.Match[str]) -> str:
block = match.group(0)
if match.group("name") not in ACTIVE_ZERO_JOINTS:
return block
masked = _mask_origin_rpy(block)
if masked == block:
raise ValueError(
f"active joint {match.group('name')} has no origin.rpy"
)
return masked
return pattern.sub(replace, text)
def _mask_endpoint_coordinate_fields(text: str) -> str:
"""Mask only limit/mimic fields induced by endpoint zero coordinates."""
pattern = re.compile(
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
re.DOTALL,
)
def replace(match: re.Match[str]) -> str:
block = match.group(0)
name = match.group("name")
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS:
return re.sub(
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
r"\1\2__CALIBRATED_UPPER__\2",
block,
count=1,
)
mimic = re.search(
r"<mimic\b[^>]*\bjoint\s*=\s*([\"'])(?P<source>[^\"']+)\1[^>]*>",
block,
re.DOTALL,
)
if (
mimic is not None
and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
):
return re.sub(
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
r"\1\2__CALIBRATED_MIMIC_OFFSET__\2",
block,
count=1,
)
return block
return pattern.sub(replace, text)
def _triplet(value: str) -> np.ndarray:
result = np.asarray([float(item) for item in value.split()], dtype=float)
if result.shape != (3,) or not np.all(np.isfinite(result)):
raise ValueError(f"invalid URDF triplet: {value}")
return result
def _joint_elements(path: str | Path) -> dict[str, ET.Element]:
return {
str(joint.get("name")): joint
for joint in ET.parse(Path(path)).getroot().findall("joint")
}
def _verify_expected_origin_offsets(
source: str | Path,
corrected: str | Path,
expected_offsets_rad: Mapping[str, float],
) -> None:
offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()}
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
not math.isfinite(value) for value in offsets.values()
):
raise ValueError("expected offsets must contain all finite active static-zero values")
before = _joint_elements(source)
after = _joint_elements(corrected)
maximum_rotation_error = 0.0
for name, original_joint in before.items():
corrected_joint = after[name]
original_origin = original_joint.find("origin")
corrected_origin = corrected_joint.find("origin")
if original_origin is None or corrected_origin is None:
if original_origin is not corrected_origin:
raise ValueError(f"corrected URDF changed origin presence for {name}")
continue
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0):
raise ValueError(f"corrected URDF changed origin.xyz for {name}")
original_rotation = Rotation.from_euler(
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
)
corrected_rotation = Rotation.from_euler(
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
)
expected_rotation = original_rotation
if name in offsets:
axis_node = original_joint.find("axis")
axis = _triplet(
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
)
norm = float(np.linalg.norm(axis))
if norm <= 1.0e-12:
raise ValueError(f"source URDF joint {name} has a degenerate axis")
expected_rotation = expected_rotation * Rotation.from_rotvec(
axis / norm * offsets[name]
)
error = float((expected_rotation.inv() * corrected_rotation).magnitude())
maximum_rotation_error = max(maximum_rotation_error, error)
# Schema v4 intentionally stores zero offsets at eight decimal places.
# Older sessions wrote the corrected URDF from the full-precision solve,
# so comparing that file with the published JSON necessarily permits half
# of one last-place unit. This is about 2.9e-7 degrees and is far below
# any calibration or URDF numerical significance.
if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD:
raise ValueError(
"corrected URDF origin.rpy does not match the published zero offsets"
)
def verify_corrected_urdf(
source: str | Path,
corrected: str | Path,
*,
expected_offsets_rad: Mapping[str, float] | None = None,
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
) -> tuple[str, ...]:
"""Prove that only active-joint origin.rpy attributes changed.
Passive joint blocks, including their mimic elements, are compared as raw
UTF-8 text so formatting and numeric spelling are protected as well.
"""
source_text = Path(source).read_text(encoding="utf-8")
corrected_text = Path(corrected).read_text(encoding="utf-8")
before = _joint_blocks(source_text)
after = _joint_blocks(corrected_text)
if set(before) != set(after):
raise ValueError("corrected URDF changed the joint set")
# Text patchers may normalize only the final newline. It has no URDF/XML
# semantics; every joint block and every non-rpy byte remains guarded
# below.
masked_source = _mask_endpoint_coordinate_fields(
_mask_active_origin_rpy_fields(source_text)
).rstrip("\r\n")
masked_corrected = _mask_endpoint_coordinate_fields(
_mask_active_origin_rpy_fields(corrected_text)
).rstrip("\r\n")
if masked_source != masked_corrected:
raise ValueError(
"corrected URDF changed content outside active origin.rpy or "
"endpoint-calibrated fields"
)
changed: list[str] = []
for name in before:
if before[name] == after[name]:
continue
if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_JOINTS:
raise ValueError(f"corrected URDF changed protected joint {name}")
if name in ACTIVE_ZERO_JOINTS:
changed.append(name)
if expected_offsets_rad is not None:
_verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
endpoint_offsets = {
str(name): float(value)
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
}
if endpoint_offsets:
source_joints = _joint_elements(source)
corrected_joints = _joint_elements(corrected)
for name, offset in endpoint_offsets.items():
original_limit = source_joints[name].find("limit")
corrected_limit = corrected_joints[name].find("limit")
expected_upper = float(original_limit.get("upper")) - offset
# Schema v4 stores the corresponding zero at eight decimal
# places, while the URDF is written from the full-precision solve.
# Match the half-last-place tolerance used for origin rotations.
if (
abs(float(corrected_limit.get("upper")) - expected_upper)
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
):
raise ValueError(f"corrected URDF has invalid {name} upper limit")
for name in PASSIVE_JOINTS:
original_mimic = source_joints[name].find("mimic")
corrected_mimic = corrected_joints[name].find("mimic")
if original_mimic is None or corrected_mimic is None:
continue
source_name = str(original_mimic.get("joint"))
multiplier = float(original_mimic.get("multiplier", "1"))
expected = float(original_mimic.get("offset", "0"))
expected += multiplier * endpoint_offsets.get(source_name, 0.0)
# The active endpoint offset comes from schema-v4 JSON rounded to
# eight decimal places, while the URDF mimic was written from the
# full-precision solve. Propagate exactly the same accepted
# quantization through the mimic multiplier; retain a much
# smaller allowance for XML decimal formatting itself.
tolerance = (
abs(multiplier)
* SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
+ 1.0e-12
)
if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance:
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
return tuple(sorted(changed))
def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
"""Return every session-local mesh after proving it resolves safely."""
path = Path(urdf).expanduser().resolve()
root = ET.parse(path).getroot()
resources: dict[str, Path] = {}
for mesh in root.findall(".//mesh"):
filename = str(mesh.get("filename", "")).strip()
if not filename or "://" in filename or filename.startswith("package:"):
continue
relative = Path(filename)
if relative.is_absolute() or ".." in relative.parts:
raise ValueError(f"URDF has an unsafe local mesh path: {filename}")
resolved = (path.parent / relative).resolve()
try:
resolved.relative_to(path.parent)
except ValueError as error:
raise ValueError(f"URDF mesh escapes the session: {filename}") from error
if not resolved.is_file():
raise ValueError(f"URDF mesh resource is missing: {filename}")
resources[relative.as_posix()] = resolved
return dict(sorted(resources.items()))
def validate_runtime_curves_against_urdf_limits(
payload: Mapping[str, Any], runtime_urdf: str | Path
) -> None:
"""Reject a curve whose q leaves its runtime URDF coordinate limits.
Curves in schema v4 are expressed in the corrected URDF joint coordinate,
not in the source-CAD coordinate. Endpoint zero calibration can therefore
move a corrected coordinate limit while preserving the same physical CAD
endpoint; callers publishing a calibrated pair must pass that corrected
URDF here.
"""
validate_compact_payload(payload)
runtime_joints = _joint_elements(runtime_urdf)
for name, calibration in payload["joints"].items():
joint = runtime_joints.get(str(name))
if joint is None:
raise ValueError(f"runtime URDF is missing joint {name}")
limit = joint.find("limit")
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
lower = float(limit.get("lower"))
upper = float(limit.get("upper"))
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
curve = np.asarray(calibration["angle_rad"], dtype=float)
minimum = float(np.min(curve))
maximum = float(np.max(curve))
tolerance = 1.0e-7
if minimum < lower - tolerance or maximum > upper + tolerance:
raise ValueError(
f"runtime curve exceeds runtime URDF limit for {name}: "
f"[{minimum:.9g}, {maximum:.9g}] not within "
f"[{lower:.9g}, {upper:.9g}]"
)
def clamp_compact_payload_to_urdf_limits(
payload: Mapping[str, Any], runtime_urdf: str | Path
) -> tuple[dict[str, Any], dict[str, int]]:
"""Return a schema-preserving payload bounded in its runtime coordinates."""
result = copy.deepcopy(dict(payload))
validate_compact_payload(result)
runtime_joints = _joint_elements(runtime_urdf)
clipped_by_joint: dict[str, int] = {}
for name, calibration in result["joints"].items():
joint = runtime_joints.get(str(name))
limit = None if joint is None else joint.find("limit")
if (
limit is None
or limit.get("lower") is None
or limit.get("upper") is None
):
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
lower = float(limit.get("lower"))
upper = float(limit.get("upper"))
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
source = np.asarray(calibration["angle_rad"], dtype=float)
bounded = np.clip(source, lower, upper)
count = int(np.count_nonzero(bounded != source))
if count:
clipped_by_joint[str(name)] = count
calibration["angle_rad"] = [
round(float(value), 8) for value in bounded
]
validate_compact_payload(result)
return result, clipped_by_joint
def build_mujoco_validation_commands(
baseline: Sequence[int],
) -> dict[str, Any]:
if len(baseline) != 20:
raise ValueError("baseline must contain exactly 20 commands")
base = [int(value) for value in baseline]
if any(not 0 <= value <= 255 for value in base):
raise ValueError("baseline commands must be in [0, 255]")
def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]:
values = list(base)
for index, value in changes.items():
values[int(index)] = int(value)
return {"name": name, "label_zh": label, "command_u8": values}
return {
"schema_version": 1,
"topic": "/g20/cb_right_hand_control_cmd",
"serial_number": "G20_RIGHT_001",
"poses": [
pose("all_open", "全开", {}),
pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}),
pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}),
pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}),
pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}),
pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}),
pose(
"half_grip",
"四指半握",
{1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160},
),
pose(
"light_pinch",
"轻捏",
{0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176},
),
],
}
def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path:
parent = Path(root).resolve()
target = Path(session).resolve()
if target.parent != parent:
raise ValueError("session pointer target must be a direct child of the serial root")
if not target.is_dir():
raise ValueError(f"session directory does not exist: {target}")
if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}:
raise ValueError("unsupported session pointer name")
parent.mkdir(parents=True, exist_ok=True)
destination = parent / name
temporary = parent / f".{name}.{os.getpid()}.tmp"
if temporary.is_symlink():
temporary.unlink()
elif temporary.exists():
raise ValueError(f"temporary pointer path is occupied: {temporary}")
os.symlink(target.name, temporary, target_is_directory=True)
os.replace(temporary, destination)
return destination
def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
validate_compact_payload(payload)
result = {
name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"])
for name in ACTIVE_ZERO_JOINTS
}
if set(result) != ACTIVE_ZERO_JOINTS or any(
not math.isfinite(value) for value in result.values()
):
raise ValueError(
"payload does not contain all finite observable active zero offsets"
)
return result
def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
"""Validate a standalone thumb artifact and expand CAD finger zeros."""
if (
payload.get("schema_version") != 1
or payload.get("artifact_type")
!= "g20_right_standalone_thumb_calibration"
or payload.get("side") != "right"
or payload.get("non_thumb_zero_policy") != "source_cad_unchanged"
or not bool(payload.get("quality", {}).get("passed"))
):
raise ValueError("standalone thumb JSON contract is invalid")
joints = payload.get("joints")
expected_joint_names = {
"thumb_cmc_roll",
"thumb_cmc_yaw",
"thumb_cmc_pitch",
"thumb_mcp",
"thumb_ip",
}
if not isinstance(joints, Mapping) or set(joints) != expected_joint_names:
raise ValueError("standalone thumb JSON has the wrong joint set")
thumb_names = expected_joint_names - {"thumb_ip"}
result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS}
for name in thumb_names:
try:
value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"])
except (KeyError, TypeError, ValueError) as error:
raise ValueError(
f"standalone thumb JSON is missing zero for {name}"
) from error
curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float)
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
raise ValueError(f"standalone thumb JSON curve is invalid: {name}")
result[name] = value
passive_curve = np.asarray(
joints["thumb_ip"].get("angle_rad", ()), dtype=float
)
if (
passive_curve.shape != (256,)
or not np.all(np.isfinite(passive_curve))
or not bool(joints["thumb_ip"].get("passive"))
or any(not math.isfinite(value) for value in result.values())
):
raise ValueError("standalone thumb JSON passive curve is invalid")
return result
def verify_partial_scope_preserves_certified_zeros(
*,
scope: str,
source_session: str | Path,
serial_root: str | Path | None = None,
serial_number: str,
current_offsets: Mapping[str, float],
) -> None:
"""Reject a partial artifact that changes any certified non-target zero."""
selected = str(scope).strip().lower()
if selected not in {"thumb", "fingers"}:
return
source = Path(source_session).expanduser()
if not source.is_absolute():
if serial_root is None:
raise ValueError(
"relative partial base session requires its serial root"
)
source = Path(serial_root).expanduser() / source
source = source.resolve()
base_payload = _load_json(
source / f"g20_right_{serial_number}_calibration.json"
)
base_offsets = (
standalone_thumb_offsets(base_payload)
if base_payload.get("artifact_type")
== "g20_right_standalone_thumb_calibration"
else active_offsets(base_payload)
)
preserve_thumb = selected == "fingers"
changed = {
name: abs(float(current_offsets[name]) - float(base_offsets[name]))
for name in ACTIVE_ZERO_JOINTS
if name.startswith("thumb_") == preserve_thumb
and abs(float(current_offsets[name]) - float(base_offsets[name]))
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
}
if changed:
raise ValueError(
f"partial {selected} calibration changed certified non-target "
"zeros: "
+ ",".join(sorted(changed))
)
def compare_session_offsets(
first: Mapping[str, Any],
second: Mapping[str, Any],
*,
maximum_difference_rad: float,
) -> dict[str, float]:
first_standalone = (
first.get("artifact_type")
== "g20_right_standalone_thumb_calibration"
)
second_standalone = (
second.get("artifact_type")
== "g20_right_standalone_thumb_calibration"
)
if first_standalone != second_standalone:
raise ValueError("cannot compare standalone thumb and full-hand artifacts")
left = (
standalone_thumb_offsets(first)
if first_standalone
else active_offsets(first)
)
right = (
standalone_thumb_offsets(second)
if second_standalone
else active_offsets(second)
)
differences = {
name: abs(left[name] - right[name])
for name in sorted(left)
}
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
if failed:
details = ", ".join(
f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items()
)
raise ValueError(f"independent-session static-zero mismatch: {details}")
return differences
def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]:
directory = Path(session).resolve()
urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf"))
if len(urdfs) != 1:
raise ValueError("session must contain exactly one corrected URDF")
return {
"json": directory / f"g20_right_{serial_number}_calibration.json",
"urdf": urdfs[0],
"summary": directory / "calibration_summary_zh.json",
"commands": directory / "mujoco_validation_commands.json",
"raw": directory / "raw_samples.jsonl",
"log": directory / "calibration.log",
}
def find_compatible_prior_session(
config: ProductConfig,
current: str | Path,
current_payload: Mapping[str, Any],
) -> tuple[Path | None, dict[str, float]]:
current_path = Path(current).resolve()
for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True):
if candidate.resolve() == current_path:
continue
json_path = candidate / f"g20_right_{config.serial_number}_calibration.json"
summary_path = candidate / "calibration_summary_zh.json"
if not json_path.is_file() or not summary_path.is_file():
continue
try:
summary = _load_json(summary_path)
if not bool(summary.get("quality", {}).get("passed")):
continue
payload = _load_json(json_path)
differences = compare_session_offsets(
payload,
current_payload,
maximum_difference_rad=config.static_repeatability_rad,
)
except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError):
continue
return candidate, differences
return None, {}
def _verify_combination_validation(combination: Mapping[str, Any]) -> None:
# The formal product uses the independently held-out fourth sweep cycle.
# The optional eight-pose check remains a developer diagnostic because the
# axis-line zero solve does not establish an absolute Cartesian hand-base
# transform. When explicitly enabled, retain its strict coverage and
# error checks.
if not bool(combination.get("enabled")):
return
position_p95 = float(combination.get("position_p95_m", float("inf")))
orientation_p95 = float(
combination.get("orientation_p95_rad", float("inf"))
)
required = {str(value) for value in combination.get("required_targets", ())}
expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS)
observations = combination.get("observation_counts")
validations = combination.get("validation_counts")
if required != expected:
raise ValueError("combination validation has the wrong required-target set")
if not isinstance(observations, Mapping) or not isinstance(validations, Mapping):
raise ValueError("combination validation is missing per-target coverage")
missing_observations = sorted(
key for key in expected if int(observations.get(key, 0)) < 2
)
missing_validations = sorted(
key for key in expected if int(validations.get(key, 0)) < 1
)
if missing_observations or missing_validations:
raise ValueError(
"combination validation target coverage is incomplete: "
f"observations={','.join(missing_observations) or '-'}; "
f"validations={','.join(missing_validations) or '-'}"
)
if (
not bool(combination.get("completed"))
or int(combination.get("completed_poses", 0)) != 8
or position_p95 > 0.003
or orientation_p95 > math.radians(2.0)
):
raise ValueError(
"eight-pose four-finger combination validation is incomplete or failed"
)
def finalize_session_artifacts(
config: ProductConfig,
session: str | Path,
*,
node_status: Mapping[str, Any],
) -> tuple[dict[str, Any], bool]:
"""Create diagnostics and update latest_passed only after every guard."""
directory = Path(session).resolve()
paths = session_artifact_paths(directory, config.serial_number)
for name in ("json", "urdf", "raw", "log"):
if not paths[name].is_file():
raise ValueError(f"session is missing {name}: {paths[name]}")
payload = _load_json(paths["json"])
standalone_thumb = bool(
payload.get("artifact_type")
== "g20_right_standalone_thumb_calibration"
)
if standalone_thumb:
standalone_thumb_offsets(payload)
else:
validate_compact_payload(payload)
if payload.get("schema_version") != 4 or payload.get("side") != "right":
raise ValueError("runtime JSON is not the compact right-hand schema v4")
if not bool(payload.get("quality", {}).get("passed")):
raise ValueError("runtime JSON quality is not passed")
combination = node_status.get("combination_validation")
if not isinstance(combination, Mapping):
raise ValueError("node status is missing combination validation")
_verify_combination_validation(combination)
resume = node_status.get("resume", {})
if not isinstance(resume, Mapping):
raise ValueError("node status has invalid resume provenance")
calibration_scope = str(
resume.get("recalibration_scope", "full")
).strip().lower()
if calibration_scope not in {"full", "thumb", "fingers"}:
raise ValueError("node status has an unsupported recalibration scope")
recalibration_tasks = tuple(
str(value) for value in resume.get("recalibration_task_keys", ())
)
if calibration_scope == "thumb" and not standalone_thumb and (
not bool(resume.get("used"))
or not str(resume.get("source_session", ""))
or len(recalibration_tasks) != 4
or any("thumb_" not in name for name in recalibration_tasks)
):
raise ValueError(
"thumb recalibration is missing its passed base-session provenance"
)
if calibration_scope == "fingers" and (
not bool(resume.get("used"))
or not str(resume.get("source_session", ""))
or len(recalibration_tasks) != 12
or any("thumb_" in name for name in recalibration_tasks)
):
raise ValueError(
"finger recalibration is missing its certified thumb-session "
"provenance"
)
if standalone_thumb and calibration_scope != "thumb":
raise ValueError("standalone thumb artifact has the wrong node scope")
offsets = (
standalone_thumb_offsets(payload)
if standalone_thumb
else active_offsets(payload)
)
if calibration_scope != "full" and not standalone_thumb:
verify_partial_scope_preserves_certified_zeros(
scope=calibration_scope,
source_session=str(resume.get("source_session", "")),
serial_root=directory.parent,
serial_number=config.serial_number,
current_offsets=offsets,
)
endpoint_offsets = {
name: offsets[name]
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
}
changed_joints = verify_corrected_urdf(
config.source_urdf,
paths["urdf"],
expected_offsets_rad=offsets,
endpoint_anchored_offsets_rad=endpoint_offsets,
)
if standalone_thumb:
clipped_runtime_joints = {}
else:
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
payload, paths["urdf"]
)
if clipped_runtime_joints:
atomic_write_json(paths["json"], payload)
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
mesh_hashes = {
name: sha256_file(path) for name, path in mesh_resources.items()
}
commands = (
{
"artifact_type": "standalone_thumb_no_full_hand_validation",
"poses": [],
}
if standalone_thumb
else build_mujoco_validation_commands(payload["baseline_command_u8"])
)
atomic_write_json(paths["commands"], commands)
prior, differences = find_compatible_prior_session(config, directory, payload)
release_ready = config.required_independent_passes == 1 or prior is not None
quality = dict(payload["quality"])
preserved_certified_zeros = sorted(
name
for name in ACTIVE_ZERO_JOINTS
if (
calibration_scope == "thumb"
and not name.startswith("thumb_")
)
or (
calibration_scope == "fingers"
and name.startswith("thumb_")
)
)
if standalone_thumb:
preserved_certified_zeros = []
calibrated_zero_joints = (
sorted(
name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_")
)
if standalone_thumb
else sorted(ACTIVE_ZERO_JOINTS)
)
summary: dict[str, Any] = {
"schema_version": 1,
"serial_number": config.serial_number,
"session_id": f"{config.serial_number}_{directory.name}",
"calibration_scope": calibration_scope,
"inherited_base_session": (
None
if calibration_scope == "full" or standalone_thumb
else str(resume.get("source_session"))
),
"freshly_calibrated_task_keys": list(recalibration_tasks),
"preserved_certified_zero_joints": preserved_certified_zeros,
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
"quality": quality,
"runtime_limit_clipped_bins": clipped_runtime_joints,
"runtime_curve_domain": (
"thumb_diagnostic_only"
if standalone_thumb
else "requested_command_u8"
),
"static_zero_calibrated_joints": calibrated_zero_joints,
"non_thumb_zero_policy": (
"source_cad_unchanged" if standalone_thumb else None
),
"retained_active_urdf_zero_joints": sorted(
RETAINED_ACTIVE_ZERO_JOINTS
),
"retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
"mimic_derived_dynamic_joints": [],
"visually_measured_passive_joints": sorted(
VISUALLY_MEASURED_FINGER_DIPS
),
"changed_urdf_joint_origins": list(changed_joints),
"hashes": {
"source_urdf_sha256": config.source_urdf_sha256,
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
"calibration_config_sha256": config.calibration_config_sha256,
"corrected_urdf_sha256": sha256_file(paths["urdf"]),
"mesh_resources_sha256": mesh_hashes,
"calibration_json_sha256": sha256_file(paths["json"]),
"raw_samples_sha256": sha256_file(paths["raw"]),
},
"holdout": {
"training_cycles": [0, 1, 2],
"validation_cycle": 3,
"isolated": True,
},
"combination_validation": dict(combination),
"formal_release": {
"passed": release_ready,
"required_independent_passes": config.required_independent_passes,
"comparison_session": None if prior is None else prior.name,
"maximum_static_difference_deg": (
None if not differences else math.degrees(max(differences.values()))
),
},
"node_status": dict(node_status),
"created_at_utc": datetime.now(timezone.utc).isoformat(),
"artifacts": {name: path.name for name, path in paths.items()},
}
atomic_write_json(paths["summary"], summary)
# Recompute hashes after all files exist and validate the pair once more
# immediately before the one atomic publication operation.
if sha256_file(config.source_urdf) != config.source_urdf_sha256:
raise ValueError("source URDF changed during calibration")
verify_corrected_urdf(
config.source_urdf,
paths["urdf"],
expected_offsets_rad=offsets,
endpoint_anchored_offsets_rad=endpoint_offsets,
)
# Revalidate the same coordinate contract immediately before publication:
# verify_corrected_urdf proves that corrected endpoint limits map back to
# the original physical CAD endpoints, while the runtime curves must stay
# inside those corrected-coordinate limits.
if not standalone_thumb:
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
final_mesh_hashes = {
name: sha256_file(path)
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
}
if final_mesh_hashes != mesh_hashes:
raise ValueError("URDF mesh resources changed during publication")
if release_ready:
atomic_session_pointer(
config.session_root,
"latest_thumb_passed" if standalone_thumb else "latest_passed",
directory,
)
return summary, release_ready
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/sample_schema.py
+192
View File
@@ -0,0 +1,192 @@
"""Canonical command/feedback schema for calibration observations.
The hand command and its measured motor feedback are different physical
domains. Durable samples always retain both. Fitting code may still use the
historical ``command_u8`` key, but it is created only as an explicit projection
of a canonical record at the fitting boundary.
"""
from __future__ import annotations
import math
from typing import Any, Iterable, Literal, Mapping
import numpy as np
SAMPLE_KINDS = frozenset(
{
'sample',
'baseline_hold_sample',
'steady_command_sample',
'palm_axis_sample',
}
)
FitDomain = Literal['default', 'requested', 'feedback']
class SampleDataContractError(ValueError):
"""A calibration observation mixes or omits command domains."""
def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float:
try:
number = float(value)
except (TypeError, ValueError) as error:
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be numeric'
) from error
if not math.isfinite(number) or not 0.0 <= number <= 255.0:
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]'
)
if integral:
rounded = int(round(number))
if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9):
raise SampleDataContractError(
f'DATA-CONTRACT-701:{field} must be an integer command'
)
return rounded
return number
def explicit_domain_value(
source: Mapping[str, Any], domain: Literal['requested', 'feedback']
) -> int | float:
"""Read and validate one explicitly named domain from any observation."""
field = (
'requested_command_u8' if domain == 'requested' else 'feedback_u8'
)
if field not in source or source[field] is None:
raise SampleDataContractError(
f'DATA-CONTRACT-701:observation is missing explicit {field}'
)
return _finite_u8(source[field], field, integral=domain == 'requested')
def canonical_sample_record(
source: Mapping[str, Any],
*,
allow_legacy_command: bool = False,
) -> dict[str, Any]:
"""Return one durable, unambiguous calibration observation.
``allow_legacy_command`` is restricted to importing historical sessions
and unit fixtures. New online observations must provide both explicit
fields and therefore cannot silently reinterpret ``command_u8``.
"""
record = dict(source)
kind = str(record.get('kind', ''))
if not kind and allow_legacy_command:
# Old in-memory steady-curve fixtures predate durable sample kinds.
# This adapter is never enabled by the new online/import contract.
kind = 'steady_command_sample'
record['kind'] = kind
if kind not in SAMPLE_KINDS:
raise SampleDataContractError(
f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}'
)
requested = record.get('requested_command_u8')
feedback = record.get('feedback_u8')
legacy = record.get('command_u8')
if requested is None or feedback is None:
if not allow_legacy_command or legacy is None:
missing = [
name
for name, value in (
('requested_command_u8', requested),
('feedback_u8', feedback),
)
if value is None
]
raise SampleDataContractError(
'DATA-CONTRACT-701:'
f'{kind} is missing explicit {",".join(missing)}'
)
# Historical in-memory records used requested commands for settled
# checkpoints and feedback bins for dense/baseline/palm observations.
if requested is None:
requested = legacy
if feedback is None:
feedback = legacy
record.pop('command_u8', None)
record['requested_command_u8'] = explicit_domain_value(
{'requested_command_u8': requested}, 'requested'
)
record['feedback_u8'] = explicit_domain_value(
{'feedback_u8': feedback}, 'feedback'
)
return record
def fitting_sample_record(
source: Mapping[str, Any],
*,
domain: FitDomain = 'default',
allow_legacy_command: bool = False,
snap_requested_endpoints: bool = False,
) -> dict[str, Any]:
"""Project a canonical sample into the legacy curve-fitter interface."""
record = canonical_sample_record(
source, allow_legacy_command=allow_legacy_command
)
kind = str(record['kind'])
selected = domain
if selected == 'default':
selected = (
'requested' if kind == 'steady_command_sample' else 'feedback'
)
if selected not in {'requested', 'feedback'}:
raise SampleDataContractError(
f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}'
)
requested = int(record['requested_command_u8'])
if selected == 'requested' or (
snap_requested_endpoints and requested in {0, 255}
):
index = requested
else:
index = int(
np.clip(np.rint(float(record['feedback_u8'])), 0, 255)
)
record['command_u8'] = index
return record
def fitting_sample_records(
records: Iterable[Mapping[str, Any]],
*,
domain: FitDomain = 'default',
allow_legacy_command: bool = False,
snap_requested_endpoints: bool = False,
) -> list[dict[str, Any]]:
"""Project several canonical samples into one explicit fitting domain."""
return [
fitting_sample_record(
record,
domain=domain,
allow_legacy_command=allow_legacy_command,
snap_requested_endpoints=snap_requested_endpoints,
)
for record in records
]
def validate_sample_records(
records: Iterable[Mapping[str, Any]],
*,
allow_legacy_command: bool = False,
) -> None:
"""Validate a collection without changing its representation."""
for index, record in enumerate(records):
try:
canonical_sample_record(
record, allow_legacy_command=allow_legacy_command
)
except SampleDataContractError as error:
raise SampleDataContractError(
f'{error};record_index={index}'
) from error
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py
+431 -42
View File
@@ -3,12 +3,14 @@
from __future__ import annotations
import re
from typing import Any, Mapping
from typing import Any, Mapping, Sequence
STATE_NAMES_ZH = {
"PREFLIGHT": "设备和标签预检",
"WAIT_START": "等待开始标定",
"IMPORTING_BASE": "正在读取基础标定会话",
"REVALIDATING_INHERITED": "正在复核继承的四指数据",
"RETURN_BASELINE": "正在恢复目标姿态",
"PREPARE_SWEEP": "正在到达扫描起点",
"SWEEP": "正在采集轨迹",
@@ -48,6 +50,10 @@ JOINT_NAMES_ZH = {
"pinky_pip": "小指PIP",
"pinky_dip": "小指DIP（被动）",
"thumb_cmc_yaw": "拇指CMC侧摆",
"index_mcp_roll_side": "食指MCP侧摆（侧面校验）",
"middle_mcp_roll_side": "中指MCP侧摆（侧面校验）",
"ring_mcp_roll_side": "无名指MCP侧摆（侧面校验）",
"pinky_mcp_roll_side": "小指MCP侧摆（侧面校验）",
}
@@ -86,6 +92,12 @@ def _task_text(active: Mapping[str, Any]) -> str:
f"{_format_u8(active.get('target_u8'))}、实际"
f"{_format_u8(active.get('actual_u8'))}"
)
if active.get("kind") == "cross_view_roll_diagnostic":
return (
f"{active.get('finger', '?')}侧摆跨机位诊断完成："
f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°，"
f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°"
)
if active.get("kind") == "validation":
return (
f"{view}机位，随机复测，电机{active.get('motor_index')}，"
@@ -99,23 +111,19 @@ def _task_text(active: Mapping[str, Any]) -> str:
target = active.get("target_u8")
cycle = active.get("cycle", "?")
repetitions = active.get("repetitions", "?")
direction_index = active.get("direction_index")
direction_text = (
"" if direction_index is None else f"第{direction_index}/2程，"
)
task = (
f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
f"第{cycle}/{repetitions}轮，{direction_text}{start}→{target}"
f"第{cycle}/{repetitions}轮，{start}→{target}"
)
sequence = active.get("cycle_sequence_u8", [])
if len(sequence) == 3:
task += "（本轮" + "→".join(str(value) for value in sequence) + "）"
fit_attempt = int(active.get("fit_attempt", 1))
if fit_attempt > 1:
task += (
f"（整关节自动重采第{fit_attempt}/"
f"{active.get('fit_attempt_limit', '?')}次）"
)
retry_cycles = active.get("fit_retry_cycles", [])
if retry_cycles:
task += "（补采异常轮" + "/".join(
str(cycle) for cycle in retry_cycles
) + "）"
else:
task += f"（拟合补采第{fit_attempt}次）"
return task
@@ -139,10 +147,19 @@ def three_camera_reason_zh(
context = fields[1] if len(fields) > 1 else "unknown"
error_match = re.search(r"error_u8=([0-9.]+)", reason)
error = error_match.group(1) if error_match else "未知"
timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason)
timeout_value = active.get("timeout_seconds")
if timeout_value is None and timeout_match is not None:
timeout_value = float(timeout_match.group(1))
duration = (
f"连续{float(timeout_value):g}秒"
if timeout_value is not None
else "在规定时间内"
)
motor = active.get("motor_index")
if motor is not None:
return (
f"电机{motor}反馈连续8秒没有向目标推进；目标"
f"电机{motor}反馈{duration}没有向目标推进；目标"
f"{_format_u8(active.get('target_u8'))}、实际"
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，"
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
@@ -151,12 +168,29 @@ def three_camera_reason_zh(
"重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。",
)
return (
f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8"
f"电机反馈{duration}没有向目标推进；停止位置距目标{error}个u8"
f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。",
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
"正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。",
)
base_reason, separator, reason_detail = reason.partition(":")
if base_reason in {
"sweep_missing_endpoint_bin",
"sweep_bins_too_few",
"sweep_bin_gap_too_large",
"task_precheck_missing_command_127",
"task_precheck_detection_rate_too_low",
"synchronised_tag_state_timeout",
}:
reason = base_reason
detail_label = (
JOINT_NAMES_ZH.get(reason_detail, reason_detail)
if separator and reason_detail
else ""
)
detail_prefix = f"{detail_label}：" if detail_label else ""
if "URDF zero offset reached the configured" in reason:
bound_match = re.search(
r"configured\s+([0-9.]+)\s+degree bound", reason
@@ -181,7 +215,7 @@ def three_camera_reason_zh(
if reason == "sweep_missing_endpoint_bin":
missing_text = "、".join(str(value) for value in missing) or "0或255"
return (
f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少"
f"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少"
f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为"
f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到"
"端点，但该时刻没有同时取得有效Tag图像和电机状态。",
@@ -190,29 +224,214 @@ def three_camera_reason_zh(
)
if reason == "sweep_bins_too_few":
return (
f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
"检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
)
if reason == "sweep_bin_gap_too_large":
gap_start = sample.get("maximum_bin_gap_start_u8")
gap_end = sample.get("maximum_bin_gap_end_u8")
gap_range = (
""
if gap_start is None or gap_end is None
else f"（{gap_start}→{gap_end}）"
)
return (
f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
f"{gap_range}允许值不超过"
f"{sample.get('allowed_maximum_bin_gap', '?')}。",
"检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
)
if reason == "synchronised_tag_state_timeout":
group_reasons = active.get("group_pnp_reasons", {})
if isinstance(group_reasons, Mapping) and group_reasons:
tag_rejections = active.get("pnp_rejection_counts", {})
group_rejections = active.get(
"group_pnp_rejection_counts", {}
)
missing_roles = active.get(
"group_missing_candidate_roles", {}
)
candidate_diagnostics = active.get(
"pnp_candidate_diagnostics", {}
)
view_details: list[str] = []
for view, value in group_reasons.items():
view_name = str(view)
parts = [str(value)]
missing = (
missing_roles.get(view_name, ())
if isinstance(missing_roles, Mapping)
else ()
)
if isinstance(missing, Sequence) and not isinstance(
missing, (str, bytes)
) and missing:
parts.append(
"缺候选=" + ",".join(str(role) for role in missing)
)
counts: dict[str, int] = {}
for source in (tag_rejections, group_rejections):
values = (
source.get(view_name)
if isinstance(source, Mapping)
else None
)
if isinstance(values, Mapping):
for name, count in values.items():
counts[str(name)] = counts.get(str(name), 0) + int(
count
)
if counts:
common = sorted(
counts.items(), key=lambda pair: (-pair[1], pair[0])
)[:3]
parts.append(
"累计拒绝="
+ ",".join(
f"{name}×{count}" for name, count in common
)
)
view_candidates = (
candidate_diagnostics.get(view_name, {})
if isinstance(candidate_diagnostics, Mapping)
else {}
)
if isinstance(view_candidates, Mapping) and missing:
summaries: list[str] = []
for role in missing:
diagnostic = view_candidates.get(str(role), {})
if not isinstance(diagnostic, Mapping):
continue
summaries.append(
f"{role}(solve="
f"{int(diagnostic.get('solved_candidate_count', 0))},"
"reproj="
f"{int(diagnostic.get('reprojection_candidate_count', 0))},"
"tilt="
f"{int(diagnostic.get('independent_tilt_candidate_count', 0))})"
)
if summaries:
parts.append("候选统计=" + ",".join(summaries))
view_details.append(
f"{VIEW_NAMES_ZH.get(view_name, view_name)}="
+ "；".join(parts)
)
reason_text = "、".join(view_details)
return (
f"{detail_prefix}已经取得部分有效轨迹，但Tag仍可见且反馈正常时，"
"后续连续图像帧"
"被整组PnP几何检查拒绝"
f"（{reason_text}），因此无法与电机状态形成有效轨迹帧。",
"不要调整或反复粘贴Tag；保留当前会话中的"
"group_pnp_candidate_event，"
"按缺失角色的候选统计检查PnP分支逻辑。",
)
return (
"运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
"按时间戳配对”的图像帧。",
"查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
"程序会重扫当前方向。",
)
if reason == "task_precheck_missing_command_127":
return (
f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。",
"检查中位姿态的Tag遮挡和反光；程序只会重扫当前物理任务。",
)
if reason == "task_precheck_detection_rate_too_low":
return (
f"{detail_prefix}低速预检的有效Tag识别率低于门限。",
"检查该机位当前任务Tag的遮挡、反光和对焦；程序只会重扫当前物理任务。",
)
if reason == "sweep_start_position_timeout":
return (
f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。",
"检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。",
)
if reason == "sweep_start_tag_timeout":
group_reasons = active.get("group_pnp_reasons", {})
progress_by_view = active.get("pnp_initialization_progress", {})
tag_rejections = active.get("pnp_rejection_counts", {})
group_rejections = active.get("group_pnp_rejection_counts", {})
if any(
isinstance(value, Mapping) and bool(value)
for value in (
group_reasons,
progress_by_view,
tag_rejections,
group_rejections,
)
):
details: list[str] = []
views = set()
for value in (
group_reasons,
progress_by_view,
tag_rejections,
group_rejections,
):
if isinstance(value, Mapping):
views.update(str(view) for view in value)
for view in sorted(views):
parts: list[str] = []
progress = (
progress_by_view.get(view)
if isinstance(progress_by_view, Mapping)
else None
)
if isinstance(progress, Mapping):
parts.append(
"初始化"
f"{int(progress.get('accepted', 0))}/"
f"{int(progress.get('required', 0))}"
)
counts: dict[str, int] = {}
for source in (tag_rejections, group_rejections):
values = (
source.get(view)
if isinstance(source, Mapping)
else None
)
if isinstance(values, Mapping):
for name, count in values.items():
counts[str(name)] = (
counts.get(str(name), 0) + int(count)
)
if counts:
common = sorted(
counts.items(), key=lambda pair: (-pair[1], pair[0])
)[:3]
parts.append(
"累计拒绝="
+ ",".join(
f"{name}×{count}" for name, count in common
)
)
latest = (
group_reasons.get(view)
if isinstance(group_reasons, Mapping)
else None
)
if latest and not str(latest).startswith(
"group_initializing:"
):
parts.append(f"最后状态={latest}")
if parts:
details.append(
f"{VIEW_NAMES_ZH.get(view, view)}=" + "；".join(parts)
)
reason_text = "、".join(details) or "未形成完整初始化窗口"
return (
"被测电机已经到达扫描起点，所需Tag也可见，但三维PnP位姿初始化"
f"没有完成（{reason_text}），因此没有生成同步端点帧。",
"不要根据可见性重复粘贴Tag；保留累计拒绝原因并检查PnP候选选择。",
)
return (
"被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的"
"同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”，不会触发此错误。",
"只检查标记为✗的实时运动Tag、反光和外部遮挡；不要移动相机或手掌底座。",
)
if reason == "sweep_timeout":
return (
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
@@ -233,7 +452,27 @@ def three_camera_reason_zh(
"随机复测位置没有采集到足够的同步有效Tag帧。",
"检查当前机位Tag可见性后调用resume。",
)
if reason == "joint_fit_check_failed":
if reason == "palm_orientation_quality_failed":
failures = active.get("failures", [])
detail = (
str(failures[0].get("reason", "方向观测不足"))
if failures
else "方向观测不足"
)
if "thumb_cmc_" in detail:
return (
"拇指CMC yaw无法由顶部Tag 8/9的零位邻近短轨迹稳定确定："
+ detail,
"保持Tag安装不变；确保顶部Tag 8/9在拇指CMC pitch和roll"
"从零位开始的前1/4行程持续可见后重新标定。",
)
return (
"掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定："
+ detail,
"保持Tag安装不变；让正面Tag 10–13在对应MCP-pitch起始段"
"至少可见15°行程后重新标定。",
)
if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
metric_names = {
"plane_rms_mm": "平面拟合RMS",
"radial_rms_mm": "圆半径拟合RMS",
@@ -244,15 +483,23 @@ def three_camera_reason_zh(
"arc_deg": "实测圆弧",
"monotonic_correction_deg": "最大单调修正",
"hysteresis_deg": "最大正反程差",
"baseline_hysteresis_deg": "baseline正反程关节角差",
"baseline_directional_gap_deg": "baseline方向分支间隙",
"baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差",
"cycle_travel_range_deg": "三轮行程差",
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
"axis_plane_rms_mm": "三维圆轴向RMS",
"axis_radial_rms_mm": "三维圆半径RMS",
"axis_pose_line_rms_mm": "姿态轨迹轴线RMS",
"axis_line_cycle_rms_mm": "四轮轴线位置RMS",
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
"axis_cycle_difference_deg": "三轮转轴方向极差",
"axis_cycle_difference_deg": "各轮转轴方向极差",
"cross_view_roll_curve": "正面/侧面关节角曲线差异RMS",
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
"zero_cycle_offset_range_deg": "训练轮零位极差",
"zero_confidence_95_half_width_deg": "零位95%置信半宽",
"state_image_sync_p95_ms": "图像与电机状态同步误差P95",
"tag_valid_rate_percent": "所需Tag同时有效率",
}
@@ -266,17 +513,25 @@ def three_camera_reason_zh(
"arc_deg": "°",
"monotonic_correction_deg": "°",
"hysteresis_deg": "°",
"baseline_hysteresis_deg": "°",
"baseline_directional_gap_deg": "°",
"baseline_directional_gap_range_deg": "°",
"cycle_travel_range_deg": "°",
"rotation_orthogonal_rms_deg": "°",
"axis_plane_rms_mm": "mm",
"axis_radial_rms_mm": "mm",
"axis_pose_line_rms_mm": "mm",
"axis_line_cycle_rms_mm": "mm",
"rotation_circle_axis_difference_deg": "°",
"axis_cycle_difference_deg": "°",
"third_cycle_axis_holdout_deg": "°",
"third_cycle_axis_line_rms_mm": "mm",
"third_cycle_trajectory_p95_deg": "°",
"zero_cycle_offset_range_deg": "°",
"zero_confidence_95_half_width_deg": "°",
"state_image_sync_p95_ms": "ms",
"tag_valid_rate_percent": "%",
"cross_view_roll_curve": "°",
}
details: list[str] = []
for failure in active.get("failures", []):
@@ -295,9 +550,16 @@ def three_camera_reason_zh(
)
cycle_travel = failure.get("cycle_travel_deg", [])
if cycle_travel:
detail += "（三轮=" + "/".join(
detail += "（各轮=" + "/".join(
f"{float(value):.2f}°" for value in cycle_travel
) + "）"
cycle_values = failure.get("cycle_values_deg", [])
if not cycle_values:
cycle_values = failure.get("cycle_offset_deg", [])
if cycle_values:
detail += "（各轮=" + "/".join(
f"{float(value):.2f}°" for value in cycle_values
) + "）"
details.append(detail)
else:
cycle = failure.get("cycle")
@@ -307,11 +569,53 @@ def three_camera_reason_zh(
f"{failure.get('reason', '未知原因')}"
)
detail_text = "；".join(details) or "当前关节的轨迹拟合未通过"
directional_gap_failure = any(
str(failure.get("metric", "")).startswith(
"baseline_directional_gap"
)
for failure in active.get("failures", [])
)
if reason == "joint_fit_systematic_failure":
cross_view_systematic = any(
failure.get("classification")
in {
"stable_cross_view_installation_or_model_bias",
"stable_cross_view_direction_conflict",
}
for failure in active.get("failures", [])
)
suggestion = (
"四轮都出现稳定的正面/侧面差异，属于Tag安装外参或跨视角模型偏差，"
"继续重扫不会改善；检查Tag刚性安装与跨视角安装变换，不要放宽门限。"
if cross_view_systematic
else "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。"
"请直接复制诊断块给开发者，不要放宽门限。"
)
else:
source_task_names = set(active.get("source_task_names", []))
thumb_yaw_source_retry = source_task_names == {
"thumb_cmc_pitch_front",
"thumb_cmc_roll_front",
}
suggestion = (
"方向分支已由软件保留，不要放宽门限；请检查传动回差或高支架刚度，"
"处理后重新执行一键标定命令，程序会从最近可靠断点继续。"
if directional_gap_failure
else (
"保持顶部Tag 8/9无遮挡；程序只替换决定yaw零位的"
"CMC pitch/roll顶部轴观测并重扫"
f"{active.get('directions_to_rescan', 16)}个方向，"
"不会无效重扫yaw侧摆。"
if thumb_yaw_source_retry
else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令；"
"程序只清除当前失败关节的数据并重扫"
f"{active.get('directions_to_rescan', 6)}个方向，"
"不需要手工调用ROS服务。"
)
)
return (
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
"修正Tag位置、遮挡或机械行程后调用"
"/g20_calibration/resume；程序只清除当前失败关节的数据"
f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。",
suggestion,
)
if reason == "zero_model_validation_failed":
reason_names = {
@@ -352,11 +656,44 @@ def three_camera_reason_zh(
"该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；"
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
)
if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}:
return (
"正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪；此阶段不以Tag可见性阻止基准恢复。",
"保持机械手运动范围无障碍；设备就绪后系统会先安全恢复基准形态，再检查掌部Tag。",
)
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
return (
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
"根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
)
if reason == "waiting_for_baseline_tags_after_recovery":
return (
"机械手已经稳定恢复到基准形态，正在用新采集的画面确认三台相机各自的固定掌部Tag。",
"若某个掌部Tag持续缺失，只调整遮挡手指或检查Tag固定情况，不要移动相机和手掌底座。",
)
if reason == "locking_fixed_base_references":
return (
"基准形态Tag预检已通过，正在把三台相机的固定掌部Tag稳健锁定为本会话参考。",
"无需操作；锁定完成后允许任务姿态遮挡固定掌部Tag。",
)
if reason == "fixed_base_reference_moved":
return (
"顶部Tag 8在本会话基准锁定后连续多帧发生角点位移；程序已立即保持机械手当前位置，"
"本会话中已采集数据不再用于发布。",
"Tag 8允许在下一次标定预检前重新摆放，但本次不能继续；固定Tag 8和顶部相机后"
"重新启动新会话。",
)
if reason == "waiting_for_task_tags_at_sweep_start":
return (
"电机已到扫描起点，正在等待当前任务的实时运动Tag；显示为“锁”的固定掌部Tag"
"允许被手指遮挡。",
"只检查标记为✗的实时运动Tag；若均为✓或锁，程序会自动开始运动。",
)
if reason == "call_start_for_baseline_recovery":
return (
"相机数据和机械手反馈已就绪，等待一键程序触发安全基准恢复。",
"保持机械手运动范围无障碍；程序会自动开始，无需手工调用ROS服务。",
)
if reason == "call_start":
return (
"三机位预检已经通过，等待操作员确认开始。",
@@ -368,14 +705,74 @@ def three_camera_reason_zh(
return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
if reason == "collecting_timestamp_synchronised_tag_centres":
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。"
if reason == "collecting_dedicated_baseline_hold":
return (
"正在从当前方向到达关节baseline并静止采集Tag与电机反馈；这批数据单独用于回差验收。",
"无需操作，保持相机、标签和底座不动。",
)
if reason == "steady checkpoint target is missing":
return (
"首轮稳态检查点已经到达最终端点，但采集状态没有及时切换到端点完成阶段。",
"程序已停止发布并保留已采样数据；这是软件状态切换问题，不需要调整相机、Tag或机械手。",
)
if reason == "cross_view_roll_front_failure_deferred":
return (
"正面侧摆回差不合格已保留，诊断模式将继续采集同一手指的侧面数据。",
"无需操作；该诊断会锁定URDF发布。",
)
if reason == "cross_view_roll_diagnostic_complete":
interpretation = str(active.get("interpretation", ""))
explanations = {
"both_views_confirm_direction_dependent_pose": (
"正面和侧面都确认了方向相关姿态，优先判断为roll输出机构或共同下游链的真实回差。"
),
"front_only_difference_check_roll_tag_bracket_or_front_pnp": (
"只有正面差异超限，优先检查roll Tag高支架刚度和正面PnP。"
),
"side_only_difference_check_side_tag_chain_or_side_pnp": (
"只有侧面差异超限，优先检查侧面Tag链和侧面PnP。"
),
"both_views_within_formal_hysteresis_limit": (
"两个机位的静止回差均满足正式门限。"
),
}
return (
explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。")
+ " 本次为诊断会话，不会生成或发布URDF。",
"保存当前状态和raw_samples.jsonl；根据两机位结论处理后重新启动正式标定。",
)
if reason == "capturing_random_validation_pose":
return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。"
if reason in {"calibration_passed", "calibration_complete"}:
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
if reason == "quality_failed":
return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
if reason.startswith("validated_endpoint_zero_state"):
return (
"轨迹和URDF零位验证已经通过，但发布前检测到端点零位状态缺失或与"
"已验证模型不一致；这是程序内部状态生命周期错误，结果未发布。",
"不要移动相机、Tag或机械手底座；保留当前会话并把原因码交给开发者。",
)
if reason.startswith("PUB-ARTIFACT-601:"):
return (
"标定节点已经生成通过质量门限的JSON和候选URDF，但一键程序在正式发布前"
"发现这对产物的坐标、限位、哈希或资源一致性检查失败；原始URDF未被覆盖。",
"不要重新标定相机或调整Tag；保留本会话产物和启动日志供开发者检查发布契约。",
)
if reason == "combination_pose_prediction_failed":
return (
"单关节、零位和URDF几何验证已通过，但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
"程序会在原姿态重新初始化PnP并自动复测；若最终仍失败，请把raw_samples.jsonl中的"
"combination_validation_failure记录交给开发者，不要重新采集16个单关节任务。",
)
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
if reason == "holding_same_finger_clearance_before_next_task":
return (
"同一根手指的上一项已经完成；相邻手指继续保持当前避让姿态，只调整"
"被测关节以衔接下一项。",
"无需操作，不要手动展开正在避让的手指。",
)
if state == "RETURN_BASELINE":
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
if state == "FITTING":
@@ -390,14 +787,9 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
reason_zh, action_zh = three_camera_reason_zh(
state, str(payload.get("reason", "")), active
)
service_prefix = str(payload.get("service_prefix", "/g20_calibration"))
if service_prefix != "/g20_calibration":
reason_zh = reason_zh.replace("/g20_calibration", service_prefix)
action_zh = action_zh.replace("/g20_calibration", service_prefix)
progress = float(payload.get("progress", 0.0))
completed = payload.get("completed_sweeps", 0)
total = payload.get("total_sweeps", 0)
executed = int(payload.get("executed_sweep_directions", completed))
scan_progress = float(
payload.get(
"scan_progress",
@@ -412,17 +804,6 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
f"扫描进度{scan_progress:.1%}）",
f"当前任务：{_task_text(active)}",
]
if executed > int(completed):
lines.append(
f"实际采集：已启动{executed}个方向（含自动重扫）；"
f"计划进度只统计{total}个唯一方向，重扫不会重复增加计划进度"
)
attempt = int(active.get("attempt", active.get("fit_attempt", 1)))
if attempt > 1:
lines.append(
f"整关节重采：当前为第{attempt}/"
f"{active.get('fit_attempt_limit', '?')}次采集结果"
)
if state == "RETURN_BASELINE":
baseline_command = payload.get("baseline_command_u8", [])
return_command = payload.get("return_command_u8", baseline_command)
@@ -454,6 +835,14 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～"
f"{_format_u8(sample.get('maximum_u8'))}"
)
detection_frames = int(active.get("detection_frames", 0))
if detection_frames:
lines.append(
"本方向Tag检出："
f"{float(active.get('detection_rate', 0.0)):.1%}"
f"（{active.get('detection_valid_frames', 0)}/"
f"{detection_frames}帧）"
)
auxiliary = active.get("auxiliary_motors", [])
if auxiliary:
lines.append(
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py
+9617 -783
View File
File diff suppressed because it is too large Load Diff
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py
+9 -4
View File
@@ -634,6 +634,7 @@ def _fit_joint_curve(
values: Sequence[float],
*,
endpoint_reference: Mapping[str, Sequence[float]] | None = None,
preserve_direction_offset: bool = False,
) -> tuple[dict[str, Any], float, float]:
by_direction: dict[str, list[list[float]]] = {
direction: [[] for _ in range(256)] for direction in DIRECTIONS
@@ -669,9 +670,11 @@ def _fit_joint_curve(
],
dtype=float,
)
raw -= raw[-1]
if not preserve_direction_offset:
raw -= raw[-1]
projected_samples = isotonic_nonincreasing(raw)
projected_samples -= projected_samples[-1]
if not preserve_direction_offset:
projected_samples -= projected_samples[-1]
maximum_correction = max(
maximum_correction,
float(np.max(np.abs(projected_samples - raw))),
@@ -681,7 +684,8 @@ def _fit_joint_curve(
commands.astype(float),
projected_samples,
)
curve -= curve[255]
if not preserve_direction_offset:
curve -= curve[255]
if endpoint_reference is not None:
curve = _regularize_coupled_zero_tail(
curve,
@@ -695,7 +699,8 @@ def _fit_joint_curve(
increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float)
hysteresis = float(np.max(np.abs(decreasing - increasing)))
combined = 0.5 * (decreasing + increasing)
combined -= combined[255]
if not preserve_direction_offset:
combined -= combined[255]
return (
{
"angle_rad": [round(float(value), 8) for value in combined],
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py
+2819 -256
View File
File diff suppressed because it is too large Load Diff
src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py
+5 -4
View File
@@ -1,4 +1,4 @@
"""Publish calibrated URDF joint angles from raw u8 commands."""
"""Publish calibrated G20 URDF angles from raw command/feedback u8 values."""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
@@ -9,7 +9,6 @@ from launch_ros.actions import Node
def generate_launch_description() -> LaunchDescription:
return LaunchDescription(
[
DeclareLaunchArgument("hand_model", default_value="G20"),
DeclareLaunchArgument("hand_type", default_value="right"),
DeclareLaunchArgument("calibration_file"),
DeclareLaunchArgument("input_topic", default_value=""),
@@ -17,12 +16,14 @@ def generate_launch_description() -> LaunchDescription:
Node(
package="g20_thumb_apriltag_calibration",
executable="calibrated_joint_state_bridge",
name="calibrated_joint_state_bridge",
name=[
"g20_calibrated_joint_state_bridge_",
LaunchConfiguration("hand_type"),
],
output="screen",
emulate_tty=True,
parameters=[
{
"hand_model": LaunchConfiguration("hand_model"),
"hand_type": LaunchConfiguration("hand_type"),
"calibration_file": LaunchConfiguration(
"calibration_file"
src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py
+128 -145
View File
@@ -1,8 +1,9 @@
"""Launch three Hikrobot views and one supported-hand calibration owner."""
"""Launch three Hikrobot views and one complete-G20 calibration owner."""
from __future__ import annotations
from datetime import datetime
import hashlib
from pathlib import Path
import re
@@ -21,32 +22,11 @@ from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch_ros.parameter_descriptions import ParameterValue
from g20_thumb_apriltag_calibration.full_hand import (
get_hand_calibration_profile,
)
VIEWS = ("front", "side", "top")
def _default_source_urdf(hand_model: str, hand_type: str) -> Path:
if hand_model == "O30":
relative = (
Path("linkerhand-urdf/O30/urdf_0803-right/src")
/ "linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf"
)
candidates = (
Path.cwd().parent / relative,
Path.home() / "projects" / relative,
)
return next(
(candidate for candidate in candidates if candidate.is_file()),
Path.home()
/ "projects"
/ "linkerhand-urdf/O30/urdf_0803-right/src"
/ "linkerhand_O30i_right.urdf"
/ "linkerhand_O30i_right-0803.urdf",
)
def _default_source_urdf(hand_type: str) -> Path:
relative = Path(
"assets/robots/hands/linker_hand"
) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
@@ -59,34 +39,66 @@ def _default_source_urdf(hand_model: str, hand_type: str) -> Path:
def _launch_stack(context):
hand_model = LaunchConfiguration("hand_model").perform(context).upper()
from g20_thumb_apriltag_calibration.product import (
get_product_calibration_contract,
)
model = LaunchConfiguration("model").perform(context).strip().upper()
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
if hand_type not in {"left", "right"}:
raise RuntimeError("hand_type must be left or right")
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
try:
profile = get_hand_calibration_profile(hand_type, hand_model)
contract = get_product_calibration_contract(
model, hand_type, tag_layout
)
except ValueError as error:
raise RuntimeError(str(error)) from error
model_key = hand_model.lower()
calibration_namespace = f"/{model_key}_calibration"
if hand_model == "O30":
command_topic = f"/cb_{hand_type}_hand_control_cmd"
state_topic = f"/cb_{hand_type}_hand_state"
hand_info_topic = f"/cb_{hand_type}_hand_info"
setting_topic = "/cb_hand_setting_cmd"
tag_config = LaunchConfiguration("o30_tag_config")
else:
command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
state_topic = f"/g20/cb_{hand_type}_hand_state"
hand_info_topic = f"/g20/cb_{hand_type}_hand_info"
setting_topic = "/g20/cb_hand_setting_cmd"
tag_config = LaunchConfiguration("tag_config")
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
)
tag_config = (
Path(requested_tag_config).expanduser().resolve()
if requested_tag_config
else package_share
/ "config"
/ (
"three_camera_tags_g20_right_19.yaml"
if tag_layout == "g20_right_19"
else "three_camera_tags_g20_right_15.yaml"
if tag_layout == "g20_right_15"
else "three_camera_tags.yaml"
)
)
if not tag_config.is_file():
raise RuntimeError(f"tag config does not exist: {tag_config}")
topic_prefix = f"/{model.lower()}"
command_topic = f"{topic_prefix}/cb_{hand_type}_hand_control_cmd"
state_topic = f"{topic_prefix}/cb_{hand_type}_hand_state"
info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info"
requested_source = LaunchConfiguration("source_urdf_path").perform(context)
source_urdf = (
Path(requested_source).expanduser().resolve()
if requested_source
else _default_source_urdf(hand_model, hand_type).resolve()
else _default_source_urdf(hand_type).resolve()
)
if not source_urdf.is_file():
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
expected_source_hash = LaunchConfiguration(
"source_urdf_expected_sha256"
).perform(context).strip().lower()
if contract.profile.supports("urdf_zero_publication"):
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
raise RuntimeError(
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
"by the CAD/hardware owner"
)
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
if actual_source_hash != expected_source_hash:
raise RuntimeError(
"source_urdf_expected_sha256 does not match source_urdf_path"
)
hand_serial = LaunchConfiguration("serial_number").perform(context)
if (
@@ -126,14 +138,14 @@ def _launch_stack(context):
info_topics = []
detection_topics = []
for view in VIEWS:
namespace = f"{calibration_namespace}/{view}/camera"
namespace = f"/g20_calibration/{view}/camera"
raw_topic = f"{namespace}/image_raw"
camera_info_topic = f"{namespace}/camera_info"
info_topic = f"{namespace}/camera_info"
rect_topic = f"{namespace}/image_rect"
detector_namespace = f"{calibration_namespace}/{view}/apriltag"
detector_namespace = f"/g20_calibration/{view}/apriltag"
detection_topic = f"{detector_namespace}/detections"
raw_topics.append(raw_topic)
info_topics.append(camera_info_topic)
info_topics.append(info_topic)
detection_topics.append(detection_topic)
cameras.append(
Node(
@@ -153,9 +165,7 @@ def _launch_stack(context):
"camera_name": LaunchConfiguration(
f"{view}_camera_name"
),
"frame_id": (
f"{model_key}_calibration_{view}_optical_frame"
),
"frame_id": f"g20_calibration_{view}_optical_frame",
"image_width": 1624,
"image_height": 1240,
"frame_rate": ParameterValue(
@@ -188,7 +198,7 @@ def _launch_stack(context):
namespace=namespace,
remappings=[
("image", raw_topic),
("camera_info", camera_info_topic),
("camera_info", info_topic),
("image_rect", rect_topic),
],
parameters=[{"queue_size": 1}],
@@ -200,7 +210,7 @@ def _launch_stack(context):
name="apriltag",
namespace=detector_namespace,
parameters=[
tag_config,
str(tag_config),
{
"detector.decimate": ParameterValue(
LaunchConfiguration("apriltag_decimate"),
@@ -210,7 +220,7 @@ def _launch_stack(context):
],
remappings=[
("image_rect", rect_topic),
("camera_info", camera_info_topic),
("camera_info", info_topic),
],
extra_arguments=[{"use_intra_process_comms": True}],
),
@@ -218,7 +228,7 @@ def _launch_stack(context):
)
vision = ComposableNodeContainer(
name=f"{model_key}_three_camera_vision",
name="g20_three_camera_vision",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
@@ -226,88 +236,74 @@ def _launch_stack(context):
output="screen",
emulate_tty=True,
)
if hand_model == "O30":
sdk = Node(
package="linker_hand_o30_ros2_sdk",
executable="linker_hand_o30_ros2_sdk",
name="linker_hand_o30_ros2_sdk",
output="screen",
condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[
{
"hand_type": hand_type,
"hand_joint": "O30",
"is_touch": False,
"canfd_device": ParameterValue(
LaunchConfiguration("canfd_device"), value_type=int
),
"comm_type": LaunchConfiguration("o30_comm_type"),
"channel": LaunchConfiguration("can_interface"),
"bitrate": ParameterValue(
LaunchConfiguration("o30_bitrate"), value_type=int
),
"dbitrate": ParameterValue(
LaunchConfiguration("o30_dbitrate"), value_type=int
),
"auto_setup": ParameterValue(
LaunchConfiguration("o30_auto_setup"), value_type=bool
),
}
],
)
else:
sdk = Node(
package="linker_hand_ros2_sdk",
executable="linker_hand_sdk",
name="linker_hand_sdk",
output="screen",
condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[
{
"hand_type": hand_type,
"hand_joint": "G20",
"can": LaunchConfiguration("can_interface"),
"modbus": "None",
"topic_prefix": "/g20",
"move_on_startup": False,
"startup_speed": ParameterValue(
LaunchConfiguration("calibration_speed"), value_type=int
),
"startup_torque": 80,
"state_poll_rate": 30.0,
"velocity_poll_rate": 1.0,
"defer_state_reads_while_commanding": False,
"repeat_position_commands": False,
"is_touch": False,
}
],
)
sdk = Node(
package="linker_hand_ros2_sdk",
executable="linker_hand_sdk",
name="linker_hand_sdk",
output="screen",
condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[
{
"hand_type": hand_type,
"hand_joint": model,
"can": LaunchConfiguration("can_interface"),
"modbus": "None",
"topic_prefix": topic_prefix,
"move_on_startup": False,
"startup_speed": ParameterValue(
LaunchConfiguration("calibration_speed"), value_type=int
),
"startup_torque": 80,
# Match 30 Hz cameras so state/image p95 skew stays below 50 ms.
"state_poll_rate": 30.0,
# Calibration does not consume measured joint velocity. A
# G20 velocity read sends another five synchronous CAN
# queries, so keep it off the trajectory-critical path.
"velocity_poll_rate": 1.0,
# Calibration sends one endpoint command per sweep. Keep
# polling the real motor state during the following motion;
# otherwise the SDK republishes a stale state for 0.2 s and
# creates 17-27 command-unit holes in the trajectory bins.
"defer_state_reads_while_commanding": False,
"repeat_position_commands": False,
"is_touch": False,
}
],
)
calibration = Node(
package="g20_thumb_apriltag_calibration",
executable="three_camera_calibration_node",
name=f"{model_key}_calibration",
name="g20_calibration",
output="screen",
emulate_tty=True,
parameters=[
LaunchConfiguration("calibration_config"),
{
"serial_number": hand_serial,
"hand_model": hand_model,
"model": model,
"hand_type": hand_type,
"tag_layout": tag_layout,
"session_dir": str(session_dir),
"resume_raw_samples_path": LaunchConfiguration(
"resume_raw_samples_path"
),
"recalibration_scope": LaunchConfiguration(
"recalibration_scope"
),
# The SDK performs roughly 25 synchronous CAN queries whenever
# cb_<side>_hand_info has a subscriber. Calibration only used
# that topic to display a speed diagnostic, while those reads
# created 17-33 command-unit holes in position trajectories.
"info_topic": f"{calibration_namespace}/disabled_hand_info",
"info_topic": "/g20_calibration/disabled_hand_info",
"command_topic": command_topic,
"state_topic": state_topic,
"setting_topic": setting_topic,
"baseline_command_u8": list(profile.baseline_command),
"camera_extrinsics_file": LaunchConfiguration(
"camera_extrinsics_file"
),
"source_urdf_path": str(source_urdf),
"source_urdf_expected_sha256": LaunchConfiguration(
"source_urdf_expected_sha256"
),
"corrected_urdf_output_dir": LaunchConfiguration(
"corrected_urdf_output_dir"
),
@@ -315,14 +311,6 @@ def _launch_stack(context):
f"{view}_camera_serial": camera_serials[view]
for view in VIEWS
},
**{
f"{view}_camera_info_topic": info_topics[index]
for index, view in enumerate(VIEWS)
},
**{
f"{view}_detections_topic": detection_topics[index]
for index, view in enumerate(VIEWS)
},
"commands_enabled": ParameterValue(
LaunchConfiguration("commands_enabled"), value_type=bool
),
@@ -337,12 +325,12 @@ def _launch_stack(context):
LaunchConfiguration("index_flex_calibration_speed"),
value_type=int,
),
"o30_internal_speed_u8": ParameterValue(
LaunchConfiguration("o30_internal_speed_u8"), value_type=int
"adaptive_formal_speed_enabled": ParameterValue(
LaunchConfiguration("adaptive_formal_speed_enabled"),
value_type=bool,
),
"o30_command_full_range_seconds": ParameterValue(
LaunchConfiguration("o30_command_full_range_seconds"),
value_type=float,
"cross_view_roll_diagnostic_finger": LaunchConfiguration(
"cross_view_roll_diagnostic_finger"
),
"validation_enabled": ParameterValue(
LaunchConfiguration("validation_enabled"), value_type=bool
@@ -369,15 +357,15 @@ def _launch_stack(context):
*detection_topics,
command_topic,
state_topic,
hand_info_topic,
f"{calibration_namespace}/status",
info_topic,
"/g20_calibration/status",
],
output="screen",
)
return [
LogInfo(
msg=(
f"{hand_model} {hand_type} three-camera session: {session_dir}; "
f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; "
f"source_urdf={source_urdf}"
)
),
@@ -418,8 +406,9 @@ def generate_launch_description() -> LaunchDescription:
name="FASTRTPS_DEFAULT_PROFILES_FILE",
value=str(package_share / "config" / "fastdds_large_images.xml"),
),
DeclareLaunchArgument("hand_model", default_value="G20"),
DeclareLaunchArgument("model", default_value="G20"),
DeclareLaunchArgument("hand_type", default_value="left"),
DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
DeclareLaunchArgument("serial_number", default_value="UNSET"),
DeclareLaunchArgument(
"front_camera_serial", default_value="DB2163742"
@@ -458,11 +447,6 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("auto_exposure", default_value="false"),
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
DeclareLaunchArgument("can_interface", default_value="can0"),
DeclareLaunchArgument("canfd_device", default_value="0"),
DeclareLaunchArgument("o30_comm_type", default_value="socketcan"),
DeclareLaunchArgument("o30_bitrate", default_value="1000000"),
DeclareLaunchArgument("o30_dbitrate", default_value="5000000"),
DeclareLaunchArgument("o30_auto_setup", default_value="true"),
DeclareLaunchArgument("calibration_speed", default_value="15"),
DeclareLaunchArgument(
"index_roll_calibration_speed", default_value="5"
@@ -470,9 +454,11 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"index_flex_calibration_speed", default_value="10"
),
DeclareLaunchArgument("o30_internal_speed_u8", default_value="0"),
DeclareLaunchArgument(
"o30_command_full_range_seconds", default_value="6.0"
"adaptive_formal_speed_enabled", default_value="true"
),
DeclareLaunchArgument(
"cross_view_roll_diagnostic_finger", default_value=""
),
DeclareLaunchArgument("validation_enabled", default_value="false"),
DeclareLaunchArgument(
@@ -484,6 +470,9 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"source_urdf_path", default_value=""
),
DeclareLaunchArgument(
"source_urdf_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"corrected_urdf_output_dir", default_value=""
),
@@ -496,6 +485,8 @@ def generate_launch_description() -> LaunchDescription:
default_value=str(Path.cwd() / "calibration_output"),
),
DeclareLaunchArgument("session_dir", default_value=""),
DeclareLaunchArgument("resume_raw_samples_path", default_value=""),
DeclareLaunchArgument("recalibration_scope", default_value="full"),
DeclareLaunchArgument(
"calibration_config",
default_value=str(
@@ -504,15 +495,7 @@ def generate_launch_description() -> LaunchDescription:
),
DeclareLaunchArgument(
"tag_config",
default_value=str(
package_share / "config" / "three_camera_tags.yaml"
),
),
DeclareLaunchArgument(
"o30_tag_config",
default_value=str(
package_share / "config" / "three_camera_tags_o30.yaml"
),
default_value="",
),
OpaqueFunction(function=_launch_stack),
]
src/g20_thumb_apriltag_calibration/package.xml
+1 -2
View File
@@ -3,7 +3,7 @@
<package format="3">
<name>g20_thumb_apriltag_calibration</name>
<version>0.1.0</version>
<description>Three-view Hikrobot AprilTag calibration for G20 hands and the O30 right hand.</description>
<description>One-command three-camera AprilTag calibration and zero-URDF correction for the G20 right hand.</description>
<maintainer email="support@linker-robotics.com">lxp</maintainer>
<license>MIT</license>
@@ -15,7 +15,6 @@
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>linker_hand_ros2_sdk</exec_depend>
<exec_depend>linker_hand_o30_ros2_sdk</exec_depend>
<exec_depend>rclcpp_components</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>rosbag2</exec_depend>
src/g20_thumb_apriltag_calibration/setup.py
+9 -1
View File
@@ -26,7 +26,7 @@ setup(
zip_safe=True,
maintainer="lxp",
maintainer_email="support@linker-robotics.com",
description="Three-view AprilTag calibration for G20 and O30 hands",
description="Profile-driven three-camera AprilTag hand calibration",
license="MIT",
entry_points={
"console_scripts": [
@@ -64,6 +64,14 @@ setup(
"g20_thumb_apriltag_calibration."
"calibrated_joint_state_bridge:main"
),
(
"calibrate_g20_right = "
"g20_thumb_apriltag_calibration.one_command:main"
),
(
"calibrate_hand = "
"g20_thumb_apriltag_calibration.one_command:main"
),
],
},
)
src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py
+76 -30
View File
@@ -5,15 +5,17 @@ import pytest
from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
G20_COMMAND_NAMES,
G20_URDF_JOINT_NAMES,
O30_COMMAND_NAMES,
O30_URDF_JOINT_NAMES,
CalibratedCommandMapper,
default_input_topic,
)
from g20_thumb_apriltag_calibration.full_hand import (
JointCurveFit,
build_compact_payload,
get_hand_calibration_profile,
)
from g20_thumb_apriltag_calibration.urdf_zero import (
get_zero_calibration_profile,
)
def _payload(side: str = "right") -> dict:
@@ -91,41 +93,85 @@ def test_mapper_rejects_incomplete_named_command() -> None:
mapper.map_positions([255.0], ["thumb_cmc_pitch"])
def test_o30_mapper_uses_sdk_names_and_twenty_active_urdf_joints() -> None:
profile = get_hand_calibration_profile("right", "O30")
def fit_for(name: str) -> JointCurveFit:
zero = profile.baseline_command[profile.joint_specs[name].motor_index]
curve = tuple((command - zero) * 0.001 for command in range(256))
return JointCurveFit(
angle_rad=curve,
decreasing_rad=curve,
increasing_rad=curve,
circle={},
maximum_monotonic_correction_rad=0.0,
maximum_hysteresis_rad=0.0,
quality={},
def test_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> None:
profile = get_hand_calibration_profile("right", "g20_right_15")
zero = get_zero_calibration_profile("right", "g20_right_15")
baseline = [255] * 20
baseline[6:10] = [127] * 4
fits = {}
diagnostics = {}
for name, spec in profile.joint_specs.items():
zero_command = baseline[spec.motor_index]
average = tuple(
0.001 * (zero_command - value) for value in range(256)
)
decreasing = tuple(1.1 * value for value in average)
increasing = tuple(0.9 * value for value in average)
fits[name] = JointCurveFit(
average,
decreasing,
increasing,
{},
0.0,
0.0,
{"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
)
diagnostics[name] = {
"cycle_travel_rad": [0.25] * 4,
"cycle_travel_range_rad": 0.0,
"baseline_hysteresis_by_cycle_rad": [0.0] * 4,
"holdout_cycle": 3,
"holdout_cycle_mae_rad": 0.0,
"holdout_cycle_max_rad": 0.0,
}
payload = build_compact_payload(
serial_number="O30_RIGHT_TEST",
measured_fits={name: fit_for(name) for name in profile.measured_joints},
serial_number="TEST_RIGHT_V5",
measured_fits=fits,
urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
validation_errors_rad=[0.0],
passed=True,
baseline=baseline,
side="right",
model="O30",
layout_id="g20_right_15",
zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
zero_cycle_offsets_rad={
name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
},
zero_observers=zero.offset_observer_joint,
artifact_hashes={
"source_urdf_sha256": "0" * 64,
"camera_extrinsics_sha256": "1" * 64,
"corrected_urdf_sha256": "2" * 64,
},
cross_view_roll_metrics={
f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
for finger in ("index", "middle", "ring", "pinky")
},
joint_dynamic_diagnostics=diagnostics,
zero_geometry_diagnostics={
"training_cycles": (0, 1, 2),
"validation_cycle": 3,
"axis_line_rms_m": 0.0,
"validation_line_error_by_joint_m": {},
},
)
mapper = CalibratedCommandMapper(payload, expected_side="right")
command = [255.0] * 20
command[0] = 100.0
mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command)))
assert mapper.model == "O30"
assert mapper.command_names == O30_COMMAND_NAMES
assert mapper.urdf_joint_names == O30_URDF_JOINT_NAMES
baseline = profile.baseline_command
assert mapper.map_positions(baseline, O30_COMMAND_NAMES) == pytest.approx(
[0.0] * 20
assert mapper.input_domain == "command_u8"
assert mapper.layout_id == "g20_right_19"
# midpoint(1.1, 0.9) is the original 0.001-rad/u8 curve.
assert mapped["thumb_cmc_pitch"] == pytest.approx(0.155)
assert payload["joints"]["pinky_dip"]["passive"] is True
assert "source_joint" not in payload["joints"]["pinky_dip"]
def test_default_topics_use_the_g20_sdk_namespace() -> None:
assert default_input_topic("right", "command_u8") == (
"/g20/cb_right_hand_control_cmd"
)
assert default_input_topic("right", "feedback_u8") == (
"/g20/cb_right_hand_state"
)
moved = list(baseline)
moved[19] = 100
result = dict(zip(O30_URDF_JOINT_NAMES, mapper.map_positions(moved)))
assert result["pinky_dip"] == pytest.approx(0.1)
src/g20_thumb_apriltag_calibration/test/test_config.py
+85 -46
View File
@@ -56,52 +56,71 @@ def test_front_tag_parameters_match_namespaced_detector() -> None:
def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
tags = yaml.safe_load(
(PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text()
)
expected = {
"front": [0, 1, 2, 3, 10],
"side": [4, 5, 6, 7],
"top": [8, 9],
}
for model in ("g20", "o30"):
suffix = "" if model == "g20" else "_o30"
tags = yaml.safe_load(
(
PACKAGE_ROOT
/ "config"
/ f"three_camera_tags{suffix}.yaml"
).read_text()
)
all_ids = set()
for view, ids in expected.items():
key = f"/{model}_calibration/{view}/apriltag/apriltag"
parameters = tags[key]["ros__parameters"]
assert parameters["tag"]["ids"] == ids
assert parameters["size"] == 0.016
assert parameters["tag"]["sizes"] == [0.016] * len(ids)
assert parameters["qos_profile"] == "sensor_data"
assert parameters["detector"]["decimate"] == 1.5
all_ids.update(ids)
assert all_ids == set(range(11))
assert tags[
f"/{model}_calibration/side/apriltag/apriltag"
]["ros__parameters"]["tag"]["frames"][0] == "side_base"
all_ids = set()
for view, ids in expected.items():
key = f"/g20_calibration/{view}/apriltag/apriltag"
parameters = tags[key]["ros__parameters"]
assert parameters["tag"]["ids"] == ids
assert parameters["size"] == 0.016
assert parameters["tag"]["sizes"] == [0.016] * len(ids)
assert parameters["qos_profile"] == "sensor_data"
assert parameters["detector"]["decimate"] == 1.5
all_ids.update(ids)
assert all_ids == set(range(11))
assert tags[
"/g20_calibration/side/apriltag/apriltag"
]["ros__parameters"]["tag"]["frames"][0] == "side_base"
def test_three_camera_launch_selects_o30_tag_parameters() -> None:
launch_text = (
PACKAGE_ROOT / "launch" / "three_camera_calibration.launch.py"
).read_text()
assert 'tag_config = LaunchConfiguration("o30_tag_config")' in launch_text
assert '"three_camera_tags_o30.yaml"' in launch_text
def test_right_19_tag_config_matches_the_physical_layout() -> None:
tags = yaml.safe_load(
(
PACKAGE_ROOT
/ "config"
/ "three_camera_tags_g20_right_19.yaml"
).read_text()
)
expected = {
"front": [0, 1, 2, 3, 10, 11, 12, 13],
"side": [4, 5, 6, 7, 14, 15, 16, 17, 18],
"top": [8, 9],
}
all_ids: set[int] = set()
for view, ids in expected.items():
parameters = tags[
f"/g20_calibration/{view}/apriltag/apriltag"
]["ros__parameters"]
assert parameters["tag"]["ids"] == ids
expected_sizes = [0.016 for _tag_id in ids]
assert parameters["tag"]["sizes"] == expected_sizes
all_ids.update(ids)
assert all_ids == set(range(19))
side_frames = tags[
"/g20_calibration/side/apriltag/apriltag"
]["ros__parameters"]["tag"]["frames"]
assert side_frames == [
"side_base", "ring_pip", "pinky_pip", "pinky_dip", "ring_dip",
"middle_pip", "middle_dip", "index_pip", "index_dip",
]
def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
config = yaml.safe_load(
(PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text()
)
parameters = config["/**"]["ros__parameters"]
parameters = config["g20_calibration"]["ros__parameters"]
assert parameters["tag_size_m"] == 0.016
assert parameters["tag_size_override_ids"] == [7, 14, 16, 18]
assert parameters["tag_size_overrides_m"] == [0.016] * 4
assert parameters["baseline_command_u8"] == [
255,
@@ -129,40 +148,52 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
assert parameters["normal_calibration_speed"] == 15
assert parameters["index_roll_calibration_speed"] == 5
assert parameters["index_flex_calibration_speed"] == 10
assert parameters["o30_internal_speed_u8"] == 0
assert parameters["o30_command_full_range_seconds"] == 6.0
assert parameters["adaptive_formal_speed_enabled"] is True
assert parameters["adaptive_formal_speed_max_scale"] == 1.5
assert parameters["adaptive_formal_speed_minimum_bins"] == 64
assert parameters["adaptive_formal_speed_maximum_bin_gap"] == 8
assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0
assert parameters["speed_setting_settle_seconds"] >= 0.2
assert parameters["top_pnp_invalid_reset_seconds"] == 1.0
assert parameters["pnp_group_initialization_frames"] == 8
assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0
assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0
assert parameters["thumb_ip_pnp_coupling_multiplier"] == 1.03
assert parameters["thumb_ip_pnp_coupling_scale_deg"] == 3.0
assert parameters["thumb_ip_pnp_maximum_coupling_residual_deg"] == 7.5
assert parameters["baseline_hold_seconds"] == 0.5
assert parameters["minimum_baseline_hold_frames"] == 10
assert parameters["directional_zero_maximum_branch_gap_deg"] == 2.0
assert parameters["directional_zero_maximum_branch_gap_range_deg"] == 0.3
assert (
parameters["cross_view_roll_maximum_branch_gap_difference_deg"]
== 0.3
)
assert parameters["cross_view_roll_diagnostic_finger"] == ""
assert parameters["repetitions"] == 3
assert parameters["g20_right_19_repetitions"] >= 4
assert parameters["validation_enabled"] is False
assert parameters["combination_validation_enabled"] is False
assert parameters["combination_validation_frames"] >= 10
assert parameters["combination_maximum_position_p95_m"] <= 0.003
assert parameters["combination_maximum_orientation_p95_deg"] <= 2.0
assert parameters["minimum_detection_rate"] == 0.95
assert parameters["minimum_detection_hz"] == 15.0
assert parameters["minimum_state_span_u8"] >= 240.0
assert parameters["endpoint_tolerance_u8"] == 2.0
assert parameters["o30_endpoint_tolerance_u8"] == 4.0
assert (
parameters["o30_thumb_cmc_roll_255_endpoint_tolerance_u8"] == 9.0
)
assert (
parameters["o30_index_mcp_roll_255_endpoint_tolerance_u8"] == 8.0
)
assert parameters["o30_thumb_mcp_zero_endpoint_tolerance_u8"] == 8.0
assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0
assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
assert parameters["minimum_sweep_bins"] >= 32
assert parameters["maximum_bin_gap"] <= 16
assert parameters["automatic_sweep_retry_limit"] == 3
assert parameters["automatic_sweep_retry_limit"] == 2
assert parameters["automatic_fit_retry_limit"] == 2
assert parameters["motor_stall_timeout_seconds"] >= 5.0
assert parameters["motor_stall_timeout_seconds"] == 2.0
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
assert parameters["motor_stall_minimum_progress_u8"] == 1.0
assert parameters["automatic_motion_retry_limit"] == 2
assert parameters["provisional_warning_ratio"] == 1.25
assert parameters["retry_speed_scales"] == [0.8, 0.6, 0.5]
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25]
assert parameters["retry_speed_scales"] == [0.8, 0.6]
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0]
assert parameters["position_timeout_seconds"] >= 20.0
assert parameters["maximum_state_image_skew_ms"] <= 50.0
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
@@ -172,15 +203,23 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5
assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75
assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0
assert parameters["zero_maximum_observability_condition_number"] >= 1.0
assert parameters["zero_maximum_offset_deg"] <= 20.0
assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
assert parameters["mechanical_endpoint_maximum_offset_deg"] == 5.0
assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
assert parameters["image_trajectory_minimum_radius_px"] >= 20.0
assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0
assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0
assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0
assert parameters["passive_maximum_hysteresis_deg"] <= 7.5
assert parameters["maximum_hysteresis_deg"] <= 2.0
assert parameters["passive_maximum_hysteresis_deg"] <= 2.0
assert parameters["command_maximum_direction_gap_deg"] <= 2.0
assert parameters["maximum_validation_mae_deg"] <= 1.0
assert parameters["maximum_validation_p95_deg"] <= 2.0
assert parameters["maximum_validation_error_deg"] <= 3.0
assert parameters["zero_maximum_confidence_half_width_deg"] <= 1.5
for view in ("front", "side", "top"):
assert parameters[f"{view}_camera_info_topic"].startswith(
f"/g20_calibration/{view}/"
src/g20_thumb_apriltag_calibration/test/test_full_hand.py
+594
View File
@@ -1,5 +1,6 @@
import math
from dataclasses import replace
from pathlib import Path
import numpy as np
import pytest
@@ -8,18 +9,27 @@ from g20_thumb_apriltag_calibration.full_hand import (
ACTIVE_JOINTS,
IMAGE_TRAJECTORY_JOINTS,
JOINT_SPECS,
JointCurveFit,
LEFT_HAND_PROFILE,
MEASURED_JOINTS,
PASSIVE_JOINTS,
RIGHT_HAND_PROFILE,
RIGHT_19_HAND_PROFILE,
SPLAY_JOINTS,
SWEEP_SPECS,
THREE_CAMERA_BASELINE_COMMAND,
VIEW_TAGS,
build_calibration_motion_command,
build_calibration_preparation_waypoints,
build_calibration_return_waypoints,
build_calibration_speed_profile,
build_compact_payload,
build_full_hand_command,
clamp_runtime_fits_to_urdf_limits,
center_splay_curve,
compare_cross_view_roll_curves,
cross_view_roll_diagnostic_metrics,
derive_mimic_passive_fits,
fit_joint_center_curve,
fit_joint_image_curve,
fit_measured_joint_curve,
@@ -30,6 +40,500 @@ from g20_thumb_apriltag_calibration.full_hand import (
)
def test_runtime_fit_saturates_at_source_urdf_limit() -> None:
values = tuple(np.linspace(0.0, 2.0, 256))
fit = JointCurveFit(
angle_rad=values,
decreasing_rad=values,
increasing_rad=values,
circle={},
maximum_monotonic_correction_rad=0.0,
maximum_hysteresis_rad=0.0,
quality={},
)
bounded = clamp_runtime_fits_to_urdf_limits(
Path(__file__).resolve().parents[3]
/ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/"
"hands/linker_hand/g20_right/linkerhand_g20_right.urdf",
{"thumb_cmc_yaw": fit},
)["thumb_cmc_yaw"]
assert max(bounded.angle_rad) == pytest.approx(1.57)
assert max(bounded.decreasing_rad) == pytest.approx(1.57)
assert bounded.circle["runtime_urdf_limit_clipped_bins"]["angle_rad"] > 0
def test_right_19_profile_has_exact_layout_tasks_and_measured_dips() -> None:
profile = get_hand_calibration_profile("right", "g20_right_19")
assert profile is RIGHT_19_HAND_PROFILE
assert len(profile.sweep_specs) == 16
assert len(profile.measured_joints) == 21
assert len(profile.record_joints) == 25
assert list(profile.view_tags["front"].values()) == [
0, 1, 2, 3, 10, 11, 12, 13
]
assert list(profile.view_tags["side"].values()) == [
4, 5, 6, 7, 14, 15, 16, 17, 18
]
assert list(profile.view_tags["top"].values()) == [8, 9]
assert profile.preflight_view_roles == {
"front": ("front_base",),
"side": ("side_base",),
"top": ("top_base",),
}
assert [spec.task_name for spec in profile.sweep_specs[:4]] == [
"thumb_cmc_pitch_front",
"thumb_cmc_roll_front",
"thumb_mcp_ip_front",
"thumb_cmc_yaw_top",
]
assert [
spec.task_name for spec in profile.sweep_specs[4:7]
] == [
"pinky_roll_multiview",
"pinky_pitch_side",
"pinky_pip_side",
]
assert profile.sweep_specs[4].joints == (
"pinky_mcp_roll",
"pinky_mcp_roll_side",
)
assert {
(
observer.source_name,
observer.task_name,
observer.view,
observer.model_joint,
observer.motor_index,
)
for observer in profile.palm_axis_observers
} == {
(
"thumb_cmc_pitch_top_axis",
"thumb_cmc_pitch_front",
"top",
"thumb_cmc_pitch",
0,
),
(
"thumb_cmc_roll_top_axis",
"thumb_cmc_roll_front",
"top",
"thumb_cmc_roll",
5,
),
}
assert profile.minimum_palm_orientation_sources == 2
assert profile.palm_orientation_minimum_arc_rad == pytest.approx(
math.radians(10.0)
)
assert profile.palm_orientation_maximum_command_distance_u8 == 64
assert profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip")
assert {
name: spec.source_joint
for name, spec in profile.joint_specs.items()
if spec.source_joint is not None
} == {}
def test_right_19_keeps_four_visual_dip_curves_independent() -> None:
profile = RIGHT_19_HAND_PROFILE
values = tuple(0.002 * (255 - command) for command in range(256))
fit = JointCurveFit(values, values, values, {}, 0.0, 0.01, {})
measured = {name: fit for name in profile.measured_joints}
dip_values = tuple(0.0013 * (255 - command) for command in range(256))
for finger in ("index", "middle", "ring", "pinky"):
measured[f"{finger}_dip"] = JointCurveFit(
dip_values,
dip_values,
dip_values,
{"visual_measurement": True},
0.0,
0.007,
{"visual_measurement": 1.0},
)
source_urdf = (
Path(__file__).resolve().parents[3]
/ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
"linker_hand/g20_right/linkerhand_g20_right.urdf"
)
runtime = derive_mimic_passive_fits(
source_urdf, measured, profile=profile
)
assert set(runtime) == set(profile.joint_specs)
for finger in ("index", "middle", "ring", "pinky"):
dip = runtime[f"{finger}_dip"]
assert dip.angle_rad == pytest.approx(dip_values)
assert dip.maximum_hysteresis_rad == pytest.approx(0.007)
assert dip.circle["visual_measurement"] is True
def _changed_motors(
previous: tuple[int, ...] | list[int], waypoint: tuple[int, ...]
) -> list[int]:
return [
index
for index, (left, right) in enumerate(zip(previous, waypoint))
if left != right
]
def _phase_changes(
previous: tuple[int, ...] | list[int],
waypoints: tuple[tuple[int, ...], ...],
) -> list[list[int]]:
phases: list[list[int]] = []
for waypoint in waypoints:
phases.append(_changed_motors(previous, waypoint))
previous = waypoint
return phases
def test_right_19_side_clearance_waypoints_group_joint_classes() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "index_roll_multiview"
)
baseline = [255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=baseline,
profile=profile,
)
# Side clearance keeps all neighbouring roll axes neutral, flexes the
# neighbour PIPs together, then their pitches, and moves the index target
# last through the cleared swept volume.
assert _phase_changes(baseline, waypoints) == [
[17, 18, 19], [2, 3, 4], [6],
]
assert waypoints[-1][1] == 255 and waypoints[-1][16] == 255
returns = build_calibration_return_waypoints(
baseline,
current_command=waypoints[-1],
profile=profile,
)
assert _phase_changes(waypoints[-1], returns) == [
[6], [2, 3, 4], [17, 18, 19],
]
assert list(returns[-1]) == baseline
def test_right_19_side_clearance_waypoints_move_one_motor_at_a_time() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "index_roll_multiview"
)
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
profile=profile,
parallel=False,
)
previous = tuple([255] * 6 + [127] * 4 + [255] * 10)
changed_motors = []
for waypoint in waypoints:
changed = _changed_motors(previous, waypoint)
assert len(changed) == 1
changed_motors.append(changed[0])
previous = waypoint
# Side clearance keeps all neighbouring roll axes neutral, flexes them out
# of the swept volume, and moves the index target last.
assert changed_motors[:7] == [19, 4, 18, 3, 17, 2, 6]
assert previous[1] == 255 and previous[16] == 255
returns = build_calibration_return_waypoints(
[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
current_command=previous,
profile=profile,
parallel=False,
)
for waypoint in returns:
changed = sum(left != right for left, right in zip(previous, waypoint))
assert changed == 1
previous = waypoint
def test_right_15_ring_multiview_keeps_pinky_roll_neutral_and_flexes_it() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "ring_roll_multiview"
)
current = tuple(THREE_CAMERA_BASELINE_COMMAND)
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=current,
profile=profile,
)
assert _phase_changes(current, waypoints) == [[6, 7], [19], [4], [8]]
assert all(waypoint[9] == 127 for waypoint in waypoints)
assert waypoints[-1][8] == 255
assert waypoints[-1][19] == 0
assert waypoints[-1][4] == 0
def test_middle_multiview_recovers_neighbour_from_observed_58_not_zero() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "middle_roll_multiview"
)
# Session 20260819_160238 stopped here: the legacy front-only avoidance
# asked neighbouring ring roll (motor 8) for 0 and it saturated at 58.
current = [254] * 20
current[6:10] = [254, 127, 58, 127]
current[10] = 252
current[11:15] = [0, 0, 0, 0]
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=current,
profile=profile,
)
# Both differing roll axes (parked motor 6 and the stuck motor 8) move in
# the single roll phase, still never commanded toward 0.
phases = _phase_changes(current, waypoints)
assert phases[0] == [6, 8]
assert waypoints[0][8] == 127
assert all(
waypoint[motor] != 0
for waypoint in waypoints
for motor in range(6, 10)
)
assert waypoints[-1][6:10] == (255, 255, 127, 127)
def test_right_19_ring_pitch_keeps_target_neutral_during_clearance() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = next(
item for item in profile.sweep_specs
if item.task_name == "ring_pitch_side"
)
current = tuple(THREE_CAMERA_BASELINE_COMMAND)
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=current,
profile=profile,
)
assert _phase_changes(current, waypoints) == [[6, 7], [19], [4]]
assert all(waypoint[8] == 127 for waypoint in waypoints)
assert all(waypoint[9] == 127 for waypoint in waypoints)
assert waypoints[-1][9] == 127
assert waypoints[-1][19] == 0
assert waypoints[-1][4] == 0
def test_right_19_waypoints_never_wait_for_reserved_feedback_channels() -> None:
profile = RIGHT_19_HAND_PROFILE
spec = profile.sweep_specs[0]
# This is the real G20 feedback shape: reserved channels 11..14 report zero
# while active endpoint channels commonly settle one or two counts low.
current = [
255, 254, 254, 254, 254, 254, 127, 127, 127, 127,
253, 0, 0, 0, 0, 255, 253, 253, 253, 253,
]
waypoints = build_calibration_preparation_waypoints(
spec,
255,
current_command=current,
profile=profile,
)
assert waypoints
assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in waypoints)
controlled = {joint.motor_index for joint in profile.joint_specs.values()}
for previous, waypoint in zip((tuple(current), *waypoints[:-1]), waypoints):
assert set(_changed_motors(previous, waypoint)) <= controlled
returns = build_calibration_return_waypoints(
list(THREE_CAMERA_BASELINE_COMMAND),
current_command=waypoints[-1],
profile=profile,
)
assert all(waypoint[11:15] == (0, 0, 0, 0) for waypoint in returns)
def test_right_19_parallel_return_moves_rolls_together_then_fingers() -> None:
# Full four-finger avoidance pose: rolls parked at 255 and every clearance
# finger flexed to 0.
current = list(THREE_CAMERA_BASELINE_COMMAND)
current[6:10] = [255, 255, 255, 255]
for motor in (1, 2, 3, 4, 16, 17, 18, 19):
current[motor] = 0
returns = build_calibration_return_waypoints(
list(THREE_CAMERA_BASELINE_COMMAND),
current_command=current,
profile=RIGHT_19_HAND_PROFILE,
anchor_roll_motor=9,
)
assert _phase_changes(current, returns) == [
[6, 7, 8, 9], [1, 2, 3, 4], [16, 17, 18, 19],
]
assert list(returns[-1]) == list(THREE_CAMERA_BASELINE_COMMAND)
def test_right_19_pinky_exit_returns_target_roll_before_other_fingers() -> None:
current = list(THREE_CAMERA_BASELINE_COMMAND)
current[6:10] = [255, 255, 255, 255]
returns = build_calibration_return_waypoints(
list(THREE_CAMERA_BASELINE_COMMAND),
current_command=current,
profile=RIGHT_19_HAND_PROFILE,
anchor_roll_motor=9,
parallel=False,
)
phases = _phase_changes(current, returns)
assert all(len(phase) == 1 for phase in phases)
assert [motor for phase in phases for motor in phase][:4] == [9, 8, 7, 6]
def test_right_19_ring_exit_returns_ring_before_neighbours() -> None:
current = list(THREE_CAMERA_BASELINE_COMMAND)
current[6:10] = [255, 255, 255, 0]
returns = build_calibration_return_waypoints(
list(THREE_CAMERA_BASELINE_COMMAND),
current_command=current,
profile=RIGHT_19_HAND_PROFILE,
anchor_roll_motor=8,
parallel=False,
)
first = returns[0]
assert first[8] == 127
assert first[6:8] == (255, 255)
assert first[9] == 0
def test_cross_view_roll_curve_must_agree_before_fusion() -> None:
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
fit = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
assert compare_cross_view_roll_curves(fit, fit)[
"angle_rad_rms_difference_rad"
] == 0.0
bad_values = tuple(value + 0.03 * (command != 127) for command, value in enumerate(values))
bad = replace(fit, angle_rad=bad_values, decreasing_rad=bad_values,
increasing_rad=bad_values)
with pytest.raises(ValueError, match="cross_view_roll_curve"):
compare_cross_view_roll_curves(fit, bad)
reverse = replace(
fit,
increasing_rad=tuple(value + math.radians(0.5) for value in values),
)
with pytest.raises(ValueError, match="branch_gap_difference"):
compare_cross_view_roll_curves(fit, reverse)
def test_cross_view_roll_curve_allows_bounded_projection_scale() -> None:
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
projected = tuple(0.8 * value for value in values)
secondary = replace(
primary,
angle_rad=projected,
decreasing_rad=projected,
increasing_rad=projected,
)
with pytest.raises(ValueError, match="cross_view_roll_curve"):
compare_cross_view_roll_curves(primary, secondary)
metrics = compare_cross_view_roll_curves(
primary,
secondary,
allow_projection_scale=True,
)
assert metrics["projection_scale"] == pytest.approx(1.25)
assert metrics["angle_rad_rms_difference_rad"] == pytest.approx(0.0)
assert metrics["raw_angle_rad_rms_difference_rad"] > 0.0
def test_projected_cross_view_keeps_branch_gap_as_diagnostic() -> None:
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
shifted_increasing = tuple(
value + math.radians(0.5) for value in values
)
secondary = replace(primary, increasing_rad=shifted_increasing)
metrics = compare_cross_view_roll_curves(
primary,
secondary,
allow_projection_scale=True,
)
assert metrics["baseline_branch_gap_difference_rad"] > math.radians(0.3)
assert metrics["baseline_branch_gap_gate_applied"] == 0.0
def test_cross_view_roll_curve_rejects_implausible_projection_scale() -> None:
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
collapsed = tuple(0.2 * value for value in values)
secondary = replace(
primary,
angle_rad=collapsed,
decreasing_rad=collapsed,
increasing_rad=collapsed,
)
with pytest.raises(ValueError, match="projection_scale_ratio"):
compare_cross_view_roll_curves(
primary,
secondary,
allow_projection_scale=True,
)
def test_validation_only_cross_view_metrics_never_create_a_retry_gate() -> None:
values = tuple(0.5 * (127 - command) / 127 for command in range(256))
primary = JointCurveFit(values, values, values, {}, 0.0, 0.0, {})
biased = tuple(
value
+ math.radians(2.5) * math.sin(math.pi * command / 255.0)
for command, value in enumerate(values)
)
secondary = replace(
primary,
angle_rad=biased,
decreasing_rad=biased,
increasing_rad=biased,
)
metrics = cross_view_roll_diagnostic_metrics(primary, secondary)
assert metrics["angle_rad_rms_difference_rad"] > math.radians(1.0)
reversed_secondary = replace(
secondary,
angle_rad=tuple(-value for value in secondary.angle_rad),
decreasing_rad=tuple(-value for value in secondary.decreasing_rad),
increasing_rad=tuple(-value for value in secondary.increasing_rad),
)
assert cross_view_roll_diagnostic_metrics(
primary, reversed_secondary
) == {"direction_disagrees": 1.0}
def _records() -> list[dict[str, object]]:
commands = list(range(0, 256, 16))
if commands[-1] != 255:
@@ -498,3 +1002,93 @@ def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
assert joint["angle_rad"] == source["angle_rad"]
assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0}
assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01}
def test_right_19_schema_v4_records_visual_dips_and_16_static_zeros() -> None:
from g20_thumb_apriltag_calibration.urdf_zero import (
get_zero_calibration_profile,
)
profile = RIGHT_19_HAND_PROFILE
zero = get_zero_calibration_profile("right", "g20_right_19")
baseline = [255] * 20
baseline[6:10] = [127] * 4
fits = {}
diagnostics = {}
for name, spec in profile.joint_specs.items():
zero_command = baseline[spec.motor_index]
values = tuple(
0.002 * (zero_command - command) for command in range(256)
)
fits[name] = JointCurveFit(
values,
values,
values,
{},
0.0,
0.0,
{"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
)
diagnostics[name] = {
"cycle_travel_rad": [0.5, 0.5, 0.5, 0.5],
"cycle_travel_range_rad": 0.0,
"baseline_hysteresis_by_cycle_rad": [0.0, 0.0, 0.0, 0.0],
"holdout_cycle": 3,
"holdout_cycle_mae_rad": 0.0,
"holdout_cycle_max_rad": 0.0,
}
offsets = {name: 0.0 for name in profile.active_joints}
payload = build_compact_payload(
serial_number="G20_RIGHT_019",
measured_fits=fits,
urdf_zero_offsets_rad=offsets,
validation_errors_rad=[0.0],
passed=True,
baseline=baseline,
side="right",
layout_id="g20_right_19",
zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
zero_cycle_offsets_rad={
name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
},
zero_observers=zero.offset_observer_joint,
artifact_hashes={
"source_urdf_sha256": "0" * 64,
"camera_extrinsics_sha256": "1" * 64,
"corrected_urdf_sha256": "2" * 64,
},
cross_view_roll_metrics={
f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
for finger in ("index", "middle", "ring", "pinky")
},
joint_dynamic_diagnostics=diagnostics,
zero_geometry_diagnostics={
"training_cycles": (0, 1, 2),
"validation_cycle": 3,
"axis_line_rms_m": 0.0,
"validation_line_error_by_joint_m": {},
},
)
validate_compact_payload(payload)
assert payload["schema_version"] == 4
assert set(payload) == {
"schema_version", "model", "side", "serial_number", "angle_unit",
"command_range", "baseline_command_u8", "joints", "quality",
}
assert len(zero.direct_zero_joints) == 16
assert all(
f"{finger}_pip" in zero.direct_zero_joints
and f"{finger}_dip" not in zero.direct_zero_joints
for finger in ("index", "middle", "ring", "pinky")
)
assert payload["joints"]["thumb_mcp"]["zero_angles"] == {
"urdf_zero_offset_rad": 0.0
}
assert payload["joints"]["thumb_ip"]["passive"] is True
assert set(payload["joints"]["thumb_ip"]) == {
"motor_index", "angle_rad", "passive"
}
assert all(
"source_joint" not in joint for joint in payload["joints"].values()
)
src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py
+2615
View File
File diff suppressed because it is too large Load Diff
src/g20_thumb_apriltag_calibration/test/test_o30_support.py
-263
View File
@@ -1,263 +0,0 @@
import math
from pathlib import Path
import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from g20_thumb_apriltag_calibration.full_hand import (
O30_COMMAND_NAMES,
O30_RIGHT_BASELINE_COMMAND,
O30_RIGHT_HAND_PROFILE,
JointCurveFit,
build_calibration_motion_command,
build_calibration_speed_profile,
build_compact_payload,
get_hand_calibration_profile,
validate_compact_payload,
)
from g20_thumb_apriltag_calibration.urdf_zero import (
JointAxisMeasurement,
UrdfKinematicModel,
_angles_from_state,
get_zero_calibration_profile,
solve_urdf_zero_offsets,
write_zero_corrected_urdf,
)
O30_SOURCE_URDF = Path(
"/home/lxp/projects/linkerhand-urdf/O30/urdf_0803-right/src/"
"linkerhand_O30i_right.urdf/linkerhand_O30i_right-0803.urdf"
)
def _curve(zero_command: int, travel_rad: float = 0.8) -> JointCurveFit:
values = np.asarray(
[travel_rad * (command - zero_command) / 255.0 for command in range(256)]
)
data = tuple(float(value) for value in values)
return JointCurveFit(
angle_rad=data,
decreasing_rad=data,
increasing_rad=data,
circle={},
maximum_monotonic_correction_rad=0.0,
maximum_hysteresis_rad=0.0,
quality={},
)
def test_o30_right_profile_matches_sdk_and_requested_baseline() -> None:
profile = get_hand_calibration_profile("right", "O30")
assert profile is O30_RIGHT_HAND_PROFILE
assert profile.command_names == O30_COMMAND_NAMES
assert profile.baseline_command == (
0, 0, 255, 205, 165, 20,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
)
assert O30_RIGHT_BASELINE_COMMAND == profile.baseline_command
assert len(profile.active_joints) == 20
assert profile.passive_joints == ()
assert set(profile.measured_joints) == {
"thumb_cmc_roll",
"thumb_cmc_yaw",
"thumb_mcp",
"thumb_ip",
"pinky_mcp_roll",
"pinky_mcp_pitch",
"pinky_pip",
"pinky_dip",
}
assert [spec.motor_index for spec in profile.sweep_specs] == [
0, 6, 15, 5, 10, 14, 19, 1,
]
assert profile.joint_specs["index_mcp_roll"].motor_index == 2
assert profile.joint_specs["pinky_mcp_roll"].motor_index == 5
assert profile.joint_specs["thumb_mcp"].motor_index == 6
assert profile.joint_specs["pinky_mcp_pitch"].motor_index == 10
assert profile.joint_specs["pinky_pip"].motor_index == 14
assert profile.joint_specs["thumb_ip"].motor_index == 15
assert profile.joint_specs["pinky_dip"].motor_index == 19
with pytest.raises(ValueError, match="only the right hand"):
get_hand_calibration_profile("left", "O30")
def test_o30_pinky_roll_clearance_and_sdk_speed_broadcast() -> None:
profile = O30_RIGHT_HAND_PROFILE
roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 5)
command = build_calibration_motion_command(roll, 100, profile=profile)
assert command[0:6] == [0, 0, 255, 255, 255, 100]
assert command[6:] == [0] * 14
assert build_calibration_speed_profile(
roll,
normal_speed=15,
index_roll_speed=5,
index_flex_speed=10,
profile=profile,
) == [5] * 5
dip = next(spec for spec in profile.sweep_specs if spec.motor_index == 19)
assert build_calibration_speed_profile(
dip,
normal_speed=15,
index_roll_speed=5,
index_flex_speed=10,
profile=profile,
) == [10] * 5
def test_o30_static_zero_policy_and_compact_payload() -> None:
profile = O30_RIGHT_HAND_PROFILE
zero = get_zero_calibration_profile("right", "O30")
assert set(zero.fixed_direct_zero_offsets_rad) == {
"thumb_ip",
"pinky_mcp_roll",
"pinky_dip",
}
assert "pinky_mcp_pitch" not in zero.fixed_direct_zero_offsets_rad
assert "pinky_pip" not in zero.fixed_direct_zero_offsets_rad
assert zero.inherited_static_zero_joints == {}
assert zero.inherited_zero_joints["index_dip"] == "pinky_dip"
measured = {
name: _curve(
profile.baseline_command[profile.joint_specs[name].motor_index]
)
for name in profile.measured_joints
}
offsets = {name: 0.0 for name in profile.active_joints}
offsets["pinky_mcp_pitch"] = 0.01
offsets["pinky_pip"] = -0.02
payload = build_compact_payload(
serial_number="O30_RIGHT_TEST",
measured_fits=measured,
urdf_zero_offsets_rad=offsets,
validation_errors_rad=[0.001],
passed=True,
side="right",
model="O30",
)
validate_compact_payload(payload)
assert payload["model"] == "O30"
assert len(payload["joints"]) == 20
for name in (
"index_mcp_roll",
"middle_mcp_roll",
"ring_mcp_roll",
"pinky_mcp_roll",
):
joint = payload["joints"][name]
assert joint["angle_rad"][joint["zero_command_u8"]] == pytest.approx(0.0)
assert joint["zero_angles"]["urdf_zero_offset_rad"] == 0.0
assert payload["joints"]["pinky_mcp_pitch"]["zero_angles"] == {
"urdf_zero_offset_rad": 0.01
}
assert payload["joints"]["index_mcp_pitch"]["zero_angles"] == {
"urdf_zero_offset_rad": 0.0
}
@pytest.mark.skipif(not O30_SOURCE_URDF.is_file(), reason="O30 source URDF absent")
def test_o30_source_urdf_has_exact_active_joint_set_and_writer(tmp_path) -> None:
profile = O30_RIGHT_HAND_PROFILE
source_bytes = O30_SOURCE_URDF.read_bytes()
model = UrdfKinematicModel(O30_SOURCE_URDF)
assert set(model.joints) == set(profile.active_joints)
assert "thumb_cmc_pitch" not in model.joints
destination = write_zero_corrected_urdf(
source_urdf=O30_SOURCE_URDF,
output_directory=tmp_path,
serial_number="O30_RIGHT_TEST",
offsets_rad={name: 0.0 for name in profile.active_joints},
timestamp="20260814_120000",
)
assert destination.is_file()
assert O30_SOURCE_URDF.read_bytes() == source_bytes
assert set(UrdfKinematicModel(destination).joints) == set(profile.active_joints)
@pytest.mark.skipif(not O30_SOURCE_URDF.is_file(), reason="O30 source URDF absent")
def test_o30_zero_solver_recovers_only_observable_offsets() -> None:
profile = O30_RIGHT_HAND_PROFILE
zero = get_zero_calibration_profile("right", "O30")
curves = {
name: _curve(
profile.baseline_command[profile.joint_specs[name].motor_index],
math.radians(55.0),
)
for name in profile.measured_joints
}
motor_by_joint = {
name: spec.motor_index for name, spec in profile.joint_specs.items()
}
expected_degrees = {
"thumb_cmc_roll": 2.0,
"thumb_cmc_yaw": -3.0,
"thumb_mcp": 4.0,
"thumb_ip": 0.0,
"pinky_mcp_roll": 0.0,
"pinky_mcp_pitch": 1.2,
"pinky_pip": -1.0,
"pinky_dip": 0.0,
}
offsets = {
name: math.radians(expected_degrees[name])
for name in zero.direct_zero_joints
}
model = UrdfKinematicModel(O30_SOURCE_URDF)
base_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6])
base_translation = np.asarray([0.25, -0.12, 0.68])
measurements = []
for cycle in range(3):
for joint in zero.axis_joints:
state = tuple(float(value) for value in profile.baseline_command)
angles = _angles_from_state(
state,
curves=curves,
motor_by_joint=motor_by_joint,
inherited_zero_joints=zero.inherited_zero_joints,
)
axis, point = model.axis_line(
joint,
zero_offsets=offsets,
joint_angles=angles,
)
measurements.append(
JointAxisMeasurement(
joint=joint,
cycle=cycle,
axis_common_xyz=tuple(base_rotation.apply(axis)),
point_common_xyz_m=tuple(
base_rotation.apply(point) + base_translation
),
condition_state_u8=state,
plane_rms_m=0.0002,
radial_rms_m=0.0002,
rotation_circle_axis_difference_rad=math.radians(0.1),
pose_axis_line_rms_m=0.0001,
)
)
result = solve_urdf_zero_offsets(
source_urdf=O30_SOURCE_URDF,
measurements=measurements,
curves=curves,
motor_by_joint=motor_by_joint,
hand_type="right",
hand_model="O30",
)
assert result.passed is True
for name, expected in expected_degrees.items():
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
expected, abs=0.05
)
assert result.all_active_offsets_rad["index_mcp_roll"] == 0.0
assert result.all_active_offsets_rad["index_mcp_pitch"] == 0.0
src/g20_thumb_apriltag_calibration/test/test_offline_replay.py
+25
View File
@@ -2,6 +2,7 @@ import pytest
from g20_thumb_apriltag_calibration.offline_replay import (
_latest_attempt_records,
_latest_palm_axis_records,
_output_suffix,
)
@@ -36,6 +37,30 @@ def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
] == [1]
def test_latest_palm_axis_attempt_is_selected_per_cycle_and_direction() -> None:
def sample(cycle: int, direction: str, attempt: int) -> dict:
return {
"kind": "palm_axis_sample",
"source_joint": "pinky_mcp_pitch_front_axis",
"cycle": cycle,
"direction": direction,
"attempt": attempt,
}
selected = _latest_palm_axis_records(
[
sample(0, "decreasing", 1),
sample(0, "decreasing", 2),
sample(0, "increasing", 1),
]
)
assert [
record["attempt"]
for record in selected["pinky_mcp_pitch_front_axis"]
] == [2, 1]
def test_output_suffix_is_safe_and_explicit() -> None:
assert _output_suffix(None) == ""
assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
src/g20_thumb_apriltag_calibration/test/test_pnp.py
+250
View File
@@ -234,6 +234,147 @@ def _pose(
)
def test_group_tracker_receives_oblique_reprojection_valid_candidates() -> None:
per_tag = SquareTagPoseTracker(
maximum_reprojection_error_px=1.5,
reprojection_tie_px=1.5,
maximum_pose_jump_rad=np.deg2rad(35.0),
maximum_translation_jump_m=0.04,
maximum_tag_tilt_rad=np.deg2rad(75.0),
reset_after_seconds=5.0,
)
oblique_rotation = Rotation.from_euler("y", 89.0, degrees=True)
independent, reason = per_tag.estimate(
"child",
_project(oblique_rotation, np.asarray([0.03, 0.0, 0.25])),
tag_size_m=0.01,
camera_matrix=_camera_matrix(),
stamp_ns=1_000_000_000,
)
assert independent is None
assert reason == "no_pose_within_reprojection_or_tilt_limit"
assert per_tag.last_candidates_by_role["child"]
diagnostics = per_tag.last_candidate_diagnostics_by_role["child"]
assert diagnostics["reprojection_candidate_count"] == 2
assert diagnostics["independent_tilt_candidate_count"] == 0
assert diagnostics["minimum_candidate_tilt_deg"] > 75.0
group = SquareTagGroupPoseTracker(
roles=("base", "child"),
adjacent_pairs=(("base", "child"),),
maximum_pose_jump_rad=np.deg2rad(35.0),
maximum_translation_jump_m=0.04,
relative_rotation_scale_rad=np.deg2rad(5.0),
relative_translation_scale_m=0.01,
reprojection_scale_px=0.1,
reprojection_weight=0.05,
reset_after_seconds=5.0,
)
selected, group_reason = group.select(
{
"base": (_pose(0.0, 0.0, 0.05),),
"child": per_tag.last_candidates_by_role["child"],
},
stamp_ns=1_000_000_000,
)
assert group_reason == ""
assert selected is not None
assert "child" in selected
def test_group_tracker_exposes_roles_without_candidates() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("base", "parent", "child"),
adjacent_pairs=(("base", "parent"), ("parent", "child")),
maximum_pose_jump_rad=np.deg2rad(35.0),
maximum_translation_jump_m=0.04,
relative_rotation_scale_rad=np.deg2rad(5.0),
relative_translation_scale_m=0.01,
reprojection_scale_px=0.1,
reprojection_weight=0.05,
reset_after_seconds=5.0,
)
selected, reason = tracker.select(
{
"base": (_pose(0.0, 0.0, 0.05),),
"parent": (),
},
stamp_ns=1_000_000_000,
)
assert selected is None
assert reason == "group_missing_pose_candidates"
assert tracker.last_missing_roles == ("parent", "child")
def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("base", "mcp", "ip"),
adjacent_pairs=(("base", "mcp"), ("mcp", "ip")),
maximum_pose_jump_rad=np.deg2rad(360.0),
maximum_translation_jump_m=0.04,
relative_rotation_scale_rad=np.deg2rad(1000.0),
relative_translation_scale_m=1.0,
reprojection_scale_px=0.1,
reprojection_weight=1.0,
reset_after_seconds=5.0,
coupled_rotation_pairs=(
("base", "mcp", "mcp", "ip", 1.03),
),
coupled_rotation_scale_rad=np.deg2rad(3.0),
maximum_coupled_rotation_residual_rad=np.deg2rad(7.5),
)
baseline = {
"base": (_pose(0.0, 0.00, 0.05),),
"mcp": (_pose(0.0, 0.03, 0.05),),
"ip": (_pose(0.0, 0.06, 0.05),),
}
selected, reason = tracker.select(
baseline,
stamp_ns=1_000_000_000,
trajectory_command_u8=255,
trajectory_direction="decreasing",
)
assert selected is not None
assert reason == ""
driver = _pose(10.0, 0.03, 0.05)
measured_ip = _pose(20.3, 0.06, 0.20)
lower_reprojection_mirror = _pose(30.0, 0.06, 0.05)
selected, reason = tracker.select(
{
"base": (_pose(0.0, 0.00, 0.05),),
"mcp": (driver,),
"ip": (lower_reprojection_mirror, measured_ip),
},
stamp_ns=1_033_000_000,
trajectory_command_u8=128,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected is not None
assert selected["ip"] == measured_ip
fallback, fallback_reason = tracker.select(
{
"base": (_pose(0.0, 0.00, 0.05),),
"mcp": (driver,),
"ip": (lower_reprojection_mirror,),
},
stamp_ns=1_066_000_000,
trajectory_command_u8=128,
trajectory_direction="decreasing",
)
assert fallback is not None
assert fallback_reason == ""
assert fallback["ip"] == lower_reprojection_mirror
def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("t0", "t3", "t4", "t5"),
@@ -324,6 +465,58 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
assert selected == {"t4": return_t4, "t5": return_t5}
def test_group_tracker_uses_outbound_pose_at_same_command_on_return() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("parent", "child"),
adjacent_pairs=(("parent", "child"),),
maximum_pose_jump_rad=np.deg2rad(35.0),
maximum_translation_jump_m=0.04,
relative_rotation_scale_rad=np.deg2rad(5.0),
relative_translation_scale_m=0.01,
reprojection_scale_px=0.1,
reprojection_weight=0.05,
reset_after_seconds=5.0,
)
parent = _pose(0.0, 0.00, 0.05)
for stamp, command, angle in (
(1_000_000_000, 255, 20.0),
(1_033_000_000, 64, 45.0),
(1_066_000_000, 0, 50.0),
):
selected, reason = tracker.select(
{"parent": (parent,), "child": (_pose(angle, 0.03, 0.05),)},
stamp_ns=stamp,
trajectory_command_u8=command,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected is not None
# The true return contains 3 deg of real hysteresis along the learned
# y-axis. The lower-error mirror candidate is temporally smoother but
# adds a 2 deg tilt outside that physical motion axis.
true_return = _pose(42.0, 0.03, 0.20)
smoother_mirror = SquareTagPose(
quaternion_xyzw=tuple(
Rotation.from_euler("xy", [2.0, 49.0], degrees=True).as_quat()
),
translation_xyz_m=(0.03, 0.0, 0.25),
reprojection_error_px=0.01,
)
selected, reason = tracker.select(
{
"parent": (parent,),
"child": (smoother_mirror, true_return),
},
stamp_ns=1_099_000_000,
trajectory_command_u8=64,
trajectory_direction="increasing",
)
assert reason == ""
assert selected == {"parent": parent, "child": true_return}
def test_group_tracker_initializes_from_multiple_static_frames() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("parent", "child"),
@@ -372,6 +565,63 @@ def test_group_tracker_initializes_from_multiple_static_frames() -> None:
] < np.deg2rad(1.0)
def test_group_tracker_preserves_task_branch_anchor_across_cycles() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("parent", "child"),
adjacent_pairs=(("parent", "child"),),
maximum_pose_jump_rad=np.deg2rad(35.0),
maximum_translation_jump_m=0.04,
relative_rotation_scale_rad=np.deg2rad(5.0),
relative_translation_scale_m=0.01,
reprojection_scale_px=0.1,
reprojection_weight=0.05,
reset_after_seconds=5.0,
initialization_frames=8,
)
parent = _pose(0.0, 0.00, 0.05)
anchored_child = _pose(20.0, 0.03, 0.20)
for index in range(8):
selected, reason = tracker.select(
{"parent": (parent,), "child": (anchored_child,)},
stamp_ns=1_000_000_000 + index * 33_000_000,
trajectory_command_u8=255,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected == {"parent": parent, "child": anchored_child}
tracker.reset(preserve_task_reference=True)
lower_error_mirror = _pose(5.0, 0.03, 0.01)
for index in range(8):
selected, reason = tracker.select(
{
"parent": (parent,),
"child": (lower_error_mirror, anchored_child),
},
stamp_ns=2_000_000_000 + index * 33_000_000,
trajectory_command_u8=255,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected == {"parent": parent, "child": anchored_child}
assert tracker.last_initialization_quality["task_reference_used"] == "true"
tracker.reset()
for index in range(8):
selected, reason = tracker.select(
{
"parent": (parent,),
"child": (lower_error_mirror, anchored_child),
},
stamp_ns=3_000_000_000 + index * 33_000_000,
trajectory_command_u8=255,
trajectory_direction="decreasing",
)
assert reason == ""
assert selected == {"parent": parent, "child": lower_error_mirror}
def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None:
tracker = SquareTagGroupPoseTracker(
roles=("mcp", "pip", "dip"),
src/g20_thumb_apriltag_calibration/test/test_sample_schema.py
+69
View File
@@ -0,0 +1,69 @@
"""Tests for the explicit calibration sample command domains."""
from g20_thumb_apriltag_calibration.sample_schema import (
SampleDataContractError,
canonical_sample_record,
fitting_sample_record,
fitting_sample_records,
)
import pytest
def _sample(kind: str = 'sample') -> dict[str, object]:
return {
'kind': kind,
'joint': 'thumb_cmc_roll',
'cycle': 0,
'direction': 'decreasing',
'requested_command_u8': 0,
'feedback_u8': 17.6,
}
def test_online_and_serialized_replay_use_identical_fit_projection() -> None:
"""Serialization must not change the curve fitter's input record."""
durable = canonical_sample_record(_sample())
online = fitting_sample_record(durable)
serialized = canonical_sample_record(online)
replay = fitting_sample_record(serialized)
assert 'command_u8' not in durable
assert serialized == durable
assert replay == online
assert online['command_u8'] == 18
def test_default_domain_is_requested_only_for_steady_checkpoints() -> None:
"""Settled checkpoints and dense trajectories use declared domains."""
steady = _sample('steady_command_sample')
assert fitting_sample_record(steady)['command_u8'] == 0
assert fitting_sample_record(steady, domain='feedback')['command_u8'] == 18
def test_feedback_projection_can_snap_proven_requested_endpoints() -> None:
"""A settled endpoint remains exact after feedback-domain projection."""
records = fitting_sample_records(
[_sample()],
domain='feedback',
snap_requested_endpoints=True,
)
assert records[0]['command_u8'] == 0
assert records[0]['feedback_u8'] == pytest.approx(17.6)
def test_new_sample_rejects_missing_explicit_feedback_domain() -> None:
"""A new observation cannot rely on the compatibility command key."""
ambiguous = {
'kind': 'sample',
'requested_command_u8': 0,
'command_u8': 0,
}
with pytest.raises(
SampleDataContractError,
match='DATA-CONTRACT-701.*feedback_u8',
):
canonical_sample_record(ambiguous)
src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py
+327 -36
View File
@@ -74,6 +74,137 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
assert "/g20_calibration/resume" in text
def test_endpoint_zero_lifecycle_failure_is_not_reported_as_quality() -> None:
payload = {
"state": "PAUSED",
"reason": (
"validated_endpoint_zero_state_incomplete:"
"missing=index_pip;extra=-"
),
"active": {},
"progress": 1.0,
"completed_sweeps": 16,
"total_sweeps": 16,
"views": {},
}
text = render_three_camera_status_text_zh(payload)
assert "程序内部状态生命周期错误" in text
assert "不要移动相机、Tag或机械手底座" in text
def test_artifact_failure_explains_that_collection_need_not_restart() -> None:
payload = {
"state": "PAUSED",
"reason": (
"PUB-ARTIFACT-601:runtime curve exceeds runtime URDF limit "
"for index_mcp_pitch"
),
"active": {},
"progress": 1.0,
"completed_sweeps": 16,
"total_sweeps": 16,
"views": {},
}
text = render_three_camera_status_text_zh(payload)
assert "正式发布前" in text
assert "原始URDF未被覆盖" in text
assert "不要重新标定相机或调整Tag" in text
def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED", "combination_pose_prediction_failed", {}
)
assert "多关节组合姿态" in explanation
assert "未分类原因码" not in explanation
assert "combination_validation_failure" in action
assert "不要重新采集16个单关节任务" in action
def test_sweep_start_visible_tags_reports_group_pnp_rejection() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED",
"sweep_start_tag_timeout",
{
"group_pnp_reasons": {
"side": "group_initializing:5/8",
},
"pnp_initialization_progress": {
"side": {"accepted": 5, "required": 8},
},
"pnp_rejection_counts": {
"side": {
"ring_pip:no_pose_within_reprojection_or_tilt_limit": 12,
},
},
},
)
assert "所需Tag也可见" in explanation
assert "初始化5/8" in explanation
assert "no_pose_within_reprojection_or_tilt_limit×12" in explanation
assert "不要根据可见性重复粘贴" in action
def test_mid_sweep_pnp_failure_names_missing_candidate_role() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED",
"synchronised_tag_state_timeout:side",
{
"valid_frames": 150,
"group_pnp_reasons": {
"side": "group_missing_pose_candidates",
},
"group_missing_candidate_roles": {
"side": ["index_dip"],
},
"pnp_rejection_counts": {
"side": {
"index_dip:no_pose_within_reprojection_or_tilt_limit": 90,
},
},
"pnp_candidate_diagnostics": {
"side": {
"index_dip": {
"solved_candidate_count": 2,
"reprojection_candidate_count": 0,
"independent_tilt_candidate_count": 0,
},
},
},
},
)
assert "已经取得部分有效轨迹" in explanation
assert "缺候选=index_dip" in explanation
assert "solve=2,reproj=0,tilt=0" in explanation
assert "group_pnp_candidate_event" in action
def test_multiview_failure_names_the_camera_specific_joint() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED",
"sweep_bin_gap_too_large:pinky_mcp_roll_side",
{
"sample": {
"maximum_bin_gap": 20,
"maximum_bin_gap_start_u8": 100,
"maximum_bin_gap_end_u8": 120,
"allowed_maximum_bin_gap": 16,
}
},
)
assert "小指MCP侧摆（侧面校验）" in explanation
assert "100→120" in explanation
assert "resume" in action
def test_preflight_lists_missing_tags_in_chinese() -> None:
payload = {
"state": "PREFLIGHT",
@@ -137,6 +268,171 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
assert "运动采样：" not in text
def test_palm_orientation_failure_explains_thumb_top_coverage() -> None:
explanation, suggestion = three_camera_reason_zh(
"PAUSED",
"palm_orientation_quality_failed",
{
"failures": [
{
"reason": (
"palm orientation cycle 2 has 1/2 usable sources: "
"thumb_cmc_roll_top_axis:cycle2=too few samples"
)
}
]
},
)
assert "顶部Tag 8/9" in explanation
assert "1/2 usable sources" in explanation
assert "CMC pitch和roll" in suggestion
def test_cross_view_side_line_rms_names_side_source() -> None:
payload = {
"state": "PAUSED",
"reason": "joint_fit_check_failed",
"progress": 0.1,
"completed_sweeps": 8,
"total_sweeps": 80,
"active": {
"kind": "fit_failure",
"view": "front",
"motor_index": 9,
"joints": ["pinky_mcp_roll", "pinky_mcp_roll_side"],
"attempt": 1,
"directions_to_rescan": 8,
"failures": [
{
"joint": "pinky_mcp_roll_side",
"model_joint": "pinky_mcp_roll",
"quality_source_joints": ["pinky_mcp_roll_side"],
"metric": "axis_line_cycle_rms_mm",
"actual": 1.2,
"limit": 1.0,
"comparison": "maximum",
}
],
},
"views": {},
"result_path": "",
}
text = render_three_camera_status_text_zh(payload)
assert "小指MCP侧摆（侧面校验）的四轮轴线位置RMS为1.20mm" in text
assert "要求不超过1.00mm" in text
def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None:
payload = {
"state": "PAUSED",
"reason": "joint_fit_check_failed",
"progress": 0.15,
"completed_sweeps": 24,
"total_sweeps": 160,
"active": {
"kind": "fit_failure",
"view": "front",
"motor_index": 15,
"joints": ["thumb_ip"],
"attempt": 3,
"directions_to_rescan": 6,
"failures": [
{
"joint": "thumb_ip",
"metric": "baseline_hysteresis_deg",
"actual": 1.34,
"limit": 0.5,
"comparison": "maximum",
"cycle_values_deg": [1.34, 0.04, 0.14],
}
],
},
"views": {},
"result_path": "",
}
text = render_three_camera_status_text_zh(payload)
assert "baseline正反程关节角差为1.34°" in text
assert "要求不超过0.50°" in text
assert "各轮=1.34°/0.04°/0.14°" in text
assert "未知原因" not in text
def test_thumb_yaw_repeatability_failure_names_real_source_retry() -> None:
explanation, suggestion = three_camera_reason_zh(
"PAUSED",
"joint_fit_check_failed",
{
"source_task_names": [
"thumb_cmc_pitch_front",
"thumb_cmc_roll_front",
],
"directions_to_rescan": 16,
"failures": [
{
"joint": "thumb_cmc_yaw",
"metric": "zero_cycle_offset_range_deg",
"actual": 0.555836,
"limit": 0.5,
"comparison": "maximum",
"cycle": 3,
"cycle_offset_deg": [-2.650924, -2.615277, -2.095088],
}
],
},
)
assert "训练轮零位极差为0.56°" in explanation
assert "各轮=-2.65°/-2.62°/-2.10°" in explanation
assert "未知原因" not in explanation
assert "CMC pitch/roll顶部轴观测" in suggestion
assert "重扫16个方向" in suggestion
assert "不会无效重扫yaw侧摆" in suggestion
def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None:
payload = {
"state": "PAUSED",
"reason": "sweep_bin_gap_too_large",
"progress": 0.1,
"completed_sweeps": 1,
"total_sweeps": 10,
"active": {
"kind": "sweep",
"view": "top",
"motor_index": 10,
"joints": ["thumb_cmc_yaw"],
"cycle": 1,
"repetitions": 3,
"start_u8": 255,
"target_u8": 0,
"actual_u8": 3.0,
"valid_frames": 610,
"detection_frames": 644,
"detection_valid_frames": 610,
"detection_rate": 610 / 644,
"sample": {
"minimum_u8": 3.0,
"maximum_u8": 252.0,
"maximum_bin_gap": 17,
"maximum_bin_gap_start_u8": 99,
"maximum_bin_gap_end_u8": 116,
"allowed_maximum_bin_gap": 16,
},
},
"views": {},
}
text = render_three_camera_status_text_zh(payload)
assert "最大空缺为17，（99→116）" in text
assert "本方向Tag检出：94.7%（610/644帧）" in text
def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
payload = {
"state": "PAUSED",
@@ -175,7 +471,7 @@ def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
assert "运动采样：" not in text
def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
def test_sweep_status_shows_localized_fit_retry_cycle() -> None:
active = {
"kind": "sweep",
"view": "front",
@@ -187,42 +483,10 @@ def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
"target_u8": 0,
"fit_attempt": 2,
"fit_attempt_limit": 3,
"fit_retry_cycles": [2],
}
assert "整关节自动重采第2/3次" in _task_text(active)
def test_o30_status_shows_cycle_order_retry_execution_and_namespace() -> None:
payload = {
"state": "PAUSED",
"reason": "joint_fit_check_failed",
"progress": 0.112,
"scan_progress": 0.125,
"completed_sweeps": 6,
"total_sweeps": 48,
"executed_sweep_directions": 20,
"service_prefix": "/o30_calibration",
"active": {
"kind": "fit_failure",
"view": "front",
"motor_index": 0,
"joints": ["thumb_cmc_roll"],
"attempt": 3,
"fit_attempt_limit": 3,
"directions_to_rescan": 6,
"failures": [],
},
"views": {},
"result_path": "",
}
text = render_three_camera_status_text_zh(payload)
assert "已启动20个方向（含自动重扫）" in text
assert "重扫不会重复增加计划进度" in text
assert "当前为第3/3次采集结果" in text
assert "/o30_calibration/resume" in text
assert "/g20_calibration/resume" not in text
assert "补采异常轮2" in _task_text(active)
def test_motor_stall_reason_is_explained_in_chinese() -> None:
@@ -243,6 +507,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
"reason": (
"motor_state_stalled:return_baseline:motor_index=10:"
"target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:"
"timeout_seconds=2.000:"
"error_u8=5.000"
),
"progress": 0.0,
@@ -257,6 +522,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
"actual_u8": 250.0,
"error_u8": 5.0,
"tolerance_u8": 4.0,
"timeout_seconds": 2.0,
},
"views": {},
"result_path": "",
@@ -264,7 +530,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
text = render_three_camera_status_text_zh(payload)
assert "电机10反馈连续8秒" in text
assert "电机10反馈连续2秒" in text
assert "目标255.0、实际250.0、误差5.000 u8" in text
assert "允许容差±4.0 u8" in text
assert "当前任务：电机10运动停滞，目标255.0、实际250.0" in text
@@ -404,3 +670,28 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None:
assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text
assert "SDK报告[15, 5, 15, 15, 15]" in text
assert "自动重试：当前方向已自动重扫1/2次" in text
def test_same_finger_transition_explains_that_clearance_stays_parked() -> None:
explanation, action = three_camera_reason_zh(
"RETURN_BASELINE",
"holding_same_finger_clearance_before_next_task",
{},
)
assert "继续保持当前避让姿态" in explanation
assert "只调整被测关节" in explanation
assert "不要手动展开" in action
def test_fixed_base_reference_movement_requires_a_new_session() -> None:
explanation, action = three_camera_reason_zh(
"PAUSED",
"fixed_base_reference_moved",
{},
)
assert "Tag 8" in explanation
assert "基准锁定后" in explanation
assert "下一次标定预检前重新摆放" in action
assert "重新启动新会话" in action
src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py
+4394 -210
View File
File diff suppressed because it is too large Load Diff
src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py
+1280 -12
View File
File diff suppressed because it is too large Load Diff
src/gui_control/gui_control/config/constants.py
+4 -4
View File
@@ -75,10 +75,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
"点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
"握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
"张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
"OK": [0, 0, 255, 255, 255, 151, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 255, 255, 255],
"拇指对中指": [0, 255, 0, 255, 255, 119, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 220, 255, 255],
"拇指对无名指": [0, 255, 255, 0, 255, 88, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 229, 254],
"拇指对小指": [0, 255, 255, 255, 0, 49, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 215],
"OK": [0, 0, 255, 255, 255, 138, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 211, 255, 255, 255],
"拇指对中指": [0, 255, 0, 255, 255, 107, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 202, 255, 255],
"拇指对无名指": [0, 255, 255, 0, 255, 88, 171, 148, 105, 42, 59, 255, 255, 255, 255, 255, 255, 255, 206, 254],
"拇指对小指": [0, 255, 255, 255, 0, 32, 170, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 203],
"准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
"壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
"贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],