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

Compare commits

base: admin/linkerhand_retarget_ros2:08fe190b3a323071e30f40b6c1ba8ace2d6ffbe5
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
..
compare: admin/linkerhand_retarget_ros2:o30
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

1 Commits
08fe190b3a .. o30

Author SHA1 Message Date
admin 57babb966b O30临时标定提交 2026-08-17 17:55:13 +08:00
234 changed files with 13384 additions and 52499 deletions
Expand all files Collapse all files
.gitignore
-4
View File
@@ -69,8 +69,6 @@ Thumbs.db
*_mapping_quality.json
# Device-specific robot descriptions derived from local calibration runs
# Includes both full and partial timestamped zero-calibration outputs.
/src/linkerhand_calibration/urdf/*/*_zero_calibrated_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
@@ -94,5 +92,3 @@ candump-*
# Local Codex/agent workspace metadata
/.agents/
/.codex/
/.codebuddy/
/.zcode/
docs/G20三相机零位标定与URDF修正操作说明.md
+669
View File
@@ -0,0 +1,669 @@
# 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
@@ -0,0 +1,157 @@
# 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/linkerhand_calibration/README.md → src/g20_thumb_apriltag_calibration/README.md
+36 -376
View File
@@ -1,338 +1,8 @@
# LinkerHand 专业标定包
# G20 左右手与 O30 右手 AprilTag 标定
## L6 右手局部标定（l6_right_8/v1）
本版只发布 `rh_thumb_cmc_pitch`、`rh_thumb_cmc_roll`、
`rh_pinky_mcp_pitch` 的静态零位与动态曲线，以及 `rh_thumb_dip`、
`rh_pinky_dip` 的视觉动态曲线。拇指 DIP 通过线性 mimic；小指 DIP 使用实测
双方向运行曲线和 MuJoCo 二次 equality，因为它的传动比会随屈曲角变化。生成的
修正 URDF 保留 `rh_pinky_dip` 的 `<mimic>`，因此在普通 URDF/RViz 中仍会跟随
MCP 运动；该线性回退精确对齐实测零位和闭合端点。中间行程的准确非线性轨迹由
MuJoCo equality 或下述标定桥提供。根据 L6 四指同机构的实机确认，食指、中指、
无名指的 MCP 零偏、行程、双向反馈曲线及 DIP 耦合从小指迁移；每指自己的
`origin.xyz`、axis、mesh 和惯量保持 CAD，不把迁移结果标成 Tag 实测。四指 MCP
以反馈 255 的展开端作为 CAD lower/zero 锚点；实测行程不会再被强制压回较短的
CAD upper，因此不会向四指 origin 写入系统性的负零偏。
结果指针为 `latest_partial_passed`，不会被当作六路主动关节的完整标定。
拇指 pitch/roll 根据两组已记录六路反馈值的实机/仿真姿态对比，以反馈 255
保持源 CAD joint zero，不叠加端点推断的静态偏置；小指及三指迁移仍以反馈 0
机械闭合姿态对齐源 CAD upper。该策略只改变坐标锚点，不改变视觉实测的双方向
行程曲线。现场姿态对比必须同时记录对应的六路反馈值。
先启动 SDK 和 GUI 做手动检查时使用：
```bash
ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
-p hand_type:=right -p hand_joint:=L6 -p can:=can0 -p topic_prefix:=/l6
ros2 run gui_control gui_control
```
正式一键标定由 runner 自己启动 SDK、三相机、AprilTag 和标定节点，不要同时
运行上面的 SDK/GUI 控制命令：
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml
```
只检查 Profile、8 张 16 mm Tag、相机/外参哈希和只读源 URDF：
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml --validate-only
```
离线回放与在线发布使用同一拟合/URDF写回路径：
```bash
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/l6_right_product.yaml \
--offline-raw calibration_output/L6_RIGHT_001/<时间戳>/raw_samples.jsonl
```
运行前将产品 YAML 中的 `serial_number` 改成实物串号。通道顺序固定为 pitch、
roll、index、middle、ring、pinky；旧 SDK 反馈中的 `thumb_cmc_yaw` 仅作为第二路
兼容别名读取，新产物和运行桥始终输出 `thumb_cmc_roll` / `rh_*` URDF 名。
预检和正式扫描均使用 L6 硬件速度 `1` 作为上限，并由 100 Hz 余弦缓入缓出
轨迹把完整 `255↔0` 行程固定为 `6 s`；短行程按距离同比缩短。SDK 会过滤 L6
在同一 CAN ID 回送的位置命令回显，标定只使用状态查询返回的真实反馈。标定
启动时还会检查重复 SDK/GUI 发布者，避免两个进程同时访问同一只手。
标定完成后，用生成的 JSON 将六路硬件反馈转换成包括小指 DIP 在内的 11 关节
`JointState`：
```bash
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
hand_type:=right \
calibration_file:=$PWD/calibration_output/L6_RIGHT_001/latest_partial_passed/l6_right_L6_RIGHT_001_partial_calibration.json
```
schema v6 默认订阅 `/l6/cb_right_hand_state`，发布
`/sim/mujoco/l6/right/joint_state`。标准 URDF 的 `<mimic>` 本身只支持线性关系，
所以只查看 URDF 时小指 DIP 中间行程是端点对齐的近似；需要实测轨迹时使用该桥
或修正 URDF 内的 MuJoCo equality。
## G20右手正式一键标定
固定三相机和19张Tag安装完成后，用户只运行：
```bash
ros2 run linkerhand_calibration calibrate_hand
```
旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名；
旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。
构建和正式调用统一为：
```bash
colcon build --packages-select linkerhand_calibration
ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml>
```
## 代码边界
- `core/`：无 ROS、无具体型号，包含领域类型、PnP/旋转数学、拟合接口、统一样本
契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。
- `runtime/`：通用会话状态机与注册 Profile 分发；ROS 消息和硬件适配只能位于
`runtime/nodes`、`runtime/adapters`。
- `models/g20/`：G20 right-19、legacy-11、运动、零位、产物和中文诊断策略。
后续型号或左右手作为新的独立 Profile 加入 `models/`，不在通用层增加分支。
- `compat/`：v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留
一版最小转发 shim，不包含算法副本。
产品配置在启动硬件前通过本地 `ProfileRegistry` 完成命令索引、任务、视角、
Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改；v2 配置使用
`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明，通用层
不要求 `front/side/top`，也不假设固定 20 个命令。
仓库中存在已审定硬件会话时，可执行只读金标准检查（不会覆盖任何产物）：
```bash
ros2 run linkerhand_calibration validate_g20_goldens \
calibration_output/G20_RIGHT_001
```
它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话；完整
整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。
完全独立地只标定大拇指4项任务时，使用：
```bash
ros2 run linkerhand_calibration calibrate_hand \
--scope thumb
```
该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机
方向观测以及拇指自己的机械端点；输出URDF从原始CAD生成，只修改4个拇指主动
关节，12个四指关节保持CAD零位。独立结果发布到`latest_thumb_passed`，其JSON是
拇指标定/诊断产物，不冒充可直接运行的完整整手曲线JSON。
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定，才额外使用：
```bash
ros2 run linkerhand_calibration calibrate_hand \
--scope thumb \
--base-session calibration_output/G20_RIGHT_001/latest_passed
```
合并模式会冻结基础会话中的12个四指主动零位；发布前再次读取基础会话JSON核对，
任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。
拇指专项结果重复性通过后，不必再做原来的16项整手扫描。以该拇指会话为基础，
只重新采集12项四指任务并合成完整整手URDF：
```bash
ros2 run linkerhand_calibration calibrate_hand \
--scope fingers \
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
```
`fingers`模式严格冻结基础会话中的4个拇指主动零位；最终完整整手URDF中的拇指
零位与专项会话schema-v4数值完全一致。默认`full`也先调用与`thumb`完全相同的
独立拇指内核，再冻结这4个结果求解12个四指零位，因此四指数据不能反向改写
拇指结果。原来的默认`full`仍保留，用于需要16项全部重新采集的情况。
采样文件中的运动域是显式且不可混用的：
`requested_command_u8` 表示下发命令，`feedback_u8` 表示电机反馈。
在线拟合和离线重放通过同一个数据契约投影到曲线索引；新会话不会把含糊的
`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为
`IMPORTING_BASE` 和 `REVALIDATING_INHERITED`，完成复核后才允许机械手运动。
提供`--base-session`时，它必须解析到同一序列号目录下的完整PASS会话；启动前会校验源CAD
URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF，不在旧校准
URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
`thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务，其余任务的原始记录导入后仍按
数据契约和产物哈希检查，但不会以历史四指拟合结果否决本次拇指专项标定。
开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经
完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的
独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与
最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据
当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收
失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记；
恢复时只读取该标记之后的替代采集，不能把同一尝试编号下重扫前后的稳态点合并。
因此已经在线硬门限验收的任务保持已完成，暂停中的任务从任务开头重采，不会因
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量
独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次
采集确定的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 linkerhand_calibration three_camera_calibration.launch.py \
hand_type:=right \
tag_layout:=g20_right_19 \
serial_number:=G20_RIGHT_001 \
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
source_urdf_expected_sha256:=<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的静态偏移。
O30 右手使用相同的三相机/11-Tag 几何采集框架，但采用独立的 20 电机
profile、8 项扫描任务和主动关节零位策略。完整映射、固定基准命令和启动方法见
[O30 右手操作说明](../../docs/O30右手三相机标定与URDF修正操作说明.md)。
## 三机位三维关节轴零位标定（schema v4）
@@ -381,7 +51,7 @@ ID 9 必须在拇指横摆的完整行程中持续可见。
```bash
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
ros2 launch linkerhand_calibration \
ros2 launch g20_thumb_apriltag_calibration \
three_camera_extrinsics.launch.py \
output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
@@ -422,11 +92,12 @@ ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
先使用禁止运动模式检查三个机位、外参、内参和标签：
```bash
ros2 launch linkerhand_calibration \
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 \
commands_enabled:=false
```
@@ -444,20 +115,19 @@ ros2 run image_view image_view --ros-args \
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程：
```bash
ros2 launch linkerhand_calibration \
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
```
原始URDF及其mesh随`linkerhand_calibration`安装，默认根据`hand_type`自动选择。
如需调试其他CAD版本，仍可通过`source_urdf_path:=<绝对路径>`显式覆盖。
右手使用同一入口，并自动选择右手SDK话题和原始URDF：
右手使用同一入口；默认自动选择右手SDK话题和原始URDF：
```bash
ros2 launch linkerhand_calibration \
ros2 launch g20_thumb_apriltag_calibration \
three_camera_calibration.launch.py \
hand_type:=right \
serial_number:=G20_RIGHT_001 \
@@ -511,7 +181,7 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。
95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
@@ -528,14 +198,13 @@ Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目Pn
约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
但拟合出的共同掌坐标相位不写入四指 `origin.rpy`；只发布各指相对四指中值的实测
装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械
端点，视觉轴链继续用于轴线、PnP和留出诊断；代码和配置中不保存任何按左右手或
序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。
但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何
按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
视觉依赖链仍为：yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指
pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再决定四个具有机械端点
的拇指主动关节绝对零位；四指PIP/DIP轴线相位也继续用于机构诊断。
7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束
拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴
约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
@@ -544,22 +213,18 @@ pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再
生成修正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倍内时会标记为黄色
预警，但只要仍超过硬门限，就在当前关节立即使用剩余重试预算
（`provisional_fit_warning_rescan`）；第三次仍超限则当场暂停，不允许预警数据继续到
后续关节。最终拟合仍按原硬门限验收，因此不会在全部任务采完后才回头重采靠前
关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的
PnP双簇行程，程序将它判为系统性分支失败并当场停止，不再浪费第3次全关节重扫。
仍保存在 `raw_samples.jsonl`。
每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
@@ -612,31 +277,27 @@ 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` 可能
改变；当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解；四指DIP和
`thumb_ip`为被动关节，发布实测动态曲线并保留CAD静态零位，其mimic坐标偏置只随
上游主动关节坐标系变换作等价调整。
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖：
```bash
python3 -m linkerhand_calibration.offline_replay \
python3 -m g20_thumb_apriltag_calibration.offline_replay \
calibration_output/G20_RIGHT_001/20260811_120146 \
--output-tag AXIS_FRAME_V3 \
--write
@@ -653,13 +314,12 @@ python3 -m linkerhand_calibration.offline_replay \
`JointState`（包括5个被动关节）：
```bash
ros2 launch linkerhand_calibration calibrated_joint_state_bridge.launch.py \
ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
hand_type:=right \
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
```
schema v5默认订阅 `/g20/cb_right_hand_state`；schema v4默认订阅
`/g20/cb_right_hand_control_cmd`。两者均发布
默认订阅 `/cb_right_hand_control_cmd`，发布
`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
因此不会静默退回旧标定。
@@ -725,7 +385,7 @@ sudo apt-get install -y \
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install \
--packages-select linker_hand_ros2_sdk linkerhand_calibration
--packages-select linker_hand_ros2_sdk g20_thumb_apriltag_calibration
source install/setup.bash
```
@@ -752,7 +412,7 @@ source install/setup.bash
先单独启动相机（不会连接机械手，也不会发送关节命令）：
```bash
ros2 run linkerhand_calibration hikrobot_camera_node --ros-args \
ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
--remap __ns:=/camera/camera/color \
-p serial_number:=DB2163742 \
-p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
@@ -790,7 +450,7 @@ ros2 run camera_calibration cameracalibrator \
但标定节点不会发送位置运动命令，也不会允许解锁全行程扫描：
```bash
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
camera_serial_number:=DB2163742 \
commands_enabled:=false
@@ -801,7 +461,7 @@ ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
速度，并使用单终点连续运动：
```bash
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
camera_serial_number:=DB2163742 \
can_interface:=can0 \
@@ -914,7 +574,7 @@ calibration_output/<序列号>/<时间戳>/
恢复时必须显式复用原目录，否则会创建新会话：
```bash
ros2 launch linkerhand_calibration front_thumb_calibration.launch.py \
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
session_dir:=/绝对路径/calibration_output/G20_LEFT_001/20260727_120000
```
@@ -980,7 +640,7 @@ for name, joint in data["joints"].items():
不影响 AprilTag 的 ROI 输入。
静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
当前末节16 mm Tag如果中心位置 RMS 持续不合格，应优先增加照明、缩短
当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
@@ -994,7 +654,7 @@ for name, joint in data["joints"].items():
URDF：
```bash
ros2 launch linkerhand_calibration \
ros2 launch g20_thumb_apriltag_calibration \
front_cmc_pitch_zero.launch.py \
serial_number:=G20_LEFT_001
```
@@ -1071,7 +731,7 @@ zero_angles.table_projected_zero_rad
Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量，不读取T3标签朝向。
```bash
ros2 launch linkerhand_calibration \
ros2 launch g20_thumb_apriltag_calibration \
front_cmc_roll_calibration.launch.py \
serial_number:=G20_LEFT_001
```
src/linkerhand_calibration/config/calibration.yaml → 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 small/far tags (historically observed at 32-38 px).
# Trial threshold for the current 10 mm tags (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/linkerhand_calibration/config/cmc_pitch_zero.yaml → src/g20_thumb_apriltag_calibration/config/cmc_pitch_zero.yaml
View File
src/linkerhand_calibration/config/cmc_roll_zero_travel.yaml → src/g20_thumb_apriltag_calibration/config/cmc_roll_zero_travel.yaml
View File
src/linkerhand_calibration/config/fastdds_large_images.xml → src/g20_thumb_apriltag_calibration/config/fastdds_large_images.xml
View File
src/linkerhand_calibration/config/front_tags.yaml → src/g20_thumb_apriltag_calibration/config/front_tags.yaml
View File
src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
+132
View File
@@ -0,0 +1,132 @@
/**:
ros__parameters:
command_topic: /g20/cb_left_hand_control_cmd
state_topic: /g20/cb_left_hand_state
info_topic: /g20/cb_left_hand_info
setting_topic: /g20/cb_hand_setting_cmd
front_camera_info_topic: /g20_calibration/front/camera/camera_info
front_detections_topic: /g20_calibration/front/apriltag/detections
side_camera_info_topic: /g20_calibration/side/camera/camera_info
side_detections_topic: /g20_calibration/side/apriltag/detections
top_camera_info_topic: /g20_calibration/top/camera/camera_info
top_detections_topic: /g20_calibration/top/apriltag/detections
# /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
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
speed_setting_settle_seconds: 0.25
# tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。
tag_size_m: 0.016
repetitions: 3
preflight_frames: 60
minimum_detection_rate: 0.95
minimum_detection_hz: 15.0
maximum_hamming: 0
minimum_decision_margin: 30.0
minimum_edge_pixels: 30.0
pnp_maximum_reprojection_error_px: 1.5
pnp_reprojection_tie_px: 1.5
pnp_maximum_pose_jump_deg: 35.0
pnp_maximum_translation_jump_m: 0.04
pnp_maximum_tag_tilt_deg: 75.0
pnp_tracker_reset_seconds: 5.0
# 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
pnp_group_initialization_frames: 8
# 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
pnp_group_normal_alignment_scale_deg: 5.0
pnp_group_maximum_normal_alignment_deg: 15.0
top_pnp_invalid_reset_seconds: 1.0
# 三维位姿必须与实测20通道状态严格按时间戳配对。
maximum_state_image_skew_ms: 50.0
axis_maximum_plane_rms_m: 0.003
# 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
# 轴线一致性仍沿用严格检查。
passive_axis_maximum_plane_rms_m: 0.004
axis_maximum_radial_rms_m: 0.003
# 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
axis_maximum_pose_line_rms_m: 0.001
# 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
# 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
axis_maximum_rotation_circle_difference_deg: 1.0
# 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
# 关节允许可重复的非理想单轴分量，但仍须通过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_offset_deg: 20.0
# 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。
zero_finger_maximum_offset_deg: 3.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
# 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
# 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
pinky_pip_zero_endpoint_tolerance_u8: 5.0
endpoint_hold_seconds: 0.5
baseline_hold_seconds: 0.5
position_timeout_seconds: 30.0
sweep_timeout_seconds: 90.0
# 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停；
# 这类故障不进入遮挡/超时的三次自动重扫。
motor_stall_timeout_seconds: 8.0
motor_stall_minimum_progress_u8: 1.0
invalid_timeout_seconds: 3.0
minimum_sweep_frames: 40
minimum_state_span_u8: 240.0
minimum_sweep_bins: 32
maximum_bin_gap: 16
# 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
automatic_sweep_retry_limit: 3
# 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
automatic_fit_retry_limit: 2
automatic_motion_retry_limit: 2
# 过程检查允许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]
trajectory_maximum_plane_rms_m: 0.004
trajectory_maximum_radial_rms_m: 0.004
trajectory_minimum_radius_m: 0.003
trajectory_minimum_arc_deg: 15.0
# 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
image_trajectory_maximum_radial_rms_px: 2.0
image_trajectory_maximum_radial_p95_px: 3.5
image_trajectory_minimum_radius_px: 20.0
trajectory_maximum_cycle_travel_difference_deg: 3.0
passive_maximum_cycle_travel_difference_deg: 10.0
maximum_monotonic_correction_deg: 2.0
maximum_hysteresis_deg: 5.0
passive_maximum_monotonic_correction_deg: 3.0
passive_maximum_hysteresis_deg: 7.5
# 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。
validation_enabled: false
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
src/linkerhand_calibration/config/three_camera_tags.yaml → src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml
View File
src/linkerhand_calibration/config/three_camera_tags_g20_right_15.yaml → src/g20_thumb_apriltag_calibration/config/three_camera_tags_o30.yaml
+25 -25
View File
@@ -1,4 +1,4 @@
/g20_calibration/front/apriltag/apriltag:
/o30_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, 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]
ids: [0, 1, 2, 3, 10]
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
/g20_calibration/top/apriltag/apriltag:
/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:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/__init__.py
+5
View File
@@ -0,0 +1,5 @@
"""Front-camera AprilTag calibration for the left LinkerHand G20 thumb."""
from .core import BASELINE_COMMAND, COMMAND_NAMES
__all__ = ["BASELINE_COMMAND", "COMMAND_NAMES"]
src/linkerhand_calibration/linkerhand_calibration/acquisition.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py
+3 -8
View File
@@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence
import numpy as np
from .core import robust_rotation_summary
from .core import PAIR_NAMES, robust_rotation_summary
from .pnp import SquareTagPose
@@ -18,7 +18,6 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
"t3_t4": ("t3", "t4"),
"t4_t5": ("t4", "t5"),
}
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
@dataclass(frozen=True)
@@ -117,7 +116,7 @@ def interpolate_state_u8(
*,
maximum_skew_ns: int,
) -> tuple[tuple[float, ...], int] | None:
"""Interpolate a profile-sized hand state at an image timestamp.
"""Interpolate the 20-D hand state at an image timestamp.
The SDK publishes state independently from the camera. Continuous
calibration must therefore use the image timestamp instead of whichever
@@ -151,11 +150,7 @@ def interpolate_state_u8(
fraction = before_gap / denominator
before_values = np.asarray(before.position_u8, dtype=float)
after_values = np.asarray(after.position_u8, dtype=float)
if (
before_values.ndim != 1
or before_values.size == 0
or after_values.shape != before_values.shape
):
if before_values.shape != (20,) or after_values.shape != (20,):
return None
interpolated = before_values + fraction * (after_values - before_values)
return (
src/linkerhand_calibration/linkerhand_calibration/alignment_view.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/alignment_view.py
View File
src/linkerhand_calibration/linkerhand_calibration/calibrated_joint_state_bridge.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py
+90 -132
View File
@@ -1,13 +1,8 @@
"""Map model SDK u8 feedback to URDF joint angles using one calibration JSON.
"""Map supported-hand u8 commands to URDF angles using 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
@@ -21,12 +16,7 @@ import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from .full_hand import (
get_hand_calibration_profile,
infer_compact_payload_layout,
validate_compact_payload,
)
from .models.l6.artifacts import validate_l6_runtime_payload
from .full_hand import get_hand_calibration_profile, validate_compact_payload
G20_COMMAND_NAMES: tuple[str, ...] = (
@@ -79,18 +69,69 @@ 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, profile-specific lookup from SDK u8 values to URDF radians."""
"""Validated, model/side-specific lookup from commands to URDF radians."""
def __init__(
self, payload: Mapping[str, Any], *, expected_side: str | None = None
) -> None:
schema_version = int(payload["schema_version"])
if schema_version == 6:
validate_l6_runtime_payload(payload)
else:
validate_compact_payload(payload)
validate_compact_payload(payload)
side = str(payload["side"]).lower()
if expected_side is not None and side != str(expected_side).lower():
raise ValueError(
@@ -100,77 +141,31 @@ class CalibratedCommandMapper:
quality = payload["quality"]
if quality.get("passed") is not True:
raise ValueError("calibration quality.passed must be true")
layout_id = (
str(payload["layout_id"])
if schema_version == 6
else infer_compact_payload_layout(payload)
)
model = str(payload["model"]).upper()
profile = get_hand_calibration_profile(side, model)
urdf_joint_names = URDF_JOINT_NAMES_BY_MODEL[model]
self.model = model
self.side = side
self.layout_id = layout_id
self.model = str(payload["model"]).upper()
self.profile_id = str(
payload.get("profile_id", f"G20/{side}/{layout_id}/v1")
)
self.serial_number = str(payload["serial_number"])
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")
if schema_version == 6:
self.command_names = tuple(str(value) for value in payload["command_names"])
self.urdf_joint_names = tuple(str(name) for name in payload["joints"])
self._motor_by_joint = {
name: int(payload["joints"][name]["motor_index"])
for name in self.urdf_joint_names
}
self.feedback_name_aliases = {"thumb_cmc_yaw": "thumb_cmc_roll"}
else:
profile = get_hand_calibration_profile(side, layout_id)
self.command_names = G20_COMMAND_NAMES
self.urdf_joint_names = G20_URDF_JOINT_NAMES
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in self.urdf_joint_names
}
self.feedback_name_aliases = {}
self.command_names = COMMAND_NAMES_BY_MODEL[model]
self.urdf_joint_names = urdf_joint_names
self._motor_by_joint = {
name: int(profile.joint_specs[name].motor_index)
for name in urdf_joint_names
}
self._curves = {
name: tuple(
float(value)
for value in payload["joints"][name]["angle_rad"]
)
for name in self.urdf_joint_names
for name in 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 self.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 self.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("G20 command positions must be finite")
raise ValueError("command positions must be finite")
return max(0, min(255, int(math.floor(command + 0.5))))
def map_positions(
@@ -185,52 +180,25 @@ 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))
for alias, canonical in self.feedback_name_aliases.items():
if alias in by_name and canonical not in by_name:
by_name[canonical] = by_name[alias]
missing = [name for name in self.command_names if name not in by_name]
if missing:
raise ValueError(
f"{self.model} feedback is missing named channels: "
f"{self.model} command is missing named channels: "
+ ",".join(missing)
)
command = tuple(by_name[name] for name in self.command_names)
else:
if len(values) != len(self.command_names):
raise ValueError(
f"unnamed {self.model} feedback must contain exactly "
f"unnamed {self.model} command must contain exactly "
f"{len(self.command_names)} positions"
)
command = values
indices = tuple(self._command_index(value) for value in command)
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 self.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)
return tuple(
self._curves[name][indices[self._motor_by_joint[name]]]
for name in self.urdf_joint_names
)
def load_calibrated_command_mapper(
@@ -243,44 +211,35 @@ def load_calibrated_command_mapper(
return CalibratedCommandMapper(payload, expected_side=expected_side)
def default_input_topic(
hand_type: str, input_domain: str, model: str = "G20"
) -> 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"/{str(model).lower()}/cb_{side}_hand_state"
if input_domain == "command_u8":
return f"/{str(model).lower()}/cb_{side}_hand_control_cmd"
raise ValueError("calibration curve_input_domain is invalid")
class CalibratedJointStateBridge(Node):
def __init__(self) -> None:
super().__init__("calibrated_joint_state_bridge")
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()
if hand_type not in {"left", "right"}:
raise ValueError("hand_type must be left or right")
get_hand_calibration_profile(hand_type, hand_model)
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 default_input_topic(
hand_type, self.mapper.input_domain, self.mapper.model
)
self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd"
self.output_topic = (
output_topic
or f"/sim/mujoco/{self.mapper.model.lower()}/{hand_type}/joint_state"
or f"/sim/mujoco/{hand_model.lower()}/{hand_type}/joint_state"
)
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
self.subscription = self.create_subscription(
@@ -288,10 +247,9 @@ class CalibratedJointStateBridge(Node):
)
self._last_error = ""
self.get_logger().info(
f"loaded {self.mapper.profile_id} calibration for "
f"loaded {hand_type} {hand_model} calibration for "
f"{self.mapper.serial_number}: "
f"{self.input_topic} ({self.mapper.input_domain}) -> "
f"{self.output_topic}"
f"{self.input_topic} -> {self.output_topic}"
)
def _command_callback(self, command: JointState) -> None:
src/linkerhand_calibration/linkerhand_calibration/compat/legacy/thumb_core.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py
View File
src/linkerhand_calibration/linkerhand_calibration/diagnostics.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/diagnostics.py
View File
src/linkerhand_calibration/linkerhand_calibration/core/geometry/extrinsics.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics.py
+81 -66
View File
@@ -13,6 +13,9 @@ import yaml
from scipy.spatial.transform import Rotation
VIEWS: tuple[str, ...] = ("front", "side", "top")
def camera_info_fingerprint(
*,
width: int,
@@ -78,61 +81,66 @@ class CameraCalibrationIdentity:
@dataclass(frozen=True)
class CameraExtrinsics:
"""Transforms every declared camera into one Profile-selected reference."""
class ThreeCameraExtrinsics:
"""Transforms points from each camera optical frame into front optical."""
cameras: Mapping[str, CameraCalibrationIdentity]
reference_view: str
reference_from_view: Mapping[str, np.ndarray]
front_from_view: Mapping[str, np.ndarray]
quality: Mapping[str, float]
def transform(self, view: str) -> np.ndarray:
if view not in self.reference_from_view:
if view not in self.front_from_view:
raise KeyError(f"extrinsics do not contain view {view}")
return np.asarray(self.reference_from_view[view], dtype=float).copy()
return np.asarray(self.front_from_view[view], dtype=float).copy()
def camera_matches(
self,
view: str,
*,
serial_number: str,
width: int,
height: int,
intrinsics_sha256: str,
) -> bool:
expected = self.cameras.get(view)
return bool(
expected is not None
and expected.serial_number == str(serial_number)
and expected.width == int(width)
and expected.height == int(height)
and expected.intrinsics_sha256 == str(intrinsics_sha256)
)
def validate_camera_extrinsics_payload(
payload: Mapping[str, Any],
*,
required_views: Sequence[str],
reference_view: str,
quality_limits: Mapping[str, float] | None = None,
minimum_capture_counts: Mapping[str, int] | None = None,
) -> None:
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
if int(payload.get("schema_version", -1)) != 1:
raise ValueError("camera extrinsics schema_version must be 1")
reference = str(reference_view)
if payload.get("reference_view") != reference:
raise ValueError(
"camera extrinsics reference_view differs from the Profile"
)
if payload.get("reference_view") != "front":
raise ValueError("camera extrinsics reference_view must be front")
cameras = payload.get("cameras")
transforms = payload.get(f"{reference}_from_view")
transforms = payload.get("front_from_view")
quality = payload.get("quality")
views = tuple(str(view) for view in required_views)
if not views or len(set(views)) != len(views):
raise ValueError("required extrinsic views must be non-empty and unique")
if reference not in views:
raise ValueError("extrinsic reference view is not required")
if not isinstance(cameras, Mapping) or set(cameras) != set(views):
raise ValueError("camera extrinsics differ from the Profile views")
if not isinstance(transforms, Mapping) or set(transforms) != set(views):
raise ValueError("camera transforms differ from the Profile views")
if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
raise ValueError("camera extrinsics must contain front/side/top cameras")
if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
raise ValueError("camera extrinsics must contain all three transforms")
if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
raise ValueError("camera extrinsics quality is not passed")
for key, limit in dict(quality_limits or {}).items():
quality_limits = {
"reprojection_rms_px": 1.2,
"maximum_rotation_repeatability_deg": 0.3,
"maximum_translation_repeatability_m": 0.0015,
}
for key, limit in quality_limits.items():
value = float(quality.get(key, float("inf")))
if not np.isfinite(value) or value > limit:
raise ValueError(
f"camera extrinsics {key}={value} exceeds {limit}"
)
for key, minimum in dict(minimum_capture_counts or {}).items():
if int(quality.get(key, 0)) < int(minimum):
raise ValueError(
f"camera extrinsics {key} must be at least {minimum}"
)
for view in views:
for key in ("front_side_captures", "front_top_captures"):
if int(quality.get(key, 0)) < 15:
raise ValueError(f"camera extrinsics {key} must be at least 15")
for view in VIEWS:
identity = cameras[view]
if not isinstance(identity, Mapping):
raise ValueError(f"{view} camera identity must be an object")
@@ -150,23 +158,14 @@ def validate_camera_extrinsics_payload(
transform.get("translation_xyz_m", ()),
transform.get("quaternion_xyzw", ()),
)
if view == reference and not np.allclose(
matrix, np.eye(4), atol=1.0e-9
):
raise ValueError("reference-view transform must be identity")
serials = [str(cameras[view]["serial_number"]) for view in views]
if len(set(serials)) != len(views):
if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
raise ValueError("front_from_view.front must be identity")
serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
if len(set(serials)) != len(VIEWS):
raise ValueError("camera extrinsics serial numbers must be unique")
def load_camera_extrinsics(
path: str | Path,
*,
required_views: Sequence[str],
reference_view: str,
quality_limits: Mapping[str, float] | None = None,
minimum_capture_counts: Mapping[str, int] | None = None,
) -> CameraExtrinsics:
def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
source = Path(path).expanduser().resolve()
if not source.is_file():
raise ValueError(f"camera extrinsics file does not exist: {source}")
@@ -174,15 +173,7 @@ def load_camera_extrinsics(
payload = yaml.safe_load(stream)
if not isinstance(payload, Mapping):
raise ValueError("camera extrinsics file must contain an object")
validate_camera_extrinsics_payload(
payload,
required_views=required_views,
reference_view=reference_view,
quality_limits=quality_limits,
minimum_capture_counts=minimum_capture_counts,
)
views = tuple(str(view) for view in required_views)
transform_key = f"{reference_view}_from_view"
validate_extrinsics_payload(payload)
cameras = {
view: CameraCalibrationIdentity(
serial_number=str(payload["cameras"][view]["serial_number"]),
@@ -192,21 +183,45 @@ def load_camera_extrinsics(
payload["cameras"][view]["intrinsics_sha256"]
),
)
for view in views
for view in VIEWS
}
transforms = {
view: transform_matrix(
payload[transform_key][view]["translation_xyz_m"],
payload[transform_key][view]["quaternion_xyzw"],
payload["front_from_view"][view]["translation_xyz_m"],
payload["front_from_view"][view]["quaternion_xyzw"],
)
for view in views
for view in VIEWS
}
return CameraExtrinsics(
return ThreeCameraExtrinsics(
cameras=cameras,
reference_view=str(reference_view),
reference_from_view=transforms,
front_from_view=transforms,
quality={
str(key): float(value) if isinstance(value, (int, float)) else value
for key, value in payload["quality"].items()
},
)
def dump_three_camera_extrinsics(
path: str | Path,
*,
cameras: Mapping[str, Mapping[str, Any]],
front_from_view: Mapping[str, Sequence[Sequence[float]]],
quality: Mapping[str, Any],
) -> None:
payload = {
"schema_version": 1,
"reference_view": "front",
"cameras": {view: dict(cameras[view]) for view in VIEWS},
"front_from_view": {
view: matrix_payload(front_from_view[view]) for view in VIEWS
},
"quality": dict(quality),
}
validate_extrinsics_payload(payload)
destination = Path(path).expanduser().resolve()
destination.parent.mkdir(parents=True, exist_ok=True)
temporary = destination.with_suffix(destination.suffix + ".tmp")
with temporary.open("w", encoding="utf-8") as stream:
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
temporary.replace(destination)
src/linkerhand_calibration/linkerhand_calibration/extrinsics_node.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics_node.py
View File
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py
+1138
View File
File diff suppressed because it is too large Load Diff
src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/hikrobot_camera.py
+1 -1
View File
@@ -203,7 +203,7 @@ def configure_fastdds_large_image_transport() -> Path:
from ament_index_python.packages import get_package_share_directory
profile = (
Path(get_package_share_directory("linkerhand_calibration"))
Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
/ "config"
/ "fastdds_large_images.xml"
)
src/linkerhand_calibration/linkerhand_calibration/node.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py
+1 -1
View File
@@ -36,7 +36,7 @@ from .acquisition import (
interpolate_state_u8,
tag_quality_is_valid,
)
from .compat.legacy.thumb_core import (
from .core import (
BASELINE_COMMAND,
COMMAND_NAMES,
DIRECTION_DECREASING,
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py
+860
View File
@@ -0,0 +1,860 @@
"""Safely replay a complete three-camera session without moving the hand."""
from __future__ import annotations
import argparse
from collections import defaultdict
from dataclasses import replace
import hashlib
import json
import math
import os
from pathlib import Path
import re
import tempfile
from typing import Any, Mapping, Sequence
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation
import yaml
from .extrinsics import load_three_camera_extrinsics
from .full_hand import (
HandCalibrationProfile,
JointCurveFit,
build_calibration_motion_command,
build_compact_payload,
get_hand_calibration_profile,
validate_compact_payload,
)
from .storage import atomic_write_json
from .urdf_zero import (
JointAxisMeasurement,
UrdfKinematicModel,
_angles_from_state,
fit_joint_axis_measurement,
fit_rotation_joint_curve,
get_zero_calibration_profile,
rotation_curve_holdout_errors,
solve_urdf_zero_offsets,
write_zero_corrected_urdf,
)
def _sha256(path: Path) -> str:
digest = hashlib.sha256()
with path.open("rb") as stream:
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
digest.update(chunk)
return digest.hexdigest()
def _output_suffix(output_tag: str | None) -> str:
"""Return a filename-safe suffix for a non-destructive replay variant."""
if output_tag is None:
return ""
tag = str(output_tag)
if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag):
raise ValueError(
"output tag must be 1-64 filename-safe characters, beginning "
"with a letter or digit"
)
return f"_{tag}"
def _load_parameters(path: Path) -> dict[str, Any]:
payload = yaml.safe_load(path.read_text(encoding="utf-8"))
if not isinstance(payload, Mapping):
raise ValueError("calibration config must contain a mapping")
node = payload.get("/**", payload.get("g20_calibration"))
if not isinstance(node, Mapping) or not isinstance(
node.get("ros__parameters"), Mapping
):
raise ValueError("calibration config is missing ros__parameters")
return dict(node["ros__parameters"])
def _latest_attempt_records(
rows: Sequence[Mapping[str, Any]],
) -> dict[str, list[dict[str, Any]]]:
"""Reproduce the online retry buffer from append-only raw samples.
Online retry clears only the failed joint/cycle/direction from memory,
while JSONL deliberately retains every attempt for audit. Offline replay
must therefore select the greatest attempt independently for each logical
trajectory rather than mixing rejected attempts into the final fit.
"""
samples = [dict(row) for row in rows if row.get("kind") == "sample"]
latest_attempt: dict[tuple[str, int, str], int] = {}
for row in samples:
key = (
str(row["joint"]),
int(row["cycle"]),
str(row["direction"]),
)
latest_attempt[key] = max(
latest_attempt.get(key, 0), int(row.get("attempt", 1))
)
records: dict[str, list[dict[str, Any]]] = defaultdict(list)
for row in samples:
key = (
str(row["joint"]),
int(row["cycle"]),
str(row["direction"]),
)
if int(row.get("attempt", 1)) == latest_attempt[key]:
records[key[0]].append(row)
return dict(records)
def _load_raw_session(
session_dir: Path,
) -> tuple[dict[str, Any], dict[str, list[dict[str, Any]]], Path]:
raw_path = session_dir / "raw_samples.jsonl"
if not raw_path.is_file():
raise ValueError(f"raw session does not exist: {raw_path}")
rows = [
json.loads(line)
for line in raw_path.read_text(encoding="utf-8").splitlines()
if line.strip()
]
starts = [row for row in rows if row.get("kind") == "session_start"]
if len(starts) != 1:
raise ValueError("raw session must contain exactly one session_start")
return starts[0], _latest_attempt_records(rows), raw_path
def _fit_curve(
name: str,
records: Sequence[Mapping[str, Any]],
*,
profile: HandCalibrationProfile,
baseline: Sequence[int],
) -> JointCurveFit:
motor = profile.joint_specs[name].motor_index
return fit_rotation_joint_curve(
records,
zero_command_u8=int(baseline[motor]),
command_increasing=profile.joint_specs[name].command_increasing,
)
def _fit_axes(
*,
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
profile: HandCalibrationProfile,
baseline: Sequence[int],
extrinsics_file: Path,
repetitions: int,
) -> list[JointAxisMeasurement]:
zero_profile = get_zero_calibration_profile(profile.side, profile.model)
extrinsics = load_three_camera_extrinsics(extrinsics_file)
cache: dict[tuple[str, int], JointAxisMeasurement] = {}
upstream_by_joint = zero_profile.parallel_axis_parent_joint
def fit_one(name: str, cycle: int) -> JointAxisMeasurement:
key = (name, cycle)
if key in cache:
return cache[key]
upstream = upstream_by_joint.get(name)
constraint = None if upstream is None else fit_one(upstream, cycle).axis_common_xyz
spec = profile.joint_specs[name]
view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray(
[0.0, 0.0, 1.0], dtype=float
)
result = fit_joint_axis_measurement(
name,
records_by_joint[name],
cycle=cycle,
zero_command_u8=int(baseline[spec.motor_index]),
axis_common_constraint=constraint,
constrained_circle_joints=zero_profile.constrained_circle_joints,
view_normal_common_xyz=view_normal,
command_increasing=spec.command_increasing,
)
condition = build_calibration_motion_command(
spec,
int(baseline[spec.motor_index]),
baseline=baseline,
profile=profile,
)
result = replace(
result,
condition_command_u8=tuple(float(value) for value in condition),
view_normal_common_xyz=tuple(float(value) for value in view_normal),
)
cache[key] = result
return result
return [
fit_one(name, cycle)
for name in zero_profile.axis_joints
for cycle in range(repetitions)
]
def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float:
maximum = 0.0
for left in axes:
for right in axes:
maximum = max(
maximum,
math.acos(
abs(
float(
np.clip(
np.asarray(left, dtype=float)
@ np.asarray(right, dtype=float),
-1.0,
1.0,
)
)
)
),
)
return maximum
def _quality_failures(
*,
records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]],
profile: HandCalibrationProfile,
baseline: Sequence[int],
fits: Mapping[str, JointCurveFit],
axes: Sequence[JointAxisMeasurement],
parameters: Mapping[str, Any],
) -> list[str]:
failures: list[str] = []
repetitions = int(parameters["repetitions"])
zero_profile = get_zero_calibration_profile(profile.side, profile.model)
axis_by_key = {(item.joint, item.cycle): item for item in axes}
expected_directions = {"decreasing", "increasing"}
for name in profile.measured_joints:
records = list(records_by_joint.get(name, ()))
if not records:
failures.append(f"{name}: no samples")
continue
attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set)
for record in records:
attempts_by_direction[
(int(record["cycle"]), str(record["direction"]))
].add(int(record.get("attempt", 1)))
for cycle in range(repetitions):
for direction in expected_directions:
selected = [
record
for record in records
if int(record["cycle"]) == cycle
and str(record["direction"]) == direction
]
attempts = attempts_by_direction.get((cycle, direction), set())
if len(attempts) != 1:
attempt_list = sorted(attempts)
failures.append(
f"{name} cycle {cycle + 1} {direction}: "
f"ambiguous attempts {attempt_list}"
)
continue
commands = sorted({int(record["command_u8"]) for record in selected})
if len(selected) < int(parameters["minimum_sweep_frames"]):
failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames")
if not commands or max(commands) - min(commands) < float(
parameters["minimum_state_span_u8"]
):
failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span")
if len(commands) < int(parameters["minimum_sweep_bins"]):
failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins")
if 0 not in commands or 255 not in commands:
failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing")
if commands and max(np.diff(commands), default=0) > int(
parameters["maximum_bin_gap"]
):
failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap")
sync_p95 = float(
np.percentile(
[float(record.get("state_image_sync_error_ms", 0.0)) for record in records],
95.0,
)
)
if sync_p95 > float(parameters["maximum_state_image_skew_ms"]):
failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms")
spec = profile.joint_specs[name]
fit = fits[name]
orthogonal_limit = math.radians(
float(
parameters[
"active_maximum_rotation_orthogonal_rms_deg"
if spec.active
else "passive_maximum_rotation_orthogonal_rms_deg"
]
)
)
if float(fit.quality["rotation_orthogonal_rms_rad"]) > orthogonal_limit:
failures.append(f"{name}: rotation orthogonal RMS")
if float(fit.quality["arc_rad"]) < math.radians(
float(parameters["trajectory_minimum_arc_deg"])
):
failures.append(f"{name}: trajectory arc")
monotonic_limit = math.radians(
float(
parameters[
"maximum_monotonic_correction_deg"
if spec.active
else "passive_maximum_monotonic_correction_deg"
]
)
)
hysteresis_limit = math.radians(
float(
parameters[
"maximum_hysteresis_deg"
if spec.active
else "passive_maximum_hysteresis_deg"
]
)
)
if fit.maximum_monotonic_correction_rad > monotonic_limit:
failures.append(f"{name}: monotonic correction")
if fit.maximum_hysteresis_rad > hysteresis_limit:
failures.append(f"{name}: hysteresis")
cycle_travels: list[float] = []
cycle_axes: list[Sequence[float]] = []
cycle_axis_sources: list[str] = []
for cycle in range(repetitions):
cycle_fit = _fit_curve(
name,
[record for record in records if int(record["cycle"]) == cycle],
profile=profile,
baseline=baseline,
)
cycle_travels.append(
abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255]))
)
axis = axis_by_key[(name, cycle)]
cycle_axes.append(axis.axis_common_xyz)
cycle_axis_sources.append(axis.axis_direction_source)
if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]):
failures.append(f"{name} cycle {cycle + 1}: radial RMS")
if axis.pose_axis_line_rms_m > float(
parameters["axis_maximum_pose_line_rms_m"]
):
failures.append(
f"{name} cycle {cycle + 1}: pose axis-line RMS"
)
if name not in zero_profile.constrained_circle_joints:
plane_limit = float(
parameters[
"axis_maximum_plane_rms_m"
if spec.active
else "passive_axis_maximum_plane_rms_m"
]
)
if axis.plane_rms_m > plane_limit:
failures.append(f"{name} cycle {cycle + 1}: plane RMS")
if (
name not in zero_profile.constrained_circle_joints
and axis.rotation_circle_axis_difference_rad > math.radians(
float(parameters["axis_maximum_rotation_circle_difference_deg"])
)
):
failures.append(f"{name} cycle {cycle + 1}: axis disagreement")
travel_limit = math.radians(
float(
parameters[
"trajectory_maximum_cycle_travel_difference_deg"
if spec.active
else "passive_maximum_cycle_travel_difference_deg"
]
)
)
if max(cycle_travels) - min(cycle_travels) > travel_limit:
failures.append(f"{name}: cycle travel difference")
if (
not all(
source == "upstream_constraint"
for source in cycle_axis_sources
)
and _maximum_undirected_axis_difference(cycle_axes) > math.radians(
float(parameters["zero_maximum_axis_cycle_difference_deg"])
)
):
failures.append(f"{name}: cycle axis difference")
return failures
def _joint_xml(path: Path) -> dict[str, ET.Element]:
return {
str(joint.get("name")): joint
for joint in ET.parse(path).getroot().findall("joint")
}
def _triplet(value: str) -> np.ndarray:
return np.asarray([float(item) for item in value.split()], dtype=float)
def _validate_corrected_urdf(
*,
source: Path,
corrected: Path,
offsets: Mapping[str, float],
axes: Sequence[JointAxisMeasurement],
curves: Mapping[str, JointCurveFit],
motor_by_joint: Mapping[str, int],
inherited_zero_joints: Mapping[str, str],
) -> dict[str, float]:
source_joints = _joint_xml(source)
corrected_joints = _joint_xml(corrected)
if set(source_joints) != set(corrected_joints):
raise ValueError("corrected URDF changed the joint set")
maximum_origin_rotation_error = 0.0
maximum_origin_translation_error = 0.0
for name, original_joint in source_joints.items():
corrected_joint = corrected_joints[name]
original_origin = original_joint.find("origin")
corrected_origin = corrected_joint.find("origin")
if original_origin is None or corrected_origin is None:
continue
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
maximum_origin_translation_error = max(
maximum_origin_translation_error,
float(np.linalg.norm(corrected_xyz - original_xyz)),
)
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 = 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")
)
axis /= np.linalg.norm(axis)
expected = original_rotation * Rotation.from_rotvec(
axis * float(offsets[name])
)
error = float((expected.inv() * corrected_rotation).magnitude())
maximum_origin_rotation_error = max(maximum_origin_rotation_error, error)
original_limit = original_joint.find("limit")
corrected_limit = corrected_joint.find("limit")
if original_limit is not None and corrected_limit is not None:
if (
original_limit.get("lower") != corrected_limit.get("lower")
or original_limit.get("upper") != corrected_limit.get("upper")
):
raise ValueError(f"corrected URDF unexpectedly changed {name} limits")
if maximum_origin_translation_error > 1.0e-12:
raise ValueError("corrected URDF changed a joint origin translation")
if maximum_origin_rotation_error > 1.0e-10:
raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)")
original_model = UrdfKinematicModel(source)
corrected_model = UrdfKinematicModel(corrected)
maximum_axis_error = 0.0
maximum_point_error = 0.0
for measurement in axes:
state = (
measurement.condition_state_u8
if measurement.condition_command_u8 is None
else measurement.condition_command_u8
)
angles = _angles_from_state(
state,
curves=curves,
motor_by_joint=motor_by_joint,
inherited_zero_joints=inherited_zero_joints,
)
expected_axis, expected_point = original_model.axis_line(
measurement.joint,
zero_offsets=offsets,
joint_angles=angles,
)
actual_axis, actual_point = corrected_model.axis_line(
measurement.joint,
zero_offsets={},
joint_angles=angles,
)
maximum_axis_error = max(
maximum_axis_error,
math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))),
)
maximum_point_error = max(
maximum_point_error,
float(np.linalg.norm(expected_point - actual_point)),
)
if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10:
raise ValueError("written URDF kinematics differ from the solved correction")
return {
"maximum_origin_rotation_error_rad": maximum_origin_rotation_error,
"maximum_origin_translation_error_m": maximum_origin_translation_error,
"maximum_axis_equivalence_error_rad": maximum_axis_error,
"maximum_axis_point_equivalence_error_m": maximum_point_error,
}
def replay_session(
session_dir: str | Path,
*,
serial_number: str | None = None,
config_file: str | Path | None = None,
write_outputs: bool = False,
output_tag: str | None = None,
) -> dict[str, Any]:
session = Path(session_dir).expanduser().resolve()
package_root = Path(__file__).resolve().parents[1]
config = (
package_root / "config" / "three_camera_calibration.yaml"
if config_file is None
else Path(config_file).expanduser().resolve()
)
parameters = _load_parameters(config)
start, records_by_joint, raw_path = _load_raw_session(session)
hand_model = str(start.get("hand_model", "G20")).upper()
side = str(start["hand_type"]).lower()
profile = get_hand_calibration_profile(side, hand_model)
zero_profile = get_zero_calibration_profile(side, hand_model)
baseline = tuple(int(value) for value in start["baseline_command_u8"])
if len(baseline) != 20:
raise ValueError("session baseline must contain exactly 20 commands")
if set(records_by_joint) != set(profile.measured_joints):
raise ValueError("raw session does not contain exactly the measured joint set")
source_urdf = Path(start["source_urdf_path"]).expanduser().resolve()
extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve()
if not source_urdf.is_file() or not extrinsics_file.is_file():
raise ValueError("session source URDF or camera extrinsics is missing")
if "zero_calibrated" in source_urdf.stem.lower():
raise ValueError("offline replay requires the original CAD URDF")
hand_serial = str(serial_number or session.parent.name)
output_suffix = _output_suffix(output_tag)
repetitions = int(parameters["repetitions"])
measured_fits = {
name: _fit_curve(
name,
records_by_joint[name],
profile=profile,
baseline=baseline,
)
for name in profile.measured_joints
}
training_fits = {
name: _fit_curve(
name,
[
record
for record in records_by_joint[name]
if int(record["cycle"]) in {0, 1}
],
profile=profile,
baseline=baseline,
)
for name in profile.measured_joints
}
axes = _fit_axes(
records_by_joint=records_by_joint,
profile=profile,
baseline=baseline,
extrinsics_file=extrinsics_file,
repetitions=repetitions,
)
failures = _quality_failures(
records_by_joint=records_by_joint,
profile=profile,
baseline=baseline,
fits=measured_fits,
axes=axes,
parameters=parameters,
)
if failures:
raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures))
holdout_by_joint = {
name: rotation_curve_holdout_errors(
training_fits[name],
[
record
for record in records_by_joint[name]
if int(record["cycle"]) == 2
],
zero_command_u8=int(
baseline[profile.joint_specs[name].motor_index]
),
)
for name in profile.measured_joints
}
trajectory_errors = np.abs(
np.asarray(
[value for values in holdout_by_joint.values() for value in values],
dtype=float,
)
)
maximum_validation_mae = math.radians(
float(parameters["maximum_validation_mae_deg"])
)
maximum_validation_p95 = math.radians(
float(parameters["maximum_validation_p95_deg"])
)
trajectory_mae = float(np.mean(trajectory_errors))
trajectory_p95 = float(np.percentile(trajectory_errors, 95.0))
if (
trajectory_mae > maximum_validation_mae
or trajectory_p95 > maximum_validation_p95
):
raise ValueError("third-cycle trajectory holdout failed")
motor_by_joint = {
name: int(spec.motor_index) for name, spec in profile.joint_specs.items()
}
joint_limits: dict[str, float] = {}
solve_arguments = {
"source_urdf": source_urdf,
"measurements": axes,
"motor_by_joint": motor_by_joint,
"maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])),
"finger_maximum_offset_rad": math.radians(
float(parameters.get("zero_finger_maximum_offset_deg", 3.0))
),
"joint_maximum_offset_rad": joint_limits,
"maximum_cycle_difference_rad": math.radians(
float(parameters["zero_maximum_axis_cycle_difference_deg"])
),
"maximum_axis_cone_mismatch_rad": math.radians(
float(parameters["zero_maximum_axis_cone_mismatch_deg"])
),
"maximum_pose_axis_line_rms_m": float(
parameters["axis_maximum_pose_line_rms_m"]
),
"maximum_validation_mae_rad": maximum_validation_mae,
"maximum_validation_p95_rad": maximum_validation_p95,
"hand_type": side,
"hand_model": hand_model,
}
holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments)
if not holdout_zero.passed:
failure = {
"reasons": dict(holdout_zero.failure_reasons),
"fitted_offsets_deg": {
name: math.degrees(value)
for name, value in holdout_zero.direct_offsets_rad.items()
},
"cycle_offsets_deg": {
name: [math.degrees(value) for value in values]
for name, values in holdout_zero.cycle_offsets_rad.items()
},
}
raise ValueError(
"third-cycle zero/URDF holdout failed: "
+ json.dumps(failure, ensure_ascii=False, sort_keys=True)
)
final_zero = solve_urdf_zero_offsets(curves=measured_fits, **solve_arguments)
if not final_zero.passed:
raise ValueError(
"all-cycle zero refit failed: "
+ json.dumps(
{
"reasons": dict(final_zero.failure_reasons),
"fitted_offsets_deg": {
name: math.degrees(value)
for name, value in final_zero.direct_offsets_rad.items()
},
},
ensure_ascii=False,
sort_keys=True,
)
)
for target, source_name in zero_profile.inherited_static_zero_joints.items():
if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]:
raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}")
for target in (
set(zero_profile.inherited_zero_joints)
- set(zero_profile.inherited_static_zero_joints)
):
if final_zero.all_active_offsets_rad[target] != 0.0:
raise ValueError(f"unobserved static zero must retain source CAD: {target}")
validation_errors = [
float(value) for values in holdout_by_joint.values() for value in values
]
validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad)
payload = build_compact_payload(
serial_number=hand_serial,
measured_fits=measured_fits,
urdf_zero_offsets_rad=final_zero.all_active_offsets_rad,
validation_errors_rad=validation_errors,
passed=True,
baseline=baseline,
side=side,
model=hand_model,
)
validate_compact_payload(payload)
stamp = session.name
final_json = session / (
f"{hand_model.lower()}_{side}_{hand_serial}_calibration"
f"{output_suffix}.json"
)
expected_urdf_name = (
f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}"
f"{output_suffix}.urdf"
)
final_urdf = source_urdf.parent / expected_urdf_name
report_path = session / (
f"{hand_model.lower()}_{side}_{hand_serial}_offline_validation"
f"{output_suffix}.json"
)
if write_outputs:
existing = [path for path in (final_json, final_urdf, report_path) if path.exists()]
if existing:
raise ValueError(
"refusing to overwrite replay outputs: "
+ ", ".join(str(path) for path in existing)
)
source_hash_before = _sha256(source_urdf)
with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary:
candidate = write_zero_corrected_urdf(
source_urdf=source_urdf,
output_directory=temporary,
serial_number=hand_serial,
offsets_rad=final_zero.all_active_offsets_rad,
timestamp=stamp,
)
urdf_checks = _validate_corrected_urdf(
source=source_urdf,
corrected=candidate,
offsets=final_zero.all_active_offsets_rad,
axes=axes,
curves=measured_fits,
motor_by_joint=motor_by_joint,
inherited_zero_joints=zero_profile.inherited_zero_joints,
)
residual_zero = solve_urdf_zero_offsets(
curves=measured_fits,
fixed_direct_zero_offsets_rad={
name: 0.0
for name in zero_profile.fixed_direct_zero_offsets_rad
},
static_output_zero_offsets_rad={
name: 0.0
for name in zero_profile.static_output_zero_offsets_rad
},
**{**solve_arguments, "source_urdf": candidate},
)
maximum_residual_offset = max(
abs(float(value)) for value in residual_zero.direct_offsets_rad.values()
)
if not residual_zero.passed or maximum_residual_offset > math.radians(0.3):
raise ValueError("written URDF retains a significant zero correction")
candidate_hash = _sha256(candidate)
if write_outputs:
os.replace(candidate, final_urdf)
if _sha256(source_urdf) != source_hash_before:
raise ValueError("source URDF changed during offline replay")
report: dict[str, Any] = {
"passed": True,
"session_dir": str(session),
"model": hand_model,
"side": side,
"serial_number": hand_serial,
"output_tag": output_tag,
"raw_samples_sha256": _sha256(raw_path),
"source_urdf": str(source_urdf),
"source_urdf_sha256": source_hash_before,
"joint_limits_deg": {
"finger_default": float(
parameters.get("zero_finger_maximum_offset_deg", 3.0)
),
"thumb_default": float(parameters["zero_maximum_offset_deg"]),
},
"static_zero_policy": "direct_measurements_only",
"fixed_zero_offsets_deg": {
name: math.degrees(value)
for name, value in zero_profile.fixed_direct_zero_offsets_rad.items()
},
"static_output_zero_offsets_deg": {
name: math.degrees(value)
for name, value in zero_profile.static_output_zero_offsets_rad.items()
},
"direct_offsets_deg": {
name: math.degrees(value)
for name, value in final_zero.direct_offsets_rad.items()
},
"cycle_offsets_deg": {
name: [math.degrees(value) for value in values]
for name, values in holdout_zero.cycle_offsets_rad.items()
},
"offset_uncertainty_deg": {
name: math.degrees(value)
for name, value in holdout_zero.offset_uncertainty_rad.items()
},
"trajectory_holdout_mae_deg": math.degrees(trajectory_mae),
"trajectory_holdout_p95_deg": math.degrees(trajectory_p95),
"zero_holdout_error_deg": {
name: math.degrees(value)
for name, value in holdout_zero.validation_error_by_joint_rad.items()
},
"zero_original_error_deg": {
name: math.degrees(value)
for name, value in holdout_zero.validation_original_error_by_joint_rad.items()
},
"zero_improvement_95pct_lower_deg": {
name: math.degrees(value)
for name, value in (
holdout_zero.validation_improvement_confidence_lower_rad.items()
)
},
"corrected_urdf_checks": {
**urdf_checks,
"maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset),
},
"corrected_urdf_sha256": candidate_hash,
"final_json": str(final_json) if write_outputs else None,
"corrected_urdf": str(final_urdf) if write_outputs else None,
}
if write_outputs:
atomic_write_json(final_json, payload)
report["final_json_sha256"] = _sha256(final_json)
if _sha256(final_urdf) != candidate_hash:
raise ValueError("formal corrected URDF differs from validated candidate")
atomic_write_json(report_path, report)
report["validation_report"] = str(report_path)
return report
def main() -> None:
parser = argparse.ArgumentParser(
description=(
"Replay and independently validate a complete supported-hand "
"calibration session."
)
)
parser.add_argument("session_dir")
parser.add_argument("--serial-number", default=None)
parser.add_argument("--config-file", default=None)
parser.add_argument("--write", action="store_true")
parser.add_argument(
"--output-tag",
default=None,
help="safe suffix for a replay variant; existing outputs are never overwritten",
)
arguments = parser.parse_args()
result = replay_session(
arguments.session_dir,
serial_number=arguments.serial_number,
config_file=arguments.config_file,
write_outputs=arguments.write,
output_tag=arguments.output_tag,
)
print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True))
if __name__ == "__main__":
main()
src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py
+22 -427
View File
@@ -477,11 +477,6 @@ 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.
@@ -502,18 +497,13 @@ 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,
*task_references,
)
for parent, child in (*pair_names, *normal_pair_names)
):
raise ValueError("geometry pairs must reference roles")
scales = (
@@ -523,8 +513,6 @@ 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")
@@ -535,8 +523,6 @@ 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]]] = []
@@ -592,16 +578,6 @@ 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")
@@ -652,9 +628,6 @@ 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
@@ -913,15 +886,11 @@ 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(
@@ -942,91 +911,28 @@ 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"
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:
usable_candidates: list[SquareTagPose] = []
for candidate in candidates:
normal = Rotation.from_quat(
candidate.quaternion_xyzw
).as_matrix()[:, 2]
tilt = math.acos(
float(np.clip(abs(normal[2]), 0.0, 1.0))
)
candidate_tilts_rad.append(float(tilt))
if tilt <= self.maximum_tag_tilt_rad:
independent_candidates.append(candidate)
# Candidate generation and candidate selection have different
# contracts. The per-Tag tilt limit protects a pose used without any
# other geometry, but it must not erase a finite, low-reprojection
# IPPE solution before SquareTagGroupPoseTracker can evaluate it
# against the fixed palm reference, the articulated chain and the
# preceding group pose. At a strongly oblique view the planar
# ambiguity is usually smaller, and rejecting both branches at a
# fixed angle caused deterministic mid-sweep holes despite continuous
# image detections. Group tracking therefore receives every
# reprojection-valid candidate; independent tracking below retains the
# original tilt safety gate.
if (
candidate.reprojection_error_px
<= self.maximum_reprojection_error_px
and tilt <= self.maximum_tag_tilt_rad
):
usable_candidates.append(candidate)
self.last_candidates_by_role[str(role)] = tuple(
reprojection_candidates
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:
if not usable_candidates:
return None, "no_pose_within_reprojection_or_tilt_limit"
previous_record = self._previous.get(str(role))
@@ -1038,7 +944,7 @@ class SquareTagPoseTracker:
previous = previous_pose
selected, reason = select_continuous_pose(
independent_candidates,
usable_candidates,
previous=previous,
maximum_reprojection_error_px=(
self.maximum_reprojection_error_px
@@ -1107,13 +1013,6 @@ 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(
@@ -1124,22 +1023,6 @@ 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(
@@ -1171,20 +1054,6 @@ 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,
@@ -1193,8 +1062,6 @@ 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")
@@ -1202,33 +1069,11 @@ 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
@@ -1238,214 +1083,28 @@ 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, *, preserve_task_reference: bool = False) -> None:
def reset(self) -> None:
self._previous.clear()
self._previous_stamp_ns = None
self._initial_candidates.clear()
self._initial_stamps_ns.clear()
self.last_initialization_quality.clear()
self.branch_correction_counts.clear()
self._decreasing_relative_rotations.clear()
self._coupled_reference_rotations.clear()
self.last_missing_roles = ()
if not preserve_task_reference:
self._task_reference_relative_poses.clear()
def _task_reference_cost(
self, combination: Mapping[str, SquareTagPose]
) -> float:
residual = 0.0
for pair, (expected_rotation, expected_translation) in (
self._task_reference_relative_poses.items()
):
rotation, translation = _relative_pose(
combination[pair[0]], combination[pair[1]]
)
residual += (
float((expected_rotation.inv() * rotation).magnitude())
/ self.return_reference_rotation_scale_rad
+ float(np.linalg.norm(translation - expected_translation))
/ self.relative_translation_scale_m
)
return residual
def _coupled_rotation_residuals(
self, combination: Mapping[str, SquareTagPose]
) -> tuple[float, ...]:
if not self.coupled_rotation_pairs:
return ()
residuals: list[float] = []
for (
driver_parent,
driver_child,
follower_parent,
follower_child,
multiplier,
) in self.coupled_rotation_pairs:
driver_pair = (driver_parent, driver_child)
follower_pair = (follower_parent, follower_child)
if (
driver_pair not in self._coupled_reference_rotations
or follower_pair not in self._coupled_reference_rotations
):
return ()
driver_rotation = _relative_pose(
combination[driver_parent], combination[driver_child]
)[0]
follower_rotation = _relative_pose(
combination[follower_parent], combination[follower_child]
)[0]
driver_travel = (
self._coupled_reference_rotations[driver_pair].inv()
* driver_rotation
).magnitude()
follower_travel = (
self._coupled_reference_rotations[follower_pair].inv()
* follower_rotation
).magnitude()
residuals.append(
abs(float(follower_travel) - multiplier * float(driver_travel))
)
return tuple(residuals)
def _informative_coupled_rotation_costs(
self,
combinations: Sequence[Mapping[str, SquareTagPose]],
) -> tuple[float, ...]:
"""Return branch costs only while the weak coupling prior is credible.
The URDF mimic ratio is useful for distinguishing two planar-IPPE
branches, but it is not measurement truth for a passive joint. Once
every otherwise viable combination disagrees with that ratio, using
it would bias the measured curve (and previously rejected every
frame). In that case fall back to visual continuity for this frame.
"""
residuals = tuple(
self._coupled_rotation_residuals(combination)
for combination in combinations
)
if not residuals or not any(residuals):
return tuple(0.0 for _ in combinations)
if (
self.maximum_coupled_rotation_residual_rad is not None
and not any(
values
and max(values)
<= self.maximum_coupled_rotation_residual_rad
for values in residuals
)
):
return tuple(0.0 for _ in combinations)
return tuple(
sum(values) / self.coupled_rotation_scale_rad
for values in residuals
)
def _return_reference(
self, command_u8: int | None
) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]:
if command_u8 is None or not self._decreasing_relative_rotations:
return {}
command = int(command_u8)
nearest = min(
self._decreasing_relative_rotations,
key=lambda candidate: abs(candidate - command),
)
if (
abs(nearest - command)
> self.return_reference_maximum_command_gap_u8
):
return {}
references = self._decreasing_relative_rotations[nearest]
commands = sorted(self._decreasing_relative_rotations)
axes: dict[tuple[str, str], np.ndarray | None] = {}
for pair in self.adjacent_pairs:
endpoint_delta = (
self._decreasing_relative_rotations[commands[-1]][pair].inv()
* self._decreasing_relative_rotations[commands[0]][pair]
).as_rotvec()
norm = float(np.linalg.norm(endpoint_delta))
axes[pair] = (
None
if norm < math.radians(5.0)
else endpoint_delta / norm
)
return {
pair: (rotation, axes[pair])
for pair, rotation in references.items()
}
def _return_reference_cost(
self,
combination: Mapping[str, SquareTagPose],
reference: Mapping[
tuple[str, str], tuple[Rotation, np.ndarray | None]
],
) -> float:
residual = 0.0
for pair, (expected, motion_axis) in reference.items():
vector = (
expected.inv()
* _relative_pose(
combination[pair[0]], combination[pair[1]]
)[0]
).as_rotvec()
if motion_axis is not None:
# The outbound trajectory identifies the physical one-DOF
# motion axis. Do not penalize return travel along that axis:
# it may contain real mechanical hysteresis that calibration
# must measure. A planar-IPPE mirror branch appears primarily
# as a large orthogonal tilt and is rejected by this residual.
vector = vector - motion_axis * float(vector @ motion_axis)
residual += float(np.linalg.norm(vector))
return residual / self.return_reference_rotation_scale_rad
def select(
self,
candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
*,
stamp_ns: int,
trajectory_command_u8: int | None = None,
trajectory_direction: str | None = None,
) -> tuple[dict[str, SquareTagPose] | None, str]:
"""Return one mutually consistent pose for every configured role."""
direction = (
None
if trajectory_direction is None
else str(trajectory_direction)
)
if direction not in {None, "decreasing", "increasing"}:
raise ValueError(
"trajectory_direction must be decreasing or increasing"
)
return_reference = (
self._return_reference(trajectory_command_u8)
if direction == "increasing"
else {}
)
candidate_lists = [
tuple(candidates_by_role.get(role, ()))
for role in self.roles
]
self.last_missing_roles = tuple(
role
for role, candidates in zip(self.roles, candidate_lists)
if not candidates
)
if self.last_missing_roles:
if any(not candidates for candidates in candidate_lists):
return None, "group_missing_pose_candidates"
self.last_missing_roles = ()
combinations = [
dict(zip(self.roles, combination))
@@ -1516,15 +1175,6 @@ 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]
@@ -1545,31 +1195,23 @@ class SquareTagGroupPoseTracker:
):
return None, "group_normal_alignment"
else:
coupling_costs = self._informative_coupled_rotation_costs(
combinations
)
selected = min(
zip(combinations, coupling_costs),
key=lambda item: (
combinations,
key=lambda combination: (
sum(
pose.reprojection_error_px
for pose in item[0].values()
for pose in combination.values()
)
/ self.reprojection_scale_px
+ sum(
_normal_alignment_rad(
item[0][first], item[0][second]
combination[first], combination[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(
@@ -1596,7 +1238,7 @@ class SquareTagGroupPoseTracker:
)
for pair in self.adjacent_pairs
}
base_scored: list[tuple[float, dict[str, SquareTagPose]]] = []
scored: list[tuple[float, dict[str, SquareTagPose]]] = []
for combination in combinations:
absolute_rotation_motion = 0.0
absolute_translation_motion = 0.0
@@ -1663,23 +1305,11 @@ class SquareTagGroupPoseTracker:
+ relative_translation_motion
/ self.relative_translation_scale_m
+ self.reprojection_weight * reprojection_penalty
+ self._return_reference_cost(
combination, return_reference
)
)
base_scored.append((float(score), combination))
scored.append((float(score), combination))
if not base_scored:
if not 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] = {}
@@ -1713,39 +1343,4 @@ 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/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py
View File
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py
+504
View File
@@ -0,0 +1,504 @@
"""Chinese, operator-facing diagnostics for three-camera calibration."""
from __future__ import annotations
import re
from typing import Any, Mapping
STATE_NAMES_ZH = {
"PREFLIGHT": "设备和标签预检",
"WAIT_START": "等待开始标定",
"RETURN_BASELINE": "正在恢复目标姿态",
"PREPARE_SWEEP": "正在到达扫描起点",
"SWEEP": "正在采集轨迹",
"FITTING": "正在拟合轨迹和零位",
"VALIDATION_MOVE": "正在移动到随机复测位置",
"VALIDATION_CAPTURE": "正在采集随机复测数据",
"PAUSED": "标定已暂停",
"ABORTED": "标定已终止",
"COMPLETE": "标定已完成",
}
VIEW_NAMES_ZH = {
"front": "正面",
"side": "侧面",
"top": "上面",
}
JOINT_NAMES_ZH = {
"thumb_cmc_pitch": "拇指CMC俯仰",
"thumb_cmc_roll": "拇指CMC滚转",
"thumb_mcp": "拇指MCP",
"thumb_ip": "拇指IP（被动）",
"index_mcp_roll": "食指MCP侧摆",
"index_mcp_pitch": "食指MCP屈伸",
"index_pip": "食指PIP",
"index_dip": "食指DIP（被动）",
"middle_mcp_roll": "中指MCP侧摆",
"middle_mcp_pitch": "中指MCP屈伸",
"middle_pip": "中指PIP",
"middle_dip": "中指DIP（被动）",
"ring_mcp_roll": "无名指MCP侧摆",
"ring_mcp_pitch": "无名指MCP屈伸",
"ring_pip": "无名指PIP",
"ring_dip": "无名指DIP（被动）",
"pinky_mcp_roll": "小指MCP侧摆",
"pinky_mcp_pitch": "小指MCP屈伸",
"pinky_pip": "小指PIP",
"pinky_dip": "小指DIP（被动）",
"thumb_cmc_yaw": "拇指CMC侧摆",
}
def _format_u8(value: Any) -> str:
if value is None:
return "尚无反馈"
return f"{float(value):.1f}"
def _task_text(active: Mapping[str, Any]) -> str:
if not active:
return "尚无活动任务"
view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
if active.get("kind") == "fit_failure":
joints = active.get("joints", [])
joint_text = "/".join(
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
)
return (
f"{view}机位，{joint_text}拟合检查失败，"
f"电机{active.get('motor_index')}，"
f"第{active.get('attempt', 1)}次尝试"
)
if active.get("kind") == "zero_model_failure":
joints = active.get("joints", [])
joint_text = "/".join(
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
)
return (
f"{view}机位，{joint_text}零位/URDF验证失败，"
f"电机{active.get('motor_index')}，不会自动重扫"
)
if active.get("kind") == "motion_stall":
return (
f"电机{active.get('motor_index', '?')}运动停滞，目标"
f"{_format_u8(active.get('target_u8'))}、实际"
f"{_format_u8(active.get('actual_u8'))}"
)
if active.get("kind") == "validation":
return (
f"{view}机位，随机复测，电机{active.get('motor_index')}，"
f"目标命令{active.get('command_u8')}"
)
joints = active.get("joints", [])
joint_text = "/".join(
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
)
start = active.get("start_u8")
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}"
)
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', '?')}次）"
)
return task
def three_camera_reason_zh(
state: str,
reason: str,
active: Mapping[str, Any],
) -> tuple[str, str]:
"""Translate a reason code and provide one concrete operator action."""
reason = str(reason)
sample = active.get("sample", {}) if active else {}
missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
sample_range = (
f"{_format_u8(sample.get('minimum_u8'))}～"
f"{_format_u8(sample.get('maximum_u8'))}"
)
tolerance = sample.get("endpoint_tolerance_u8", "?")
if reason.startswith("motor_state_stalled:"):
fields = reason.split(":")
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 "未知"
motor = active.get("motor_index")
if motor is not None:
return (
f"电机{motor}反馈连续8秒没有向目标推进；目标"
f"{_format_u8(active.get('target_u8'))}、实际"
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，"
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
f"（阶段={context}）。程序已保持当前位置。",
"若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后"
"重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。",
)
return (
f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8"
f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。",
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
"正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。",
)
if "URDF zero offset reached the configured" in reason:
bound_match = re.search(
r"configured\s+([0-9.]+)\s+degree bound", reason
)
bound = bound_match.group(1) if bound_match else "配置的"
hit_text = ""
if "bound:" in reason:
hit_text = reason.split("bound:", 1)[1].split(
"; all_offsets:", 1
)[0]
for name, label in JOINT_NAMES_ZH.items():
hit_text = hit_text.replace(name, label)
hit_suffix = f"；触边关节：{hit_text}" if hit_text else ""
return (
f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
"零位结果，而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
"不要调用resume，也不要增大零位边界。先确认Tag有效黑框边长、三相机"
"内外参和原始CAD URDF；Tag尺寸修正后必须调用start重新采集，旧尺度"
"产生的轨迹不能直接生成修正URDF。",
)
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"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为"
f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到"
"端点，但该时刻没有同时取得有效Tag图像和电机状态。",
"确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用"
"/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。",
)
if reason == "sweep_bins_too_few":
return (
f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
"检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
)
if reason == "sweep_bin_gap_too_large":
return (
f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
"检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
)
if reason == "synchronised_tag_state_timeout":
return (
"运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
"按时间戳配对”的图像帧。",
"查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
"程序会重扫当前方向。",
)
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_timeout":
return (
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
"检查电机实际值、Tag连续识别和标定速度，修正后调用resume。",
)
if reason == "return_baseline_timeout":
return (
"一个或多个标定电机未在规定时间返回基准命令。",
"检查机械手状态、CAN和机械卡阻，确认安全后调用resume。",
)
if reason == "validation_move_timeout":
return (
"随机复测时电机未在规定时间到达目标命令。",
"检查机械手状态和机械卡阻，确认安全后调用resume。",
)
if reason == "validation_capture_timeout":
return (
"随机复测位置没有采集到足够的同步有效Tag帧。",
"检查当前机位Tag可见性后调用resume。",
)
if reason == "joint_fit_check_failed":
metric_names = {
"plane_rms_mm": "平面拟合RMS",
"radial_rms_mm": "圆半径拟合RMS",
"radius_mm": "拟合半径",
"image_radial_rms_px": "二维圆半径拟合RMS",
"image_radial_p95_px": "二维圆半径误差P95",
"image_radius_px": "二维拟合半径",
"arc_deg": "实测圆弧",
"monotonic_correction_deg": "最大单调修正",
"hysteresis_deg": "最大正反程差",
"cycle_travel_range_deg": "三轮行程差",
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
"axis_plane_rms_mm": "三维圆轴向RMS",
"axis_radial_rms_mm": "三维圆半径RMS",
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
"axis_cycle_difference_deg": "三轮转轴方向极差",
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
"state_image_sync_p95_ms": "图像与电机状态同步误差P95",
"tag_valid_rate_percent": "所需Tag同时有效率",
}
metric_units = {
"plane_rms_mm": "mm",
"radial_rms_mm": "mm",
"radius_mm": "mm",
"image_radial_rms_px": "px",
"image_radial_p95_px": "px",
"image_radius_px": "px",
"arc_deg": "°",
"monotonic_correction_deg": "°",
"hysteresis_deg": "°",
"cycle_travel_range_deg": "°",
"rotation_orthogonal_rms_deg": "°",
"axis_plane_rms_mm": "mm",
"axis_radial_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": "°",
"state_image_sync_p95_ms": "ms",
"tag_valid_rate_percent": "%",
}
details: list[str] = []
for failure in active.get("failures", []):
joint = JOINT_NAMES_ZH.get(
str(failure.get("joint")), str(failure.get("joint"))
)
metric = str(failure.get("metric", ""))
if metric in metric_names:
comparison = str(failure.get("comparison", ""))
requirement = "不超过" if comparison == "maximum" else "至少"
unit = metric_units[metric]
detail = (
f"{joint}的{metric_names[metric]}为"
f"{float(failure.get('actual', 0.0)):.2f}{unit}，"
f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
)
cycle_travel = failure.get("cycle_travel_deg", [])
if cycle_travel:
detail += "（三轮=" + "/".join(
f"{float(value):.2f}°" for value in cycle_travel
) + "）"
details.append(detail)
else:
cycle = failure.get("cycle")
cycle_text = "" if cycle is None else f"第{cycle}轮"
details.append(
f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败："
f"{failure.get('reason', '未知原因')}"
)
detail_text = "；".join(details) or "当前关节的轨迹拟合未通过"
return (
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
"修正Tag位置、遮挡或机械行程后调用"
"/g20_calibration/resume；程序只清除当前失败关节的数据"
f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。",
)
if reason == "zero_model_validation_failed":
reason_names = {
"zero_offset_reached_configured_bound": "零位解触及安全边界",
"zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
"zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
"zero_offset_cycle_difference_too_large": "三轮零位离散过大",
"zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
"zero_axis_cone_mismatch_too_large": (
"父子轴夹角与原始URDF不一致，零位旋转无法解释"
),
"zero_phase_axis_line_residual_too_large": (
"整段SE(3)运动无法稳定确定平行轴线相位"
),
"zero_offset_did_not_improve_with_95pct_confidence": (
"第三轮留出验证未以95%置信度改善"
),
}
details: list[str] = []
for failure in active.get("failures", []):
joint = JOINT_NAMES_ZH.get(
str(failure.get("joint")), str(failure.get("joint"))
)
if failure.get("metric") == "zero_guard":
reason_text = reason_names.get(
str(failure.get("reason")), str(failure.get("reason"))
)
if "actual_deg" in failure and "limit_deg" in failure:
reason_text += (
f"（估计{float(failure['actual_deg']):+.2f}°，"
f"允许±{float(failure['limit_deg']):.2f}°）"
)
details.append(f"{joint}：{reason_text}")
return (
"轨迹采集已完成，但零位/URDF几何验证失败"
+ ("（" + "；".join(details) + "）" if details else "")
+ "。程序没有生成正式JSON或修正URDF。",
"该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；"
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
)
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
return (
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
"根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
)
if reason == "call_start":
return (
"三机位预检已经通过，等待操作员确认开始。",
"清空机械手运动范围后调用/g20_calibration/start。",
)
if reason == "operator_pause":
return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
if reason == "operator_abort":
return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
if reason == "collecting_timestamp_synchronised_tag_centres":
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。"
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("prepare_") or state == "PREPARE_SWEEP":
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
if state == "RETURN_BASELINE":
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
if state == "FITTING":
return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
"""Render the complete operator status; the JSON topic remains unchanged."""
state = str(payload.get("state", ""))
active = payload.get("active", {})
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",
0.0 if not total else float(completed) / float(total),
)
)
lines = [
f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）",
f"原因：{reason_zh}",
f"建议：{action_zh}",
f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，"
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)
label = "恢复姿态" if return_command != baseline_command else "基准姿态"
lines.append(f"正在确认{label}：{return_command}")
if active and active.get("kind") not in {
"fit_failure",
"zero_model_failure",
"motion_stall",
}:
retry_count = int(active.get("automatic_retry_count", 0))
if retry_count:
lines.append(
"自动重试：当前方向已自动重扫"
f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，"
f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，"
f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
)
sample = active.get("sample", {})
motion_progress = active.get("motion_progress")
motion_text = (
"未知" if motion_progress is None else f"{float(motion_progress):.1%}"
)
lines.append(
"运动采样："
f"目标{active.get('target_u8', active.get('command_u8', '?'))}，"
f"实际{_format_u8(active.get('actual_u8'))}，"
f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，"
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～"
f"{_format_u8(sample.get('maximum_u8'))}"
)
auxiliary = active.get("auxiliary_motors", [])
if auxiliary:
lines.append(
"避挡姿态："
+ "，".join(
f"电机{item.get('motor_index')}目标"
f"{item.get('command_u8')}、实际"
f"{_format_u8(item.get('actual_u8'))}"
for item in auxiliary
)
)
speed = active.get("speed", {})
if speed:
lines.append(
"阶段速度：五指目标"
f"{speed.get('commanded_finger_speed')}，SDK报告"
f"{speed.get('reported_finger_speed')}"
)
if active.get("sweep_timeout_seconds") is not None:
lines.append(
"运动保护：扫描超时"
f"{float(active['sweep_timeout_seconds']):.1f}s，"
"连续"
f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
"进展不足"
f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
"则立即暂停"
)
lines.append("机位：")
for name, view in payload.get("views", {}).items():
missing = view.get("missing_tag_ids", [])
missing_text = "无" if not missing else ",".join(map(str, missing))
lines.append(
f"- {VIEW_NAMES_ZH.get(str(name), str(name))}："
f"{'就绪' if view.get('ready') else '等待'}，"
f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'}，"
f"{float(view.get('detection_hz', 0.0)):.1f}Hz，"
f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，"
f"当前缺失Tag={missing_text}"
)
extrinsics_error = payload.get("camera_extrinsics_error")
if extrinsics_error:
lines.append(f"外参文件：{extrinsics_error}")
lines.append(f"JSON结果：{payload.get('result_path') or '尚未生成'}")
lines.append(
f"修正URDF：{payload.get('corrected_urdf_path') or '尚未生成'}"
)
return "\n".join(lines)
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py
+4179
View File
File diff suppressed because it is too large Load Diff
src/linkerhand_calibration/linkerhand_calibration/trajectory.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py
+4 -9
View File
@@ -634,7 +634,6 @@ 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
@@ -670,11 +669,9 @@ def _fit_joint_curve(
],
dtype=float,
)
if not preserve_direction_offset:
raw -= raw[-1]
raw -= raw[-1]
projected_samples = isotonic_nonincreasing(raw)
if not preserve_direction_offset:
projected_samples -= projected_samples[-1]
projected_samples -= projected_samples[-1]
maximum_correction = max(
maximum_correction,
float(np.max(np.abs(projected_samples - raw))),
@@ -684,8 +681,7 @@ def _fit_joint_curve(
commands.astype(float),
projected_samples,
)
if not preserve_direction_offset:
curve -= curve[255]
curve -= curve[255]
if endpoint_reference is not None:
curve = _regularize_coupled_zero_tail(
curve,
@@ -699,8 +695,7 @@ 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)
if not preserve_direction_offset:
combined -= combined[255]
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
+2061
View File
File diff suppressed because it is too large Load Diff
src/linkerhand_calibration/linkerhand_calibration/zero_calibration.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_calibration.py
View File
src/linkerhand_calibration/linkerhand_calibration/zero_node.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_node.py
+1 -5
View File
@@ -21,11 +21,7 @@ from sensor_msgs.msg import Image, JointState
from std_msgs.msg import String
from std_srvs.srv import Trigger
from .compat.legacy.thumb_core import (
BASELINE_COMMAND,
COMMAND_NAMES,
build_command,
)
from .core import BASELINE_COMMAND, build_command, COMMAND_NAMES
from .storage import atomic_write_json
from .zero_calibration import (
build_trajectory_zero_angle_payload,
src/linkerhand_calibration/launch/calibrated_joint_state_bridge.launch.py → src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py
+5 -6
View File
@@ -1,4 +1,4 @@
"""Publish profile-calibrated URDF angles from raw command/feedback u8 values."""
"""Publish calibrated URDF joint angles from raw u8 commands."""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
@@ -9,21 +9,20 @@ 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=""),
DeclareLaunchArgument("output_topic", default_value=""),
Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="calibrated_joint_state_bridge",
name=[
"calibrated_joint_state_bridge_",
LaunchConfiguration("hand_type"),
],
name="calibrated_joint_state_bridge",
output="screen",
emulate_tty=True,
parameters=[
{
"hand_model": LaunchConfiguration("hand_model"),
"hand_type": LaunchConfiguration("hand_type"),
"calibration_file": LaunchConfiguration(
"calibration_file"
src/linkerhand_calibration/launch/front_cmc_pitch_zero.launch.py → src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py
+3 -3
View File
@@ -48,7 +48,7 @@ def _launch_stack(context):
tag_config = LaunchConfiguration("tag_config").perform(context)
zero_config = LaunchConfiguration("zero_config").perform(context)
camera = Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -158,7 +158,7 @@ def _launch_stack(context):
)
zero_node = Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="cmc_pitch_zero_node",
name="g20_thumb_cmc_pitch_zero",
output="screen",
@@ -203,7 +203,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("linkerhand_calibration")
get_package_share_directory("g20_thumb_apriltag_calibration")
)
return LaunchDescription(
[
src/linkerhand_calibration/launch/front_cmc_roll_calibration.launch.py → src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py
+3 -3
View File
@@ -50,7 +50,7 @@ def _launch_stack(context):
"calibration_config"
).perform(context)
camera = Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -160,7 +160,7 @@ def _launch_stack(context):
)
calibration_node = Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="cmc_roll_calibration_node",
name="g20_thumb_cmc_roll_calibration",
output="screen",
@@ -205,7 +205,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("linkerhand_calibration")
get_package_share_directory("g20_thumb_apriltag_calibration")
)
return LaunchDescription(
[
src/linkerhand_calibration/launch/front_thumb_calibration.launch.py → src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py
+3 -3
View File
@@ -82,7 +82,7 @@ def _launch_stack(context):
)
camera = Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
@@ -234,7 +234,7 @@ def _launch_stack(context):
)
calibration = Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="calibration_node",
name="g20_thumb_calibration",
output="screen",
@@ -315,7 +315,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("linkerhand_calibration")
get_package_share_directory("g20_thumb_apriltag_calibration")
)
default_output = str(Path.cwd() / "calibration_output")
return LaunchDescription(
src/linkerhand_calibration/launch/three_camera_calibration.launch.py → src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py
+149 -166
View File
@@ -1,9 +1,8 @@
"""Launch three Hikrobot views and one registered hand calibration owner."""
"""Launch three Hikrobot views and one supported-hand calibration owner."""
from __future__ import annotations
from datetime import datetime
import hashlib
from pathlib import Path
import re
@@ -22,92 +21,72 @@ 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(model: str, hand_type: str) -> Path:
relative = (
Path("urdf") / "l6_right" / "linkerhand_l6v3.1_right.urdf"
if model.upper() == "L6" and hand_type == "right"
else Path("urdf")
/ f"{model.lower()}_{hand_type}"
/ f"linkerhand_{model.lower()}_{hand_type}.urdf"
)
package_source_or_share = Path(__file__).resolve().parents[1] / relative
try:
installed = (
Path(get_package_share_directory("linkerhand_calibration"))
/ relative
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",
)
relative = Path(
"assets/robots/hands/linker_hand"
) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative
try:
installed = Path(get_package_share_directory("linkerhand_retarget")) / relative
except Exception:
installed = package_source_or_share
return installed if installed.is_file() else package_source_or_share
installed = workspace
return workspace if workspace.is_file() else installed
def _launch_stack(context):
from linkerhand_calibration.product import (
get_product_calibration_contract,
)
model = LaunchConfiguration("model").perform(context).strip().upper()
hand_model = LaunchConfiguration("hand_model").perform(context).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:
contract = get_product_calibration_contract(
model, hand_type, tag_layout
)
profile = get_hand_calibration_profile(hand_type, hand_model)
except ValueError as error:
raise RuntimeError(str(error)) from error
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
package_share = Path(
get_package_share_directory("linkerhand_calibration")
)
tag_config = (
Path(requested_tag_config).expanduser().resolve()
if requested_tag_config
else package_share
/ "config"
/ (
"three_camera_tags_g20_right_19.yaml"
if tag_layout == "g20_right_19"
else "l6_right_8_tags.yaml"
if tag_layout == "l6_right_8"
else "three_camera_tags_g20_right_15.yaml"
if tag_layout == "g20_right_15"
else "three_camera_tags.yaml"
)
)
if not 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"
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_source = LaunchConfiguration("source_urdf_path").perform(context)
source_urdf = (
Path(requested_source).expanduser().resolve()
if requested_source
else _default_source_urdf(model, hand_type).resolve()
else _default_source_urdf(hand_model, 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.typed_profile.artifacts.publish_corrected_urdf:
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
raise RuntimeError(
"this profile 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 (
@@ -146,20 +125,19 @@ def _launch_stack(context):
raw_topics = []
info_topics = []
detection_topics = []
calibration_namespace = contract.typed_profile.namespace
for view in VIEWS:
namespace = f"{calibration_namespace}/{view}/camera"
raw_topic = f"{namespace}/image_raw"
info_topic = f"{namespace}/camera_info"
camera_info_topic = f"{namespace}/camera_info"
rect_topic = f"{namespace}/image_rect"
detector_namespace = f"{calibration_namespace}/{view}/apriltag"
detection_topic = f"{detector_namespace}/detections"
raw_topics.append(raw_topic)
info_topics.append(info_topic)
info_topics.append(camera_info_topic)
detection_topics.append(detection_topic)
cameras.append(
Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace=namespace,
@@ -176,7 +154,7 @@ def _launch_stack(context):
f"{view}_camera_name"
),
"frame_id": (
f"{model.lower()}_calibration_{view}_optical_frame"
f"{model_key}_calibration_{view}_optical_frame"
),
"image_width": 1624,
"image_height": 1240,
@@ -210,7 +188,7 @@ def _launch_stack(context):
namespace=namespace,
remappings=[
("image", raw_topic),
("camera_info", info_topic),
("camera_info", camera_info_topic),
("image_rect", rect_topic),
],
parameters=[{"queue_size": 1}],
@@ -222,7 +200,7 @@ def _launch_stack(context):
name="apriltag",
namespace=detector_namespace,
parameters=[
str(tag_config),
tag_config,
{
"detector.decimate": ParameterValue(
LaunchConfiguration("apriltag_decimate"),
@@ -232,7 +210,7 @@ def _launch_stack(context):
],
remappings=[
("image_rect", rect_topic),
("camera_info", info_topic),
("camera_info", camera_info_topic),
],
extra_arguments=[{"use_intra_process_comms": True}],
),
@@ -240,7 +218,7 @@ def _launch_stack(context):
)
vision = ComposableNodeContainer(
name=f"{model.lower()}_three_camera_vision",
name=f"{model_key}_three_camera_vision",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
@@ -248,64 +226,75 @@ def _launch_stack(context):
output="screen",
emulate_tty=True,
)
sdk = Node(
package="linker_hand_ros2_sdk",
executable="linker_hand_sdk",
name="linker_hand_sdk",
output="screen",
condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[
{
"hand_type": 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,
# G20 sends an endpoint and L6 streams a bounded trajectory.
# Keep polling the real motor state during either command path;
# otherwise the SDK republishes stale state and creates large
# command-unit holes in the trajectory bins.
"defer_state_reads_while_commanding": False,
"repeat_position_commands": False,
"is_touch": False,
}
],
)
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,
}
],
)
calibration = Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="three_camera_calibration_node",
name=f"{model.lower()}_calibration",
name=f"{model_key}_calibration",
output="screen",
emulate_tty=True,
arguments=[
"--profile-id",
contract.typed_profile.key.profile_id,
],
parameters=[
LaunchConfiguration("calibration_config"),
{
"serial_number": hand_serial,
"model": model,
"hand_model": hand_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
@@ -313,22 +302,12 @@ def _launch_stack(context):
"info_topic": f"{calibration_namespace}/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"
),
"camera_extrinsics_expected_sha256": LaunchConfiguration(
"camera_extrinsics_expected_sha256"
),
"calibration_config_expected_sha256": LaunchConfiguration(
"calibration_config_expected_sha256"
),
"tag_config_expected_sha256": LaunchConfiguration(
"tag_config_expected_sha256"
),
"corrected_urdf_output_dir": LaunchConfiguration(
"corrected_urdf_output_dir"
),
@@ -336,6 +315,14 @@ 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
),
@@ -350,12 +337,12 @@ def _launch_stack(context):
LaunchConfiguration("index_flex_calibration_speed"),
value_type=int,
),
"adaptive_formal_speed_enabled": ParameterValue(
LaunchConfiguration("adaptive_formal_speed_enabled"),
value_type=bool,
"o30_internal_speed_u8": ParameterValue(
LaunchConfiguration("o30_internal_speed_u8"), value_type=int
),
"cross_view_roll_diagnostic_finger": LaunchConfiguration(
"cross_view_roll_diagnostic_finger"
"o30_command_full_range_seconds": ParameterValue(
LaunchConfiguration("o30_command_full_range_seconds"),
value_type=float,
),
"validation_enabled": ParameterValue(
LaunchConfiguration("validation_enabled"), value_type=bool
@@ -382,7 +369,7 @@ def _launch_stack(context):
*detection_topics,
command_topic,
state_topic,
info_topic,
hand_info_topic,
f"{calibration_namespace}/status",
],
output="screen",
@@ -390,7 +377,7 @@ def _launch_stack(context):
return [
LogInfo(
msg=(
f"{model} {hand_type} {tag_layout} three-camera session: {session_dir}; "
f"{hand_model} {hand_type} three-camera session: {session_dir}; "
f"source_urdf={source_urdf}"
)
),
@@ -411,7 +398,7 @@ def _launch_stack(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("linkerhand_calibration")
get_package_share_directory("g20_thumb_apriltag_calibration")
)
info_root = Path.home() / ".ros" / "camera_info"
return LaunchDescription(
@@ -431,9 +418,8 @@ def generate_launch_description() -> LaunchDescription:
name="FASTRTPS_DEFAULT_PROFILES_FILE",
value=str(package_share / "config" / "fastdds_large_images.xml"),
),
DeclareLaunchArgument("model", default_value="G20"),
DeclareLaunchArgument("hand_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"
@@ -472,6 +458,11 @@ 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"
@@ -479,11 +470,9 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"index_flex_calibration_speed", default_value="10"
),
DeclareLaunchArgument("o30_internal_speed_u8", default_value="0"),
DeclareLaunchArgument(
"adaptive_formal_speed_enabled", default_value="true"
),
DeclareLaunchArgument(
"cross_view_roll_diagnostic_finger", default_value=""
"o30_command_full_range_seconds", default_value="6.0"
),
DeclareLaunchArgument("validation_enabled", default_value="false"),
DeclareLaunchArgument(
@@ -495,18 +484,6 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"source_urdf_path", default_value=""
),
DeclareLaunchArgument(
"source_urdf_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"camera_extrinsics_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"calibration_config_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"tag_config_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"corrected_urdf_output_dir", default_value=""
),
@@ -519,8 +496,6 @@ 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(
@@ -529,7 +504,15 @@ def generate_launch_description() -> LaunchDescription:
),
DeclareLaunchArgument(
"tag_config",
default_value="",
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"
),
),
OpaqueFunction(function=_launch_stack),
]
src/linkerhand_calibration/launch/three_camera_extrinsics.launch.py → src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py
+3 -3
View File
@@ -26,7 +26,7 @@ def _launch(context):
namespace = f"/g20_extrinsics/{view}/camera"
cameras.append(
Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace=namespace,
@@ -85,7 +85,7 @@ def _launch(context):
output="screen",
)
solver = Node(
package="linkerhand_calibration",
package="g20_thumb_apriltag_calibration",
executable="three_camera_extrinsics_node",
name="g20_camera_extrinsics",
output="screen",
@@ -138,7 +138,7 @@ def _launch(context):
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("linkerhand_calibration")
get_package_share_directory("g20_thumb_apriltag_calibration")
)
camera_info = Path.home() / ".ros" / "camera_info"
return LaunchDescription(
src/linkerhand_calibration/package.xml → src/g20_thumb_apriltag_calibration/package.xml
+3 -2
View File
@@ -1,9 +1,9 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>linkerhand_calibration</name>
<name>g20_thumb_apriltag_calibration</name>
<version>0.1.0</version>
<description>Profile-driven hand calibration and validated URDF correction.</description>
<description>Three-view Hikrobot AprilTag calibration for G20 hands and the O30 right hand.</description>
<maintainer email="support@linker-robotics.com">lxp</maintainer>
<license>MIT</license>
@@ -15,6 +15,7 @@
<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/linkerhand_calibration/pyproject.toml → src/g20_thumb_apriltag_calibration/pyproject.toml
View File
src/linkerhand_calibration/resource/linkerhand_calibration → src/g20_thumb_apriltag_calibration/resource/g20_thumb_apriltag_calibration
View File
src/g20_thumb_apriltag_calibration/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/g20_thumb_apriltag_calibration
[install]
install_scripts=$base/lib/g20_thumb_apriltag_calibration
src/g20_thumb_apriltag_calibration/setup.py
+69
View File
@@ -0,0 +1,69 @@
from glob import glob
from setuptools import find_packages, setup
package_name = "g20_thumb_apriltag_calibration"
setup(
name=package_name,
version="0.1.0",
packages=find_packages(),
data_files=[
(
"share/ament_index/resource_index/packages",
["resource/" + package_name],
),
("share/" + package_name, ["package.xml", "README.md"]),
(
"share/" + package_name + "/config",
glob("config/*.yaml") + glob("config/*.xml"),
),
("share/" + package_name + "/launch", glob("launch/*.launch.py")),
],
install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
tests_require=["pytest"],
zip_safe=True,
maintainer="lxp",
maintainer_email="support@linker-robotics.com",
description="Three-view AprilTag calibration for G20 and O30 hands",
license="MIT",
entry_points={
"console_scripts": [
(
"hikrobot_camera_node = "
"g20_thumb_apriltag_calibration.hikrobot_camera:main"
),
"calibration_node = g20_thumb_apriltag_calibration.node:main",
(
"cmc_pitch_zero_node = "
"g20_thumb_apriltag_calibration.zero_node:main"
),
(
"cmc_roll_calibration_node = "
"g20_thumb_apriltag_calibration.zero_node:main"
),
(
"three_camera_calibration_node = "
"g20_thumb_apriltag_calibration.three_camera_node:main"
),
(
"three_camera_extrinsics_node = "
"g20_thumb_apriltag_calibration.extrinsics_node:main"
),
(
"offline_replay = "
"g20_thumb_apriltag_calibration.offline_replay:main"
),
(
"camera_alignment_view = "
"g20_thumb_apriltag_calibration.alignment_view:main"
),
(
"calibrated_joint_state_bridge = "
"g20_thumb_apriltag_calibration."
"calibrated_joint_state_bridge:main"
),
],
},
)
src/linkerhand_calibration/test/test_acquisition.py → src/g20_thumb_apriltag_calibration/test/test_acquisition.py
+2 -2
View File
@@ -6,7 +6,7 @@ import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from linkerhand_calibration.acquisition import (
from g20_thumb_apriltag_calibration.acquisition import (
ContinuousSweepCollector,
Observation,
PointCollector,
@@ -19,7 +19,7 @@ from linkerhand_calibration.acquisition import (
tag_quality_is_valid,
update_pnp_reset_watchdog,
)
from linkerhand_calibration.acquisition import PAIR_NAMES
from g20_thumb_apriltag_calibration.core import PAIR_NAMES
def test_pnp_watchdog_resets_after_one_continuous_invalid_second() -> None:
src/linkerhand_calibration/test/test_alignment_view.py → src/g20_thumb_apriltag_calibration/test/test_alignment_view.py
+1 -1
View File
@@ -2,7 +2,7 @@
import pytest
from linkerhand_calibration.alignment_view import (
from g20_thumb_apriltag_calibration.alignment_view import (
summarize_alignment_measurements,
)
src/linkerhand_calibration/test/test_calibrated_joint_state_bridge.py → src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py
+32 -78
View File
@@ -2,20 +2,18 @@ import copy
import pytest
from linkerhand_calibration.calibrated_joint_state_bridge import (
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 linkerhand_calibration.full_hand import (
from g20_thumb_apriltag_calibration.full_hand import (
JointCurveFit,
build_compact_payload,
get_hand_calibration_profile,
)
from linkerhand_calibration.urdf_zero import (
get_zero_calibration_profile,
)
def _payload(side: str = "right") -> dict:
@@ -93,85 +91,41 @@ def test_mapper_rejects_incomplete_named_command() -> None:
mapper.map_positions([255.0], ["thumb_cmc_pitch"])
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)
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={},
)
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="TEST_RIGHT_V5",
measured_fits=fits,
serial_number="O30_RIGHT_TEST",
measured_fits={name: fit_for(name) for name in profile.measured_joints},
urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
validation_errors_rad=[0.0],
passed=True,
baseline=baseline,
side="right",
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": {},
},
model="O30",
)
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.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"
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
)
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/linkerhand_calibration/test/test_config.py → src/g20_thumb_apriltag_calibration/test/test_config.py
+46 -103
View File
@@ -7,24 +7,6 @@ import yaml
PACKAGE_ROOT = Path(__file__).resolve().parents[1]
def test_package_owns_both_g20_source_urdfs_and_their_meshes() -> None:
for side in ("left", "right"):
urdf = (
PACKAGE_ROOT
/ "urdf"
/ f"g20_{side}"
/ f"linkerhand_g20_{side}.urdf"
)
assert urdf.is_file()
root = ElementTree.parse(urdf).getroot()
mesh_paths = {
mesh.attrib["filename"] for mesh in root.findall(".//mesh")
}
assert mesh_paths
assert all(not Path(path).is_absolute() for path in mesh_paths)
assert all((urdf.parent / path).is_file() for path in mesh_paths)
def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None:
root = ElementTree.parse(
PACKAGE_ROOT / "config" / "fastdds_large_images.xml"
@@ -74,71 +56,52 @@ 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],
}
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"
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"
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_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_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["g20_calibration"]["ros__parameters"]
parameters = config["/**"]["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,
@@ -166,52 +129,40 @@ 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["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["o30_internal_speed_u8"] == 0
assert parameters["o30_command_full_range_seconds"] == 6.0
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"] == 2
assert parameters["automatic_sweep_retry_limit"] == 3
assert parameters["automatic_fit_retry_limit"] == 2
assert parameters["motor_stall_timeout_seconds"] == 2.0
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
assert parameters["motor_stall_timeout_seconds"] >= 5.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]
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0]
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["position_timeout_seconds"] >= 20.0
assert parameters["maximum_state_image_skew_ms"] <= 50.0
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
@@ -221,23 +172,15 @@ 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["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
assert parameters["passive_maximum_hysteresis_deg"] <= 7.5
for view in ("front", "side", "top"):
assert parameters[f"{view}_camera_info_topic"].startswith(
f"/g20_calibration/{view}/"
src/linkerhand_calibration/test/test_core.py → src/g20_thumb_apriltag_calibration/test/test_core.py
+1 -1
View File
@@ -6,7 +6,7 @@ import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from linkerhand_calibration.compat.legacy.thumb_core import (
from g20_thumb_apriltag_calibration.core import (
BASELINE_COMMAND,
DIRECTION_DECREASING,
DIRECTION_INCREASING,
src/linkerhand_calibration/test/test_diagnostics.py → src/g20_thumb_apriltag_calibration/test/test_diagnostics.py
+2 -2
View File
@@ -1,5 +1,5 @@
from linkerhand_calibration.acquisition import TagQuality
from linkerhand_calibration.diagnostics import (
from g20_thumb_apriltag_calibration.acquisition import TagQuality
from g20_thumb_apriltag_calibration.diagnostics import (
build_tag_quality_diagnostics,
render_status_text_zh,
status_guidance_zh,
src/linkerhand_calibration/test/test_extrinsics_node.py → src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py
+2 -2
View File
@@ -4,8 +4,8 @@ import cv2
import numpy as np
from scipy.spatial.transform import Rotation
import linkerhand_calibration.extrinsics_node as extrinsics_node
from linkerhand_calibration.extrinsics_node import (
import g20_thumb_apriltag_calibration.extrinsics_node as extrinsics_node
from g20_thumb_apriltag_calibration.extrinsics_node import (
BoardPose,
StereoCapture,
_fit_stereo_robust,
src/g20_thumb_apriltag_calibration/test/test_full_hand.py
+500
View File
@@ -0,0 +1,500 @@
import math
from dataclasses import replace
import numpy as np
import pytest
from g20_thumb_apriltag_calibration.full_hand import (
ACTIVE_JOINTS,
IMAGE_TRAJECTORY_JOINTS,
JOINT_SPECS,
LEFT_HAND_PROFILE,
MEASURED_JOINTS,
PASSIVE_JOINTS,
RIGHT_HAND_PROFILE,
SPLAY_JOINTS,
SWEEP_SPECS,
VIEW_TAGS,
build_calibration_motion_command,
build_calibration_speed_profile,
build_compact_payload,
build_full_hand_command,
center_splay_curve,
fit_joint_center_curve,
fit_joint_image_curve,
fit_measured_joint_curve,
fit_projected_zero,
get_hand_calibration_profile,
measure_joint_observation,
validate_compact_payload,
)
def _records() -> list[dict[str, object]]:
commands = list(range(0, 256, 16))
if commands[-1] != 255:
commands.append(255)
records: list[dict[str, object]] = []
centre = np.asarray([0.006, -0.004, 0.012])
radius = 0.025
image_centre = np.asarray([30.0, -12.0])
image_radius = 100.0
for cycle in range(3):
for direction, sequence in (
("decreasing", reversed(commands)),
("increasing", commands),
):
for command in sequence:
angle = 0.70 * (255.0 - command) / 255.0
point = centre + np.asarray(
[radius * math.cos(angle), radius * math.sin(angle), 0.0]
)
# At command 255 the inward vector points along image +x, so
# table_projected_zero_rad is exactly zero.
image_point = image_centre + np.asarray(
[
-image_radius * math.cos(angle),
image_radius * math.sin(angle),
]
)
records.append(
{
"cycle": cycle,
"direction": direction,
"command_u8": command,
"relative_translation_xyz_m": point.tolist(),
"image_relative_xy_px": image_point.tolist(),
}
)
return records
def test_joint_layout_covers_16_active_and_5_passive_joints() -> None:
assert len(JOINT_SPECS) == 21
assert len(ACTIVE_JOINTS) == 16
assert len(PASSIVE_JOINTS) == 5
def test_right_profile_measures_pinky_and_inherits_to_other_fingers() -> None:
profile = get_hand_calibration_profile("right")
assert profile is RIGHT_HAND_PROFILE
assert profile.reference_finger == "pinky"
assert [spec.motor_index for spec in profile.sweep_specs] == [
0, 5, 15, 9, 4, 19, 10
]
assert profile.view_tags["front"]["pinky_roll"] == 10
assert profile.view_tags["side"] == {
"side_base": 4,
"pinky_mcp": 5,
"pinky_pip": 6,
"pinky_dip": 7,
}
assert profile.preflight_view_roles["side"] == (
"side_base",
"pinky_mcp",
"pinky_pip",
"pinky_dip",
)
thumb_pitch = profile.joint_specs["thumb_cmc_pitch"]
assert thumb_pitch.view == "front"
assert thumb_pitch.parent_role == "front_base"
assert thumb_pitch.child_role == "thumb_cmc"
assert profile.joint_specs["index_mcp_roll"].source_joint == (
"pinky_mcp_roll"
)
assert profile.joint_specs["middle_mcp_pitch"].source_joint == (
"pinky_mcp_pitch"
)
assert profile.joint_specs["ring_pip"].source_joint == "pinky_pip"
assert profile.joint_specs["index_dip"].source_joint == "pinky_dip"
roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 9)
command = build_calibration_motion_command(
roll, 127, profile=profile
)
assert command[9] == 127
assert command[6:9] == [255, 255, 255]
speeds = build_calibration_speed_profile(
roll,
normal_speed=15,
index_roll_speed=5,
index_flex_speed=10,
profile=profile,
)
assert speeds == [15, 15, 15, 15, 5]
assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set(
range(11)
)
assert VIEW_TAGS["front"]["index_roll"] == 10
assert VIEW_TAGS["side"] == {
"side_base": 4,
"index_mcp": 5,
"index_pip": 6,
"index_dip": 7,
}
assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9}
assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10]
def test_joint_trajectory_spaces_match_observation_geometry() -> None:
assert IMAGE_TRAJECTORY_JOINTS == {
"thumb_cmc_pitch",
"thumb_cmc_roll",
"thumb_mcp",
"thumb_ip",
"index_mcp_roll",
"index_mcp_pitch",
"index_pip",
}
records = _records()
for name in IMAGE_TRAJECTORY_JOINTS:
fit = fit_measured_joint_curve(name, records)
assert fit.circle["space"] == "image_2d"
for name in ("index_dip", "thumb_cmc_yaw"):
assert "space" not in fit_measured_joint_curve(name, records).circle
def test_full_hand_command_changes_exactly_one_controlled_motor() -> None:
result = build_full_hand_command(6, 27)
assert result[6] == 27
assert result[:6] == [255] * 6
with pytest.raises(ValueError, match="controlled"):
build_full_hand_command(11, 27)
def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
baseline = [255] * 20
baseline[6:10] = [127, 127, 127, 127]
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
result = build_calibration_motion_command(index_roll, 27, baseline)
assert result[6:10] == [27, 0, 0, 0]
assert result[:6] == baseline[:6]
assert result[10:] == baseline[10:]
def test_right_pinky_roll_moves_other_three_fingers_camera_right() -> None:
profile = RIGHT_HAND_PROFILE
pinky_roll = next(
spec for spec in profile.sweep_specs if spec.motor_index == 9
)
result = build_calibration_motion_command(
pinky_roll, 27, profile=profile
)
assert result[6:10] == [255, 255, 255, 27]
assert profile.roll_clearance_commands == {6: 255, 7: 255, 8: 255}
def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
baseline = [255] * 20
baseline[6:10] = [127, 127, 127, 127]
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
result = build_calibration_motion_command(thumb_pitch, 17, baseline)
assert result[0] == 17
assert result[6:10] == [127, 127, 127, 127]
def test_right_thumb_pitch_uses_front_visible_yaw_and_roll_pose() -> None:
profile = RIGHT_HAND_PROFILE
thumb_pitch = next(
spec for spec in profile.sweep_specs if spec.motor_index == 0
)
result = build_calibration_motion_command(
thumb_pitch, 17, profile=profile
)
assert result[0] == 17
assert result[10] == 255
assert result[5] == 255
assert profile.thumb_pitch_clearance_commands == {10: 255, 5: 255}
def test_left_thumb_pitch_keeps_legacy_baseline_pose() -> None:
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
result = build_calibration_motion_command(thumb_pitch, 17)
assert result[0] == 17
assert result[5] == 255
assert result[10] == 255
assert LEFT_HAND_PROFILE.thumb_pitch_clearance_commands == {}
def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None:
baseline = [255] * 20
baseline[6:10] = [127, 127, 127, 127]
thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10)
result = build_calibration_motion_command(thumb_yaw, 27, baseline)
assert result[5] == 145
assert result[10] == 27
assert result[:5] == baseline[:5]
assert result[6:10] == baseline[6:10]
assert result[11:] == baseline[11:]
def test_only_index_roll_uses_the_slow_index_finger_speed() -> None:
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1)
assert build_calibration_speed_profile(
index_roll,
normal_speed=15,
index_roll_speed=5,
index_flex_speed=10,
) == [15, 5, 15, 15, 15]
assert build_calibration_speed_profile(
index_pitch,
normal_speed=15,
index_roll_speed=5,
index_flex_speed=10,
) == [15, 10, 15, 15, 15]
index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16)
assert build_calibration_speed_profile(
index_pip,
normal_speed=15,
index_roll_speed=5,
index_flex_speed=10,
) == [15, 10, 15, 15, 15]
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
assert build_calibration_speed_profile(
thumb_pitch,
normal_speed=15,
index_roll_speed=5,
index_flex_speed=10,
) == [15, 15, 15, 15, 15]
def test_calibration_speed_profile_rejects_out_of_range_values() -> None:
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
with pytest.raises(ValueError, match="speeds"):
build_calibration_speed_profile(
index_roll,
normal_speed=15,
index_roll_speed=256,
index_flex_speed=10,
)
def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None:
records = _records()
fit = fit_joint_center_curve(records)
assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8)
assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4)
assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8)
assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6)
def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None:
records = _records()
for record in records:
command = int(record["command_u8"])
record["relative_translation_xyz_m"] = [
0.001 * command,
0.0,
0.0,
]
fit = fit_measured_joint_curve("thumb_mcp", records)
assert fit.circle["space"] == "image_2d"
assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02)
assert fit.angle_rad[255] == pytest.approx(0.0)
assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01
assert math.degrees(fit.maximum_hysteresis_rad) < 0.01
command_zero = next(
record for record in records if int(record["command_u8"]) == 0
)
observed = measure_joint_observation(
fit,
vector_xyz_m=command_zero["relative_translation_xyz_m"],
image_vector_xy_px=command_zero["image_relative_xy_px"],
)
assert observed == pytest.approx(0.70, abs=0.02)
def test_image_curve_rejects_insufficient_projected_arc() -> None:
records = _records()
for record in records:
command = float(record["command_u8"])
angle = math.radians(2.0) * (255.0 - command) / 255.0
record["image_relative_xy_px"] = [
100.0 * math.cos(angle),
100.0 * math.sin(angle),
]
with pytest.raises(
ValueError, match="joint_image_trajectory_quality_failed:arc"
):
fit_joint_image_curve(records)
def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None:
fit = fit_joint_center_curve(_records())
centred, zero_command, midpoint = center_splay_curve(fit)
assert midpoint == pytest.approx(0.35, abs=1.0e-4)
assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4)
assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4)
assert zero_command in {127, 128}
assert abs(centred.angle_rad[zero_command]) <= 0.002
def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
base = fit_joint_center_curve(_records())
splay, zero_command, _ = center_splay_curve(base)
splay = replace(
splay,
angle_rad=tuple(
value - splay.angle_rad[zero_command]
for value in splay.angle_rad
),
)
measured = {
name: splay if name == "index_mcp_roll" else base
for name in MEASURED_JOINTS
}
offsets = {name: 0.01 for name in ACTIVE_JOINTS}
baseline = [255] * 20
baseline[6:10] = [zero_command] * 4
payload = build_compact_payload(
serial_number="G20_LEFT_001",
measured_fits=measured,
urdf_zero_offsets_rad=offsets,
validation_errors_rad=[0.01, -0.02],
passed=True,
baseline=baseline,
)
validate_compact_payload(payload)
assert set(payload) == {
"schema_version",
"model",
"side",
"serial_number",
"angle_unit",
"command_range",
"baseline_command_u8",
"joints",
"quality",
}
assert len(payload["joints"]) == 21
assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values())
assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16
assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5
assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll"
assert (
payload["joints"]["middle_mcp_roll"]["angle_rad"]
== payload["joints"]["index_mcp_roll"]["angle_rad"]
)
assert payload["joints"]["index_mcp_roll"]["zero_angles"] == {
"urdf_zero_offset_rad": pytest.approx(0.01)
}
for name in SPLAY_JOINTS:
assert payload["joints"][name]["zero_command_u8"] == zero_command
def test_compact_payload_allows_skipped_random_validation() -> None:
base = fit_joint_center_curve(_records())
splay, zero_command, _ = center_splay_curve(base)
splay = replace(
splay,
angle_rad=tuple(
value - splay.angle_rad[zero_command]
for value in splay.angle_rad
),
)
measured = {
name: splay if name == "index_mcp_roll" else base
for name in MEASURED_JOINTS
}
offsets = {name: 0.0 for name in ACTIVE_JOINTS}
baseline = [255] * 20
baseline[6:10] = [zero_command] * 4
payload = build_compact_payload(
serial_number="G20_LEFT_001",
measured_fits=measured,
urdf_zero_offsets_rad=offsets,
validation_errors_rad=[],
passed=True,
baseline=baseline,
)
validate_compact_payload(payload)
assert payload["quality"] == {
"passed": True,
"validation_mae_rad": None,
"validation_p95_rad": None,
}
def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None:
profile = RIGHT_HAND_PROFILE
base = fit_joint_center_curve(_records())
splay, zero_command, _ = center_splay_curve(base)
splay = replace(
splay,
angle_rad=tuple(
value - splay.angle_rad[zero_command]
for value in splay.angle_rad
),
)
measured = {
name: splay if name == "pinky_mcp_roll" else base
for name in profile.measured_joints
}
offsets = {
name: (0.01 if profile.joint_specs[name].source_joint is None else 0.0)
for name in profile.active_joints
}
baseline = [255] * 20
baseline[6:10] = [zero_command] * 4
payload = build_compact_payload(
serial_number="G20_RIGHT_001",
measured_fits=measured,
urdf_zero_offsets_rad=offsets,
validation_errors_rad=[0.01],
passed=True,
baseline=baseline,
side="right",
)
validate_compact_payload(payload)
assert payload["schema_version"] == 4
assert payload["side"] == "right"
assert set(payload) == {
"schema_version",
"model",
"side",
"serial_number",
"angle_unit",
"command_range",
"baseline_command_u8",
"joints",
"quality",
}
for finger in ("index", "middle", "ring"):
assert payload["joints"][f"{finger}_mcp_roll"]["source_joint"] == (
"pinky_mcp_roll"
)
assert payload["joints"][f"{finger}_mcp_pitch"]["source_joint"] == (
"pinky_mcp_pitch"
)
assert payload["joints"][f"{finger}_pip"]["source_joint"] == (
"pinky_pip"
)
for suffix in ("mcp_roll", "mcp_pitch", "pip"):
joint = payload["joints"][f"{finger}_{suffix}"]
source = payload["joints"][joint["source_joint"]]
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}
src/linkerhand_calibration/test/test_hikrobot_camera.py → src/g20_thumb_apriltag_calibration/test/test_hikrobot_camera.py
+1 -1
View File
@@ -3,7 +3,7 @@ from pathlib import Path
import pytest
import yaml
from linkerhand_calibration.hikrobot_camera import (
from g20_thumb_apriltag_calibration.hikrobot_camera import (
DeviceDescriptor,
decode_c_string,
load_camera_calibration,
src/g20_thumb_apriltag_calibration/test/test_o30_support.py
+263
View File
@@ -0,0 +1,263 @@
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
+44
View File
@@ -0,0 +1,44 @@
import pytest
from g20_thumb_apriltag_calibration.offline_replay import (
_latest_attempt_records,
_output_suffix,
)
def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict:
return {
"kind": "sample",
"joint": joint,
"cycle": cycle,
"direction": direction,
"attempt": attempt,
}
def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
rows = [
{"kind": "session_start"},
_sample("pinky_pip", 0, "decreasing", 1),
_sample("pinky_pip", 0, "decreasing", 3),
_sample("pinky_pip", 0, "increasing", 1),
_sample("pinky_pip", 1, "decreasing", 2),
_sample("thumb_cmc_yaw", 0, "decreasing", 1),
]
selected = _latest_attempt_records(rows)
assert [
record["attempt"] for record in selected["pinky_pip"]
] == [3, 1, 2]
assert [
record["attempt"] for record in selected["thumb_cmc_yaw"]
] == [1]
def test_output_suffix_is_safe_and_explicit() -> None:
assert _output_suffix(None) == ""
assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
with pytest.raises(ValueError, match="output tag"):
_output_suffix(invalid)
src/linkerhand_calibration/test/test_pnp.py → src/g20_thumb_apriltag_calibration/test/test_pnp.py
+1 -251
View File
@@ -5,7 +5,7 @@ import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from linkerhand_calibration.pnp import (
from g20_thumb_apriltag_calibration.pnp import (
SquareTagGroupPoseTracker,
SquareTagPose,
SquareTagPoseTracker,
@@ -234,147 +234,6 @@ def _pose(
)
def test_group_tracker_receives_oblique_reprojection_valid_candidates() -> None:
per_tag = SquareTagPoseTracker(
maximum_reprojection_error_px=1.5,
reprojection_tie_px=1.5,
maximum_pose_jump_rad=np.deg2rad(35.0),
maximum_translation_jump_m=0.04,
maximum_tag_tilt_rad=np.deg2rad(75.0),
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"),
@@ -465,58 +324,6 @@ 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"),
@@ -565,63 +372,6 @@ 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/linkerhand_calibration/test/test_storage.py → src/g20_thumb_apriltag_calibration/test/test_storage.py
+1 -1
View File
@@ -1,4 +1,4 @@
from linkerhand_calibration.storage import (
from g20_thumb_apriltag_calibration.storage import (
append_jsonl,
atomic_write_json,
completed_scan_keys,
src/linkerhand_calibration/test/test_three_camera_diagnostics.py → src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py
+37 -328
View File
@@ -2,7 +2,7 @@ import json
import numpy as np
from linkerhand_calibration.three_camera_diagnostics import (
from g20_thumb_apriltag_calibration.three_camera_diagnostics import (
_task_text,
render_three_camera_status_text_zh,
three_camera_reason_zh,
@@ -74,137 +74,6 @@ 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",
@@ -268,171 +137,6 @@ 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",
@@ -471,7 +175,7 @@ def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
assert "运动采样：" not in text
def test_sweep_status_shows_localized_fit_retry_cycle() -> None:
def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
active = {
"kind": "sweep",
"view": "front",
@@ -483,10 +187,42 @@ def test_sweep_status_shows_localized_fit_retry_cycle() -> None:
"target_u8": 0,
"fit_attempt": 2,
"fit_attempt_limit": 3,
"fit_retry_cycles": [2],
}
assert "补采异常轮2" in _task_text(active)
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
def test_motor_stall_reason_is_explained_in_chinese() -> None:
@@ -507,7 +243,6 @@ 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,
@@ -522,7 +257,6 @@ 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": "",
@@ -530,7 +264,7 @@ def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
text = render_three_camera_status_text_zh(payload)
assert "电机10反馈连续2秒" in text
assert "电机10反馈连续8秒" 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
@@ -670,28 +404,3 @@ 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
+1249
View File
File diff suppressed because it is too large Load Diff
src/linkerhand_calibration/test/test_trajectory.py → src/g20_thumb_apriltag_calibration/test/test_trajectory.py
+4 -6
View File
@@ -4,19 +4,17 @@ import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from linkerhand_calibration.core import (
from g20_thumb_apriltag_calibration.core import (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
PHASE_ROOT,
PHASE_TIP,
)
from linkerhand_calibration.compat.legacy.thumb_core import (
PAIR_IP,
PAIR_MCP,
PAIR_ROOT,
PHASE_ROOT,
PHASE_TIP,
create_final_payload,
)
from linkerhand_calibration.trajectory import (
from g20_thumb_apriltag_calibration.trajectory import (
_regularize_coupled_zero_tail,
fit_center_trajectory_curves,
maximum_center_non_target_drift_rad,
src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py
+977
View File
@@ -0,0 +1,977 @@
import math
from pathlib import Path
import xml.etree.ElementTree as ET
import numpy as np
import pytest
from scipy.spatial.transform import Rotation
from g20_thumb_apriltag_calibration.extrinsics import (
camera_info_fingerprint,
dump_three_camera_extrinsics,
load_three_camera_extrinsics,
)
from g20_thumb_apriltag_calibration.urdf_zero import (
AXIS_JOINTS,
DIRECT_ZERO_JOINTS,
INHERITED_ZERO_JOINTS,
JointAxisMeasurement,
UrdfKinematicModel,
_angles_from_state,
_zero_sensitive_axis_error_rad,
fit_joint_axis_measurement,
fit_rotation_joint_curve,
solve_urdf_zero_offsets,
get_zero_calibration_profile,
write_zero_corrected_urdf,
)
from g20_thumb_apriltag_calibration.full_hand import (
ACTIVE_JOINTS,
JOINT_SPECS,
MEASURED_JOINTS,
PASSIVE_JOINTS,
JointCurveFit,
get_hand_calibration_profile,
)
REPOSITORY = Path(__file__).resolve().parents[3]
SOURCE_URDF = REPOSITORY / (
"src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
"linker_hand/g20_left/linkerhand_g20_left.urdf"
)
RIGHT_SOURCE_URDF = REPOSITORY / (
"src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
"linker_hand/g20_right/linkerhand_g20_right.urdf"
)
def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch():
parent = np.asarray([0.0, 0.0, 1.0])
predicted = np.asarray([1.0, 0.0, 0.0])
cone_mismatch = np.asarray(
[math.cos(math.radians(10.0)), 0.0, math.sin(math.radians(10.0))]
)
zero_mismatch = np.asarray(
[math.cos(math.radians(3.0)), math.sin(math.radians(3.0)), 0.0]
)
assert _zero_sensitive_axis_error_rad(
predicted, cone_mismatch, parent
) == pytest.approx(0.0, abs=1.0e-12)
assert math.degrees(
_zero_sensitive_axis_error_rad(predicted, zero_mismatch, parent)
) == pytest.approx(3.0, abs=1.0e-9)
def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None:
parent = np.asarray([0.0, 0.0, 1.0])
cone = math.radians(32.0)
phase = math.radians(7.0)
predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)])
observed = Rotation.from_rotvec(parent * phase).apply(predicted)
error = _zero_sensitive_axis_error_rad(predicted, observed, parent)
assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9)
def _payload(transform: np.ndarray) -> dict[str, list[float]]:
return {
"translation_xyz_m": transform[:3, 3].tolist(),
"quaternion_xyzw": Rotation.from_matrix(
transform[:3, :3]
).as_quat().tolist(),
}
def _arbitrary_tag_records() -> tuple[list[dict], np.ndarray, np.ndarray]:
axis_parent = np.asarray([0.23, -0.31, 0.922], dtype=float)
axis_parent /= np.linalg.norm(axis_parent)
centre_parent = np.asarray([0.012, -0.008, 0.021])
radial = np.cross(axis_parent, np.asarray([0.7, 0.1, -0.2]))
radial = 0.035 * radial / np.linalg.norm(radial)
child_tag_mount = Rotation.from_euler(
"xyz", [1.1, -0.7, 0.45]
)
common_from_parent = np.eye(4)
common_from_parent[:3, :3] = Rotation.from_euler(
"xyz", [-0.8, 0.55, 1.3]
).as_matrix()
common_from_parent[:3, 3] = [0.41, -0.12, 0.73]
expected_axis = common_from_parent[:3, :3] @ axis_parent
expected_point = (
common_from_parent[:3, :3] @ centre_parent
+ common_from_parent[:3, 3]
)
commands = list(range(0, 256, 16)) + [255]
records = []
for cycle in range(3):
for direction in ("decreasing", "increasing"):
for command in commands:
angle = math.radians(62.0) * (255.0 - command) / 255.0
motion = Rotation.from_rotvec(axis_parent * angle)
relative_rotation = motion * child_tag_mount
relative_translation = centre_parent + motion.apply(radial)
child_common = common_from_parent.copy()
child_common[:3, :3] = (
common_from_parent[:3, :3]
@ relative_rotation.as_matrix()
)
child_common[:3, 3] = (
common_from_parent[:3, :3] @ relative_translation
+ common_from_parent[:3, 3]
)
state = [255.0] * 20
state[5] = float(command)
records.append(
{
"cycle": cycle,
"direction": direction,
"command_u8": command,
"relative_translation_xyz_m": relative_translation.tolist(),
"relative_quaternion_xyzw": relative_rotation.as_quat().tolist(),
"parent_pose_common": _payload(common_from_parent),
"child_pose_common": _payload(child_common),
"state_u8": state,
}
)
return records, expected_axis, expected_point
def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None:
records, expected_axis, expected_point = _arbitrary_tag_records()
curve = fit_rotation_joint_curve(records, zero_command_u8=255)
measurement = fit_joint_axis_measurement(
"thumb_cmc_roll", records, cycle=0, zero_command_u8=255
)
observed_axis = np.asarray(measurement.axis_common_xyz)
observed_point = np.asarray(measurement.point_common_xyz_m)
assert float(observed_axis @ expected_axis) > math.cos(math.radians(0.05))
assert np.linalg.norm(
np.cross(observed_point - expected_point, expected_axis)
) < 1.0e-6
assert curve.angle_rad[255] == pytest.approx(0.0, abs=1.0e-9)
assert curve.angle_rad[0] == pytest.approx(math.radians(62.0), abs=1.0e-6)
@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"])
def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth(
joint: str,
) -> None:
records, expected_axis, expected_point = _arbitrary_tag_records()
parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
axis_parent = parent_rotation.inv().apply(expected_axis)
tangent = np.cross(axis_parent, np.asarray([0.4, -0.2, 0.7]))
tangent /= np.linalg.norm(tangent)
# Reproduce monocular planar-PnP depth bias: the centre trajectory remains
# precise in its dominant directions but receives a command-correlated
# component that makes a free 3-D plane normal substantially wrong.
biased_records = []
for record in records:
biased = dict(record)
point = np.asarray(record["relative_translation_xyz_m"], dtype=float)
depth_bias = 0.30 * float(point @ tangent)
biased["relative_translation_xyz_m"] = (
point + depth_bias * axis_parent
).tolist()
biased_records.append(biased)
measurement = fit_joint_axis_measurement(
joint, biased_records, cycle=0, zero_command_u8=255
)
observed_axis = np.asarray(measurement.axis_common_xyz)
observed_point = np.asarray(measurement.point_common_xyz_m)
assert abs(float(observed_axis @ expected_axis)) > math.cos(
math.radians(0.05)
)
assert np.linalg.norm(
np.cross(observed_point - expected_point, expected_axis)
) < 0.003
assert measurement.rotation_circle_axis_difference_rad > math.radians(5.0)
assert measurement.plane_rms_m < 0.003
assert measurement.radial_rms_m < 0.003
def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None:
records, expected_axis, expected_point = _arbitrary_tag_records()
common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
axis_parent = common_from_parent.inv().apply(expected_axis)
# Exact end-on depth is a gauge along the physical axis and therefore
# cannot alter the observable axis line. An oblique camera has a small
# irreducible coupling between monocular depth and radial position; that
# case must be bounded by the residual/holdout gates, not asserted to be
# exactly recoverable from one view.
view_normal_parent = axis_parent
view_normal_common = common_from_parent.apply(view_normal_parent)
biased = []
for record in records:
changed = dict(record)
fraction = (255.0 - float(record["command_u8"])) / 255.0
depth_bias = 0.03 * (fraction - 0.5)
changed["relative_translation_xyz_m"] = (
np.asarray(record["relative_translation_xyz_m"], dtype=float)
+ depth_bias * view_normal_parent
).tolist()
biased.append(changed)
measurement = fit_joint_axis_measurement(
"thumb_cmc_pitch",
biased,
cycle=0,
zero_command_u8=255,
view_normal_common_xyz=view_normal_common,
)
observed_point = np.asarray(measurement.point_common_xyz_m)
assert measurement.axis_point_source == "pose_trajectory_image_plane"
assert measurement.pose_axis_line_rms_m < 1.0e-6
assert np.linalg.norm(
np.cross(observed_point - expected_point, expected_axis)
) < 1.0e-6
def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None:
records, expected_axis, _ = _arbitrary_tag_records()
assert not any(record["command_u8"] == 127 for record in records)
curve = fit_rotation_joint_curve(records, zero_command_u8=127)
measurement = fit_joint_axis_measurement(
"index_mcp_roll", records, cycle=0, zero_command_u8=127
)
observed_axis = np.asarray(measurement.axis_common_xyz)
assert abs(float(observed_axis @ expected_axis)) > math.cos(
math.radians(0.05)
)
assert curve.angle_rad[127] == pytest.approx(0.0, abs=1.0e-9)
def test_passive_axis_can_use_trusted_upstream_direction_constraint() -> None:
records, expected_axis, expected_point = _arbitrary_tag_records()
common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3])
physical_axis_parent = common_from_parent.inv().apply(expected_axis)
wrong_axis_parent = np.cross(
physical_axis_parent, np.asarray([0.2, 0.8, -0.1])
)
wrong_axis_parent /= np.linalg.norm(wrong_axis_parent)
mount = Rotation.from_quat(records[0]["relative_quaternion_xyzw"])
contradictory = []
for record in records:
changed = dict(record)
angle = math.radians(62.0) * (
255.0 - float(record["command_u8"])
) / 255.0
changed["relative_quaternion_xyzw"] = (
Rotation.from_rotvec(wrong_axis_parent * angle) * mount
).as_quat().tolist()
contradictory.append(changed)
measurement = fit_joint_axis_measurement(
"index_dip",
contradictory,
cycle=0,
zero_command_u8=255,
axis_common_constraint=expected_axis,
)
observed_axis = np.asarray(measurement.axis_common_xyz)
observed_point = np.asarray(measurement.point_common_xyz_m)
assert abs(float(observed_axis @ expected_axis)) > math.cos(
math.radians(0.05)
)
assert np.linalg.norm(
np.cross(observed_point - expected_point, expected_axis)
) < 1.0e-6
def test_extrinsics_round_trip_keeps_camera_identity(tmp_path: Path) -> None:
cameras = {
view: {
"serial_number": f"SERIAL_{view}",
"width": 1624,
"height": 1240,
"intrinsics_sha256": camera_info_fingerprint(
width=1624,
height=1240,
camera_matrix=np.asarray(
[[1100.0, 0.0, 812.0], [0.0, 1099.0, 620.0], [0.0, 0.0, 1.0]]
),
),
}
for view in ("front", "side", "top")
}
transforms = {"front": np.eye(4), "side": np.eye(4), "top": np.eye(4)}
transforms["side"][:3, :3] = Rotation.from_euler("y", 0.7).as_matrix()
transforms["side"][:3, 3] = [0.2, 0.0, 0.1]
transforms["top"][:3, :3] = Rotation.from_euler("x", -0.9).as_matrix()
transforms["top"][:3, 3] = [-0.1, 0.3, 0.2]
destination = tmp_path / "extrinsics.yaml"
dump_three_camera_extrinsics(
destination,
cameras=cameras,
front_from_view=transforms,
quality={
"passed": True,
"reprojection_rms_px": 0.3,
"maximum_rotation_repeatability_deg": 0.2,
"maximum_translation_repeatability_m": 0.001,
"front_side_captures": 15,
"front_top_captures": 15,
},
)
loaded = load_three_camera_extrinsics(destination)
assert loaded.cameras["front"].serial_number == "SERIAL_front"
assert np.allclose(loaded.transform("side"), transforms["side"])
assert np.allclose(loaded.transform("top"), transforms["top"])
assert loaded.camera_matches(
"front",
serial_number="SERIAL_front",
width=1624,
height=1240,
intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
)
assert not loaded.camera_matches(
"front",
serial_number="WRONG_SERIAL",
width=1624,
height=1240,
intrinsics_sha256=cameras["front"]["intrinsics_sha256"],
)
def _joint_origin(path: Path, name: str) -> tuple[np.ndarray, np.ndarray]:
joint = next(
element
for element in ET.parse(path).getroot().findall("joint")
if element.get("name") == name
)
origin = joint.find("origin")
axis = joint.find("axis")
xyz = np.asarray([float(value) for value in origin.get("xyz").split()])
rpy = np.asarray([float(value) for value in origin.get("rpy").split()])
axis_xyz = np.asarray([float(value) for value in axis.get("xyz").split()])
return np.block(
[
[Rotation.from_euler("xyz", rpy).as_matrix(), xyz[:, None]],
[np.asarray([[0.0, 0.0, 0.0, 1.0]])],
]
), axis_xyz / np.linalg.norm(axis_xyz)
def _joint_limit(path: Path, name: str) -> tuple[float, float]:
joint = next(
element
for element in ET.parse(path).getroot().findall("joint")
if element.get("name") == name
)
limit = joint.find("limit")
return float(limit.get("lower")), float(limit.get("upper"))
def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Path) -> None:
offset = math.radians(7.3)
destination = write_zero_corrected_urdf(
source_urdf=SOURCE_URDF,
output_directory=tmp_path,
serial_number="G20_LEFT_001",
offsets_rad={"thumb_cmc_yaw": offset},
timestamp="20260806_120000",
)
original, axis = _joint_origin(SOURCE_URDF, "thumb_cmc_yaw")
corrected, _ = _joint_origin(destination, "thumb_cmc_yaw")
expected = original.copy()
expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
axis * offset
).as_matrix()
assert destination != SOURCE_URDF
assert np.allclose(corrected, expected, atol=1.0e-12)
with pytest.raises(ValueError, match="refusing to overwrite"):
write_zero_corrected_urdf(
source_urdf=SOURCE_URDF,
output_directory=tmp_path,
serial_number="G20_LEFT_001",
offsets_rad={"thumb_cmc_yaw": offset},
timestamp="20260806_120000",
)
with pytest.raises(ValueError, match="original CAD URDF"):
write_zero_corrected_urdf(
source_urdf=destination,
output_directory=tmp_path,
serial_number="G20_LEFT_001",
offsets_rad={"thumb_cmc_yaw": offset},
timestamp="20260806_120001",
)
with pytest.raises(ValueError, match="finite and within"):
write_zero_corrected_urdf(
source_urdf=SOURCE_URDF,
output_directory=tmp_path,
serial_number="G20_LEFT_001",
offsets_rad={"thumb_cmc_yaw": math.nan},
timestamp="20260806_120002",
)
def test_urdf_writer_changes_only_the_16_active_zero_origins(
tmp_path: Path,
) -> None:
before = SOURCE_URDF.read_bytes()
offsets = {
name: math.radians(0.25 * (index + 1))
for index, name in enumerate(ACTIVE_JOINTS)
}
destination = write_zero_corrected_urdf(
source_urdf=SOURCE_URDF,
output_directory=tmp_path,
serial_number="G20_LEFT_001",
offsets_rad=offsets,
timestamp="20260807_180000",
)
assert len(offsets) == 16
assert SOURCE_URDF.read_bytes() == before
for name in ACTIVE_JOINTS:
original, axis = _joint_origin(SOURCE_URDF, name)
corrected, corrected_axis = _joint_origin(destination, name)
expected = original.copy()
expected[:3, :3] = original[:3, :3] @ Rotation.from_rotvec(
axis * offsets[name]
).as_matrix()
assert np.allclose(corrected, expected, atol=1.0e-12)
assert np.allclose(corrected_axis, axis, atol=1.0e-12)
for name in PASSIVE_JOINTS:
original, axis = _joint_origin(SOURCE_URDF, name)
corrected, corrected_axis = _joint_origin(destination, name)
assert np.allclose(corrected, original, atol=1.0e-12)
assert np.allclose(corrected_axis, axis, atol=1.0e-12)
# A zero calibration must not silently expand mechanical/CAD safety
# limits. Dynamic measured ranges remain in the calibration JSON.
for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS):
assert _joint_limit(destination, name) == pytest.approx(
_joint_limit(SOURCE_URDF, name)
)
def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit:
values = np.asarray(
[travel * (255.0 - command) / 255.0 for command in range(256)]
)
values -= values[zero_command]
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 _solve_synthetic_offsets(
side: str,
offset_degrees: list[float],
*,
inject_oblique_optical_depth_bias: bool = False,
inject_secondary_root_axis_bias_degrees: float = 0.0,
inject_secondary_root_point_bias_m: float = 0.0,
inject_observer_cone_bias_degrees: float = 0.0,
pose_axis_line_rms_by_joint_m: dict[str, float] | None = None,
joint_maximum_offset_degrees: dict[str, float] | None = None,
):
hand = get_hand_calibration_profile(side)
zero = get_zero_calibration_profile(side)
source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF
baseline = [255.0] * 20
baseline[6:10] = [127.0] * 4
curves = {
name: _synthetic_curve(
int(baseline[hand.joint_specs[name].motor_index]),
math.radians(50.0),
)
for name in hand.measured_joints
}
if inject_secondary_root_axis_bias_degrees:
# Make the thumb root the higher-travel, directly observed direction,
# matching the real right-hand data where the short pinky splay arc is
# the less reliable root-axis orientation estimate.
curves["thumb_cmc_roll"] = _synthetic_curve(
int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]),
math.radians(70.0),
)
motor_by_joint = {
name: spec.motor_index for name, spec in hand.joint_specs.items()
}
offsets = {
name: math.radians(value)
for name, value in zip(zero.direct_zero_joints, offset_degrees)
}
model = UrdfKinematicModel(source)
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
base_translation = np.asarray([0.31, -0.19, 0.72])
measurements: list[JointAxisMeasurement] = []
for cycle in range(3):
for joint in zero.axis_joints:
state = list(baseline)
if joint == "thumb_cmc_yaw":
state[5] = 145.0
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
)
point_common = base_rotation.apply(point) + base_translation
if (
inject_secondary_root_point_bias_m
and joint == f"{zero.reference_finger}_mcp_roll"
):
# A repeatable monocular depth error on the second parallel
# root line must affect translation only, never palm rotation
# or the inferred thumb-roll zero.
point_common = point_common + base_rotation.apply(
np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m])
)
view_normal_common = None
if (
inject_oblique_optical_depth_bias
and joint in zero.phase_parent_joint
):
parent_axis = model.axis_line(
zero.phase_parent_joint[joint],
zero_offsets=offsets,
joint_angles=angles,
)[0]
helper = (
np.asarray([1.0, 0.0, 0.0])
if abs(float(parent_axis[0])) < 0.8
else np.asarray([0.0, 1.0, 0.0])
)
tilt_axis = np.cross(parent_axis, helper)
tilt_axis /= np.linalg.norm(tilt_axis)
view_normal = Rotation.from_rotvec(
math.radians(15.0) * tilt_axis
).apply(parent_axis)
view_normal_common = tuple(base_rotation.apply(view_normal))
# Simulate an independent planar-PnP depth error on the child
# Tag. It is large enough to drive the old 3-D phase solve to
# a configured offset bound.
point_common = point_common + 0.03 * np.asarray(
view_normal_common
)
axis_common = base_rotation.apply(axis)
if (
inject_observer_cone_bias_degrees
and joint == "thumb_cmc_pitch"
):
parent_axis = model.axis_line(
zero.axis_parent_joint[joint],
zero_offsets=offsets,
joint_angles=angles,
)[0]
cone_normal = np.cross(axis, parent_axis)
cone_normal /= np.linalg.norm(cone_normal)
axis_common = base_rotation.apply(
Rotation.from_rotvec(
math.radians(inject_observer_cone_bias_degrees)
* cone_normal
).apply(axis)
)
if (
inject_secondary_root_axis_bias_degrees
and joint == f"{zero.reference_finger}_mcp_roll"
):
helper = np.asarray([0.0, 0.0, 1.0])
if abs(float(axis_common @ helper)) > 0.8:
helper = np.asarray([0.0, 1.0, 0.0])
bias_axis = np.cross(axis_common, helper)
bias_axis /= np.linalg.norm(bias_axis)
axis_common = Rotation.from_rotvec(
math.radians(inject_secondary_root_axis_bias_degrees)
* bias_axis
).apply(axis_common)
measurements.append(
JointAxisMeasurement(
joint=joint,
cycle=cycle,
axis_common_xyz=tuple(axis_common),
point_common_xyz_m=tuple(point_common),
condition_state_u8=tuple(state),
plane_rms_m=0.0002,
radial_rms_m=0.0002,
rotation_circle_axis_difference_rad=math.radians(0.1),
view_normal_common_xyz=view_normal_common,
pose_axis_line_rms_m=(
pose_axis_line_rms_by_joint_m or {}
).get(joint, 0.0),
)
)
result = solve_urdf_zero_offsets(
source_urdf=source,
measurements=measurements,
curves=curves,
motor_by_joint=motor_by_joint,
hand_type=side,
joint_maximum_offset_rad={
name: math.radians(value)
for name, value in (joint_maximum_offset_degrees or {}).items()
},
)
return zero, result
def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None:
zero, result = _solve_synthetic_offsets("right", [0.1] * 7)
assert result.passed is True
static_policy = {
**zero.fixed_direct_zero_offsets_rad,
**zero.static_output_zero_offsets_rad,
}
for name, value in result.direct_offsets_rad.items():
assert value == pytest.approx(static_policy.get(name, 0.0))
def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None:
right = get_zero_calibration_profile("right")
left = get_zero_calibration_profile("left")
assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad
assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad
assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad
assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad
def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None:
zero, result = _solve_synthetic_offsets(
"right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0]
)
reference_roll = f"{zero.reference_finger}_mcp_roll"
assert result.passed is True
assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx(
0.0, abs=1.0e-12
)
def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None:
zero, result = _solve_synthetic_offsets(
"right",
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
inject_secondary_root_axis_bias_degrees=15.0,
)
assert result.passed is True
expected = dict(
zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
)
expected.update(
{
name: math.degrees(value)
for name, value in {
**zero.fixed_direct_zero_offsets_rad,
**zero.static_output_zero_offsets_rad,
}.items()
}
)
for name, value in expected.items():
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
value, abs=0.05
)
def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None:
zero, result = _solve_synthetic_offsets(
"right",
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
inject_secondary_root_point_bias_m=0.02,
)
assert result.passed is True
assert math.degrees(
result.direct_offsets_rad["thumb_cmc_roll"]
) == pytest.approx(2.0, abs=0.05)
assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0
def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None:
offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0]
_, result = _solve_synthetic_offsets(
"right",
offsets,
joint_maximum_offset_degrees={"thumb_mcp": 45.0},
)
assert result.passed is True
assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0)
assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0))
assert "thumb_ip" not in result.validation_error_by_joint_rad
for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"):
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0
for name in ("pinky_mcp_pitch", "pinky_pip"):
assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0
assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0)
def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None:
zero, result = _solve_synthetic_offsets(
"right",
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
inject_oblique_optical_depth_bias=True,
)
assert result.passed is True
expected = dict(
zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0])
)
expected.update(
{
name: math.degrees(value)
for name, value in {
**zero.fixed_direct_zero_offsets_rad,
**zero.static_output_zero_offsets_rad,
}.items()
}
)
for name, value in expected.items():
assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx(
value, abs=0.05
)
def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None:
_, result = _solve_synthetic_offsets(
"right",
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
inject_observer_cone_bias_degrees=8.0,
)
assert result.passed is False
assert result.failure_reasons["thumb_cmc_yaw"] == (
"zero_axis_cone_mismatch_too_large"
)
def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None:
_, result = _solve_synthetic_offsets(
"right",
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002},
)
assert result.passed is False
assert result.failure_reasons["thumb_cmc_pitch"] == (
"zero_phase_axis_line_residual_too_large"
)
def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
zero = get_zero_calibration_profile("left")
baseline = [255.0] * 20
baseline[6:10] = [127.0] * 4
curves = {
name: _synthetic_curve(
int(baseline[JOINT_SPECS[name].motor_index]),
math.radians(50.0),
)
for name in MEASURED_JOINTS
}
motor_by_joint = {
name: spec.motor_index for name, spec in JOINT_SPECS.items()
}
true_offsets = {
name: math.radians(value)
for name, value in zip(
DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]
)
}
model = UrdfKinematicModel(SOURCE_URDF)
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
base_translation = np.asarray([0.31, -0.19, 0.72])
measurements = []
yaw_axis_without_clearance = None
yaw_axis_with_clearance = None
for cycle in range(3):
for joint in AXIS_JOINTS:
state = list(baseline)
if joint == "thumb_cmc_yaw":
state[5] = 145.0
angles = _angles_from_state(
state, curves=curves, motor_by_joint=motor_by_joint
)
axis, point = model.axis_line(
joint,
zero_offsets=true_offsets,
joint_angles=angles,
)
if joint == "thumb_cmc_yaw":
yaw_axis_with_clearance = axis.copy()
baseline_angles = _angles_from_state(
baseline, curves=curves, motor_by_joint=motor_by_joint
)
yaw_axis_without_clearance = model.axis_line(
joint,
zero_offsets=true_offsets,
joint_angles=baseline_angles,
)[0]
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=tuple(state),
plane_rms_m=0.0002,
radial_rms_m=0.0002,
rotation_circle_axis_difference_rad=math.radians(0.1),
)
)
result = solve_urdf_zero_offsets(
source_urdf=SOURCE_URDF,
measurements=measurements,
curves=curves,
motor_by_joint=motor_by_joint,
)
assert math.degrees(
math.acos(
np.clip(yaw_axis_with_clearance @ yaw_axis_without_clearance, -1.0, 1.0)
)
) > 1.0
assert result.passed is True
static_policy = {
**zero.fixed_direct_zero_offsets_rad,
**zero.static_output_zero_offsets_rad,
}
for name, expected in true_offsets.items():
if name in static_policy:
expected = static_policy[name]
assert result.direct_offsets_rad[name] == pytest.approx(
expected, abs=math.radians(0.05)
)
assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS)
assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx(
0.0, abs=math.radians(0.05)
)
for target in INHERITED_ZERO_JOINTS:
assert result.all_active_offsets_rad[target] == pytest.approx(
0.0, abs=1.0e-12
)
assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys()
def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None:
hand = get_hand_calibration_profile("right")
zero = get_zero_calibration_profile("right")
baseline = [255.0] * 20
baseline[6:10] = [127.0] * 4
curves = {
name: _synthetic_curve(
int(baseline[hand.joint_specs[name].motor_index]),
math.radians(50.0),
)
for name in hand.measured_joints
}
motor_by_joint = {
name: spec.motor_index for name, spec in hand.joint_specs.items()
}
true_offsets = {
name: math.radians(value)
for name, value in zip(
zero.direct_zero_joints,
[2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0],
)
}
model = UrdfKinematicModel(RIGHT_SOURCE_URDF)
base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8])
base_translation = np.asarray([0.31, -0.19, 0.72])
measurements: list[JointAxisMeasurement] = []
for cycle in range(3):
states: dict[str, list[float]] = {}
lines: dict[str, tuple[np.ndarray, np.ndarray]] = {}
for joint in zero.axis_joints:
state = list(baseline)
if joint == "thumb_cmc_yaw":
state[5] = 145.0
states[joint] = state
angles = _angles_from_state(
state,
curves=curves,
motor_by_joint=motor_by_joint,
inherited_zero_joints=zero.inherited_zero_joints,
)
lines[joint] = model.axis_line(
joint,
zero_offsets=true_offsets,
joint_angles=angles,
)
original_lines = {
name: (axis.copy(), point.copy())
for name, (axis, point) in lines.items()
}
for observer, parent in zero.phase_parent_joint.items():
parent_axis, parent_point = lines[parent]
original_parent_axis, original_parent_point = original_lines[parent]
child_axis, original_child_point = original_lines[observer]
radial = original_child_point - original_parent_point
radial -= original_parent_axis * float(
radial @ original_parent_axis
)
# Preserve angular phase while deliberately corrupting link radius
# and along-axis depth. These components must not move a zero.
lines[observer] = (
child_axis,
parent_point + 1.25 * radial + 0.02 * parent_axis,
)
for joint in zero.axis_joints:
axis, point = lines[joint]
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=tuple(states[joint]),
plane_rms_m=0.0002,
radial_rms_m=0.0002,
rotation_circle_axis_difference_rad=math.radians(0.1),
)
)
result = solve_urdf_zero_offsets(
source_urdf=RIGHT_SOURCE_URDF,
measurements=measurements,
curves=curves,
motor_by_joint=motor_by_joint,
hand_type="right",
)
assert result.passed is True
static_policy = {
**zero.fixed_direct_zero_offsets_rad,
**zero.static_output_zero_offsets_rad,
}
for name, expected in true_offsets.items():
if name in static_policy:
expected = static_policy[name]
assert result.direct_offsets_rad[name] == pytest.approx(
expected, abs=math.radians(0.05)
)
for target in zero.inherited_zero_joints:
assert result.all_active_offsets_rad[target] == pytest.approx(
0.0, abs=1.0e-12
)
assert set(result.all_active_offsets_rad) == set(hand.active_joints)
src/linkerhand_calibration/test/test_zero_calibration.py → src/g20_thumb_apriltag_calibration/test/test_zero_calibration.py
+1 -1
View File
@@ -3,7 +3,7 @@ from __future__ import annotations
import math
import cv2
from linkerhand_calibration.zero_calibration import (
from g20_thumb_apriltag_calibration.zero_calibration import (
build_trajectory_zero_angle_payload,
build_trajectory_zero_travel_payload,
circular_median_rad,
src/gui_control/gui_control/config/constants.py
+5 -5
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, 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],
"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],
"准备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],
@@ -230,7 +230,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
}
),
"L6": HandConfig(
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
init_pos=[250] * 6,
preset_actions={
src/gui_control/gui_control/gui_control.py
-5
View File
@@ -33,10 +33,6 @@ _CANONICAL_COMMAND_NAMES = {
"thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch",
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
],
"L6": [
"thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch",
"middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
],
}
_CANONICAL_COMMAND_BOUNDS = {
@@ -47,7 +43,6 @@ _CANONICAL_COMMAND_BOUNDS = {
*[(0, 255)] * 5,
],
"O6": [(0, 255)] * 6,
"L6": [(0, 255)] * 6,
}
src/gui_control/test/test_l6_config.py
-12
View File
@@ -1,12 +0,0 @@
from gui_control.config.constants import HAND_CONFIGS
def test_l6_gui_uses_the_sdk_channel_order() -> None:
assert HAND_CONFIGS["L6"].joint_names_en == [
"thumb_cmc_pitch",
"thumb_cmc_roll",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
]
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py
+2 -36
View File
@@ -1,5 +1,3 @@
from collections import deque
import can
import time, sys
import threading
@@ -59,14 +57,6 @@ class LinkerHandL6Can:
self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
self.is_lock = False
self.version = None
# L6 replies to a six-byte 0x01 position command with an immediate
# byte-for-byte echo on the same CAN ID. A zero-payload 0x01 state
# query also replies on that ID, but with the measured positions.
# Keep the two transactions distinct so command echoes never enter
# the published feedback stream used by calibration.
self._position_echo_lock = threading.Lock()
self._pending_position_echoes = deque(maxlen=32)
self._position_echo_timeout_seconds = 0.02
# Start the receiving thread
self.running = True
self.receive_thread = threading.Thread(target=self.receive_response)
@@ -121,11 +111,6 @@ class LinkerHandL6Can:
frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
data = [frame_property_value] + [int(val) for val in data_list]
msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
if frame_property_value == 0x01 and len(data_list) == 6:
with self._position_echo_lock:
self._pending_position_echoes.append(
(time.monotonic(), tuple(int(value) for value in data_list))
)
try:
self.bus.send(msg)
except can.CanError as e:
@@ -216,23 +201,7 @@ class LinkerHandL6Can:
except:
return
if frame_type == 0x01: # 0x01
response = tuple(int(value) for value in response_data)
now = time.monotonic()
is_position_echo = False
with self._position_echo_lock:
while (
self._pending_position_echoes
and now - self._pending_position_echoes[0][0]
> self._position_echo_timeout_seconds
):
self._pending_position_echoes.popleft()
for pending in tuple(self._pending_position_echoes):
if pending[1] == response:
self._pending_position_echoes.remove(pending)
is_position_echo = True
break
if not is_position_echo:
self.x01 = list(response)
self.x01 = list(response_data)
elif frame_type == 0x02: # 0x02
self.x02 = list(response_data)
elif frame_type == 0x05: # Set speed
@@ -422,10 +391,7 @@ class LinkerHandL6Can:
return self.x35
def get_finger_order(self):
# L6 channel 1 is the physical CMC roll actuator. Older SDK releases
# exposed the channel as ``thumb_cmc_yaw`` even though the wire order
# and mechanism have always been roll.
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
def show_fun_table(self):
pass
src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py
+1 -1
View File
@@ -372,7 +372,7 @@ class LinkerHandL6RS485:
return [0] * 6
def get_finger_order(self):
return ["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
# --------------------------------------------------
# 便捷方法
src/linker_hand_ros2_sdk/test/test_l6_channel_order.py
-72
View File
@@ -1,72 +0,0 @@
import ast
from collections import deque
from pathlib import Path
import sys
import threading
import time
from types import SimpleNamespace
PACKAGE = Path(__file__).resolve().parents[1] / "linker_hand_ros2_sdk/LinkerHand/core"
EXPECTED = [
"thumb_cmc_pitch",
"thumb_cmc_roll",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
]
def _finger_order(path: Path, class_name: str) -> list[str]:
module = ast.parse(path.read_text(encoding="utf-8"))
selected = next(
item
for item in module.body
if isinstance(item, ast.ClassDef) and item.name == class_name
)
method = next(
item
for item in selected.body
if isinstance(item, ast.FunctionDef) and item.name == "get_finger_order"
)
returned = next(item for item in method.body if isinstance(item, ast.Return))
return ast.literal_eval(returned.value)
def test_l6_can_and_rs485_publish_the_same_physical_channel_order() -> None:
assert _finger_order(PACKAGE / "can/linker_hand_l6_can.py", "LinkerHandL6Can") == EXPECTED
assert _finger_order(
PACKAGE / "rs485/linker_hand_l6_rs485.py", "LinkerHandL6RS485"
) == EXPECTED
def test_l6_can_position_echo_does_not_replace_measured_feedback() -> None:
linker_hand_root = PACKAGE.parent
sys.path.insert(0, str(linker_hand_root))
try:
from core.can.linker_hand_l6_can import LinkerHandL6Can
finally:
sys.path.remove(str(linker_hand_root))
hand = LinkerHandL6Can.__new__(LinkerHandL6Can)
hand.can_id = 0x27
hand.x01 = [10, 20, 30, 40, 50, 60]
hand._position_echo_lock = threading.Lock()
command = (255, 2, 253, 253, 253, 253)
hand._pending_position_echoes = deque(
[(time.monotonic(), command)], maxlen=32
)
hand._position_echo_timeout_seconds = 0.02
hand.process_response(
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *command)))
)
assert hand.x01 == [10, 20, 30, 40, 50, 60]
assert not hand._pending_position_echoes
measured = (250, 3, 252, 252, 252, 252)
hand.process_response(
SimpleNamespace(arbitration_id=0x27, data=bytes((0x01, *measured)))
)
assert hand.x01 == list(measured)
src/linkerhand_calibration/config/g20_right_product.yaml
-37
View File
@@ -1,37 +0,0 @@
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: package://linkerhand_calibration/urdf/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/linkerhand_calibration/config/l6_right_8_tags.yaml
-59
View File
@@ -1,59 +0,0 @@
/l6_calibration/front/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [0, 1, 2]
frames: [front_base, thumb_pitch, thumb_dip]
sizes: [0.016, 0.016, 0.016]
/l6_calibration/side/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [3, 4, 5]
frames: [side_base, pinky_pitch, pinky_dip]
sizes: [0.016, 0.016, 0.016]
/l6_calibration/top/apriltag/apriltag:
ros__parameters:
image_transport: raw
qos_profile: sensor_data
family: 36h11
size: 0.016
max_hamming: 0
detector:
threads: 4
decimate: 1.0
blur: 0.0
refine: true
sharpening: 0.25
debug: false
pose_estimation_method: pnp
tag:
ids: [6, 7]
frames: [top_base, thumb_roll]
sizes: [0.016, 0.016]
src/linkerhand_calibration/config/l6_right_product.yaml
-37
View File
@@ -1,37 +0,0 @@
schema_version: 2
profile_id: L6/right/l6_right_8/v1
model: L6
side: right
tag_layout: l6_right_8
namespace: /l6_calibration
serial_number: L6_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: package://linkerhand_calibration/urdf/l6_right/linkerhand_l6v3.1_right.urdf
source_urdf_sha256: 298c1fbf5189648911426f530b50bdbeea4830cab9c54e20f46c532485df4666
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
calibration_config: package://linkerhand_calibration/config/l6_three_camera_calibration.yaml
calibration_config_sha256: 0934699c8225891e748deefef6791eb28355821b89aeadd1f7ff0b7f7b4d265f
tag_config: package://linkerhand_calibration/config/l6_right_8_tags.yaml
tag_config_sha256: be1499eb947b61d2fe360ae2c92307a87710480fae8a9dd4cd171fc959fdcbf5
release:
required_independent_passes: 1
static_repeatability_deg: 1.0
src/linkerhand_calibration/config/l6_three_camera_calibration.yaml
-67
View File
@@ -1,67 +0,0 @@
l6_calibration:
ros__parameters:
command_topic: /l6/cb_right_hand_control_cmd
state_topic: /l6/cb_right_hand_state
setting_topic: /l6/cb_hand_setting_cmd
front_camera_info_topic: /l6_calibration/front/camera/camera_info
front_detections_topic: /l6_calibration/front/apriltag/detections
side_camera_info_topic: /l6_calibration/side/camera/camera_info
side_detections_topic: /l6_calibration/side/apriltag/detections
top_camera_info_topic: /l6_calibration/top/camera/camera_info
top_detections_topic: /l6_calibration/top/apriltag/detections
baseline_command_u8: [255, 255, 255, 255, 255, 255]
# L6_RIGHT_001 measured a 250->5 travel of only ~0.9 s at speed 10,
# which left fewer than 32 useful feedback bins. Speed 1 is still only a
# firmware ceiling: different L6 motors complete a full stroke in 0.7-1.3 s.
# A 100 Hz cosine trajectory therefore sets the actual, model-level pace.
preflight_speed_u8: 1
formal_speed_u8: 1
speed_settle_seconds: 0.2
command_trajectory_full_range_seconds: 6.0
torque_u8: 80
repetitions: 4
preflight_checkpoints_u8: [255, 127, 0]
tag_size_m: 0.016
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7]
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
minimum_detection_rate: 0.95
# Per-Tag quality remains >=95%. With three independently detected Tags,
# the fully joined frame rate may be 0.95^3 ~= 85.7%.
minimum_joint_frame_rate: 0.85
minimum_feedback_hz: 25.0
maximum_state_image_skew_ms: 50.0
maximum_hamming: 0
minimum_decision_margin: 30.0
minimum_edge_pixels: 30.0
pnp_maximum_reprojection_error_px: 1.5
pnp_reprojection_tie_px: 1.5
pnp_maximum_pose_jump_deg: 35.0
pnp_maximum_translation_jump_m: 0.04
pnp_maximum_tag_tilt_deg: 75.0
pnp_tracker_reset_seconds: 5.0
minimum_sweep_frames: 40
minimum_state_span_u8: 240.0
minimum_sweep_bins: 32
maximum_bin_gap: 16
maximum_monotonic_correction_deg: 2.0
passive_maximum_monotonic_correction_deg: 3.0
maximum_validation_mae_deg: 1.0
maximum_validation_p95_deg: 2.0
maximum_validation_error_deg: 3.0
mimic_minimum_multiplier: 0.5
mimic_maximum_multiplier: 1.5
mimic_maximum_cycle_range: 0.03
mimic_maximum_residual_p95_deg: 2.0
endpoint_tolerance_u8: 2.0
endpoint_hold_seconds: 1.0
motor_stall_timeout_seconds: 2.0
position_timeout_seconds: 30.0
sweep_timeout_seconds: 90.0
automatic_sweep_retry_limit: 2
non_target_motion_tolerance_u8: 3.0
fixed_base_maximum_corner_drift_px: 2.0
fixed_base_movement_confirmation_frames: 5
src/linkerhand_calibration/config/three_camera_calibration.yaml
-193
View File
@@ -1,193 +0,0 @@
g20_calibration:
ros__parameters:
command_topic: /g20/cb_left_hand_control_cmd
state_topic: /g20/cb_left_hand_state
info_topic: /g20/cb_left_hand_info
setting_topic: /g20/cb_hand_setting_cmd
front_camera_info_topic: /g20_calibration/front/camera/camera_info
front_detections_topic: /g20_calibration/front/apriltag/detections
side_camera_info_topic: /g20_calibration/side/camera/camera_info
side_detections_topic: /g20_calibration/side/apriltag/detections
top_camera_info_topic: /g20_calibration/top/camera/camera_info
top_detections_topic: /g20_calibration/top/apriltag/detections
# /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
normal_calibration_speed: 15
index_roll_calibration_speed: 5
index_flex_calibration_speed: 10
# 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尺寸是检测角点围成的黑色正方形边长，不包含外围白边。
# 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
pnp_maximum_reprojection_error_px: 1.5
pnp_reprojection_tie_px: 1.5
pnp_maximum_pose_jump_deg: 35.0
pnp_maximum_translation_jump_m: 0.04
pnp_maximum_tag_tilt_deg: 75.0
pnp_tracker_reset_seconds: 5.0
# 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
pnp_group_initialization_frames: 8
# 侧面当前任务所需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
axis_maximum_plane_rms_m: 0.003
# 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
# 轴线一致性仍沿用严格检查。
passive_axis_maximum_plane_rms_m: 0.004
axis_maximum_radial_rms_m: 0.003
# 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
axis_maximum_pose_line_rms_m: 0.001
# 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
# 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
axis_maximum_rotation_circle_difference_deg: 1.0
# 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
# 关节允许可重复的非理想单轴分量，但仍须通过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 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
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈
# 必须已经稳定；大残差仍由机械卡滞保护处理。
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。
right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
# 右手小指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
# 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
# 这类故障不进入遮挡/超时的三次自动重扫。
# 低速5也应持续产生反馈进展；5秒无进展即停，减少机构持续顶死时间。
motor_stall_timeout_seconds: 2.0
motor_stall_startup_grace_seconds: 1.0
motor_stall_minimum_progress_u8: 1.0
invalid_timeout_seconds: 3.0
minimum_sweep_frames: 40
minimum_state_span_u8: 240.0
minimum_sweep_bins: 32
maximum_bin_gap: 16
# 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
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]
retry_endpoint_hold_seconds: [0.75, 1.0]
trajectory_maximum_plane_rms_m: 0.004
trajectory_maximum_radial_rms_m: 0.004
trajectory_minimum_radius_m: 0.003
trajectory_minimum_arc_deg: 15.0
# 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
image_trajectory_maximum_radial_rms_px: 2.0
image_trajectory_maximum_radial_p95_px: 3.5
image_trajectory_minimum_radius_px: 20.0
trajectory_maximum_cycle_travel_difference_deg: 3.0
passive_maximum_cycle_travel_difference_deg: 10.0
maximum_monotonic_correction_deg: 2.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: 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/linkerhand_calibration/config/three_camera_tags_g20_right_19.yaml
-63
View File
@@ -1,63 +0,0 @@
/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/linkerhand_calibration/g20_thumb_apriltag_calibration/__init__.py
-21
View File
@@ -1,21 +0,0 @@
"""One-release compatibility surface for the former Python package name.
New code must import :mod:`linkerhand_calibration`. Only the documented
configuration loader is re-exported here; calibration algorithms continue to
have a single implementation in the renamed package.
"""
from __future__ import annotations
import warnings
warnings.warn(
"g20_thumb_apriltag_calibration is deprecated; "
"import linkerhand_calibration instead",
DeprecationWarning,
stacklevel=2,
)
from linkerhand_calibration.product import ProductConfig, load_product_config
__all__ = ["ProductConfig", "load_product_config"]
src/linkerhand_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py
-9
View File
@@ -1,9 +0,0 @@
"""Deprecated forwarding entry point for the runtime joint-state bridge."""
from linkerhand_calibration.calibrated_joint_state_bridge import main
__all__ = ["main"]
if __name__ == "__main__":
main()
src/linkerhand_calibration/g20_thumb_apriltag_calibration/offline_replay.py
-9
View File
@@ -1,9 +0,0 @@
"""Deprecated forwarding entry point for offline replay."""
from linkerhand_calibration.offline_replay import main
__all__ = ["main"]
if __name__ == "__main__":
main()
src/linkerhand_calibration/g20_thumb_apriltag_calibration/one_command.py
-9
View File
@@ -1,9 +0,0 @@
"""Deprecated forwarding entry point for the former Python package."""
from linkerhand_calibration.one_command import main
__all__ = ["main"]
if __name__ == "__main__":
main()
src/linkerhand_calibration/linkerhand_calibration/__init__.py
-5
View File
@@ -1,5 +0,0 @@
"""Profile-driven LinkerHand calibration and validated URDF correction."""
from .core import CalibrationProfile, ProfileKey
__all__ = ["CalibrationProfile", "ProfileKey"]
src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py
-21
View File
@@ -1,21 +0,0 @@
"""Compatibility adapters for one-release calibration migrations."""
from .config_v1 import (
legacy_default_profile_key,
product_profile_key,
resolve_legacy_profile_alias,
)
from .defaults import (
default_product_config_path,
default_three_camera_config_path,
)
from .paths import resolve_renamed_package_path
__all__ = [
"default_product_config_path",
"default_three_camera_config_path",
"legacy_default_profile_key",
"product_profile_key",
"resolve_legacy_profile_alias",
"resolve_renamed_package_path",
]
src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py
-47
View File
@@ -1,47 +0,0 @@
"""Identity migration for deployed product configuration schemas."""
from __future__ import annotations
from typing import Any, Mapping
from ..core import ProfileKey
def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey:
version = int(raw.get("schema_version", -1))
if version == 2:
key = ProfileKey.parse(str(raw.get("profile_id", "")))
for field, actual in (
("model", key.model),
("side", key.side),
("tag_layout", key.layout),
):
configured = str(raw.get(field, "")).strip()
if configured and configured.lower() != actual.lower():
raise ValueError(f"{field} differs from profile_id")
return key
if version != 1:
raise ValueError("product config schema_version must be 1 or 2")
model = str(raw.get("model", "")).strip().upper()
side = str(raw.get("side", "")).strip().lower()
layout = str(raw.get("tag_layout", "")).strip().lower()
if not layout and (model, side) == ("G20", "right"):
layout = "g20_right_19"
return ProfileKey(model, side, layout, 1)
def legacy_default_profile_key() -> ProfileKey:
"""Preserve the former no-argument executable for one release."""
return ProfileKey("G20", "right", "g20_right_19", 1)
def resolve_legacy_profile_alias(key: ProfileKey) -> ProfileKey:
"""Map retired layout identifiers to their reviewed physical profile."""
if (
key.model == "G20"
and key.side == "right"
and key.layout == "g20_right_15"
and key.revision == 1
):
return ProfileKey("G20", "right", "g20_right_19", 1)
return key
src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py
-16
View File
@@ -1,16 +0,0 @@
"""One-release default selection for invocations without ``--config``."""
from pathlib import Path
from ament_index_python.packages import get_package_share_directory
def default_product_config_path() -> Path:
share = Path(get_package_share_directory("linkerhand_calibration"))
return share / "config/g20_right_product.yaml"
def default_three_camera_config_path() -> Path:
"""Resolve the installed calibration defaults through the ROS index."""
share = Path(get_package_share_directory("linkerhand_calibration"))
return share / "config/three_camera_calibration.yaml"
src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py
-4
View File
@@ -1,4 +0,0 @@
"""Legacy single-camera algorithms retained for one compatibility release."""
from .session_v1 import uses_coupled_full_hand_zero_solver
__all__ = ["uses_coupled_full_hand_zero_solver"]
src/linkerhand_calibration/linkerhand_calibration/compat/legacy/session_v1.py
-24
View File
@@ -1,24 +0,0 @@
"""Version selection for replaying durable pre-v3 hardware sessions."""
from __future__ import annotations
from typing import Any, Mapping
def uses_coupled_full_hand_zero_solver(
session_start: Mapping[str, Any],
) -> bool:
"""Return the solver contract recorded by the legacy session header.
Capabilities are not consulted by the live runtime. This adapter reads
the durable v1 header only so offline replay can reproduce an artifact
created before the independent thumb solver was introduced.
"""
capabilities = {
str(value) for value in session_start.get("capabilities", ())
}
return (
int(session_start.get("sample_schema_version", 1)) == 1
and "palm_axis_side_channel_v2" in capabilities
and "palm_axis_relative_motion_v3" not in capabilities
)
src/linkerhand_calibration/linkerhand_calibration/compat/paths.py
-52
View File
@@ -1,52 +0,0 @@
"""Path compatibility for immutable v1 product configurations."""
from __future__ import annotations
from pathlib import Path
_LEGACY_SOURCE_PREFIX = Path("src/g20_thumb_apriltag_calibration")
_CURRENT_SOURCE_PREFIX = Path("src/linkerhand_calibration")
def _resolve_package_uri(value: str, workspace: Path) -> Path | None:
prefix = "package://"
if not value.startswith(prefix):
return None
package_name, separator, relative = value[len(prefix) :].partition("/")
if not separator or not package_name or not relative:
raise ValueError(f"invalid ROS package resource path: {value}")
workspace_candidate = (workspace / "src" / package_name / relative).resolve()
if workspace_candidate.exists():
return workspace_candidate
try:
from ament_index_python.packages import get_package_share_directory
package_share = Path(get_package_share_directory(package_name))
except Exception:
return workspace_candidate
return (package_share / relative).resolve()
def resolve_renamed_package_path(value: str | Path, workspace: Path) -> Path:
"""Resolve workspace paths, ROS package resources and the former prefix.
Deployed v1 product YAML files are kept byte-for-byte stable because the
artifact paths participate in operational review. Existing paths always
win; package URIs prefer a source-workspace copy, and the rename mapping
is used only when the literal legacy path no longer exists.
"""
text = str(value).strip()
package_resource = _resolve_package_uri(text, workspace)
if package_resource is not None:
return package_resource
raw = Path(text).expanduser()
candidate = raw if raw.is_absolute() else workspace / raw
candidate = candidate.resolve()
if candidate.exists() or raw.is_absolute():
return candidate
try:
suffix = raw.relative_to(_LEGACY_SOURCE_PREFIX)
except ValueError:
return candidate
return (workspace / _CURRENT_SOURCE_PREFIX / suffix).resolve()
src/linkerhand_calibration/linkerhand_calibration/core/__init__.py
-76
View File
@@ -1,76 +0,0 @@
"""Hardware- and model-independent calibration kernel."""
from .domain import (
ArtifactPolicy,
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
ProfileValidationError,
QualityPolicy,
SampleRecord,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
validate_profile,
)
from .domain.task import (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
DIRECTIONS,
PHASE_ROOT,
PHASE_TIP,
)
from .fitting import FitResult, isotonic_nonincreasing
from .geometry import (
delta_rotation_vector,
fit_rotation_axis,
image_plane_tag_quaternion_xyzw,
normalize_quaternion_xyzw,
relative_quaternion_xyzw,
robust_rotation_summary,
rotation_inlier_fraction,
rotation_rms_rad,
rotation_spread_rad,
)
__all__ = [
"ArtifactPolicy",
"CalibrationProfile",
"CommandLayout",
"DIRECTION_DECREASING",
"DIRECTION_INCREASING",
"DIRECTIONS",
"FitResult",
"MeasurementPolicy",
"MeasurementSpec",
"MotionPolicy",
"PHASE_ROOT",
"PHASE_TIP",
"ProfileKey",
"ProfileValidationError",
"QualityPolicy",
"SampleRecord",
"ScopePolicy",
"TagSpec",
"TaskSpec",
"ViewSpec",
"VisionRigSpec",
"ZeroSolvePolicy",
"delta_rotation_vector",
"fit_rotation_axis",
"image_plane_tag_quaternion_xyzw",
"isotonic_nonincreasing",
"normalize_quaternion_xyzw",
"relative_quaternion_xyzw",
"robust_rotation_summary",
"rotation_inlier_fraction",
"rotation_rms_rad",
"rotation_spread_rad",
"validate_profile",
]
src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py
-5
View File
@@ -1,5 +0,0 @@
"""Artifact schema and release validation contracts."""
from .release import ReleaseValidation, ReleaseValidator
__all__ = ["ReleaseValidation", "ReleaseValidator"]
src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py
-27
View File
@@ -1,27 +0,0 @@
"""Release validation protocol used before atomic publication."""
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
from typing import Mapping, Protocol
from ..domain import CalibrationProfile
from ..urdf import UrdfCorrectionPlan
@dataclass(frozen=True)
class ReleaseValidation:
passed: bool
errors: tuple[str, ...] = ()
verified_hashes: Mapping[str, str] | None = None
class ReleaseValidator(Protocol):
def validate_release(
self,
profile: CalibrationProfile,
plan: UrdfCorrectionPlan,
calibration_json: Path,
corrected_urdf: Path,
) -> ReleaseValidation: ...
src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py
-41
View File
@@ -1,41 +0,0 @@
"""Calibration domain types."""
from .profile import (
ArtifactPolicy,
CalibrationProfile,
CommandLayout,
MeasurementPolicy,
MeasurementSpec,
MotionPolicy,
ProfileKey,
ProfileValidationError,
QualityPolicy,
ScopePolicy,
TagSpec,
TaskSpec,
ViewSpec,
VisionRigSpec,
ZeroSolvePolicy,
validate_profile,
)
from .sample import SampleRecord
__all__ = [
"ArtifactPolicy",
"CalibrationProfile",
"CommandLayout",
"MeasurementPolicy",
"MeasurementSpec",
"MotionPolicy",
"ProfileKey",
"ProfileValidationError",
"QualityPolicy",
"SampleRecord",
"ScopePolicy",
"TagSpec",
"TaskSpec",
"ViewSpec",
"VisionRigSpec",
"ZeroSolvePolicy",
"validate_profile",
]
src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py
-390
View File
@@ -1,390 +0,0 @@
"""Typed, hardware-independent calibration profile contracts."""
from __future__ import annotations
from dataclasses import dataclass, field
from pathlib import PurePath
from typing import Mapping
@dataclass(frozen=True, order=True)
class ProfileKey:
"""Stable identity for one independently reviewed hand profile."""
model: str
side: str
layout: str
revision: int = 1
def __post_init__(self) -> None:
object.__setattr__(self, "model", str(self.model).strip().upper())
object.__setattr__(self, "side", str(self.side).strip().lower())
object.__setattr__(self, "layout", str(self.layout).strip().lower())
object.__setattr__(self, "revision", int(self.revision))
if not self.model or not self.side or not self.layout:
raise ValueError("profile identity fields must be non-empty")
if self.side not in {"left", "right"}:
raise ValueError("profile side must be left or right")
if self.revision < 1:
raise ValueError("profile revision must be positive")
@property
def profile_id(self) -> str:
return f"{self.model}/{self.side}/{self.layout}/v{self.revision}"
@classmethod
def parse(cls, value: str) -> "ProfileKey":
parts = str(value).strip().split("/")
if len(parts) != 4 or not parts[3].startswith("v"):
raise ValueError(
"profile_id must be MODEL/side/layout/vREVISION"
)
return cls(parts[0], parts[1], parts[2], int(parts[3][1:]))
@dataclass(frozen=True)
class CommandLayout:
"""Command channels, joint bindings, and the reviewed baseline pose."""
names: tuple[str, ...]
baseline_u8: tuple[int, ...]
command_index_by_joint: Mapping[str, int]
disabled_indices: frozenset[int] = frozenset()
# Calibration names are allowed to stay model-neutral while the source
# URDF keeps any vendor/side prefixes (for example ``rh_``).
urdf_joint_by_joint: Mapping[str, str] = field(default_factory=dict)
# Older SDKs occasionally published a wrong label for a physically stable
# channel. Aliases are accepted only at the declared channel index.
feedback_name_aliases: Mapping[str, str] = field(default_factory=dict)
# SDK speed commands are not necessarily one value per position channel.
# This mapping makes that protocol detail explicit in a profile.
speed_slot_by_command_index: Mapping[int, int] = field(default_factory=dict)
@property
def command_count(self) -> int:
return len(self.names)
@dataclass(frozen=True)
class TagSpec:
role: str
tag_id: int
fixed_reference: bool = False
@dataclass(frozen=True)
class ViewSpec:
name: str
tags: tuple[TagSpec, ...]
@dataclass(frozen=True)
class VisionRigSpec:
"""Any number of named views and their Tag roles."""
views: tuple[ViewSpec, ...]
common_frame: str
extrinsic_reference_view: str
extrinsics_quality_limits: Mapping[str, float] = field(
default_factory=dict
)
minimum_capture_counts: Mapping[str, int] = field(default_factory=dict)
@property
def view_names(self) -> tuple[str, ...]:
return tuple(view.name for view in self.views)
@property
def tag_ids(self) -> frozenset[int]:
return frozenset(tag.tag_id for view in self.views for tag in view.tags)
@dataclass(frozen=True)
class TaskSpec:
key: str
view: str
command_index: int
joints: tuple[str, ...]
auxiliary_commands: tuple[tuple[int, int], ...] = ()
validation_only: bool = False
start_u8: int = 255
end_u8: int = 0
preflight_speed_u8: int | None = None
formal_speed_u8: int | None = None
@dataclass(frozen=True)
class MotionPolicy:
"""Reviewed motion tasks and optional safe waypoint sequences."""
tasks: tuple[TaskSpec, ...]
preparation_waypoints_u8: tuple[tuple[int, ...], ...] = ()
safe_return_waypoints_u8: tuple[tuple[int, ...], ...] = ()
speed_parameters: Mapping[str, float] = field(default_factory=dict)
precheck_sweeps: bool = False
steady_command_checkpoints: bool = False
@dataclass(frozen=True)
class MeasurementSpec:
joint: str
kind: str
view: str | None
parent_role: str | None
child_role: str | None
validation_source: str | None = None
# Some measured trajectories publish only a dynamic curve while their
# static URDF zero/axis remains CAD- or mimic-owned. For those joints a
# monocular 3-D axis-line residual is useful diagnostic evidence, but it
# must not reject an otherwise clean image/SO(3) trajectory merely because
# the hand was placed at a different valid position in the camera view.
pose_axis_line_required: bool = True
@dataclass(frozen=True)
class MeasurementPolicy:
measurements: Mapping[str, MeasurementSpec]
cross_view_sources: Mapping[str, str] = field(default_factory=dict)
image_curve_joints: frozenset[str] = frozenset()
directional_zero: bool = False
cross_view_roll_curve: bool = False
stable_cross_view_cone_bias: bool = False
@dataclass(frozen=True)
class ZeroSolvePolicy:
active_joints: frozenset[str]
passive_joints: frozenset[str]
direct_zero_joints: tuple[str, ...]
axis_joints: tuple[str, ...]
mechanical_endpoint_joints: frozenset[str]
post_solve_endpoint_joints: frozenset[str]
mimic_source_by_joint: Mapping[str, str]
cad_frozen_joints: frozenset[str]
# ``upper_at_end`` means TaskSpec.end_u8 is the trusted source-URDF upper
# physical endpoint. The measured travel then defines the electrical
# zero and corrected [0, travel] coordinate range.
endpoint_anchor_by_joint: Mapping[str, str] = field(default_factory=dict)
fitted_mimic_joints: frozenset[str] = frozenset()
# Passive coupling is not necessarily representable by the linear URDF
# ``mimic`` element. Profiles must opt in explicitly before a nonlinear
# runtime/MuJoCo relation may be published.
coupling_model_by_joint: Mapping[str, str] = field(default_factory=dict)
@dataclass(frozen=True)
class QualityPolicy:
training_cycles: tuple[int, ...]
holdout_cycle: int | None
hard_threshold_keys: frozenset[str]
retry_metric_scope: Mapping[str, str] = field(default_factory=dict)
isolated_holdout: bool = False
@dataclass(frozen=True)
class ScopePolicy:
calibrate_joints: Mapping[str, frozenset[str]]
frozen_joints: Mapping[str, frozenset[str]]
default_scope: str = "full"
def selected_joints(self, scope: str) -> frozenset[str]:
try:
return self.calibrate_joints[str(scope)]
except KeyError as error:
raise ValueError(f"unsupported calibration scope: {scope}") from error
@dataclass(frozen=True)
class ArtifactPolicy:
output_schema_version: int
calibration_filename: str
corrected_urdf_filename: str
protected_input_fields: frozenset[str]
publication_pointer: str = "latest_passed"
session_compatibility_tokens: frozenset[str] = frozenset()
publish_corrected_urdf: bool = False
@dataclass(frozen=True)
class CalibrationProfile:
key: ProfileKey
namespace: str
command: CommandLayout
vision: VisionRigSpec
motion: MotionPolicy
measurement: MeasurementPolicy
zero: ZeroSolvePolicy
quality: QualityPolicy
scope: ScopePolicy
artifacts: ArtifactPolicy
# Per-URDF-joint provenance used by partial calibration artifacts.
# Known values are: measured_static_dynamic, measured_dynamic_cad_static,
# transferred_static_dynamic, transferred_dynamic_cad_static, cad_nominal,
# and mimic_nominal.
joint_coverage: Mapping[str, str] = field(default_factory=dict)
class ProfileValidationError(ValueError):
"""Raised before hardware startup when a profile is internally unsafe."""
def validate_profile(profile: CalibrationProfile) -> None:
"""Hard-check all cross-policy references before hardware is enabled."""
errors: list[str] = []
command = profile.command
if not command.names or len(command.names) != len(command.baseline_u8):
errors.append("command names and baseline must be non-empty and aligned")
if len(set(command.names)) != len(command.names):
errors.append("command names must be unique")
if any(value < 0 or value > 255 for value in command.baseline_u8):
errors.append("baseline command values must be in [0, 255]")
indices = set(range(command.command_count))
if not set(command.disabled_indices).issubset(indices):
errors.append("disabled command index is out of range")
if any(index not in indices for index in command.command_index_by_joint.values()):
errors.append("joint command index is out of range")
if command.urdf_joint_by_joint:
if not set(command.command_index_by_joint).issubset(
command.urdf_joint_by_joint
):
errors.append("every commanded joint must map to a URDF joint")
urdf_names = tuple(command.urdf_joint_by_joint.values())
if len(set(urdf_names)) != len(urdf_names):
errors.append("URDF joint mappings must be unique")
if any(
index not in indices or slot < 0
for index, slot in command.speed_slot_by_command_index.items()
):
errors.append("speed-slot mapping is invalid")
if any(
not str(alias).strip() or canonical not in command.names
for alias, canonical in command.feedback_name_aliases.items()
):
errors.append("feedback name alias is not part of the command schema")
view_names = profile.vision.view_names
if not view_names or len(set(view_names)) != len(view_names):
errors.append("vision views must be non-empty and unique")
if profile.vision.extrinsic_reference_view not in view_names:
errors.append("extrinsic reference view is not declared")
tag_ids = [tag.tag_id for view in profile.vision.views for tag in view.tags]
tag_roles = [tag.role for view in profile.vision.views for tag in view.tags]
if len(set(tag_ids)) != len(tag_ids):
errors.append("Tag IDs must be unique across views")
if len(set(tag_roles)) != len(tag_roles):
errors.append("Tag roles must be unique across views")
if not any(
tag.fixed_reference for view in profile.vision.views for tag in view.tags
):
errors.append("at least one fixed reference Tag is required")
task_keys = [task.key for task in profile.motion.tasks]
if not task_keys or len(set(task_keys)) != len(task_keys):
errors.append("motion task keys must be non-empty and unique")
measurement_names = set(profile.measurement.measurements)
for task in profile.motion.tasks:
if task.view not in view_names:
errors.append(f"task {task.key} uses an unknown view")
if task.command_index not in indices:
errors.append(f"task {task.key} command index is out of range")
if not task.joints or not set(task.joints).issubset(measurement_names):
errors.append(f"task {task.key} references unknown measurements")
if any(index not in indices for index, _ in task.auxiliary_commands):
errors.append(f"task {task.key} auxiliary index is out of range")
if not 0 <= task.start_u8 <= 255 or not 0 <= task.end_u8 <= 255:
errors.append(f"task {task.key} sweep endpoint is out of range")
if task.start_u8 == task.end_u8:
errors.append(f"task {task.key} sweep endpoints must differ")
for speed in (task.preflight_speed_u8, task.formal_speed_u8):
if speed is not None and not 0 <= speed <= 255:
errors.append(f"task {task.key} speed is out of range")
for name, spec in profile.measurement.measurements.items():
if name != spec.joint:
errors.append(f"measurement mapping key differs for {name}")
if spec.view is not None and spec.view not in view_names:
errors.append(f"measurement {name} uses an unknown view")
for primary, validation in profile.measurement.cross_view_sources.items():
if primary not in measurement_names or validation not in measurement_names:
errors.append("cross-view measurement source is unknown")
zero = profile.zero
if zero.active_joints & zero.passive_joints:
errors.append("active and passive joints must be disjoint")
all_joints = zero.active_joints | zero.passive_joints
if not zero.active_joints.issubset(command.command_index_by_joint):
errors.append("every active joint must bind to a command channel")
if not set(zero.direct_zero_joints).issubset(zero.active_joints):
errors.append("direct zero targets must be active joints")
if not set(zero.axis_joints).issubset(all_joints):
errors.append("axis targets must be known joints")
if not zero.mechanical_endpoint_joints.issubset(zero.active_joints):
errors.append("mechanical endpoint targets must be active joints")
if not zero.post_solve_endpoint_joints.issubset(zero.active_joints):
errors.append("post-solve endpoint targets must be active joints")
if not set(zero.mimic_source_by_joint).issubset(zero.passive_joints):
errors.append("mimic targets must be passive joints")
if not set(zero.mimic_source_by_joint.values()).issubset(all_joints):
errors.append("mimic sources must be known joints")
if not set(zero.endpoint_anchor_by_joint).issubset(zero.active_joints):
errors.append("endpoint anchors must target active joints")
if not set(zero.endpoint_anchor_by_joint.values()).issubset(
{
"upper_at_end",
"lower_at_start",
"zero_at_start",
"cad_range_center",
}
):
errors.append("endpoint anchor policy is unsupported")
if not zero.fitted_mimic_joints.issubset(zero.passive_joints):
errors.append("fitted mimic targets must be passive joints")
if not zero.fitted_mimic_joints.issubset(zero.mimic_source_by_joint):
errors.append("fitted mimic target has no source mapping")
if not set(zero.coupling_model_by_joint).issubset(
zero.mimic_source_by_joint
):
errors.append("coupling model target has no source mapping")
if not set(zero.coupling_model_by_joint.values()).issubset(
{"linear_mimic", "quadratic_runtime"}
):
errors.append("coupling model policy is unsupported")
scopes = set(profile.scope.calibrate_joints)
if profile.scope.default_scope not in scopes:
errors.append("default scope is not declared")
if scopes != set(profile.scope.frozen_joints):
errors.append("scope calibration and frozen mappings must align")
for name in scopes:
selected = profile.scope.calibrate_joints[name]
frozen = profile.scope.frozen_joints[name]
if selected & frozen or selected | frozen != zero.active_joints:
errors.append(f"scope {name} must partition all active joints")
artifacts = profile.artifacts
if artifacts.output_schema_version < 1:
errors.append("artifact schema version must be positive")
for label, filename in (
("calibration", artifacts.calibration_filename),
("corrected URDF", artifacts.corrected_urdf_filename),
("publication pointer", artifacts.publication_pointer),
):
if not filename or PurePath(filename).name != filename:
errors.append(f"{label} filename must not contain a directory")
if not profile.namespace.startswith("/"):
errors.append("runtime namespace must be absolute")
if profile.joint_coverage:
valid_coverage = {
"measured_static_dynamic",
"measured_dynamic_cad_static",
"transferred_static_dynamic",
"transferred_dynamic_cad_static",
"cad_nominal",
"mimic_nominal",
}
if set(profile.joint_coverage) != all_joints:
errors.append("joint coverage must describe every profile joint")
if not set(profile.joint_coverage.values()).issubset(valid_coverage):
errors.append("joint coverage contains an unsupported status")
if errors:
raise ProfileValidationError("; ".join(errors))
src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py
-27
View File
@@ -1,27 +0,0 @@
"""Normalized records shared by online evaluation and offline replay."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Mapping
@dataclass(frozen=True)
class SampleRecord:
task_key: str
measurement: str
view: str
cycle: int
direction: str
command_u8: int
timestamp_ns: int
values: Mapping[str, Any]
quality: Mapping[str, float] = field(default_factory=dict)
def __post_init__(self) -> None:
if not self.task_key or not self.measurement or not self.view:
raise ValueError("sample task, measurement, and view are required")
if self.cycle < 0 or not 0 <= self.command_u8 <= 255:
raise ValueError("sample cycle or command is out of range")
if self.timestamp_ns < 0:
raise ValueError("sample timestamp must be non-negative")
src/linkerhand_calibration/linkerhand_calibration/core/domain/sample_schema.py
-192
View File
@@ -1,192 +0,0 @@
"""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/linkerhand_calibration/linkerhand_calibration/core/domain/task.py
-19
View File
@@ -1,19 +0,0 @@
"""Shared task direction vocabulary."""
DIRECTION_DECREASING = "decreasing"
DIRECTION_INCREASING = "increasing"
DIRECTIONS: tuple[str, ...] = (
DIRECTION_DECREASING,
DIRECTION_INCREASING,
)
PHASE_ROOT = "root"
PHASE_TIP = "tip"
__all__ = [
"DIRECTION_DECREASING",
"DIRECTION_INCREASING",
"DIRECTIONS",
"PHASE_ROOT",
"PHASE_TIP",
]
src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py
-5
View File
@@ -1,5 +0,0 @@
"""Pure curve and axis fitting."""
from .curve import FitResult, isotonic_nonincreasing
__all__ = ["FitResult", "isotonic_nonincreasing"]
src/linkerhand_calibration/linkerhand_calibration/core/fitting/curve.py
-70
View File
@@ -1,70 +0,0 @@
"""Model-independent curve fitting result and monotonic projection."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Sequence
import numpy as np
from ..geometry import delta_rotation_vector
def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray:
"""Unweighted PAVA projection onto non-increasing values."""
original = np.asarray(values, dtype=float)
if original.ndim != 1 or not np.all(np.isfinite(original)):
raise ValueError("values must be a finite vector")
negated = -original
levels: list[float] = []
weights: list[int] = []
starts: list[int] = []
for index, value in enumerate(negated):
levels.append(float(value))
weights.append(1)
starts.append(index)
while len(levels) >= 2 and levels[-2] > levels[-1]:
total_weight = weights[-2] + weights[-1]
merged = (
levels[-2] * weights[-2] + levels[-1] * weights[-1]
) / total_weight
levels[-2:] = [merged]
weights[-2:] = [total_weight]
starts.pop()
projected = np.empty_like(original)
for block_index, (level, start) in enumerate(zip(levels, starts)):
end = (
starts[block_index + 1]
if block_index + 1 < len(starts)
else len(original)
)
projected[start:end] = -level
return projected
@dataclass(frozen=True)
class FitResult:
joints: dict[str, dict[str, Any]]
axes: dict[str, tuple[float, float, float]]
references: dict[str, tuple[float, float, float, float]]
ip_coupling: dict[str, float]
max_monotonic_correction_rad: float
max_hysteresis_rad: float
measurement_mode: str = "rotation"
trajectory_models: dict[str, Any] = field(default_factory=dict)
trajectory_quality: dict[str, Any] = field(default_factory=dict)
def measure_from_reference(
self,
joint_name: str,
observed_quaternion_xyzw: Sequence[float],
reference_quaternion_xyzw: Sequence[float] | None = None,
) -> float:
reference = (
reference_quaternion_xyzw
if reference_quaternion_xyzw is not None
else self.references[joint_name]
)
vector = delta_rotation_vector(reference, observed_quaternion_xyzw)
axis = np.asarray(self.axes[joint_name], dtype=float)
return float(vector @ axis)
src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py
-41
View File
@@ -1,41 +0,0 @@
"""Pure geometry used by online and offline calibration."""
from .extrinsics import (
CameraCalibrationIdentity,
CameraExtrinsics,
camera_info_fingerprint,
load_camera_extrinsics,
matrix_payload,
transform_matrix,
validate_camera_extrinsics_payload,
)
from .rotation import (
delta_rotation_vector,
fit_rotation_axis,
image_plane_tag_quaternion_xyzw,
normalize_quaternion_xyzw,
relative_quaternion_xyzw,
robust_rotation_summary,
rotation_inlier_fraction,
rotation_rms_rad,
rotation_spread_rad,
)
__all__ = [
"CameraCalibrationIdentity",
"CameraExtrinsics",
"camera_info_fingerprint",
"delta_rotation_vector",
"fit_rotation_axis",
"image_plane_tag_quaternion_xyzw",
"load_camera_extrinsics",
"matrix_payload",
"normalize_quaternion_xyzw",
"relative_quaternion_xyzw",
"robust_rotation_summary",
"rotation_inlier_fraction",
"rotation_rms_rad",
"rotation_spread_rad",
"transform_matrix",
"validate_camera_extrinsics_payload",
]
src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py
-157
View File
@@ -1,157 +0,0 @@
"""Pure quaternion summaries and rotation-axis fitting."""
from __future__ import annotations
import math
from typing import Sequence
import numpy as np
from scipy.spatial.transform import Rotation
def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray:
quaternion = np.asarray(values, dtype=float)
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
raise ValueError("quaternion must contain four finite xyzw values")
norm = float(np.linalg.norm(quaternion))
if norm < 1e-12:
raise ValueError("quaternion norm is zero")
return quaternion / norm
def relative_quaternion_xyzw(
parent_camera_quaternion: Sequence[float],
child_camera_quaternion: Sequence[float],
) -> tuple[float, float, float, float]:
"""Compute parent-to-child orientation from two camera-to-Tag rotations."""
parent = Rotation.from_quat(
normalize_quaternion_xyzw(parent_camera_quaternion)
)
child = Rotation.from_quat(
normalize_quaternion_xyzw(child_camera_quaternion)
)
quaternion = (parent.inv() * child).as_quat()
return tuple(float(value) for value in quaternion)
def image_plane_tag_quaternion_xyzw(
corners_xy: Sequence[Sequence[float]],
) -> tuple[float, float, float, float]:
"""Estimate Tag orientation about the optical axis from ordered corners."""
corners = np.asarray(corners_xy, dtype=float)
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
raise ValueError("corners_xy must contain four finite xy points")
x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3])
if float(np.linalg.norm(x_axis)) < 1e-9:
raise ValueError("tag x-axis is degenerate")
angle = -math.atan2(float(x_axis[1]), float(x_axis[0]))
quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
return tuple(float(value) for value in quaternion)
def robust_rotation_summary(
quaternions_xyzw: Sequence[Sequence[float]],
) -> tuple[tuple[float, float, float, float], float]:
"""Return a robust orientation and maximum angular residual in radians."""
if not quaternions_xyzw:
raise ValueError("at least one quaternion is required")
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
reference = rotations[0]
delta_vectors = (reference.inv() * rotations).as_rotvec()
median_delta = np.median(delta_vectors, axis=0)
robust = reference * Rotation.from_rotvec(median_delta)
residuals = (robust.inv() * rotations).magnitude()
maximum = float(np.max(residuals)) if residuals.size else 0.0
return tuple(float(value) for value in robust.as_quat()), maximum
def rotation_spread_rad(
quaternions_xyzw: Sequence[Sequence[float]],
) -> float:
"""Return the maximum geodesic residual around a robust orientation."""
_, spread = robust_rotation_summary(quaternions_xyzw)
return spread
def rotation_rms_rad(
quaternions_xyzw: Sequence[Sequence[float]],
*,
outlier_threshold_rad: float | None = None,
) -> float:
"""Return RMS geodesic noise around a robust orientation."""
robust, _ = robust_rotation_summary(quaternions_xyzw)
reference = Rotation.from_quat(robust)
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
residuals = (reference.inv() * rotations).magnitude()
if outlier_threshold_rad is not None:
threshold = float(outlier_threshold_rad)
if threshold <= 0.0:
raise ValueError("outlier_threshold_rad must be positive")
residuals = residuals[residuals <= threshold]
if residuals.size == 0:
return float("inf")
return float(np.sqrt(np.mean(np.square(residuals))))
def rotation_inlier_fraction(
quaternions_xyzw: Sequence[Sequence[float]],
*,
outlier_threshold_rad: float,
) -> float:
"""Return the fraction close to the robust orientation."""
threshold = float(outlier_threshold_rad)
if threshold <= 0.0:
raise ValueError("outlier_threshold_rad must be positive")
robust, _ = robust_rotation_summary(quaternions_xyzw)
reference = Rotation.from_quat(robust)
rotations = Rotation.from_quat(
np.asarray(
[normalize_quaternion_xyzw(value) for value in quaternions_xyzw],
dtype=float,
)
)
residuals = (reference.inv() * rotations).magnitude()
return float(np.mean(residuals <= threshold))
def delta_rotation_vector(
reference_xyzw: Sequence[float],
observed_xyzw: Sequence[float],
) -> np.ndarray:
reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw))
observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw))
return (reference.inv() * observed).as_rotvec()
def fit_rotation_axis(
vectors: Sequence[Sequence[float]],
commands: Sequence[int],
) -> np.ndarray:
"""Fit and orient the single rotational axis used by one command sweep."""
matrix = np.asarray(vectors, dtype=float)
command_values = np.asarray(commands, dtype=int)
if matrix.ndim != 2 or matrix.shape[1] != 3:
raise ValueError("vectors must have shape (N, 3)")
if command_values.shape != (matrix.shape[0],):
raise ValueError("commands must match vectors")
useful = np.linalg.norm(matrix, axis=1) > 1e-6
if int(np.count_nonzero(useful)) < 3:
raise ValueError("insufficient non-zero rotations to fit an axis")
_, _, vh = np.linalg.svd(matrix[useful], full_matrices=False)
axis = vh[0]
projections = matrix @ axis
low = projections[command_values <= 16]
high = projections[command_values >= 239]
if low.size and high.size and float(np.median(low)) < float(np.median(high)):
axis = -axis
return axis / np.linalg.norm(axis)
src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py
-15
View File
@@ -1,15 +0,0 @@
"""Task acceptance and final-session solver contracts."""
from .interfaces import (
SessionSolution,
SessionSolver,
TaskEvaluation,
TaskEvaluator,
)
__all__ = [
"SessionSolution",
"SessionSolver",
"TaskEvaluation",
"TaskEvaluator",
]
src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py
-42
View File
@@ -1,42 +0,0 @@
"""Shared evaluator and final-solver interfaces."""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Mapping, Protocol, Sequence
from ..domain import CalibrationProfile, SampleRecord, TaskSpec
@dataclass(frozen=True)
class TaskEvaluation:
accepted: bool
failures: tuple[Mapping[str, Any], ...] = ()
rescan_measurements: frozenset[str] = frozenset()
rescan_cycles: frozenset[int] = frozenset()
@dataclass(frozen=True)
class SessionSolution:
passed: bool
calibration: Mapping[str, Any]
zero_offsets_rad: Mapping[str, float]
failures: tuple[Mapping[str, Any], ...] = ()
diagnostics: Mapping[str, Any] = field(default_factory=dict)
class TaskEvaluator(Protocol):
def evaluate_task(
self,
profile: CalibrationProfile,
task: TaskSpec,
samples: Sequence[SampleRecord],
) -> TaskEvaluation: ...
class SessionSolver(Protocol):
def solve_session(
self,
profile: CalibrationProfile,
samples: Sequence[SampleRecord],
) -> SessionSolution: ...
src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py
-5
View File
@@ -1,5 +0,0 @@
"""URDF correction authorization and validation types."""
from .plan import UrdfCorrectionPlan, build_correction_plan
__all__ = ["UrdfCorrectionPlan", "build_correction_plan"]
src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py
-104
View File
@@ -1,104 +0,0 @@
"""One authorization plan shared by URDF writers and validators."""
from __future__ import annotations
from dataclasses import dataclass, field
import hashlib
from pathlib import Path
from typing import Mapping
from ..domain import CalibrationProfile
@dataclass(frozen=True)
class UrdfCorrectionPlan:
source_sha256: str
allowed_active_joints: frozenset[str]
endpoint_limit_joints: frozenset[str]
mimic_source_by_joint: Mapping[str, str]
frozen_joints: frozenset[str]
frozen_offsets_rad: Mapping[str, float] = field(default_factory=dict)
forbid_calibrated_source: bool = True
forbid_overwrite: bool = True
preserve_passive_joints: bool = True
def __post_init__(self) -> None:
if len(self.source_sha256) != 64 or any(
character not in "0123456789abcdef"
for character in self.source_sha256.lower()
):
raise ValueError("source URDF SHA-256 is invalid")
if self.allowed_active_joints & self.frozen_joints:
raise ValueError("allowed and frozen URDF joints overlap")
applied = self.allowed_active_joints | set(self.frozen_offsets_rad)
if not set(self.frozen_offsets_rad).issubset(self.frozen_joints):
raise ValueError("frozen offsets must belong to frozen joints")
if not self.endpoint_limit_joints.issubset(applied):
raise ValueError("endpoint limit joint is not an applied active joint")
if set(self.mimic_source_by_joint) & self.allowed_active_joints:
raise ValueError("dependent mimic joints cannot be active edit targets")
def authorize_offsets(self, offsets_rad: Mapping[str, float]) -> None:
required = self.allowed_active_joints | set(self.frozen_offsets_rad)
unexpected = set(offsets_rad) - required
if unexpected:
raise ValueError(
"URDF correction contains unauthorized joints: "
+ ", ".join(sorted(unexpected))
)
missing = required - set(offsets_rad)
if missing:
raise ValueError(
"URDF correction is missing active joints: "
+ ", ".join(sorted(missing))
)
changed_frozen = {
name
for name, expected in self.frozen_offsets_rad.items()
if abs(float(offsets_rad[name]) - float(expected)) > 1.0e-12
}
if changed_frozen:
raise ValueError(
"URDF correction changed frozen offsets: "
+ ", ".join(sorted(changed_frozen))
)
def verify_source(self, source_urdf: str | Path) -> None:
digest = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest()
if digest != self.source_sha256.lower():
raise ValueError("source URDF SHA-256 differs from correction plan")
def build_correction_plan(
profile: CalibrationProfile,
*,
source_sha256: str,
scope: str,
frozen_offsets_rad: Mapping[str, float] | None = None,
) -> UrdfCorrectionPlan:
"""Build one scope-aware edit authorization from typed policies."""
selected = profile.scope.selected_joints(scope)
frozen = profile.scope.frozen_joints[str(scope)]
expected_frozen = {
str(name): float(value)
for name, value in dict(frozen_offsets_rad or {}).items()
}
if set(expected_frozen) != set(frozen):
missing = set(frozen) - set(expected_frozen)
extra = set(expected_frozen) - set(frozen)
raise ValueError(
"frozen URDF offset state differs from scope policy: "
f"missing={','.join(sorted(missing)) or '-'};"
f"extra={','.join(sorted(extra)) or '-'}"
)
applied = selected | frozen
return UrdfCorrectionPlan(
source_sha256=source_sha256,
allowed_active_joints=selected,
endpoint_limit_joints=(
profile.zero.mechanical_endpoint_joints & applied
),
mimic_source_by_joint=profile.zero.mimic_source_by_joint,
frozen_joints=frozen | profile.zero.cad_frozen_joints,
frozen_offsets_rad=expected_frozen,
)

Some files were not shown because too many files have changed in this diff Show More