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:f94cf2c50005439a681d383b1716803801fbef05
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
f94cf2c500 .. o30

Author SHA1 Message Date
admin 57babb966b O30临时标定提交 2026-08-17 17:55:13 +08:00
487 changed files with 21690 additions and 57867 deletions
Expand all files Collapse all files
.gitignore
-6
View File
@@ -69,10 +69,6 @@ Thumbs.db
*_mapping_quality.json
# Device-specific robot descriptions derived from local calibration runs
# Includes full/partial zero-calibration outputs and local copies.
/src/linkerhand_calibration/urdf/*/*_calibrated_*.urdf
/src/linkerhand_calibration/urdf/*/*_zero_calibrated*.urdf
/src/linkerhand_calibration/urdf/*/*_transferred_from_*.urdf
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
/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
@@ -96,5 +92,3 @@ candump-*
# Local Codex/agent workspace metadata
/.agents/
/.codex/
/.codebuddy/
/.zcode/
AGENTS.md
-1
View File
@@ -1 +0,0 @@
避免产生屎山代码，要求代码结构清晰可读性好
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/agillink_omnihand_sdk
-1
Submodule src/agillink_omnihand_sdk deleted from 026740d9fd
src/g20_thumb_apriltag_calibration/README.md
+778
View File
@@ -0,0 +1,778 @@
# G20 左右手与 O30 右手 AprilTag 标定
O30 右手使用相同的三相机/11-Tag 几何采集框架，但采用独立的 20 电机
profile、8 项扫描任务和主动关节零位策略。完整映射、固定基准命令和启动方法见
[O30 右手操作说明](../../docs/O30右手三相机标定与URDF修正操作说明.md)。
## 三机位三维关节轴零位标定（schema v4）
正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有
`/g20_calibration` 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要
贴正；相机和Tag在一次标定中必须固定。默认绑定为：
```text
front = DB2163742，Tag 0/1/2/3/10
side = DB2163749，Tag 4/5/6/7
top = DB2163739，Tag 8/9
```
11 张 `tag36h11` 的程序角色必须与贴纸所在刚性件一致：
| ID | 机位 | 固定位置/运动件 |
|---:|---|---|
| 0 | 正面 | 正面掌壳固定基准 |
| 1 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 |
| 2 | 正面 | 拇指 MCP 后连杆 |
| 3 | 正面 | 拇指 IP 后末节 |
| 4 | 侧面 | 掌壳侧面固定基准（最底下） |
| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 |
| 9 | 上面 | 拇指 CMC yaw 运动件 |
| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见，
ID 9 必须在拇指横摆的完整行程中持续可见。
贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。
每台相机必须有独立内参文件：
```text
~/.ros/camera_info/hikrobot_DB2163742.yaml
~/.ros/camera_info/hikrobot_DB2163749.yaml
~/.ros/camera_info/hikrobot_DB2163739.yaml
```
### 1. 一次性三相机外参
三相机第一次安装、任何相机移动、镜头重新聚焦或内参变化后，必须重标外参。使用
`8x5` 内角点、实测方格边长 `27 mm`、粘在硬质平板上的棋盘：
```bash
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
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
```
启动后默认打开 `G20 Three-Camera Extrinsics` 交互窗口。可切换
`FRONT + SIDE` 和 `FRONT + TOP`；窗口实时显示棋盘角点、单相机/组合
RMS、时间差、联合拟合稳定性和候选/内点数量。单张只要棋盘完整、
同步、RMS和姿态差异合格，`ADD CANDIDATE` 就会变绿；不再用单张
PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒会自动
采集，移到新姿态后再自动采下一组。
外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加
一个候选；随后以相同方法采集正面/上面。程序使用固定内参的
`stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和
配对相机的单帧RMS分别不得超过1.5 px，同时组合RMS不得超过1.2 px；界面中
单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。
新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组，
再在不低于15个内点的前提下有界裁剪联合误差最高的候选，最终1.2 px门限不会被
放宽。两组均得到
至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。
```bash
ros2 service call /g20_camera_extrinsics/capture_front_side std_srvs/srv/Trigger {}
ros2 service call /g20_camera_extrinsics/capture_front_top std_srvs/srv/Trigger {}
ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
```
采集时可分别查看 `/g20_extrinsics/{front,side,top}/camera/image_rect`。界面始终
显示当前配对的整批RMS、旋转稳定性和平移稳定性；`BATCH FAIL` 后会直接列出
`INLIERS`、`RMS`、`ROT` 或 `TRANS` 失败项。保存门限为：联合重投影RMS不超过
1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过1.5 mm。文件同时
绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存通过结果，正式
标定也不会运动。
### 2. 预检和正式标定
先使用禁止运动模式检查三个机位、外参、内参和标签：
```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 \
commands_enabled:=false
```
分别查看正式流程的三个画面：
```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
```
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程：
```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
```
右手使用同一入口；默认自动选择右手SDK话题和原始URDF：
```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 \
can_interface:=can0
```
状态显示三个机位均“就绪”后只调用一次：
```bash
ros2 topic echo /g20_calibration/status_text
ros2 service call /g20_calibration/start std_srvs/srv/Trigger {}
```
收到 `start` 后，程序先下发并确认以下20通道基准姿态，稳定保持0.5秒后才开始
第一条轨迹扫描：
```text
[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
```
左手依次扫描电机 `0/5/15/6/1/16/10`，右手依次扫描
`0/5/15/9/4/19/10`，每项三轮 `255→0→255`。轨迹角由父/子Tag完整相对
四元数的旋转向量投影到三维拟合轴得到。
轴方向使用相对姿态旋转轴和可信上游轴约束；仅对斜视、非约束关节将中心圆作为独立
交叉检查并参与融合。轴线上一点则由整段
相对SE(3)轨迹的 `(I-R)p=t` 方程鲁棒拟合，不再把单目Tag中心自由三维圆的圆心直接
当成机械轴心。正面/侧面端视关节只使用图像平面内可观分量，丢弃无法由单目确定的
光轴深度；斜视轨迹仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令
严格归零：普通通道255，四指侧摆127。左手将食指动态轨迹、右手将小指动态轨迹继承
给其余三指。11-Tag布局只能可靠恢复参考指的动态命令—角度曲线，不能证明四根独立
电机的绝对装配相位相同；因此四指全部MCP侧摆、MCP屈伸和PIP静态URDF零偏都保留
原始CAD的0，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。
零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定
掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免
稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节
进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游
轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差
超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴
Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过
1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°；
小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。
`thumb_mcp` 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过
被动 `thumb_ip` 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴
偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手
`thumb_mcp` 都保留原始CAD零位0。该保护只冻结静态 `origin.rpy`，不会冻结或复制其
`angle_rad[256]` 实测轨迹。
前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。
四指参考源的MCP pitch虽有约70°大行程，但侧面机位接近沿转轴观察，单目PnP深度偏差
仍可能把低残差的Tag中心圆平面稳定地倾斜。因此MCP pitch与其他端视关节一样，始终用
完整相对姿态确定轴方向，Tag中心轨迹只参与轴线位置拟合；不再按10°分界在两种轴模型
之间切换。固定Tag安装旋转会在相对旋转中抵消，不需要中心圆回退。被动PIP/DIP继承
上游轴方向时不重复报告同一项跨轮轴失败。
四指MCP侧摆的动态曲线仍由参考指三轮实测并继承，但绝对静态侧摆零位固定使用原始CAD
的0。仅凭下游pitch轴相对CAD掌坐标反推roll相位，会把稳定的跨视角/固定几何偏差写成
约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零
修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何
按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。
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
会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索
不会放宽正式结果的硬门限。
生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。
坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终
拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
仍保存在 `raw_samples.jsonl`。
每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
分支；侧面Tag 4/5/6/7贴面在该端点应近似平行，初始化还会比较相邻Tag法向，避免
错误镜像分支虽然8帧稳定且重投影很小仍被选中。每轮 `255→0` 前都会在静止端点独立
重置并重新选择分支，使第三轮同时成为PnP初始化留出，而不是三轮共享同一错误分支。
再开始正式轨迹采集。初始化帧不写入轨迹；最终单轴、跨轮和留出门限不变。
左手测食指roll时将电机7/8/9固定到0；右手测小指roll时因左右手侧摆机构镜像，
将电机6/7/8固定到255。两者均为相机画面向右的物理避挡方向，速度分别为
`[15,5,15,15,15]` 和 `[15,15,15,15,5]`。参考指MCP pitch/PIP扫描分别使用
电机1/16（左）或4/19（右），参考指速度10。
右手扫描拇指CMC俯仰（电机0）前，程序将拇指横摆电机10和拇指侧摆电机5都固定到
255，确认两个辅助关节到位后才允许电机0执行全行程。该关节由正面机位使用掌部
Tag 0和运动Tag 1同帧测量。每帧都会保留两个辅助关节
的实测条件值，轴线经外参转换到公共坐标系，零位求解按URDF上游关节链补偿；左手
仍沿用原有正面机位和基准姿态。
标定 `thumb_cmc_yaw`（电机10）时，程序将
`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag
ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 `resume` 都保持
电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才
恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员
暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的
`baseline_command_u8` 不变。
右手标定小指PIP（电机19）时，命令0对应的固件反馈可能稳定饱和在5。只有电机19
的0端使用±5反馈容差，并将该实测机械端点归入命令0端点分箱；255端和其他电机仍
使用默认±2。轨迹仍须覆盖至少240个u8并通过完整拟合门限，所以中途卡滞不会被误判
为端点到达。
右手拇指横摆电机10在命令255时多次实测稳定饱和在250，因此仅右手电机10的255端
使用±5反馈容差；其命令0端实测反馈为4，仍使用±4，其他电机和中间位置不放宽。基准姿态、作为
电机0辅助避挡姿态以及电机10自身扫描端点都使用同一条专用判定。
三机位流程默认 `validation_enabled:=false`，即不增加随机机械动作，但第三轮留出验证
始终启用且不能关闭；最终 `quality.validation_mae_rad/p95_rad` 正是第三轮轨迹误差。
状态中的扫描进度和总体进度分开显示：42/42只表示计划轨迹已采完，总体进度在拟合和
验证完成、正式JSON与URDF成功写入之前不会显示100%。
上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag
和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求
当前活动机位就绪；首次调用 `start` 仍要求三个机位全部通过预检。
### 3. 输出
通过后生成精简JSON和一个新URDF：
```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
calibration_output/G20_RIGHT_001/<时间戳>/
g20_right_G20_RIGHT_001_calibration.json
src/.../g20_right/
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
```
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖：
```bash
python3 -m g20_thumb_apriltag_calibration.offline_replay \
calibration_output/G20_RIGHT_001/20260811_120146 \
--output-tag AXIS_FRAME_V3 \
--write
```
下面保留原有正面拇指独立标定说明和兼容入口。
### 4. 修正URDF的运行时关节映射
修正URDF已经把 `zero_angles.urdf_zero_offset_rad` 写入关节
`origin.rpy`。仿真运行时只能再使用同一台、同一侧机械手JSON中的256点
`angle_rad` 动态曲线，不能把 `urdf_zero_offset_rad` 再加一次，也不能把左手曲线
用于右手URDF。可用桥接节点将GUI的20通道u8命令转换为完整21关节
`JointState`（包括5个被动关节）：
```bash
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
```
默认订阅 `/cb_right_hand_control_cmd`，发布
`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
因此不会静默退回旧标定。
该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`apriltag_ros`、
Linker Hand SDK 和标定状态机，
只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次
终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置，
程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。
完成 `255→0→255` 后分别拟合正反方向并检查回差，最终运行时 JSON 将两条曲线逐点
平均，只为每个关节保存一个 256 项 `angle_rad`。最后用 5 个随机静态命令复测精度。
当前默认使用 `trajectory_center_3d`。节点由四个亚像素角点和 `CameraInfo.P`
计算每张 Tag 的三维中心，但不把小尺寸平面 Tag 的 PnP 朝向直接当作关节角：
- 根部扫描先减去掌心 T0 的位置，再用 T3/T4/T5 三条圆轨迹共同拟合 CMC 旋转轴；
每帧三个角度取中位数。
- 尖部扫描用 T4 相对 T3 的圆轨迹直接拟合 MCP。G20 只有电机 15 这一个尖部输入，
URDF 将被动 IP 定义为 `thumb_ip = 1.02 × thumb_mcp`，因此运行时 IP 曲线严格按
这个机械耦合生成。这样不会把不同相机角度下 T5 的平面 PnP 深度偏差误认为 IP
真实运动。
- 程序仍会按 MCP 角将 T5 反向旋转并拟合剩余小圆，但该结果只用于
`trajectory_center_quality.tip` 中的观测一致性诊断，不参与最终 IP 数组。
- 每条曲线都减去命令 255 的测量角，所以最终文件严格满足
`angle_rad[255] == 0.0`；`angle_rad[0]` 是该关节相对零位的最大角度。
这种方法对固定的相机摆放角度、Tag 在同一刚性连杆上的固定位置和贴纸朝向更不敏感。
但相机或贴纸在一次扫描过程中移动、Tag 翘起、角点严重抖动仍会破坏圆轨迹。程序会
检查平面残差、圆残差、轨迹半径、实际弧长和根部三个轨迹点的角度一致性。
PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维中心及辅助质量检查，不再
直接生成运行时角度。根部扫描用固定的 T3–T4、T4–T5 中心间距共同选择分支；
尖部扫描用固定的 T0–T3 中心间距约束非目标部分。中心间距漂移超过阈值仍会暂停，
避免错误中心进入圆拟合，但 Tag 的 PnP 朝向抖动不会触发该门限。
## 1. 标记和安全检查
- `T0` 必须保留并固定在掌壳，作为整体平移参考；`T3` 固定在拇指根部运动连杆，`T4` 固定在 MCP 后的连杆，
`T5` 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。
- 当前实物使用 `tag36h11` 的 ID `0/1/2/3`，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改
`config/front_tags.yaml` 里检测节点和标定节点的两组数组。
- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前实物黑色正方形实测为
`16 mm`，因此配置为 `0.016`。
测量检测角点所围成的正方形边长，不包含外围白色留边。
- 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由
静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时，
启动参数增加 `publish_debug_image:=true`，再订阅
`/g20_thumb_calibration/debug_image`；正式长时间扫描建议保持默认关闭。
- 执行全行程前清空拇指周围空间并准备断开电机电源。确认这只手的下标 0 和 15
均可安全走完整 `255→0→255`。标定节点发现命令话题上另有发布者时不会解锁扫描。
## 2. 安装与构建
```bash
sudo apt-get update
sudo apt-get install -y \
ros-jazzy-image-pipeline \
ros-jazzy-apriltag-ros \
ros-jazzy-apriltag-msgs \
ros-jazzy-camera-calibration \
python3-yaml
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
```
相机节点直接使用海康 MVS SDK。当前机器的默认安装位置是 `/opt/MVS`，需要存在：
```text
/opt/MVS/lib/64/libMvCameraControl.so
/opt/MVS/Samples/64/Python/MvImport/MvCameraControl_class.py
```
正面相机默认按序列号 `DB2163742` 绑定（MVS 显示的 GUID 是
`2BDFB2163742`），型号校验为 `MV-CS020-10UM`。三台相机同时连接时程序不会按枚举
顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。
`1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。
三相机标定 launch 会固定使用 `rmw_fastrtps_cpp`，并通过新旧两个 Fast DDS 环境变量
加载 `config/fastdds_large_images.xml`，使用 64 MB 共享内存段；否则相机内部虽为 30 Hz，
大图订阅端通常只能收到约 1～4 Hz。修改配置后必须重启相关 ROS 进程才能生效。
首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从
`~/.ros/camera_info/hikrobot_DB2163742.yaml` 加载标准 ROS CameraInfo YAML；文件缺失时
仍可预览 `mono8` 原图，但发布的内参无效，轨迹标定预检不会解锁运动。
先单独启动相机（不会连接机械手，也不会发送关节命令）：
```bash
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
```
测速时优先检查同帧发布的小消息和原图；两者正常值都应接近 30 Hz：
```bash
ros2 topic hz /camera/camera/color/camera_info
ros2 topic hz /camera/camera/color/image_raw
```
使用标定板采集内参。下面的 `8x6` 是内角点数量、`0.020` 是单格边长 20 mm，必须按
实际标定板修改：
```bash
ros2 run camera_calibration cameracalibrator \
--size 8x6 --square 0.020 \
--camera_name hikrobot_front_DB2163742 \
--ros-args \
--remap image:=/camera/camera/color/image_raw \
--remap camera/set_camera_info:=/camera/camera/color/set_camera_info
```
在标定界面完成采样后点击 `CALIBRATE`，确认重投影误差，再点击 `COMMIT`。相机节点会
原子写入上述 YAML，并立即开始发布有效内参。内参只适用于标定时的镜头焦距、对焦、
分辨率和 ROI；改变任何一项都要重新标定。
连接 CAN 后先确认 `can0` 已启动。不要同时运行其他会发布
`/g20/cb_left_hand_control_cmd` 的程序。
## 3. 启动和操作
首次使用时可先用 `commands_enabled:=false` 做预检；SDK 仍会设置速度/扭矩并读取状态，
但标定节点不会发送位置运动命令，也不会允许解锁全行程扫描：
```bash
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
camera_serial_number:=DB2163742 \
commands_enabled:=false
```
确认 T0、T3、T4、T5 在根部和尖部全行程中不会被遮挡，且拇指运动不会碰撞后，
停止预检并启动一个新的正式会话。默认使用 AprilTag 内部 `decimate=1.5` 提升检测
速度，并使用单终点连续运动：
```bash
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
serial_number:=G20_LEFT_001 \
camera_serial_number:=DB2163742 \
can_interface:=can0 \
calibration_speed:=15 \
continuous_motion_mode:=endpoint \
angle_estimation_mode:=trajectory_center_3d \
apriltag_decimate:=1.5 \
use_roi:=false
```
默认关闭 ROI，AprilTag 使用完整的 1624×1240 校正画面。查看实际送入 AprilTag
的完整画面：
```bash
ros2 run image_view image_view --ros-args \
--remap image:=/camera/camera/color/image_rect
```
图像检测链路使用 `sensor_data`（BEST_EFFORT）QoS，只保留最新帧，避免完整分辨率
下可靠队列积压反压相机；这不会裁剪图像，也不会降低相机分辨率。
若以后需要以帧率优先，可传入 `use_roi:=true`；默认 ROI 是原图中的
`x=128, y=192, width=1024, height=528`，也可用 `roi_x`、`roi_y`、
`roi_width`、`roi_height` 覆盖。
监控状态：
```bash
ros2 topic echo /g20_thumb_calibration/status
```
预检通过后状态为 `WAIT_ROOT_CONFIRM`，`reason` 为 `call_start`。只需调用一次：
```bash
ros2 service call /g20_thumb_calibration/start std_srvs/srv/Trigger {}
```
节点随后自动完成下标 0 的 `255→0→255`、下标 15 的 `255→0→255` 和 5 点随机复测，
正常结束状态为 `COMPLETE`，无需在根部和尖部之间再次确认。为安全起见，调用 `start`
前必须一次性确认两个关节的完整行程都已清空。原来的
`confirm_root_full_range`、`confirm_tip_full_range` 服务仍保留用于兼容。
暂停、恢复和终止：
```bash
ros2 service call /g20_thumb_calibration/pause std_srvs/srv/Trigger {}
ros2 service call /g20_thumb_calibration/resume std_srvs/srv/Trigger {}
ros2 service call /g20_thumb_calibration/abort std_srvs/srv/Trigger {}
```
预检要求四 Tag 有效帧率至少 95%，且检测消息频率至少 15 Hz。PnP 有效率也必须
至少 95%，每个候选解的重投影 RMS 不超过 1.5 px。中心轨迹模式以三组相对中心
的静止 RMS 不超过 2 mm、5 mm 范围内位置内点不少于 90% 为硬判据；PnP 朝向抖动
只作为诊断，不会阻止静态捕获。
状态中的
`pnp_rejections` 会指出当前是哪张 Tag 因丢失、重投影/倾角超限或姿态跳变而被拒绝，
`pnp_reprojection_error_px` 显示四张 Tag 最近一次有效解的误差。连续扫描要求
图像与状态的时间差不超过 150 ms、全行程至少得到 40 个有效帧、
至少覆盖 32 个整数位置且相邻实测位置间隔不超过 16。Tag 或同步状态持续丢失 3 秒、
90 秒内未到达终点，或覆盖不足时，节点保持当前命令并进入 `PAUSED`。恢复时会先回到
该方向的起点，再完整重扫这个方向，避免把半程数据混入结果。`abort` 也只停止队列，
不会主动移动机械手。正常扫描和随机复测最后一项均为命令 255。
PnP 跟踪在整个会话中对四张 Tag 都优先保持同一个 IPPE 平面分支；最多 5 秒的短暂检测
间隔不会重新初始化分支。随机复测只有在同步电机反馈与目标相差不超过 2、且稳定
窗口与捕获窗口内三个相对中心的最大偏差都不超过 3 mm 时才会写入，否则继续等待并最终暂停，
不会再生成明知不可靠但字段完整的结果。
单终点连续模式共有 4 个端到端命令：根部和尖部各一个往返。每个方向运动前会先用
实际电机反馈确认已经到达起点，再做一次短暂静态确认；随机验证的“接近位置”只等待
电机反馈到位，不再重复采图。若实际 AprilTag 检测仍低于 15 Hz，先优化检测链路，
不要降低到固件低速区。必须临时回退时可启动
`continuous_motion_mode:=paced`，该模式按步长 8 到位即发下一段。
连续扫描中的主要状态字段：
- `state_zh`/`reason_zh`/`action_zh`：当前阶段、失败原因和下一步操作的中文说明；
原有 `state`/`reason` 英文机器码继续保留。
- `tag_quality`：逐张显示 T0/T3/T4/T5 的边长、hamming、识别置信度、重投影误差、
是否有效和具体中文问题，不再需要手工解析 `/apriltag/detections`。
- `/g20_thumb_calibration/status_text`：适合终端直接查看的多行中文状态。使用
`ros2 topic echo --once /g20_thumb_calibration/status_text --field data`
即可看到原因、建议及四张标签的质量。
- `scan_progress`：4 个方向的完成比例，依次约为 0、0.25、0.5、0.75、1.0。
- `sweep_valid_frames_seen`：当前连续方向已收到的同步有效帧数。
- `sweep_state_span_u8`：当前方向实际覆盖的电机范围，接近 255 才算完整。
- `active_phase`/`active_direction`：当前是根部或尖部、下降或上升方向。
- `pnp_branch_corrections`：四张 Tag 联合跟踪为维持相邻关节姿态连续，而没有选择
单张 Tag 最小重投影分支的累计次数。
- `pnp_trajectory_quality`：最近一个完整方向的整段分支修正帧数，以及相对整段稳健
参考的旋转、相对平移和中心间距漂移。中心轨迹模式只按欧氏中心间距判断：
P95 超过 3 mm 或单帧最大值超过 6 mm 时暂停；旋转及随 Tag 坐标轴表达的相对平移
只保留为诊断。
- `trajectory_center_quality`：四个方向完成并拟合后，显示三维平面/圆残差、拟合半径、
实际弧长、T0/T3 锚点漂移和根部三个轨迹点的角度一致性。其中
`tip.ip_observed_vs_constrained_*` 显示T5残余小圆与URDF被动耦合之间的差异；
它用于发现T5识别误差、标签松动或机构异常，但不会改变最终IP曲线。
根部扫描中 T3/T4/T5 作为完整刚性组共同选择 IPPE 分支，不再把 T3 固定为在线解；
尖部扫描仍固定 T3，只用静止的 T0/T3 约束修正非目标根部姿态。
## 4. 中断恢复和输出
默认会话目录是启动命令当前目录下：
```text
calibration_output/<序列号>/<时间戳>/
```
恢复时必须显式复用原目录，否则会创建新会话：
```bash
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
```
恢复会校验序列号、Tag 配置、基准命令、扫描模式、采集参数和代码哈希；
任一项变化都会拒绝混用旧样本，
此时应新建会话。
目录内文件：
- `raw_samples.jsonl`：连续帧按实际整数电机位置分箱后的 Tag 三维中心、姿态辅助统计及复测点；每完成一个
扫描方向后落盘。
- `checkpoint.json`：当前状态和进度。
- `session_manifest.json`：Tag、相机内参、SDK、代码哈希和会话信息。
- `validation.json`：随机复测及全部质量判据。
- `rosbag/`：仅在 `record_bag:=true` 时生成，用于保存相机、检测、命令和状态等诊断数据。
- `g20_left_<序列号>_thumb_angle.json`：精简后的运行时标定文件。
最终文件使用 `schema_version: 2`。每个关节只包含：
```json
{
"motor_index": 0,
"angle_rad": ["按命令0～255索引的256个弧度值"]
}
```
`thumb_ip.angle_rad` 由 `thumb_mcp.angle_rad` 乘
`ip_coupling.multiplier`（默认 `1.02`）得到，二者在命令255处都严格为零。
`thumb_ip` 另外包含 `"passive": true`。正反方向原始曲线不进入最终 JSON，但仍保留
在 `raw_samples.jsonl` 中，并用于最大回差和质量判定。
零位和最大角度可直接读取：
```python
import json
from pathlib import Path
data = json.loads(Path("g20_left_G20_LEFT_001_thumb_angle.json").read_text())
for name, joint in data["joints"].items():
print(name, "zero(rad)=", joint["angle_rad"][255],
"max(rad)=", joint["angle_rad"][0])
```
如果相机或 SDK 已由外部进程启动，可传
`start_camera:=false` 或 `start_sdk:=false`。`camera_serial_number` 同时接受 MVS
序列号和 GUID，但推荐使用稳定且简短的序列号 `DB2163742`。
海康相机默认输出 `1624x1240@30Hz mono8`，全局快门，曝光时间 `5000us`、增益
`0dB`，并使用“只取最新帧”策略避免视觉延迟。现场亮度不足时优先增加照明；必要时可用
`exposure_time_us`、`gain_db` 调整，或临时传 `auto_exposure:=true`。正式轨迹采集建议固定
曝光，避免自动曝光在运动过程中改变角点质量。rosbag 默认关闭；需要诊断留档时增加
`record_bag:=true`。
默认对完整 1624×1240 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
`image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。
校正和 AprilTag 组件运行
在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。
可选 ROI 模式会额外在同一容器内加入裁剪组件并同步修正 `CameraInfo`。标定节点默认
不订阅整幅图像，只订阅检测结果和 TF。
若启用调试图，预览会缩放到 50%、限速 10 Hz 并使用最新帧优先的传输方式，
不影响 AprilTag 的 ROI 输入。
静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制；
若用于正式可追溯验收，再启用并检查磁盘空间。
## 5. CMC Pitch 零位角测量
只测量命令 255 时 `thumb_cmc_pitch` 的画面水平投影零位角时，使用独立启动文件。
它只拟合 CMC 的二维零位轨迹圆，不运行完整 0～255 角度映射，也不会生成或修改
URDF：
```bash
ros2 launch g20_thumb_apriltag_calibration \
front_cmc_pitch_zero.launch.py \
serial_number:=G20_LEFT_001
```
该流程只要求 T0（ID 0）和 T3（ID 1）有效。T4/T5 可以留在手上，但丢失不会阻塞。
预检完成后查看中文状态：
```bash
ros2 topic echo --once --full-length \
/g20_thumb_cmc_pitch_zero/status_text \
--field data
```
状态显示“等待开始”后启动三轮测量：
```bash
ros2 service call \
/g20_thumb_cmc_pitch_zero/start \
std_srvs/srv/Trigger {}
```
查看带红色画面水平线、T0/T3标签中心、青色轨迹点、紫色拟合圆心和径向零位线
的调试画面：
```bash
ros2 run image_view image_view --ros-args \
--remap image:=/g20_thumb_cmc_pitch_zero/debug_image
```
画面底部红线是固定的相机水平与构图目标。程序会在画面下部自动寻找一条足够长、
接近水平的物理桌边或高对比参考直线，并画成蓝线。开始前调整相机，使蓝线与红线
重合；画面和 `status_text` 会实时显示红蓝线夹角及垂直偏差。`±0.5°` 和
`±12 px` 只用于显示 `ALIGNED/ADJUST`，完全不参与预检或 `start` 服务判断。
由操作人员确认相机位置后手动开始标定。参考直线应清晰、连续并尽量横跨画面；
该检测不使用 T0/T3 标签朝向。
一条二维直线只能确认相机滚转角和上下构图位置，不能单独证明相机的距离、俯仰、
偏航或完整三维位置。若需要严格复现这些量，还应使用固定相机支架或专用标定板。
每轮只控制电机 0 执行一次 `255→64` 端点运动和一次 `64→255` 返回运动。运动期间
连续采集 `T3中心−T0中心`，按机械手状态分箱后拟合图像平面圆；返回 255 后使用
“T3零位中心→拟合圆心”的固定内向径向矢量计算角度。运动前和返回后各采集30帧静态零位，
三轮轨迹合并后得到最终圆心。其他 19 个命令保持固定基准。任何其他节点同时发布
`/g20/cb_left_hand_control_cmd` 时，`start` 服务会拒绝启动。
T0中心用于消除相机或整只手的平移抖动。T0和T3标签自身的朝向与角点 `+x`
都不参与零位或行程计算；标签可以任意平面内旋转或反贴180°，只需标签平整、
固定且中心始终可见。若轨迹跨度、圆弧、半径、径向RMS/P95或回零误差不合格，
节点暂停或写出 `quality.passed=false`。
完成后只生成：
```text
calibration_output/G20_LEFT_001/<时间戳>/
g20_left_G20_LEFT_001_thumb_cmc_pitch_zero.json
```
核心字段是：
```text
zero_angles.table_projected_zero_rad
```
该值是内向径向零位矢量相对相机画面水平向右方向的角度。它不使用 T0 的方向，
但会用 T0 中心抵消平移；它仍不是真实三维桌面检测，因此会随相机滚转和机械手
摆放改变。
## 6. CMC Roll 零位与行程标定
`thumb_cmc_roll` 复用上节的 T0 平移补偿、T3 中心轨迹分箱和稳健圆拟合，
但控制的是电机 5。每轮执行 `255→0→255`：在 255 零位、0 行程端点和返回
255 后各静态采集 30 帧，因此可以同时测量零位角和完整 `0～255` 实际角行程。
命令 0 是完整行程端点，开始前必须确认拇指没有机械碰撞或硬限位顶死风险。
Roll同样固定使用“T3中心→拟合圆心”的内向径向矢量，不读取T3标签朝向。
```bash
ros2 launch g20_thumb_apriltag_calibration \
front_cmc_roll_calibration.launch.py \
serial_number:=G20_LEFT_001
```
预检通过后启动三轮标定：
```bash
ros2 topic echo --once --full-length \
/g20_thumb_cmc_roll_calibration/status_text \
--field data
ros2 service call \
/g20_thumb_cmc_roll_calibration/start \
std_srvs/srv/Trigger {}
```
调试画面：
```bash
ros2 run image_view image_view --ros-args \
--remap image:=/g20_thumb_cmc_roll_calibration/debug_image
```
Roll 使用与 Pitch 相同的红蓝参考线显示，但是否对齐由操作人员确认，程序不会用
蓝线状态阻止 `start` 进入电机运动。
完成后生成：
```text
calibration_output/G20_LEFT_001/<时间戳>/
g20_left_G20_LEFT_001_thumb_cmc_roll_zero_travel.json
```
核心输出字段：
```text
zero_angles.table_projected_zero_rad
travel.signed_rad
travel.range_rad
```
`travel.signed_rad` 是从命令 255 到 0 的有符号转角，`travel.range_rad` 是三轮
行程大小的中值。只有轨迹圆质量、T0/T3 检出率、三轮零位/行程一致性、端点径向
误差和回零误差全部通过时，`quality.passed` 才为 `true`。
src/linkerhand_calibration/config/calibration.yaml → src/g20_thumb_apriltag_calibration/config/calibration.yaml
+6 -3
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.
@@ -60,8 +60,11 @@ g20_thumb_calibration:
# 30-38 px tags are usable, but only if IPPE gives a tight image fit and
# a pose continuous with the preceding frame.
pnp_maximum_reprojection_error_px: 1.5
# Independent IPPE selection inherits the shared near-tie tolerance from
# core/geometry/pnp.py; continuity cannot override a clear image-fit lead.
# All four tags keep a temporally continuous IPPE solution throughout the
# complete session. With 30 px planar tags, tiny reprojection differences
# do not reliably identify the physical branch and previously caused
# stationary T0 to flip by about 25 deg between scan and validation.
pnp_reprojection_tie_px: 1.5
pnp_maximum_pose_jump_deg: 35.0
pnp_maximum_translation_jump_m: 0.04
pnp_maximum_tag_tilt_deg: 75.0
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
+4 -14
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 (
@@ -167,16 +162,11 @@ def interpolate_state_u8(
if nearest is None:
return None
gap = abs(int(stamp_ns) - int(nearest.stamp_ns))
if gap > maximum_skew_ns or not nearest.position_u8:
if gap > maximum_skew_ns or len(nearest.position_u8) != 20:
return None
return (tuple(float(value) for value in nearest.position_u8), gap)
# Physical-angle profiles use the same timestamp interpolation. Keep the old
# public name for compatibility and offer a unit-neutral spelling to new code.
interpolate_state = interpolate_state_u8
class ContinuousSweepCollector:
"""Collect timestamp-synchronised observations during one end-to-end move."""
src/linkerhand_calibration/linkerhand_calibration/alignment_view.py → src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/alignment_view.py
View File
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py
+288
View File
@@ -0,0 +1,288 @@
"""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.
"""
from __future__ import annotations
import json
import math
from pathlib import Path
from typing import Any, Mapping, Sequence
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from .full_hand import get_hand_calibration_profile, validate_compact_payload
G20_COMMAND_NAMES: tuple[str, ...] = (
"thumb_cmc_pitch",
"index_mcp_pitch",
"middle_mcp_pitch",
"ring_mcp_pitch",
"pinky_mcp_pitch",
"thumb_cmc_roll",
"index_mcp_roll",
"middle_mcp_roll",
"ring_mcp_roll",
"pinky_mcp_roll",
"thumb_cmc_yaw",
"reserved_11",
"reserved_12",
"reserved_13",
"reserved_14",
"thumb_mcp",
"index_pip",
"middle_pip",
"ring_pip",
"pinky_pip",
)
# Match the stable ordering used by the existing MuJoCo bridge. JointState
# consumers must use names, but retaining the ordering also keeps logs and
# direct comparisons deterministic.
G20_URDF_JOINT_NAMES: tuple[str, ...] = (
"index_dip",
"index_mcp_pitch",
"index_mcp_roll",
"index_pip",
"middle_dip",
"middle_mcp_pitch",
"middle_mcp_roll",
"middle_pip",
"pinky_dip",
"pinky_mcp_pitch",
"pinky_mcp_roll",
"pinky_pip",
"ring_dip",
"ring_mcp_pitch",
"ring_mcp_roll",
"ring_pip",
"thumb_cmc_pitch",
"thumb_cmc_roll",
"thumb_cmc_yaw",
"thumb_ip",
"thumb_mcp",
)
O30_COMMAND_NAMES: tuple[str, ...] = (
"thumb_roll",
"thumb_yaw",
"index_yaw",
"middle_yaw",
"ring_yaw",
"little_yaw",
"thumb_root1",
"index_root1",
"middle_root1",
"ring_root1",
"little_root1",
"index_root2",
"middle_root2",
"ring_root2",
"little_root2",
"thumb_tip",
"index_tip",
"middle_tip",
"ring_tip",
"little_tip",
)
O30_URDF_JOINT_NAMES: tuple[str, ...] = (
"thumb_cmc_roll",
"thumb_cmc_yaw",
"thumb_mcp",
"thumb_ip",
"index_mcp_roll",
"index_mcp_pitch",
"index_pip",
"index_dip",
"middle_mcp_roll",
"middle_mcp_pitch",
"middle_pip",
"middle_dip",
"ring_mcp_roll",
"ring_mcp_pitch",
"ring_pip",
"ring_dip",
"pinky_mcp_roll",
"pinky_mcp_pitch",
"pinky_pip",
"pinky_dip",
)
COMMAND_NAMES_BY_MODEL = {
"G20": G20_COMMAND_NAMES,
"O30": O30_COMMAND_NAMES,
}
URDF_JOINT_NAMES_BY_MODEL = {
"G20": G20_URDF_JOINT_NAMES,
"O30": O30_URDF_JOINT_NAMES,
}
class CalibratedCommandMapper:
"""Validated, model/side-specific lookup from commands to URDF radians."""
def __init__(
self, payload: Mapping[str, Any], *, expected_side: str | None = None
) -> None:
validate_compact_payload(payload)
side = str(payload["side"]).lower()
if expected_side is not None and side != str(expected_side).lower():
raise ValueError(
f"calibration side {side!r} does not match requested side "
f"{str(expected_side).lower()!r}"
)
quality = payload["quality"]
if quality.get("passed") is not True:
raise ValueError("calibration quality.passed must be true")
model = str(payload["model"]).upper()
profile = get_hand_calibration_profile(side, model)
urdf_joint_names = URDF_JOINT_NAMES_BY_MODEL[model]
self.model = model
self.side = side
self.serial_number = str(payload["serial_number"])
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 urdf_joint_names
}
@staticmethod
def _command_index(value: float) -> int:
command = float(value)
if not math.isfinite(command):
raise ValueError("command positions must be finite")
return max(0, min(255, int(math.floor(command + 0.5))))
def map_positions(
self, positions: Sequence[float], names: Sequence[str] = ()
) -> tuple[float, ...]:
values = tuple(float(value) for value in positions)
if names:
if len(names) != len(values):
raise ValueError(
"JointState names and positions must have equal length"
)
if len(set(names)) != len(names):
raise ValueError("JointState names must be unique")
by_name = dict(zip((str(name) for name in names), values))
missing = [name for name in self.command_names if name not in by_name]
if missing:
raise ValueError(
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} command must contain exactly "
f"{len(self.command_names)} positions"
)
command = values
indices = tuple(self._command_index(value) for value in command)
return tuple(
self._curves[name][indices[self._motor_by_joint[name]]]
for name in self.urdf_joint_names
)
def load_calibrated_command_mapper(
calibration_file: str | Path, *, expected_side: str | None = None
) -> CalibratedCommandMapper:
path = Path(calibration_file).expanduser().resolve()
if not path.is_file():
raise ValueError(f"calibration JSON does not exist: {path}")
payload = json.loads(path.read_text(encoding="utf-8"))
return CalibratedCommandMapper(payload, expected_side=expected_side)
class CalibratedJointStateBridge(Node):
def __init__(self) -> None:
super().__init__("g20_calibrated_joint_state_bridge")
self.declare_parameter("hand_model", "G20")
self.declare_parameter("hand_type", "right")
self.declare_parameter("calibration_file", "")
self.declare_parameter("input_topic", "")
self.declare_parameter("output_topic", "")
hand_model = str(self.get_parameter("hand_model").value).upper()
hand_type = str(self.get_parameter("hand_type").value).lower()
get_hand_calibration_profile(hand_type, hand_model)
calibration_file = str(self.get_parameter("calibration_file").value)
if not calibration_file:
raise ValueError("calibration_file is required")
self.mapper = load_calibrated_command_mapper(
calibration_file, expected_side=hand_type
)
if self.mapper.model != hand_model:
raise ValueError(
f"calibration model {self.mapper.model!r} does not match "
f"requested model {hand_model!r}"
)
input_topic = str(self.get_parameter("input_topic").value).strip()
output_topic = str(self.get_parameter("output_topic").value).strip()
self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd"
self.output_topic = (
output_topic
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(
JointState, self.input_topic, self._command_callback, 10
)
self._last_error = ""
self.get_logger().info(
f"loaded {hand_type} {hand_model} calibration for "
f"{self.mapper.serial_number}: "
f"{self.input_topic} -> {self.output_topic}"
)
def _command_callback(self, command: JointState) -> None:
try:
positions = self.mapper.map_positions(command.position, command.name)
except ValueError as error:
message = str(error)
if message != self._last_error:
self.get_logger().error(message)
self._last_error = message
return
self._last_error = ""
result = JointState()
result.header = command.header
result.name = list(self.mapper.urdf_joint_names)
result.position = list(positions)
self.publisher.publish(result)
def main(args: Sequence[str] | None = None) -> None:
rclpy.init(args=args)
node: CalibratedJointStateBridge | None = None
try:
node = CalibratedJointStateBridge()
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
if node is not None:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":
main()
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
+5 -36
View File
@@ -605,9 +605,6 @@ class ThreeCameraExtrinsicsNode(Node):
self.declare_parameter("minimum_captures_per_pair", 15)
self.declare_parameter("maximum_pair_skew_ms", 100.0)
self.declare_parameter("maximum_reprojection_rms_px", 1.2)
self.declare_parameter(
"maximum_candidate_pair_reprojection_rms_px", 1.5
)
self.declare_parameter(
"maximum_single_camera_reprojection_rms_px", 1.5
)
@@ -643,9 +640,6 @@ class ThreeCameraExtrinsicsNode(Node):
self.maximum_reprojection_rms_px = float(
value("maximum_reprojection_rms_px")
)
self.maximum_candidate_pair_reprojection_rms_px = float(
value("maximum_candidate_pair_reprojection_rms_px")
)
self.maximum_single_camera_reprojection_rms_px = float(
value("maximum_single_camera_reprojection_rms_px")
)
@@ -677,20 +671,12 @@ class ThreeCameraExtrinsicsNode(Node):
if self.maximum_reprojection_rms_px <= 0.0:
raise ValueError("maximum_reprojection_rms_px must be positive")
if (
self.maximum_candidate_pair_reprojection_rms_px
self.maximum_single_camera_reprojection_rms_px
< self.maximum_reprojection_rms_px
):
raise ValueError(
"maximum_candidate_pair_reprojection_rms_px must be at "
"least maximum_reprojection_rms_px"
)
if (
self.maximum_single_camera_reprojection_rms_px
< self.maximum_candidate_pair_reprojection_rms_px
):
raise ValueError(
"maximum_single_camera_reprojection_rms_px must be at "
"least maximum_candidate_pair_reprojection_rms_px"
"least maximum_reprojection_rms_px"
)
if self.gui_refresh_hz <= 0.0 or self.gui_refresh_hz > 10.0:
raise ValueError("gui_refresh_hz must be in (0, 10]")
@@ -883,20 +869,14 @@ class ThreeCameraExtrinsicsNode(Node):
selected.reprojection_rms_px,
self.maximum_single_camera_reprojection_rms_px,
)
if (
camera_rms_failed
or pair_rms
> self.maximum_candidate_pair_reprojection_rms_px
):
if camera_rms_failed or pair_rms > self.maximum_reprojection_rms_px:
return PairAssessment(
ready=False,
reason_code="reprojection_rms_too_large",
message=(
f"当前组未通过采集门限，未计入。单相机上限="
f"{self.maximum_single_camera_reprojection_rms_px:.3f}px，"
"候选组合上限="
f"{self.maximum_candidate_pair_reprojection_rms_px:.3f}px，"
f"最终批次上限={self.maximum_reprojection_rms_px:.3f}px；"
f"组合上限={self.maximum_reprojection_rms_px:.3f}px；"
f"组合RMS={pair_rms:.3f}px，"
f"front={front.reprojection_rms_px:.3f}px，"
f"{other}={selected.reprojection_rms_px:.3f}px；"
@@ -1372,9 +1352,7 @@ class ThreeCameraExtrinsicsNode(Node):
)
cv2.putText(
canvas,
"Pair RMS "
f"{pair_rms}/"
f"{self.maximum_candidate_pair_reprojection_rms_px:.3f}px "
f"Pair RMS {pair_rms}/{self.maximum_reprojection_rms_px:.3f}px "
f"Skew {skew}/{self.maximum_pair_skew_ns / 1e6:.1f}ms "
f"Batch RMS {batch_rms}/{self.maximum_reprojection_rms_px:.3f}px "
f"stability {rotation}/"
@@ -1548,15 +1526,6 @@ class ThreeCameraExtrinsicsNode(Node):
cameras=self.camera_identities,
front_from_view=transforms,
quality=quality,
quality_limits={
"reprojection_rms_px": self.maximum_reprojection_rms_px,
"maximum_rotation_repeatability_deg": math.degrees(
self.maximum_rotation_repeatability_rad
),
"maximum_translation_repeatability_m": (
self.maximum_translation_repeatability_m
),
},
)
response.success = True
response.message = f"外参已保存：{self.output_file}；{quality}"
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/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py
+2891
View File
File diff suppressed because it is too large Load Diff
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/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py
+1346
View File
File diff suppressed because it is too large Load Diff
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
+317 -3
View File
@@ -21,6 +21,7 @@ import math
from typing import Any, Iterable, Mapping, Sequence
import numpy as np
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation
from .core import (
@@ -39,9 +40,11 @@ _ROOT_ROLES = ("t3", "t4", "t5")
DEFAULT_PASSIVE_IP_MULTIPLIER = 1.02
from .core.fitting.trajectory_geometry import (
_vector3, _plane_basis, _fit_plane_axis, _fit_circle_with_axis, _project_radial, _signed_angle, _reference_radial, _angle_for_circle, _low_command_median, _orient_circle_positive, _fit_joint_curve, _regularize_coupled_zero_tail
)
def _vector3(value: Sequence[float], *, name: str) -> np.ndarray:
vector = np.asarray(value, dtype=float)
if vector.shape != (3,) or not np.all(np.isfinite(vector)):
raise ValueError(f"{name} must contain three finite values")
return vector
def _record_translations(
@@ -61,22 +64,204 @@ def _record_translations(
}
def _plane_basis(axis: Sequence[float]) -> tuple[np.ndarray, np.ndarray]:
normal = _vector3(axis, name="axis")
normal /= np.linalg.norm(normal)
candidates = np.eye(3)
seed = candidates[int(np.argmin(np.abs(candidates @ normal)))]
first = np.cross(normal, seed)
first /= np.linalg.norm(first)
second = np.cross(normal, first)
second /= np.linalg.norm(second)
return first, second
def _fit_plane_axis(point_sets: Sequence[np.ndarray]) -> tuple[np.ndarray, float]:
centred: list[np.ndarray] = []
for points in point_sets:
array = np.asarray(points, dtype=float)
if array.ndim != 2 or array.shape[1] != 3 or len(array) < 3:
raise ValueError("each trajectory must contain at least three 3-D points")
# The arithmetic centroid of points in a plane remains in that plane
# and transforms correctly under a camera rotation. A component-wise
# median generally does neither.
centred.append(array - np.mean(array, axis=0))
matrix = np.concatenate(centred, axis=0)
_, _, vh = np.linalg.svd(matrix, full_matrices=False)
axis = vh[-1]
axis /= np.linalg.norm(axis)
residuals = matrix @ axis
plane_rms = float(np.sqrt(np.mean(np.square(residuals))))
return axis, plane_rms
def _fit_circle_with_axis(
points_xyz: Sequence[Sequence[float]],
axis_xyz: Sequence[float],
) -> dict[str, Any]:
points = np.asarray(points_xyz, dtype=float)
if (
points.ndim != 2
or points.shape[1] != 3
or len(points) < 6
or not np.all(np.isfinite(points))
):
raise ValueError("a circle requires at least six finite 3-D points")
axis = _vector3(axis_xyz, name="circle axis")
axis /= np.linalg.norm(axis)
basis_x, basis_y = _plane_basis(axis)
origin = np.mean(points, axis=0)
local = points - origin
xy = np.column_stack((local @ basis_x, local @ basis_y))
design = np.column_stack((2.0 * xy[:, 0], 2.0 * xy[:, 1], np.ones(len(xy))))
target = np.sum(np.square(xy), axis=1)
initial_x, initial_y, constant = np.linalg.lstsq(
design, target, rcond=None
)[0]
initial_radius = math.sqrt(
max(
float(constant + initial_x * initial_x + initial_y * initial_y),
np.finfo(float).eps,
)
)
def residual(parameters: np.ndarray) -> np.ndarray:
centre = parameters[:2]
radius = float(parameters[2])
return np.linalg.norm(xy - centre, axis=1) - radius
fitted = least_squares(
residual,
np.asarray([initial_x, initial_y, initial_radius], dtype=float),
loss="soft_l1",
f_scale=0.0005,
max_nfev=2000,
)
centre_xy = fitted.x[:2]
radius = abs(float(fitted.x[2]))
radial_residuals = residual(
np.asarray([centre_xy[0], centre_xy[1], radius], dtype=float)
)
centre_xyz = origin + centre_xy[0] * basis_x + centre_xy[1] * basis_y
plane_offsets = (points - centre_xyz) @ axis
centre_xyz += float(np.median(plane_offsets)) * axis
plane_residuals = (points - centre_xyz) @ axis
return {
"axis_xyz": [float(value) for value in axis],
"center_xyz_m": [float(value) for value in centre_xyz],
"radius_m": radius,
"radial_rms_m": float(
np.sqrt(np.mean(np.square(radial_residuals)))
),
"plane_rms_m": float(
np.sqrt(np.mean(np.square(plane_residuals)))
),
}
def _project_radial(
point_xyz: Sequence[float],
circle: Mapping[str, Any],
) -> np.ndarray:
point = _vector3(point_xyz, name="trajectory point")
centre = _vector3(circle["center_xyz_m"], name="circle centre")
axis = _vector3(circle["axis_xyz"], name="circle axis")
axis /= np.linalg.norm(axis)
radial = point - centre
radial -= float(radial @ axis) * axis
if float(np.linalg.norm(radial)) < 1.0e-9:
raise ValueError("trajectory point lies on the fitted rotation axis")
return radial
def _signed_angle(
reference_radial_xyz: Sequence[float],
observed_radial_xyz: Sequence[float],
axis_xyz: Sequence[float],
) -> float:
reference = _vector3(reference_radial_xyz, name="reference radial")
observed = _vector3(observed_radial_xyz, name="observed radial")
axis = _vector3(axis_xyz, name="angle axis")
axis /= np.linalg.norm(axis)
reference -= float(reference @ axis) * axis
observed -= float(observed @ axis) * axis
reference /= np.linalg.norm(reference)
observed /= np.linalg.norm(observed)
return math.atan2(
float(axis @ np.cross(reference, observed)),
float(np.clip(reference @ observed, -1.0, 1.0)),
)
def _reference_radial(
records: Sequence[Mapping[str, Any]],
points: Sequence[np.ndarray],
circle: Mapping[str, Any],
) -> np.ndarray:
references = [
_project_radial(point, circle)
for record, point in zip(records, points)
if int(record.get("command_u8", -1)) == 255
]
if not references:
raise ValueError("trajectory is missing command-255 reference points")
radial = np.median(np.asarray(references, dtype=float), axis=0)
axis = _vector3(circle["axis_xyz"], name="circle axis")
axis /= np.linalg.norm(axis)
radial -= float(radial @ axis) * axis
if float(np.linalg.norm(radial)) < 1.0e-9:
raise ValueError("command-255 reference is degenerate")
return radial
def _angle_for_circle(
point_xyz: Sequence[float],
circle: Mapping[str, Any],
) -> float:
return _signed_angle(
circle["reference_radial_xyz_m"],
_project_radial(point_xyz, circle),
circle["axis_xyz"],
)
def _low_command_median(
records: Sequence[Mapping[str, Any]],
values: Sequence[float],
) -> float:
selected = [
float(value)
for record, value in zip(records, values)
if int(record.get("command_u8", 255)) <= 16
]
if not selected:
selected = [
float(value)
for _, value in sorted(
zip(records, values),
key=lambda item: int(item[0].get("command_u8", 255)),
)[: max(3, len(records) // 20)]
]
return float(np.median(selected))
def _orient_circle_positive(
circle: dict[str, Any],
records: Sequence[Mapping[str, Any]],
points: Sequence[np.ndarray],
) -> dict[str, Any]:
reference = _reference_radial(records, points, circle)
circle["reference_radial_xyz_m"] = [
float(value) for value in reference
]
values = [_angle_for_circle(point, circle) for point in points]
if _low_command_median(records, values) < 0.0:
axis = -_vector3(circle["axis_xyz"], name="circle axis")
circle["axis_xyz"] = [float(value) for value in axis]
values = [_angle_for_circle(point, circle) for point in points]
circle["observed_arc_rad"] = float(max(values) - min(values))
return circle
def _rotation_about_axis(
@@ -444,8 +629,137 @@ def maximum_center_non_target_drift_rad(
return float(maximum)
def _fit_joint_curve(
records: Sequence[Mapping[str, Any]],
values: Sequence[float],
*,
endpoint_reference: Mapping[str, Sequence[float]] | None = None,
) -> tuple[dict[str, Any], float, float]:
by_direction: dict[str, list[list[float]]] = {
direction: [[] for _ in range(256)] for direction in DIRECTIONS
}
for record, value in zip(records, values):
direction = str(record["direction"])
command = int(record["command_u8"])
by_direction[direction][command].append(float(value))
fitted: dict[str, list[float]] = {}
maximum_correction = 0.0
for direction in DIRECTIONS:
commands = np.asarray(
[
command
for command, samples in enumerate(by_direction[direction])
if samples
],
dtype=int,
)
if (
commands.size < 3
or int(commands[0]) != 0
or int(commands[-1]) != 255
):
raise ValueError(
f"{direction} centre trajectory requires commands 0 and 255"
)
raw = np.asarray(
[
float(np.median(by_direction[direction][command]))
for command in commands
],
dtype=float,
)
raw -= raw[-1]
projected_samples = isotonic_nonincreasing(raw)
projected_samples -= projected_samples[-1]
maximum_correction = max(
maximum_correction,
float(np.max(np.abs(projected_samples - raw))),
)
curve = np.interp(
np.arange(256, dtype=float),
commands.astype(float),
projected_samples,
)
curve -= curve[255]
if endpoint_reference is not None:
curve = _regularize_coupled_zero_tail(
curve,
endpoint_reference[f"{direction}_rad"],
)
fitted[direction] = [
round(float(value), 8) for value in curve
]
decreasing = np.asarray(fitted[DIRECTION_DECREASING], dtype=float)
increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float)
hysteresis = float(np.max(np.abs(decreasing - increasing)))
combined = 0.5 * (decreasing + increasing)
combined -= combined[255]
return (
{
"angle_rad": [round(float(value), 8) for value in combined],
"decreasing_rad": fitted[DIRECTION_DECREASING],
"increasing_rad": fitted[DIRECTION_INCREASING],
},
maximum_correction,
hysteresis,
)
def _regularize_coupled_zero_tail(
values: Sequence[float],
reference_values: Sequence[float],
*,
maximum_tail_commands: int = 16,
zero_tolerance_rad: float = 1.0e-10,
) -> np.ndarray:
"""Replace a short noise-created zero tail using a coupled joint shape.
The passive IP and active MCP share motor 15. Close to command 255 the
IP centre trajectory is small enough that measurement noise can become
negative. Isotonic projection correctly prevents a negative angle, but
otherwise turns all remaining commands into an artificial zero plateau.
Continue the last resolved IP/MCP ratio over a short tail instead. This
changes neither the resolved part of the curve nor the exact 255 zero.
"""
curve = np.asarray(values, dtype=float).copy()
reference = np.asarray(reference_values, dtype=float)
if curve.shape != (256,) or reference.shape != (256,):
raise ValueError("endpoint curves must each contain 256 values")
if not np.all(np.isfinite(curve)) or not np.all(np.isfinite(reference)):
raise ValueError("endpoint curves must be finite")
maximum_tail = int(maximum_tail_commands)
tolerance = float(zero_tolerance_rad)
if maximum_tail < 2 or tolerance < 0.0:
raise ValueError("invalid endpoint regularization settings")
anchor = 254
while anchor >= 0 and abs(float(curve[anchor])) <= tolerance:
anchor -= 1
tail_commands = 255 - anchor
if (
anchor < 0
or tail_commands < 2
or tail_commands > maximum_tail
or curve[anchor] <= tolerance
or reference[anchor] <= tolerance
or abs(float(curve[255])) > tolerance
or abs(float(reference[255])) > tolerance
):
curve[255] = 0.0
return curve
ratio = float(curve[anchor] / reference[anchor])
continuation = np.maximum(
0.0,
ratio * reference[anchor + 1 :],
)
continuation = np.minimum.accumulate(continuation)
continuation = np.minimum(continuation, float(curve[anchor]))
continuation[-1] = 0.0
curve[anchor + 1 :] = continuation
return curve
def _derive_mimic_joint_curves(
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
+123 -3
View File
@@ -7,13 +7,20 @@ from typing import Any, Mapping, Sequence
import cv2
import numpy as np
from scipy.optimize import least_squares
from .core.fitting.circle_geometry import (
wrap_angle_rad, signed_angle_difference_rad, circular_mean_rad, circular_median_rad, circular_std_rad, maximum_pairwise_angle_difference_rad, _fit_circle, _trajectory_arc_rad
)
def wrap_angle_rad(angle_rad: float) -> float:
"""Wrap one finite angle to [-pi, pi)."""
angle = float(angle_rad)
if not math.isfinite(angle):
raise ValueError("angle must be finite")
return (angle + math.pi) % (2.0 * math.pi) - math.pi
def signed_angle_difference_rad(angle_rad: float, reference_rad: float) -> float:
"""Return the wrapped signed rotation from reference to angle."""
return wrap_angle_rad(float(angle_rad) - float(reference_rad))
def tag_x_axis_angle_rad(
@@ -31,12 +38,61 @@ def tag_x_axis_angle_rad(
)
def circular_mean_rad(angles_rad: Sequence[float]) -> float:
"""Return the circular mean of at least one finite angle."""
angles = np.asarray(angles_rad, dtype=float)
if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)):
raise ValueError("angles_rad must contain finite angles")
vector = complex(
float(np.mean(np.cos(angles))),
float(np.mean(np.sin(angles))),
)
if abs(vector) < 1.0e-12:
raise ValueError("angles do not have a unique circular mean")
return wrap_angle_rad(math.atan2(vector.imag, vector.real))
def circular_median_rad(angles_rad: Sequence[float]) -> float:
"""Return a robust circular median around the circular mean."""
angles = np.asarray(angles_rad, dtype=float)
reference = circular_mean_rad(angles)
deltas = np.asarray(
[signed_angle_difference_rad(value, reference) for value in angles],
dtype=float,
)
return wrap_angle_rad(reference + float(np.median(deltas)))
def circular_std_rad(angles_rad: Sequence[float]) -> float:
"""Return circular standard deviation in radians."""
angles = np.asarray(angles_rad, dtype=float)
if angles.ndim != 1 or angles.size == 0 or not np.all(np.isfinite(angles)):
raise ValueError("angles_rad must contain finite angles")
resultant = float(
math.hypot(
float(np.mean(np.cos(angles))),
float(np.mean(np.sin(angles))),
)
)
resultant = min(1.0, max(1.0e-15, resultant))
return math.sqrt(max(0.0, -2.0 * math.log(resultant)))
def maximum_pairwise_angle_difference_rad(
angles_rad: Sequence[float],
) -> float:
"""Return the largest wrapped distance between any two angles."""
angles = [float(value) for value in angles_rad]
if not angles or not all(math.isfinite(value) for value in angles):
raise ValueError("angles_rad must contain finite angles")
return max(
(
abs(signed_angle_difference_rad(left, right))
for index, left in enumerate(angles)
for right in angles[index + 1:]
),
default=0.0,
)
def detect_reference_alignment_line(
@@ -197,8 +253,72 @@ def summarize_zero_frames(
}
def _fit_circle(points_xy: np.ndarray) -> tuple[np.ndarray, float]:
"""Fit a geometric circle with robust nonlinear least squares."""
points = np.asarray(points_xy, dtype=float)
if points.ndim != 2 or points.shape[1] != 2 or len(points) < 3:
raise ValueError("circle fit requires at least three 2D points")
origin = np.mean(points, axis=0)
centered = points - origin
matrix = np.column_stack(
(2.0 * centered[:, 0], 2.0 * centered[:, 1], np.ones(len(points)))
)
rhs = np.sum(np.square(centered), axis=1)
solution, _, rank, _ = np.linalg.lstsq(matrix, rhs, rcond=None)
if rank < 3:
raise ValueError("trajectory points are degenerate")
centre0 = solution[:2] + origin
radius0_squared = (
float(solution[2]) + float(np.dot(solution[:2], solution[:2]))
)
if radius0_squared <= 0.0:
raise ValueError("trajectory circle radius is invalid")
radius0 = math.sqrt(radius0_squared)
def residual(parameters: np.ndarray) -> np.ndarray:
centre = parameters[:2]
radius = parameters[2]
return np.linalg.norm(points - centre, axis=1) - radius
initial_error = residual(np.asarray([*centre0, radius0], dtype=float))
robust_scale = max(
0.25,
1.4826
* float(
np.median(
np.abs(initial_error - float(np.median(initial_error)))
)
),
)
result = least_squares(
residual,
np.asarray([*centre0, radius0], dtype=float),
loss="soft_l1",
f_scale=robust_scale,
max_nfev=1000,
)
centre = np.asarray(result.x[:2], dtype=float)
radius = float(abs(result.x[2]))
if not result.success or not np.all(np.isfinite(centre)):
raise ValueError("trajectory circle optimization failed")
if not math.isfinite(radius) or radius <= 0.0:
raise ValueError("trajectory circle radius is invalid")
return centre, radius
def _trajectory_arc_rad(points_xy: np.ndarray, centre_xy: np.ndarray) -> float:
angles = np.mod(
np.arctan2(
points_xy[:, 1] - centre_xy[1],
points_xy[:, 0] - centre_xy[0],
),
2.0 * math.pi,
)
if len(angles) < 2:
return 0.0
angles = np.sort(angles)
gaps = np.diff(np.r_[angles, angles[0] + 2.0 * math.pi])
return float(2.0 * math.pi - np.max(gaps))
def fit_image_circle_trajectory(
src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/zero_node.py
+1604
View File
File diff suppressed because it is too large Load Diff
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/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py
+272
View File
@@ -0,0 +1,272 @@
"""Launch front-camera trajectory-circle CMC pitch zero measurement."""
from __future__ import annotations
from datetime import datetime
from pathlib import Path
import re
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
LogInfo,
OpaqueFunction,
SetEnvironmentVariable,
)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch_ros.parameter_descriptions import ParameterValue
def _launch_stack(context):
serial_number = LaunchConfiguration("serial_number").perform(context)
if (
not serial_number
or serial_number == "UNSET"
or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
or serial_number in {".", ".."}
):
raise RuntimeError(
"serial_number is required and may contain only letters, "
"digits, dot, underscore and dash"
)
requested_session = LaunchConfiguration("session_dir").perform(context)
output_root = Path(
LaunchConfiguration("output_root").perform(context)
).expanduser().resolve()
if requested_session:
session_dir = Path(requested_session).expanduser().resolve()
else:
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session_dir = output_root / serial_number / timestamp
session_dir.mkdir(parents=True, exist_ok=True)
tag_config = LaunchConfiguration("tag_config").perform(context)
zero_config = LaunchConfiguration("zero_config").perform(context)
camera = Node(
package="g20_thumb_apriltag_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
output="screen",
emulate_tty=True,
condition=IfCondition(LaunchConfiguration("start_camera")),
parameters=[
{
"serial_number": LaunchConfiguration("camera_serial_number"),
"expected_model": LaunchConfiguration("camera_model"),
"camera_name": LaunchConfiguration("camera_name"),
"frame_id": LaunchConfiguration("camera_frame_id"),
"image_width": ParameterValue(
LaunchConfiguration("image_width"), value_type=int
),
"image_height": ParameterValue(
LaunchConfiguration("image_height"), value_type=int
),
"frame_rate": ParameterValue(
LaunchConfiguration("camera_frame_rate"), value_type=float
),
"exposure_time_us": ParameterValue(
LaunchConfiguration("exposure_time_us"), value_type=float
),
"gain_db": ParameterValue(
LaunchConfiguration("gain_db"), value_type=float
),
"auto_exposure": ParameterValue(
LaunchConfiguration("auto_exposure"), value_type=bool
),
"camera_info_url": LaunchConfiguration("camera_info_url"),
}
],
)
raw_topic = "/camera/camera/color/image_raw"
camera_info_topic = "/camera/camera/color/camera_info"
rect_topic = "/camera/camera/color/image_rect"
vision_container = ComposableNodeContainer(
name="g20_thumb_zero_vision_container",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=[
ComposableNode(
package="image_proc",
plugin="image_proc::RectifyNode",
name="rectify_color",
namespace="/camera/camera/color",
remappings=[
("image", raw_topic),
("camera_info", camera_info_topic),
("image_rect", rect_topic),
],
parameters=[{"queue_size": 1}],
extra_arguments=[{"use_intra_process_comms": True}],
),
ComposableNode(
package="apriltag_ros",
plugin="AprilTagNode",
name="apriltag",
namespace="/apriltag",
parameters=[
tag_config,
{
"detector.decimate": ParameterValue(
LaunchConfiguration("apriltag_decimate"),
value_type=float,
)
},
],
remappings=[
("image_rect", rect_topic),
("camera_info", camera_info_topic),
],
extra_arguments=[{"use_intra_process_comms": True}],
),
],
output="screen",
emulate_tty=True,
)
sdk = Node(
package="linker_hand_ros2_sdk",
executable="linker_hand_sdk",
name="linker_hand_sdk",
output="screen",
condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[
{
"hand_type": "left",
"hand_joint": "G20",
"can": LaunchConfiguration("can_interface"),
"modbus": "None",
"topic_prefix": "/g20",
"move_on_startup": False,
"startup_speed": ParameterValue(
LaunchConfiguration("calibration_speed"),
value_type=int,
),
"startup_torque": 80,
"state_poll_rate": 10.0,
"repeat_position_commands": False,
"is_touch": False,
}
],
)
zero_node = Node(
package="g20_thumb_apriltag_calibration",
executable="cmc_pitch_zero_node",
name="g20_thumb_cmc_pitch_zero",
output="screen",
parameters=[
zero_config,
{
"serial_number": serial_number,
"session_dir": str(session_dir),
"commands_enabled": ParameterValue(
LaunchConfiguration("commands_enabled"),
value_type=bool,
),
"image_topic": rect_topic,
"publish_debug_image": ParameterValue(
LaunchConfiguration("publish_debug_image"),
value_type=bool,
),
},
],
)
return [
LogInfo(msg=f"G20 CMC pitch zero session: {session_dir}"),
LogInfo(
msg=(
"Only T0(ID 0) and T3(ID 1) are required; "
"T4/T5 detections are ignored"
)
),
LogInfo(
msg=(
"Motor 0 performs three 255->64->255 sweeps; "
"zero angles come from the fitted T3-centre trajectory radius"
)
),
camera,
vision_container,
sdk,
zero_node,
]
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
)
return LaunchDescription(
[
SetEnvironmentVariable(
name="FASTRTPS_DEFAULT_PROFILES_FILE",
value=str(
package_share / "config" / "fastdds_large_images.xml"
),
),
DeclareLaunchArgument("serial_number", default_value="UNSET"),
DeclareLaunchArgument(
"camera_serial_number", default_value="DB2163742"
),
DeclareLaunchArgument(
"camera_model", default_value="MV-CS020-10UM"
),
DeclareLaunchArgument(
"camera_name", default_value="hikrobot_front_DB2163742"
),
DeclareLaunchArgument(
"camera_frame_id", default_value="camera_color_optical_frame"
),
DeclareLaunchArgument("image_width", default_value="1624"),
DeclareLaunchArgument("image_height", default_value="1240"),
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
DeclareLaunchArgument("gain_db", default_value="0.0"),
DeclareLaunchArgument("auto_exposure", default_value="false"),
DeclareLaunchArgument(
"camera_info_url",
default_value=str(
Path.home()
/ ".ros"
/ "camera_info"
/ "hikrobot_DB2163742.yaml"
),
),
DeclareLaunchArgument("can_interface", default_value="can0"),
DeclareLaunchArgument("calibration_speed", default_value="15"),
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
DeclareLaunchArgument("commands_enabled", default_value="true"),
DeclareLaunchArgument(
"publish_debug_image", default_value="true"
),
DeclareLaunchArgument("start_camera", default_value="true"),
DeclareLaunchArgument("start_sdk", default_value="true"),
DeclareLaunchArgument(
"output_root",
default_value=str(Path.cwd() / "calibration_output"),
),
DeclareLaunchArgument("session_dir", default_value=""),
DeclareLaunchArgument(
"zero_config",
default_value=str(
package_share / "config" / "cmc_pitch_zero.yaml"
),
),
DeclareLaunchArgument(
"tag_config",
default_value=str(
package_share / "config" / "front_tags.yaml"
),
),
OpaqueFunction(function=_launch_stack),
]
)
src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py
+276
View File
@@ -0,0 +1,276 @@
"""Launch front-camera trajectory-circle CMC roll zero/travel calibration."""
from __future__ import annotations
from datetime import datetime
from pathlib import Path
import re
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
LogInfo,
OpaqueFunction,
SetEnvironmentVariable,
)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch_ros.parameter_descriptions import ParameterValue
def _launch_stack(context):
serial_number = LaunchConfiguration("serial_number").perform(context)
if (
not serial_number
or serial_number == "UNSET"
or re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
or serial_number in {".", ".."}
):
raise RuntimeError(
"serial_number is required and may contain only letters, "
"digits, dot, underscore and dash"
)
requested_session = LaunchConfiguration("session_dir").perform(context)
output_root = Path(
LaunchConfiguration("output_root").perform(context)
).expanduser().resolve()
if requested_session:
session_dir = Path(requested_session).expanduser().resolve()
else:
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session_dir = output_root / serial_number / timestamp
session_dir.mkdir(parents=True, exist_ok=True)
tag_config = LaunchConfiguration("tag_config").perform(context)
calibration_config = LaunchConfiguration(
"calibration_config"
).perform(context)
camera = Node(
package="g20_thumb_apriltag_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
output="screen",
emulate_tty=True,
condition=IfCondition(LaunchConfiguration("start_camera")),
parameters=[
{
"serial_number": LaunchConfiguration("camera_serial_number"),
"expected_model": LaunchConfiguration("camera_model"),
"camera_name": LaunchConfiguration("camera_name"),
"frame_id": LaunchConfiguration("camera_frame_id"),
"image_width": ParameterValue(
LaunchConfiguration("image_width"), value_type=int
),
"image_height": ParameterValue(
LaunchConfiguration("image_height"), value_type=int
),
"frame_rate": ParameterValue(
LaunchConfiguration("camera_frame_rate"), value_type=float
),
"exposure_time_us": ParameterValue(
LaunchConfiguration("exposure_time_us"), value_type=float
),
"gain_db": ParameterValue(
LaunchConfiguration("gain_db"), value_type=float
),
"auto_exposure": ParameterValue(
LaunchConfiguration("auto_exposure"), value_type=bool
),
"camera_info_url": LaunchConfiguration("camera_info_url"),
}
],
)
raw_topic = "/camera/camera/color/image_raw"
camera_info_topic = "/camera/camera/color/camera_info"
rect_topic = "/camera/camera/color/image_rect"
vision_container = ComposableNodeContainer(
name="g20_thumb_roll_vision_container",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=[
ComposableNode(
package="image_proc",
plugin="image_proc::RectifyNode",
name="rectify_color",
namespace="/camera/camera/color",
remappings=[
("image", raw_topic),
("camera_info", camera_info_topic),
("image_rect", rect_topic),
],
parameters=[{"queue_size": 1}],
extra_arguments=[{"use_intra_process_comms": True}],
),
ComposableNode(
package="apriltag_ros",
plugin="AprilTagNode",
name="apriltag",
namespace="/apriltag",
parameters=[
tag_config,
{
"detector.decimate": ParameterValue(
LaunchConfiguration("apriltag_decimate"),
value_type=float,
)
},
],
remappings=[
("image_rect", rect_topic),
("camera_info", camera_info_topic),
],
extra_arguments=[{"use_intra_process_comms": True}],
),
],
output="screen",
emulate_tty=True,
)
sdk = Node(
package="linker_hand_ros2_sdk",
executable="linker_hand_sdk",
name="linker_hand_sdk",
output="screen",
condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[
{
"hand_type": "left",
"hand_joint": "G20",
"can": LaunchConfiguration("can_interface"),
"modbus": "None",
"topic_prefix": "/g20",
"move_on_startup": False,
"startup_speed": ParameterValue(
LaunchConfiguration("calibration_speed"),
value_type=int,
),
"startup_torque": 80,
"state_poll_rate": 10.0,
"repeat_position_commands": False,
"is_touch": False,
}
],
)
calibration_node = Node(
package="g20_thumb_apriltag_calibration",
executable="cmc_roll_calibration_node",
name="g20_thumb_cmc_roll_calibration",
output="screen",
parameters=[
calibration_config,
{
"serial_number": serial_number,
"session_dir": str(session_dir),
"commands_enabled": ParameterValue(
LaunchConfiguration("commands_enabled"),
value_type=bool,
),
"image_topic": rect_topic,
"publish_debug_image": ParameterValue(
LaunchConfiguration("publish_debug_image"),
value_type=bool,
),
},
],
)
return [
LogInfo(msg=f"G20 CMC roll calibration session: {session_dir}"),
LogInfo(
msg=(
"Only T0(ID 0) and T3(ID 1) are required; "
"T4/T5 detections are ignored"
)
),
LogInfo(
msg=(
"Motor 5 performs three 255->0->255 sweeps; "
"static captures measure both zero and angular travel"
)
),
camera,
vision_container,
sdk,
calibration_node,
]
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
)
return LaunchDescription(
[
SetEnvironmentVariable(
name="FASTRTPS_DEFAULT_PROFILES_FILE",
value=str(
package_share / "config" / "fastdds_large_images.xml"
),
),
DeclareLaunchArgument("serial_number", default_value="UNSET"),
DeclareLaunchArgument(
"camera_serial_number", default_value="DB2163742"
),
DeclareLaunchArgument(
"camera_model", default_value="MV-CS020-10UM"
),
DeclareLaunchArgument(
"camera_name", default_value="hikrobot_front_DB2163742"
),
DeclareLaunchArgument(
"camera_frame_id", default_value="camera_color_optical_frame"
),
DeclareLaunchArgument("image_width", default_value="1624"),
DeclareLaunchArgument("image_height", default_value="1240"),
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
DeclareLaunchArgument("gain_db", default_value="0.0"),
DeclareLaunchArgument("auto_exposure", default_value="false"),
DeclareLaunchArgument(
"camera_info_url",
default_value=str(
Path.home()
/ ".ros"
/ "camera_info"
/ "hikrobot_DB2163742.yaml"
),
),
DeclareLaunchArgument("can_interface", default_value="can0"),
DeclareLaunchArgument("calibration_speed", default_value="15"),
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
DeclareLaunchArgument("commands_enabled", default_value="true"),
DeclareLaunchArgument(
"publish_debug_image", default_value="true"
),
DeclareLaunchArgument("start_camera", default_value="true"),
DeclareLaunchArgument("start_sdk", default_value="true"),
DeclareLaunchArgument(
"output_root",
default_value=str(Path.cwd() / "calibration_output"),
),
DeclareLaunchArgument("session_dir", default_value=""),
DeclareLaunchArgument(
"calibration_config",
default_value=str(
package_share
/ "config"
/ "cmc_roll_zero_travel.yaml"
),
),
DeclareLaunchArgument(
"tag_config",
default_value=str(
package_share / "config" / "front_tags.yaml"
),
),
OpaqueFunction(function=_launch_stack),
]
)
src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py
+391
View File
@@ -0,0 +1,391 @@
"""Launch the complete front-camera G20 thumb calibration stack."""
from __future__ import annotations
from datetime import datetime
from pathlib import Path
import re
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
ExecuteProcess,
LogInfo,
OpaqueFunction,
SetEnvironmentVariable,
)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from launch_ros.parameter_descriptions import ParameterValue
def _launch_stack(context):
serial_number = LaunchConfiguration("serial_number").perform(context)
if not serial_number or serial_number == "UNSET":
raise RuntimeError(
"serial_number is required, for example serial_number:=G20_LEFT_001"
)
if (
re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None
or serial_number in {".", ".."}
):
raise RuntimeError(
"serial_number may contain only letters, digits, dot, underscore and dash"
)
requested_session = LaunchConfiguration("session_dir").perform(context)
output_root = Path(LaunchConfiguration("output_root").perform(context)).resolve()
if requested_session:
session_dir = Path(requested_session).expanduser().resolve()
else:
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
session_dir = output_root / serial_number / timestamp
session_dir.mkdir(parents=True, exist_ok=True)
bag_path = session_dir / "rosbag"
calibration_config = LaunchConfiguration("calibration_config").perform(context)
tag_config = LaunchConfiguration("tag_config").perform(context)
use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower()
if use_roi_text not in {"true", "false"}:
raise RuntimeError("use_roi must be true or false")
use_roi = use_roi_text == "true"
roi_values = {}
for name in ("roi_x", "roi_y", "roi_width", "roi_height"):
text = LaunchConfiguration(name).perform(context)
try:
roi_values[name] = int(text)
except ValueError as error:
raise RuntimeError(f"{name} must be an integer") from error
if roi_values["roi_x"] < 0 or roi_values["roi_y"] < 0:
raise RuntimeError("roi_x and roi_y must be non-negative")
if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0:
raise RuntimeError("roi_width and roi_height must be positive")
try:
image_width = int(LaunchConfiguration("image_width").perform(context))
image_height = int(LaunchConfiguration("image_height").perform(context))
except ValueError as error:
raise RuntimeError("image_width and image_height must be integers") from error
if image_width <= 0 or image_height <= 0:
raise RuntimeError("image_width and image_height must be positive")
if use_roi:
if (
roi_values["roi_x"] + roi_values["roi_width"] > image_width
or roi_values["roi_y"] + roi_values["roi_height"] > image_height
):
raise RuntimeError(
"ROI lies outside camera image "
f"{image_width}x{image_height}"
)
camera = Node(
package="g20_thumb_apriltag_calibration",
executable="hikrobot_camera_node",
name="hikrobot_camera",
namespace="/camera/camera/color",
output="screen",
emulate_tty=True,
condition=IfCondition(LaunchConfiguration("start_camera")),
parameters=[
{
"serial_number": LaunchConfiguration("camera_serial_number"),
"expected_model": LaunchConfiguration("camera_model"),
"camera_name": LaunchConfiguration("camera_name"),
"frame_id": LaunchConfiguration("camera_frame_id"),
"image_width": ParameterValue(
LaunchConfiguration("image_width"), value_type=int
),
"image_height": ParameterValue(
LaunchConfiguration("image_height"), value_type=int
),
"frame_rate": ParameterValue(
LaunchConfiguration("camera_frame_rate"), value_type=float
),
"exposure_time_us": ParameterValue(
LaunchConfiguration("exposure_time_us"), value_type=float
),
"gain_db": ParameterValue(
LaunchConfiguration("gain_db"), value_type=float
),
"auto_exposure": ParameterValue(
LaunchConfiguration("auto_exposure"), value_type=bool
),
"camera_info_url": LaunchConfiguration("camera_info_url"),
}
],
)
vision_components = []
if use_roi:
processed_image_raw_topic = "/g20_thumb_roi/image_raw"
processed_camera_info_topic = "/g20_thumb_roi/camera_info"
processed_image_rect_topic = "/g20_thumb_roi/image_rect"
vision_components.append(
ComposableNode(
package="image_proc",
plugin="image_proc::CropDecimateNode",
name="crop_color_roi",
namespace="/g20_thumb_roi",
remappings=[
("in/image_raw", "/camera/camera/color/image_raw"),
("in/camera_info", "/camera/camera/color/camera_info"),
("out/image_raw", processed_image_raw_topic),
("out/camera_info", processed_camera_info_topic),
],
parameters=[
{
"queue_size": 5,
"decimation_x": 1,
"decimation_y": 1,
"offset_x": roi_values["roi_x"],
"offset_y": roi_values["roi_y"],
"width": roi_values["roi_width"],
"height": roi_values["roi_height"],
}
],
extra_arguments=[{"use_intra_process_comms": True}],
)
)
rectifier_namespace = "/g20_thumb_roi"
rectifier_name = "rectify_color_roi"
else:
processed_image_raw_topic = "/camera/camera/color/image_raw"
processed_camera_info_topic = "/camera/camera/color/camera_info"
processed_image_rect_topic = "/camera/camera/color/image_rect"
rectifier_namespace = "/camera/camera/color"
rectifier_name = "rectify_color"
vision_components.append(
ComposableNode(
package="image_proc",
plugin="image_proc::RectifyNode",
name=rectifier_name,
namespace=rectifier_namespace,
remappings=[
("image", processed_image_raw_topic),
("camera_info", processed_camera_info_topic),
("image_rect", processed_image_rect_topic),
],
parameters=[{"queue_size": 1}],
extra_arguments=[{"use_intra_process_comms": True}],
)
)
vision_components.append(
ComposableNode(
package="apriltag_ros",
plugin="AprilTagNode",
name="apriltag",
namespace="/apriltag",
parameters=[
tag_config,
{
"detector.decimate": ParameterValue(
LaunchConfiguration("apriltag_decimate"),
value_type=float,
)
},
],
remappings=[
("image_rect", processed_image_rect_topic),
("camera_info", processed_camera_info_topic),
],
extra_arguments=[{"use_intra_process_comms": True}],
)
)
vision_container = ComposableNodeContainer(
name="g20_thumb_vision_container",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
composable_node_descriptions=vision_components,
output="screen",
emulate_tty=True,
)
sdk = Node(
package="linker_hand_ros2_sdk",
executable="linker_hand_sdk",
name="linker_hand_sdk",
output="screen",
condition=IfCondition(LaunchConfiguration("start_sdk")),
parameters=[
{
"hand_type": "left",
"hand_joint": "G20",
"can": LaunchConfiguration("can_interface"),
"modbus": "None",
"topic_prefix": "/g20",
"move_on_startup": False,
"startup_speed": ParameterValue(
LaunchConfiguration("calibration_speed"),
value_type=int,
),
"startup_torque": 80,
"state_poll_rate": 10.0,
"repeat_position_commands": False,
"is_touch": False,
}
],
)
calibration = Node(
package="g20_thumb_apriltag_calibration",
executable="calibration_node",
name="g20_thumb_calibration",
output="screen",
parameters=[
calibration_config,
tag_config,
{
"serial_number": serial_number,
"session_dir": str(session_dir),
"commands_enabled": LaunchConfiguration("commands_enabled"),
"calibration_speed": ParameterValue(
LaunchConfiguration("calibration_speed"),
value_type=int,
),
"continuous_motion_mode": ParameterValue(
LaunchConfiguration("continuous_motion_mode"),
value_type=str,
),
"angle_estimation_mode": ParameterValue(
LaunchConfiguration("angle_estimation_mode"),
value_type=str,
),
"camera_serial_number": LaunchConfiguration("camera_serial_number"),
"rosbag_path": str(bag_path),
"camera_info_topic": processed_camera_info_topic,
"image_topic": processed_image_rect_topic,
"publish_debug_image": LaunchConfiguration(
"publish_debug_image"
),
},
],
)
bag = ExecuteProcess(
condition=IfCondition(LaunchConfiguration("record_bag")),
cmd=[
"ros2",
"bag",
"record",
"--storage",
"mcap",
"--storage-preset-profile",
"zstd_fast",
"--max-bag-size",
"10737418240",
"--output",
str(bag_path),
processed_image_raw_topic,
processed_camera_info_topic,
"/apriltag/detections",
"/tf",
"/g20/cb_left_hand_control_cmd",
"/g20/cb_left_hand_state",
"/g20/cb_left_hand_info",
"/g20_thumb_calibration/status",
],
output="screen",
)
actions = [
LogInfo(msg=f"G20 thumb calibration session: {session_dir}"),
LogInfo(
msg=(
"G20 thumb image ROI: "
f"x={roi_values['roi_x']}, y={roi_values['roi_y']}, "
f"width={roi_values['roi_width']}, "
f"height={roi_values['roi_height']}"
if use_roi
else "G20 thumb image ROI: disabled"
)
),
camera,
vision_container,
]
actions.extend([sdk, calibration, bag])
return actions
def generate_launch_description() -> LaunchDescription:
package_share = Path(
get_package_share_directory("g20_thumb_apriltag_calibration")
)
default_output = str(Path.cwd() / "calibration_output")
return LaunchDescription(
[
SetEnvironmentVariable(
name="FASTRTPS_DEFAULT_PROFILES_FILE",
value=str(
package_share / "config" / "fastdds_large_images.xml"
),
),
DeclareLaunchArgument("serial_number", default_value="UNSET"),
DeclareLaunchArgument(
"camera_serial_number", default_value="DB2163742"
),
DeclareLaunchArgument(
"camera_model", default_value="MV-CS020-10UM"
),
DeclareLaunchArgument(
"camera_name", default_value="hikrobot_front_DB2163742"
),
DeclareLaunchArgument(
"camera_frame_id", default_value="camera_color_optical_frame"
),
DeclareLaunchArgument("image_width", default_value="1624"),
DeclareLaunchArgument("image_height", default_value="1240"),
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
DeclareLaunchArgument("gain_db", default_value="0.0"),
DeclareLaunchArgument("auto_exposure", default_value="false"),
DeclareLaunchArgument(
"camera_info_url",
default_value=str(
Path.home()
/ ".ros"
/ "camera_info"
/ "hikrobot_DB2163742.yaml"
),
),
DeclareLaunchArgument(
"publish_debug_image", default_value="false"
),
DeclareLaunchArgument("use_roi", default_value="false"),
DeclareLaunchArgument("roi_x", default_value="128"),
DeclareLaunchArgument("roi_y", default_value="192"),
DeclareLaunchArgument("roi_width", default_value="1024"),
DeclareLaunchArgument("roi_height", default_value="528"),
DeclareLaunchArgument("can_interface", default_value="can0"),
DeclareLaunchArgument("calibration_speed", default_value="15"),
DeclareLaunchArgument(
"continuous_motion_mode", default_value="endpoint"
),
DeclareLaunchArgument(
"angle_estimation_mode",
default_value="trajectory_center_3d",
),
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
DeclareLaunchArgument("commands_enabled", default_value="true"),
DeclareLaunchArgument("start_camera", default_value="true"),
DeclareLaunchArgument("start_sdk", default_value="true"),
DeclareLaunchArgument("record_bag", default_value="false"),
DeclareLaunchArgument("output_root", default_value=default_output),
DeclareLaunchArgument("session_dir", default_value=""),
DeclareLaunchArgument(
"calibration_config",
default_value=str(package_share / "config" / "calibration.yaml"),
),
DeclareLaunchArgument(
"tag_config",
default_value=str(package_share / "config" / "front_tags.yaml"),
),
OpaqueFunction(function=_launch_stack),
]
)
src/linkerhand_calibration/launch/three_camera_calibration.launch.py → src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py
+238 -171
View File
@@ -1,9 +1,8 @@
"""Launch Profile-declared Hikrobot views and one 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,65 +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(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 = workspace
return workspace if workspace.is_file() else installed
def _launch_stack(context):
from linkerhand_calibration.product import (
ProductCalibrationContract,
)
from linkerhand_calibration.profiles import load_hand_profile
from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics
model = LaunchConfiguration("model").perform(context).strip().upper()
hand_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:
profile_path = LaunchConfiguration("profile_config").perform(context).strip()
if not profile_path:
raise ValueError("online calibration requires a protected YAML Profile; use calibrate_hand --config")
expected = LaunchConfiguration("profile_config_expected_sha256").perform(context).strip()
if not expected or hashlib.sha256(Path(profile_path).read_bytes()).hexdigest() != expected:
raise ValueError("Profile changed between product validation and launch")
contract = ProductCalibrationContract(declarative=load_hand_profile(profile_path))
key = contract.typed_profile.key
if (key.model, key.side, key.layout) != (model, hand_type, tag_layout):
raise ValueError("launch identity differs from the protected Profile")
profile = get_hand_calibration_profile(hand_type, hand_model)
except ValueError as error:
raise RuntimeError(str(error)) from error
views = contract.typed_profile.vision.view_names
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
if not requested_tag_config:
raise RuntimeError("tag_config is required; use calibrate_hand --config")
tag_config = Path(requested_tag_config).expanduser().resolve()
if not tag_config.is_file():
raise RuntimeError(f"tag config does not exist: {tag_config}")
topic_prefix = f"/{model.lower()}"
uses_hcan = contract.typed_profile.sdk_adapter == "o12_hcan_sdk"
if contract.typed_profile.sdk_adapter not in {"legacy_byte_sdk", "o12_hcan_sdk"}:
raise RuntimeError("SDK adapter has no ROS launch binding")
topics = sdk_topics(contract.typed_profile)
command_topic, state_topic = topics.command, topics.feedback
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)
if not requested_source:
raise RuntimeError("source_urdf_path is required; use calibrate_hand --config")
source_urdf = Path(requested_source).expanduser().resolve()
source_urdf = (
Path(requested_source).expanduser().resolve()
if requested_source
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 (
@@ -107,32 +113,31 @@ def _launch_stack(context):
camera_serials = {
view: LaunchConfiguration(f"{view}_camera_serial").perform(context)
for view in views
for view in VIEWS
}
if any(not serial for serial in camera_serials.values()):
raise RuntimeError("all Profile camera serial numbers are required")
if len(set(camera_serials.values())) != len(views):
raise RuntimeError("Profile camera serial numbers must be unique")
raise RuntimeError("all three camera serial numbers are required")
if len(set(camera_serials.values())) != 3:
raise RuntimeError("front/side/top camera serial numbers must be unique")
cameras = []
components = []
raw_topics = []
info_topics = []
detection_topics = []
calibration_namespace = contract.typed_profile.namespace
for view in views:
for view in VIEWS:
namespace = f"{calibration_namespace}/{view}/camera"
raw_topic = f"{namespace}/image_raw"
info_topic = f"{namespace}/camera_info"
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,
@@ -149,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,
@@ -183,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}],
@@ -195,7 +200,7 @@ def _launch_stack(context):
name="apriltag",
namespace=detector_namespace,
parameters=[
str(tag_config),
tag_config,
{
"detector.decimate": ParameterValue(
LaunchConfiguration("apriltag_decimate"),
@@ -205,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}],
),
@@ -213,7 +218,7 @@ def _launch_stack(context):
)
vision = ComposableNodeContainer(
name=f"{model.lower()}_calibration_vision",
name=f"{model_key}_three_camera_vision",
namespace="/",
package="rclcpp_components",
executable="component_container_mt",
@@ -221,103 +226,127 @@ def _launch_stack(context):
output="screen",
emulate_tty=True,
)
sdk = (
Node(
package="linkerhand_calibration",
executable="o12_sdk_bridge",
name="o12_sdk_bridge",
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=[{
"vendor_config": LaunchConfiguration("vendor_sdk_config"),
"vendor_config_sha256": LaunchConfiguration("sdk_config_expected_sha256"),
"vendor_python_package": LaunchConfiguration("vendor_sdk_python_package"),
"vendor_package_sha256": LaunchConfiguration("sdk_package_expected_sha256"),
"hand_type": hand_type,
"topic_prefix": f"/{model.lower()}/{hand_type}",
}],
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
),
}
],
)
if uses_hcan
else Node(
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": 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,
}],
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,
"--profile-config", profile_path, "--profile-sha256", expected,
],
parameters=[
LaunchConfiguration("calibration_config"),
{
"serial_number": hand_serial,
"hand_model": hand_model,
"hand_type": hand_type,
"session_dir": str(session_dir),
"resume_raw_samples_path": LaunchConfiguration(
"resume_raw_samples_path"
),
# The SDK performs roughly 25 synchronous CAN queries whenever
# cb_<side>_hand_info has a subscriber. Calibration only used
# that topic to display a speed diagnostic, while those reads
# created 17-33 command-unit holes in position trajectories.
"info_topic": f"{calibration_namespace}/disabled_hand_info",
"command_topic": command_topic,
"state_topic": state_topic,
"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"
),
"sdk_config_expected_sha256": LaunchConfiguration(
"sdk_config_expected_sha256"
),
"sdk_package_expected_sha256": LaunchConfiguration("sdk_package_expected_sha256"),
"profile_config_expected_sha256": LaunchConfiguration(
"profile_config_expected_sha256"
"corrected_urdf_output_dir": LaunchConfiguration(
"corrected_urdf_output_dir"
),
**{
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
),
"normal_calibration_speed": ParameterValue(
LaunchConfiguration("calibration_speed"), value_type=int
),
"index_roll_calibration_speed": ParameterValue(
LaunchConfiguration("index_roll_calibration_speed"),
value_type=int,
),
"index_flex_calibration_speed": ParameterValue(
LaunchConfiguration("index_flex_calibration_speed"),
value_type=int,
),
"o30_internal_speed_u8": ParameterValue(
LaunchConfiguration("o30_internal_speed_u8"), value_type=int
),
"o30_command_full_range_seconds": ParameterValue(
LaunchConfiguration("o30_command_full_range_seconds"),
value_type=float,
),
"validation_enabled": ParameterValue(
LaunchConfiguration("validation_enabled"), value_type=bool
),
},
],
)
@@ -340,12 +369,7 @@ def _launch_stack(context):
*detection_topics,
command_topic,
state_topic,
*(
[
f"/{model.lower()}/{hand_type}/calibration_health",
]
if uses_hcan else []
),
hand_info_topic,
f"{calibration_namespace}/status",
],
output="screen",
@@ -353,13 +377,15 @@ def _launch_stack(context):
return [
LogInfo(
msg=(
f"{model} {hand_type} {tag_layout} calibration session: {session_dir}; "
f"{hand_model} {hand_type} three-camera session: {session_dir}; "
f"source_urdf={source_urdf}"
)
),
LogInfo(
msg=(
"Camera mapping: " + " ".join(f"{view}={serial}" for view, serial in camera_serials.items())
"Camera mapping: front="
f"{camera_serials['front']} side={camera_serials['side']} "
f"top={camera_serials['top']}"
)
),
*cameras,
@@ -372,8 +398,9 @@ 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(
[
# Camera processes publish ~2 MB frames across DDS. Force the
@@ -391,44 +418,75 @@ def generate_launch_description() -> LaunchDescription:
name="FASTRTPS_DEFAULT_PROFILES_FILE",
value=str(package_share / "config" / "fastdds_large_images.xml"),
),
DeclareLaunchArgument("model", default_value=""),
DeclareLaunchArgument("hand_type", default_value=""),
DeclareLaunchArgument("tag_layout", default_value=""),
DeclareLaunchArgument("hand_model", default_value="G20"),
DeclareLaunchArgument("hand_type", default_value="left"),
DeclareLaunchArgument("serial_number", default_value="UNSET"),
DeclareLaunchArgument(
"front_camera_serial", default_value="DB2163742"
),
DeclareLaunchArgument(
"side_camera_serial", default_value="DB2163749"
),
DeclareLaunchArgument(
"top_camera_serial", default_value="DB2163739"
),
DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
DeclareLaunchArgument(
"front_camera_name", default_value="hikrobot_front_DB2163742"
),
DeclareLaunchArgument(
"side_camera_name", default_value="hikrobot_side_DB2163749"
),
DeclareLaunchArgument(
"top_camera_name", default_value="hikrobot_top_DB2163739"
),
DeclareLaunchArgument(
"front_camera_info_url",
default_value=str(info_root / "hikrobot_DB2163742.yaml"),
),
DeclareLaunchArgument(
"side_camera_info_url",
default_value=str(info_root / "hikrobot_DB2163749.yaml"),
),
DeclareLaunchArgument(
"top_camera_info_url",
default_value=str(info_root / "hikrobot_DB2163739.yaml"),
),
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
DeclareLaunchArgument("gain_db", default_value="0.0"),
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("camera_extrinsics_file", default_value=""),
DeclareLaunchArgument(
"index_roll_calibration_speed", default_value="5"
),
DeclareLaunchArgument(
"index_flex_calibration_speed", default_value="10"
),
DeclareLaunchArgument("o30_internal_speed_u8", default_value="0"),
DeclareLaunchArgument(
"o30_command_full_range_seconds", default_value="6.0"
),
DeclareLaunchArgument("validation_enabled", default_value="false"),
DeclareLaunchArgument(
"camera_extrinsics_file",
default_value=str(
Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"
),
),
DeclareLaunchArgument(
"source_urdf_path", default_value=""
),
DeclareLaunchArgument(
"source_urdf_expected_sha256", default_value=""
"corrected_urdf_output_dir", default_value=""
),
DeclareLaunchArgument(
"camera_extrinsics_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"calibration_config_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"tag_config_expected_sha256", default_value=""
),
DeclareLaunchArgument("vendor_sdk_config", default_value=""),
DeclareLaunchArgument("vendor_sdk_python_package", default_value=""),
DeclareLaunchArgument("sdk_package_expected_sha256", default_value=""),
DeclareLaunchArgument(
"sdk_config_expected_sha256", default_value=""
),
DeclareLaunchArgument(
"profile_config_expected_sha256", default_value=""
),
DeclareLaunchArgument("profile_config", default_value=""),
DeclareLaunchArgument("commands_enabled", default_value="true"),
DeclareLaunchArgument("start_cameras", default_value="true"),
DeclareLaunchArgument("start_sdk", default_value="true"),
@@ -438,14 +496,23 @@ 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(
"calibration_config",
default_value="",
default_value=str(
package_share / "config" / "three_camera_calibration.yaml"
),
),
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 -13
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",
@@ -112,12 +112,6 @@ def _launch(context):
LaunchConfiguration("maximum_reprojection_rms_px"),
value_type=float,
),
"maximum_candidate_pair_reprojection_rms_px": ParameterValue(
LaunchConfiguration(
"maximum_candidate_pair_reprojection_rms_px"
),
value_type=float,
),
"maximum_single_camera_reprojection_rms_px": ParameterValue(
LaunchConfiguration(
"maximum_single_camera_reprojection_rms_px"
@@ -144,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(
@@ -188,10 +182,6 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument(
"maximum_reprojection_rms_px", default_value="1.2"
),
DeclareLaunchArgument(
"maximum_candidate_pair_reprojection_rms_px",
default_value="1.5",
),
DeclareLaunchArgument(
"maximum_single_camera_reprojection_rms_px",
default_value="1.5",
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
+49 -106
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"] == 1
assert parameters["automatic_fit_retry_limit"] == 0
assert parameters["motor_stall_timeout_seconds"] == 2.0
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
assert parameters["automatic_sweep_retry_limit"] == 3
assert parameters["automatic_fit_retry_limit"] == 2
assert parameters["motor_stall_timeout_seconds"] >= 5.0
assert parameters["motor_stall_minimum_progress_u8"] == 1.0
assert parameters["automatic_motion_retry_limit"] == 0
assert parameters["automatic_motion_retry_limit"] == 2
assert parameters["provisional_warning_ratio"] == 1.25
assert parameters["retry_speed_scales"] == [1.0]
assert parameters["retry_endpoint_hold_seconds"] == [0.5]
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}/"
@@ -257,7 +200,7 @@ def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
assert parameters["passive_ip_multiplier"] == 1.02
assert parameters["pnp_minimum_valid_rate"] == 0.95
assert parameters["pnp_maximum_reprojection_error_px"] <= 1.5
assert "pnp_reprojection_tie_px" not in parameters # Shared runtime policy.
assert parameters["pnp_reprojection_tie_px"] == 1.5
assert parameters["pnp_tracker_reset_seconds"] == 5.0
assert parameters["pnp_group_relative_rotation_scale_deg"] == 5.0
assert parameters["pnp_group_relative_translation_scale_m"] == 0.01
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
+38 -264
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,
@@ -140,31 +140,54 @@ def test_clear_reprojection_advantage_releases_stale_mirror_branch() -> None:
assert selected == true_pose
@pytest.mark.parametrize("tie", [-0.01, 1.5, 2.0, float("nan"), float("inf")])
def test_invalid_branch_tolerance_is_rejected_before_tracking(tie) -> None:
parameters = dict(
maximum_reprojection_error_px=1.5, reprojection_tie_px=tie,
maximum_pose_jump_rad=np.deg2rad(35.0),
maximum_translation_jump_m=0.04, maximum_tag_tilt_rad=np.deg2rad(75.0),
def test_active_motion_can_prioritise_continuous_branch() -> None:
previous = SquareTagPose(
quaternion_xyzw=tuple(
Rotation.from_euler("y", 55.0, degrees=True).as_quat()
),
translation_xyz_m=(0.0, 0.0, 0.25),
reprojection_error_px=0.25,
)
with pytest.raises(ValueError, match="reprojection_tie_px"):
SquareTagPoseTracker(**parameters, reset_after_seconds=5.0)
with pytest.raises(ValueError, match="reprojection_tie_px"):
select_continuous_pose([], previous=None, **parameters)
continuous = SquareTagPose(
quaternion_xyzw=tuple(
Rotation.from_euler("y", 54.0, degrees=True).as_quat()
),
translation_xyz_m=(0.0, 0.0, 0.25),
reprojection_error_px=0.24,
)
discontinuous = SquareTagPose(
quaternion_xyzw=tuple(
Rotation.from_euler("y", 2.0, degrees=True).as_quat()
),
translation_xyz_m=(0.001, 0.0, 0.25),
reprojection_error_px=0.05,
)
selected, reason = select_continuous_pose(
[continuous, discontinuous],
previous=previous,
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),
)
assert reason == ""
assert selected == continuous
def test_tracker_recovers_after_timestamp_gap() -> None:
tracker = SquareTagPoseTracker(
maximum_reprojection_error_px=1.5,
maximum_pose_jump_rad=np.deg2rad(1.0),
reprojection_tie_px=1.5,
maximum_pose_jump_rad=np.deg2rad(5.0),
maximum_translation_jump_m=0.04,
maximum_tag_tilt_rad=np.deg2rad(75.0),
reset_after_seconds=0.5,
)
first_rotation = Rotation.from_euler("y", 0.0, degrees=True)
# Keep both IPPE solutions near-tied so this exercises gap recovery,
# rather than correction of a branch with clearly worse image fit.
second_rotation = Rotation.from_euler("y", 2.0, degrees=True)
second_rotation = Rotation.from_euler("y", 20.0, degrees=True)
first, first_reason = tracker.estimate(
"t0",
_project(first_rotation, np.asarray([0.0, 0.0, 0.25])),
@@ -211,146 +234,6 @@ def _pose(
)
def test_group_tracker_receives_oblique_reprojection_valid_candidates() -> None:
per_tag = SquareTagPoseTracker(
maximum_reprojection_error_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"),
@@ -441,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"),
@@ -541,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
+11 -79
View File
@@ -9,20 +9,6 @@ class HandConfig:
joint_names_en: Optional[List[str]] = None
init_pos: List[int] = field(default_factory=list)
preset_actions: Optional[Dict[str, List[int]]] = None
preset_action_overrides: Optional[Dict[str, Dict[str, List[int]]]] = None
# Integer GUI values are divided by this scale before publishing.
# Legacy hands use raw u8 values (scale=1); O12 uses milliradians.
position_scale: int = 1
position_unit: str = "u8"
def get_preset_actions(self, hand_type: str) -> Dict[str, List[int]]:
"""Return preset actions with hand-specific values applied."""
actions = dict(self.preset_actions or {})
if self.preset_action_overrides:
actions.update(
self.preset_action_overrides.get(hand_type.lower(), {})
)
return actions
# ------------------------------------------------------------------
# 常量字典（仅构建一次）
@@ -89,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],
@@ -114,6 +100,8 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
"动作9": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
"根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
"根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
"根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
"根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
"末端1": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
"末端2": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255],
"末端3": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
@@ -236,20 +224,13 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
"叁": [92, 87, 255, 255, 255, 0],
"肆": [92, 87, 255, 255, 255, 255],
"伍": [255, 255, 255, 255, 255, 255],
"OK": [95, 75, 116, 255, 255, 255],
"拇指对中指": [88, 2, 255, 114, 255, 255],
"OK": [139, 91, 103, 250, 250, 250],
"点赞": [250, 79, 0, 0, 0, 0],
"握拳": [102, 18, 0, 0, 0, 0],
},
preset_action_overrides={
"right": {
"OK": [95, 83, 122, 255, 255, 255],
"拇指对中指": [95, 8, 255, 114, 255, 255],
},
},
}
),
"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={
@@ -259,7 +240,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
"叁": [0, 39, 255, 255, 255, 0],
"肆": [0, 0, 255, 255, 255, 255],
"伍": [255, 255, 255, 255, 255, 255],
"OK": [62, 5, 151, 255, 255, 255],
"OK": [74, 13, 153, 255, 255, 255],
"点赞": [255, 255, 0, 0, 0, 0],
"握拳": [79, 11, 0, 0, 0, 0],
"序列动作1": [250, 250, 250, 250, 250, 250],
@@ -275,56 +256,7 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
"拇指压感准备1": [139, 18, 130, 0, 0, 0],
"拇指压感测试": [39, 30, 122, 250, 250, 250],
"拇指压感准备2": [139, 18, 130, 0, 0, 0]
},
preset_action_overrides={
"right": {
"OK": [58, 6, 153, 255, 255, 255],
},
},
),
"O12": HandConfig(
# O12 active-angle API order. Keep this independent from the
# physical motor order exposed by some SDK metadata.
joint_names_en=[
"thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
"index_abad", "index_mcp", "index_pip",
"middle_abad", "middle_mcp", "middle_pip",
"ring_mcp", "pinky_mcp",
],
joint_names=[
"拇指旋转", "拇指侧摆", "拇指根部", "拇指中部",
"食指侧摆", "食指根部", "食指中部",
"中指侧摆", "中指根部", "中指中部",
"无名指根部", "小指根部",
],
# Values are milliradians so QSlider can retain 0.001 rad resolution.
init_pos=[0] * 12,
preset_actions={
"展开": [0] * 12,
"食指轻弯": [0, 0, 0, 0, 0, 300, 400, 0, 0, 0, 0, 0],
"半握": [
300, -200, -200, -500,
0, 650, 700, 0, 650, 800, 700, 700,
],
},
preset_action_overrides={
"right": {
"拇指对食指": [
245, -766, -818, 0, 0, 1046, 0, 0, 0, 0, 0, 0,
],
"拇指对中指": [
489, -843, -685, 0, 0, 0, 0, 0, 1147, 0, 0, 0,
],
"拇指对无名指": [
699, -779, -827, 0, 0, 0, 0, 0, 0, 0, 690, 0,
],
"拇指对小指": [
786, -1065, -827, 0, 0, 0, 0, 0, 0, 0, 0, 645,
],
},
},
position_scale=1000,
position_unit="rad",
}
),
}
HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS)
src/gui_control/gui_control/gui_control.py
+37 -180
View File
@@ -1,6 +1,5 @@
import sys
import time, json
import math
import threading
from dataclasses import dataclass
from typing import List, Dict
@@ -34,16 +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",
],
"O12": [
"thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
"index_abad", "index_mcp", "index_pip",
"middle_abad", "middle_mcp", "middle_pip",
"ring_mcp", "pinky_mcp",
],
}
_CANONICAL_COMMAND_BOUNDS = {
@@ -54,38 +43,17 @@ _CANONICAL_COMMAND_BOUNDS = {
*[(0, 255)] * 5,
],
"O6": [(0, 255)] * 6,
"L6": [(0, 255)] * 6,
}
# Integer slider bounds in milliradians, derived from the O12 active-angle
# limits. Right and left thumb signs differ; all other limits are shared.
_O12_COMMAND_BOUNDS_MRAD = {
"right": [
(0, 942), (-1387, 0), (-827, 0), (-1291, 0),
(-261, 261), (0, 1352), (0, 1530), (-261, 261),
(0, 1357), (0, 1815), (0, 1535), (0, 1535),
],
"left": [
(-942, 0), (0, 1387), (-827, 0), (-1291, 0),
(-261, 261), (0, 1352), (0, 1530), (-261, 261),
(0, 1357), (0, 1815), (0, 1535), (0, 1535),
],
}
class ROS2NodeManager(QObject):
"""ROS2节点管理器，处理ROS通信"""
status_updated = pyqtSignal(str, str) # 状态类型, 消息内容
feedback_updated = pyqtSignal(object)
def __init__(self, node_name: str = "hand_control_node"):
super().__init__()
self.node = None
self.publisher = None
self.publisher_arc = None
self.state_subscription = None
self.speed_pub = None
self.torque_pub = None
self.joint_state = JointState()
self.joint_state.header = Header()
@@ -115,40 +83,27 @@ class ROS2NodeManager(QObject):
self.node.get_parameter('topic_prefix').value
)
if self.hand_joint == "O12":
topic_base = f'/o12/{self.hand_type}'
self.publisher = self.node.create_publisher(
JointState, f'{topic_base}/joint_cmd', 10
)
self.state_subscription = self.node.create_subscription(
JointState,
f'{topic_base}/joint_states',
self._o12_feedback_callback,
10,
)
elif self.is_arc == True:
if self.is_arc == True:
# 创建发布者
self.publisher_arc = self.node.create_publisher(
JointState,
self.topic(f'/cb_{self.hand_type}_hand_control_cmd_arc'),
10
)
# 创建旧型号发布者
if self.hand_joint != "O12":
self.publisher = self.node.create_publisher(
JointState,
self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
10
)
# 创建发布者
self.publisher = self.node.create_publisher(
JointState,
self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
10
)
self.snapshot_publisher = self.node.create_publisher(
JointState, self.topic('/calibration_pose_snapshot'), 10
)
# 新增 speed / torque 发布者
if self.hand_joint != "O12":
self.speed_pub = self.node.create_publisher(
String, self.topic('/cb_hand_setting_cmd'), 10)
self.torque_pub = self.node.create_publisher(
String, self.topic('/cb_hand_setting_cmd'), 10)
self.speed_pub = self.node.create_publisher(
String, self.topic('/cb_hand_setting_cmd'), 10)
self.torque_pub = self.node.create_publisher(
String, self.topic('/cb_hand_setting_cmd'), 10)
self.status_updated.emit(
"info",
f"ROS2节点初始化成功: {self.topic_prefix or '/'} "
@@ -181,36 +136,15 @@ class ROS2NodeManager(QObject):
while rclpy.ok() and self.node:
rclpy.spin_once(self.node, timeout_sec=0.1)
def _o12_feedback_callback(self, msg: JointState):
"""Forward 12-channel O12 radian feedback safely into the Qt thread."""
if len(msg.position) != 12:
self.status_updated.emit(
"error", f"O12位置反馈长度错误: {len(msg.position)}，应为12"
)
return
self.feedback_updated.emit([float(value) for value in msg.position])
def command_bounds(self, count: int):
if self.hand_joint == "O12":
bounds = _O12_COMMAND_BOUNDS_MRAD.get(self.hand_type)
else:
bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
if not bounds or len(bounds) != count:
return [(0, 255)] * count
return bounds
def bound_positions(self, positions: List[int]) -> List[int]:
bounds = self.command_bounds(len(positions))
bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
if not bounds or len(bounds) != len(positions):
return list(positions)
return [
max(minimum, min(maximum, int(value)))
for value, (minimum, maximum) in zip(positions, bounds)
]
def wire_positions(self, positions: List[int]) -> List[float]:
positions = self.bound_positions(positions)
scale = _HAND_CONFIGS[self.hand_joint].position_scale
return [float(value) / float(scale) for value in positions]
def publish_joint_state(self, positions: List[int]):
"""发布关节状态消息"""
if not self.publisher or not self.node:
@@ -219,21 +153,15 @@ class ROS2NodeManager(QObject):
try:
positions = self.bound_positions(positions)
wire_positions = self.wire_positions(positions)
self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
self.joint_state.position = wire_positions
self.joint_state.position = [float(pos) for pos in positions]
# self.joint_state.velocity = [0.1] * len(positions)
# self.joint_state.effort = [0.01] * len(positions)
# 如果有关节名称，添加到消息中
#hand_config = HandConfig.from_hand_type(self.hand_joint)
hand_config = _HAND_CONFIGS[self.hand_joint]
canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
# O12 SDK 1.1.8 may expose names in physical motor order while the
# position array uses active-angle order. An empty name array makes
# the documented position order unambiguous to the driver.
if self.hand_joint == "O12":
self.joint_state.name = []
elif canonical_names and len(canonical_names) == len(positions):
if canonical_names and len(canonical_names) == len(positions):
self.joint_state.name = canonical_names
elif len(hand_config.joint_names) == len(positions):
if hand_config.joint_names_en != None:
@@ -242,7 +170,7 @@ class ROS2NodeManager(QObject):
self.joint_state.name = hand_config.joint_names
self.publisher.publish(self.joint_state)
if self.is_arc == True and self.hand_joint != "O12":
if self.is_arc == True:
if self.hand_joint == "O6":
if self.hand_type == "left":
pose = range_to_arc_left(positions,self.hand_joint)
@@ -277,21 +205,14 @@ class ROS2NodeManager(QObject):
positions = self.bound_positions(positions)
self.publish_joint_state(positions)
self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
self.joint_state.position = self.wire_positions(positions)
self.joint_state.position = [float(pos) for pos in positions]
canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
if canonical_names and self.hand_joint != "O12":
if canonical_names:
self.joint_state.name = canonical_names
elif self.hand_joint == "O12":
self.joint_state.name = []
self.snapshot_publisher.publish(self.joint_state)
self.status_updated.emit("info", "当前标定姿势快照已发布")
def publish_speed(self, val: int):
if self.hand_joint == "O12":
self.status_updated.emit(
"warning", "O12不使用旧版u8速度接口；请通过角度轨迹限制速度"
)
return
if self.hand_joint.upper() in ("O6", "L6"):
joint_len = 6
elif self.hand_joint == "L7":
@@ -311,11 +232,6 @@ class ROS2NodeManager(QObject):
self.speed_pub.publish(msg)
def publish_torque(self, val: int):
if self.hand_joint == "O12":
self.status_updated.emit(
"warning", "O12不使用旧版u8扭矩接口；位置GUI已禁用该设置"
)
return
if self.hand_joint.upper() in ("O6", "L6"):
joint_len = 6
elif self.hand_joint == "L7":
@@ -357,17 +273,11 @@ class HandControlGUI(QWidget):
# 设置ROS管理器
self.ros_manager = ros_manager
self.ros_manager.status_updated.connect(self.update_status)
self.ros_manager.feedback_updated.connect(self.on_feedback_updated)
# 获取手部配置
self.hand_joint = self.ros_manager.hand_joint
self.hand_type = self.ros_manager.hand_type
self.hand_config = _HAND_CONFIGS[self.hand_joint]
self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
self.feedback_positions = None
# O12 only publishes after an explicit slider/preset action. This
# prevents opening or closing the real hand merely by launching GUI.
self.command_dirty = self.hand_joint != "O12"
# 初始化UI
self.init_ui()
@@ -378,20 +288,6 @@ class HandControlGUI(QWidget):
self.publish_timer.timeout.connect(self.publish_joint_state)
self.publish_timer.start()
def format_position(self, value: int) -> str:
"""Format an integer GUI value in the hand's actual command unit."""
scale = self.hand_config.position_scale
if self.hand_config.position_unit == "rad":
radians = float(value) / float(scale)
return f"{radians:.3f} rad / {math.degrees(radians):.1f}°"
return str(value)
def command_positions(self) -> List[float]:
"""Return current slider values converted to wire units."""
return self.ros_manager.wire_positions(
[slider.value() for slider in self.sliders]
)
def init_ui(self):
"""初始化用户界面"""
# 设置窗口属性
@@ -569,11 +465,13 @@ class HandControlGUI(QWidget):
for i, (name, value) in enumerate(zip(
self.hand_config.joint_names, self.hand_config.init_pos
)):
bounds = self.ros_manager.command_bounds(len(self.hand_config.init_pos))
minimum, maximum = bounds[i]
bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
minimum, maximum = (
bounds[i] if bounds and i < len(bounds) else (0, 255)
)
value = max(minimum, min(maximum, int(value)))
# 创建标签
label = QLabel(f"{name}: {self.format_position(value)}")
label = QLabel(f"{name}: {value}")
label.setMinimumWidth(120)
# 创建滑动条
@@ -637,9 +535,9 @@ class HandControlGUI(QWidget):
def create_system_preset_buttons(self, parent_layout):
"""创建系统预设动作按钮"""
self.preset_buttons = [] # 清空按钮列表
if self.preset_actions:
if self.hand_config.preset_actions:
buttons = []
for idx, (name, positions) in enumerate(self.preset_actions.items()):
for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
button = QPushButton(name)
button.setProperty("category", "preset")
button.clicked.connect(
@@ -666,9 +564,6 @@ class HandControlGUI(QWidget):
# —— 2. 新增：速度与扭矩设置（每行一个）——
quick_set_gb = QGroupBox("快速设置")
if self.hand_joint == "O12":
quick_set_gb.setEnabled(False)
quick_set_gb.setToolTip("O12不使用旧版0-255速度/扭矩设置接口")
qv_layout = QVBoxLayout(quick_set_gb)
# 速度行
@@ -797,10 +692,7 @@ class HandControlGUI(QWidget):
"""滑动条值改变事件处理"""
if 0 <= index < len(self.slider_labels):
joint_name = self.hand_config.joint_names[index]
self.slider_labels[index].setText(
f"{joint_name}: {self.format_position(value)}"
)
self.command_dirty = True
self.slider_labels[index].setText(f"{joint_name}: {value}")
# 更新数值显示
self.update_value_display()
@@ -808,23 +700,10 @@ class HandControlGUI(QWidget):
def update_value_display(self):
"""更新数值显示面板内容"""
# 获取所有滑动条的当前值
command = self.command_positions()
command_text = ", ".join(f"{value:.3f}" for value in command)
lines = [f"目标({self.hand_config.position_unit}): [{command_text}]"]
if self.feedback_positions is not None:
feedback_text = ", ".join(
f"{value:.3f}" for value in self.feedback_positions
)
lines.append(f"反馈(rad): [{feedback_text}]")
self.value_display.setText("\n".join(lines))
def on_feedback_updated(self, positions):
"""Display actual O12 joint feedback without moving command sliders."""
self.feedback_positions = list(positions)
self.update_value_display()
if hasattr(self, "connection_status"):
self.connection_status.setText("O12右手已连接并收到位置反馈")
self.connection_status.setObjectName("StatusInfo")
values = [slider.value() for slider in self.sliders]
# 格式化显示为列表形式
self.value_display.setText(f"{values}")
def on_preset_action_clicked(self, positions: List[int]):
"""预设动作按钮点击事件处理"""
@@ -863,25 +742,7 @@ class HandControlGUI(QWidget):
self.cycle_button.setText("循环运行预设动作")
self.reset_preset_buttons_color()
if self.hand_joint == "O12" and self.feedback_positions is not None:
# Hold the latest measured pose instead of merely stopping topic
# publication while a previous position target is still active.
scale = self.hand_config.position_scale
hold_values = [
int(round(value * scale)) for value in self.feedback_positions
]
hold_values = self.ros_manager.bound_positions(hold_values)
for slider, value in zip(self.sliders, hold_values):
slider.blockSignals(True)
slider.setValue(value)
slider.blockSignals(False)
self.command_dirty = True
self.publish_joint_state()
self.update_value_display()
self.status_updated.emit("warning", "已下发当前反馈位置并保持")
else:
self.command_dirty = False
self.status_updated.emit("warning", "已停止连续动作和新的位置发布")
self.status_updated.emit("warning", "已停止所有动作")
def on_save_pose_clicked(self):
"""发布当前滑块姿势，供hand_pose_capture写入profile。"""
@@ -890,7 +751,7 @@ class HandControlGUI(QWidget):
def on_cycle_clicked(self):
"""循环运行预设动作按钮点击事件处理"""
if not self.preset_actions:
if not self.hand_config.preset_actions:
QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行")
return
@@ -913,19 +774,19 @@ class HandControlGUI(QWidget):
def run_next_action(self):
"""运行下一个预设动作"""
if not self.preset_actions:
if not self.hand_config.preset_actions:
return
# 重置所有按钮颜色
self.reset_preset_buttons_color()
# 计算下一个动作索引
self.current_action_index = (self.current_action_index + 1) % len(self.preset_actions)
self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
# 获取下一个动作
action_names = list(self.preset_actions.keys())
action_names = list(self.hand_config.preset_actions.keys())
action_name = action_names[self.current_action_index]
action_positions = self.preset_actions[action_name]
action_positions = self.hand_config.preset_actions[action_name]
# 执行动作
self.on_preset_action_clicked(action_positions)
@@ -950,7 +811,6 @@ class HandControlGUI(QWidget):
"""关节类型改变事件处理"""
self.hand_joint = joint_type
self.hand_config = _HAND_CONFIGS[self.hand_joint]
self.preset_actions = self.hand_config.get_preset_actions(self.hand_type)
# 更新手部信息
info_text = f"""手部类型: {self.hand_type}
@@ -969,11 +829,8 @@ class HandControlGUI(QWidget):
def publish_joint_state(self):
"""发布当前关节状态"""
if self.hand_joint == "O12" and not self.command_dirty:
return
positions = [slider.value() for slider in self.sliders]
self.ros_manager.publish_joint_state(positions)
self.command_dirty = False
def update_status(self, status_type: str, message: str):
"""更新状态显示"""
src/gui_control/launch/o12_right_gui.launch.py
-42
View File
@@ -1,42 +0,0 @@
#!/usr/bin/env python3
"""Start the HCAN-connected O12 right hand and its safe position GUI."""
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
o12_driver = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("omnihand_node"),
"launch",
"omnihand_pro_2025_node.launch.py",
])
)
)
o12_gui = Node(
package="gui_control",
executable="gui_control",
name="o12_right_gui_control",
output="screen",
parameters=[{
"hand_type": "right",
"hand_joint": "O12",
# Commands are coalesced, so dragging a slider sends at most 10 Hz.
"topic_hz": 10,
"is_arc": False,
"topic_prefix": "",
}],
)
return LaunchDescription([
o12_driver,
# Give the CANFD driver a moment to claim and initialise the adapter.
TimerAction(period=1.5, actions=[o12_gui]),
])
src/gui_control/package.xml
-1
View File
@@ -13,7 +13,6 @@
<test_depend>python3-pytest</test_depend>
<exec_depend>linker_hand_ros2_sdk</exec_depend>
<exec_depend>omnihand_node</exec_depend>
<export>
<build_type>ament_python</build_type>
src/gui_control/test/test_l6_config.py
-23
View File
@@ -1,23 +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",
]
def test_l6_ok_preset_is_hand_specific() -> None:
config = HAND_CONFIGS["L6"]
assert config.get_preset_actions("left")["OK"] == [
62, 5, 151, 255, 255, 255,
]
assert config.get_preset_actions("right")["OK"] == [
58, 6, 153, 255, 255, 255,
]
src/gui_control/test/test_o12_config.py
-46
View File
@@ -1,46 +0,0 @@
from gui_control.config.constants import HAND_CONFIGS
def test_o12_gui_uses_active_angle_order_and_milliradians() -> None:
config = HAND_CONFIGS["O12"]
assert config.joint_names_en == [
"thumb_roll", "thumb_abad", "thumb_mcp", "thumb_pip",
"index_abad", "index_mcp", "index_pip",
"middle_abad", "middle_mcp", "middle_pip",
"ring_mcp", "pinky_mcp",
]
assert config.position_scale == 1000
assert config.position_unit == "rad"
assert len(config.init_pos) == 12
assert all(len(pose) == 12 for pose in config.preset_actions.values())
def test_o12_right_fingertip_preset_actions_use_requested_radians() -> None:
config = HAND_CONFIGS["O12"]
actions = config.get_preset_actions("right")
assert actions["拇指对食指"] == [
245, -766, -818, 0, 0, 1046, 0, 0, 0, 0, 0, 0,
]
assert actions["拇指对中指"] == [
489, -843, -685, 0, 0, 0, 0, 0, 1147, 0, 0, 0,
]
assert actions["拇指对无名指"] == [
699, -779, -827, 0, 0, 0, 0, 0, 0, 0, 690, 0,
]
assert actions["拇指对小指"] == [
786, -1065, -827, 0, 0, 0, 0, 0, 0, 0, 0, 645,
]
assert all(len(actions[name]) == 12 for name in (
"拇指对食指", "拇指对中指", "拇指对无名指", "拇指对小指",
))
def test_o12_fingertip_preset_actions_are_right_hand_only() -> None:
actions = HAND_CONFIGS["O12"].get_preset_actions("left")
assert "拇指对食指" not in actions
assert "拇指对中指" not in actions
assert "拇指对无名指" not in actions
assert "拇指对小指" not in actions
src/gui_control/test/test_o6_config.py
-18
View File
@@ -1,18 +0,0 @@
from gui_control.config.constants import HAND_CONFIGS
def test_o6_ok_preset_is_hand_specific() -> None:
config = HAND_CONFIGS["O6"]
assert config.get_preset_actions("left")["OK"] == [
95, 75, 116, 255, 255, 255,
]
assert config.get_preset_actions("right")["OK"] == [
95, 83, 122, 255, 255, 255,
]
assert config.get_preset_actions("left")["拇指对中指"] == [
88, 2, 255, 114, 255, 255,
]
assert config.get_preset_actions("right")["拇指对中指"] == [
95, 8, 255, 114, 255, 255,
]
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-l30-sdk
-1
Submodule src/linkerhand-l30-sdk deleted from 0103fc55f9
src/linkerhand_calibration/CALIBRATION_CODE_REVIEW_20260910.md
-169
View File
@@ -1,169 +0,0 @@
# 统一标定需求代码审查（2026-09-10）
## 结论与审查范围
审查当前工作区（包含未提交的统一重构、精简 JSON v2 和本会话启动参数修复），
不是只审查 Git HEAD，也没有把历史 PASS 当作当前实机验收。
通用采集、拟合、URDF 修正、最终文件验收和发布架构已经存在，但仍有迁移残留和精度验证缺口。
本轮修正可由代码及隔离测试确认的问题，没有改变原始 CAD、机械限位、SDK、贴 Tag 配置或相机外参。
没有启动 SDK、相机或发送机械手运动指令。
## 已修复的问题
| 问题 | 影响 | 修改 |
| --- | --- | --- |
| `vendor_sdk_config:=` 等空 launch 实参 | O6/L6/G20 可能在节点启动前退出 | runner 省略空参数，使用 ROS CLI 的实际解析器覆盖四型号 |
| 在线 IO 使用固定 front/side/top 外参加载器 | 新 Profile 即使静态校验通过，在线仍可能不能使用其他机位 | 直接用通用外参加载器，视图和参考机位来自 Profile |
| launch 固定三机位、型号文件名回退和过期参数 | 新型号需修改启动代码；部分开关没有实际作用 | 按 Profile 创建机位；要求受保护 Profile/原始 URDF/Tag 路径；移除旧诊断、速度、scope 和输出目录覆盖参数 |
| MVS 无图像时只显示“等待设备” | 操作者无法区分 SDK、相机或检测链路故障 | 统一显示每机位有效内参/检测消息等待项、SDK 条件和内参不匹配原因 |
| READY 后设备断流仍保留可开始状态 | 启动请求可能使用已失效的就绪条件 | 开始前可撤销 READY；Start 再次检查完整设备条件 |
| 实时 CameraInfo 只比较 P 的部分字段 | K、D、R 改变但 P 未变时，整流图可能与原外参不一致 | 实时校验宽高及 K/D/R/P 指纹；开始后不匹配按原有坐标变化故障停止继续采集 |
| 每个稳态采样点清空整方向统计 | 显示有效帧和覆盖率归零，误导进度判断 | 分离方向数据与当前稳态点数据，重扫/换方向才重置方向统计 |
| 标题只显示序列号 | 序列号不含型号时无法确认当前型号 | 所有型号统一显示 model、side、serial_number |
| 正式 finalizer 仍允许旧格式绕过指令拟合 | 新型号配置旧版本可能没有所需的 SDK 指令→rad 产物 | 正式 Profile 与 finalizer 均只接受统一输出版本 2，取消旧输出分派 |
| 完成回调读取精简 JSON 已删除的 `quality` | 文件可能已经发布，但节点在结束时抛 KeyError | 删除没有消费者的 `final_quality` 赋值，增加无 quality 字段的完成回调回归 |
| 视觉暂停与控制定时器之间的状态竞争 | 定时器等待锁期间已暂停，取得锁后仍可能继续动作 | 在控制锁内再次检查终止状态 |
开始前等待相机消息的 2 秒窗口不用于扫描中的实时暂停；空检测数组算视觉链路存活。
短时 Tag 丢失、低检测率等仍按方向结束的数据质量策略处理，同速重扫一次后仍不足才暂停。
清理的在线残留包括无人读取的回调计数、命令频率缓存/方法、型号质量标签、旧 tracker reset hook，
以及每次反馈回调不必要构造的备用 Adapter。没有删除 SDK 协议接口的抽象方法。
五个过渡期写出器已移出生产包并保留原文件：
`calibration_output/code_review_retired_serializers_umhxlepj/`。
包括 `select.py`、`generic_v1.py`、`legacy_v4.py`、`legacy_v6.py`、`native_v7.py`。
历史读取器和历史诊断工具仍在；此归档不包含或改写用户的历史标定产物。
## 视频复核后的补充修复：共用姿态分支筛选
视频复核发现旧配置把 `pnp_reprojection_tie_px` 和姿态拒绝阈值均设为 `1.5 px`，
导致通过质量门槛的 IPPE 候选都参加时间连续性比较，旧镜像分支可能压过明确更好的图像拟合。
用现有候选姿态复现时，旧分支 `0.24 px` 会压过另一分支 `0.05 px`。
公共模块统一默认近似同误差阈值为 `0.03 px`；ROS 与离线采集继承该值。
五份旧 YAML 的重复覆盖已移除，四产品的 `calibration_config_sha256` 已同步更新。
新增校验拒绝非有限、负值或不小于图像拒绝阈值的分支容差，防止旧的 `1.5/1.5` 配置再次生效。
保持实际近似同误差时的连续性选择、原有倾角/图像质量限制和方向结束重扫策略。
因此前文“配置保持不变”仅描述首轮结构审查；这次补充修复修改了上述算法配置及其哈希。
覆盖五份配置和四型号默认采集的九个复现用例，在修复前全部失败，修复后全部通过。
第一组姿态、配置、采集与加载实际参数文件的四型号隔离 ROS host 检查共 51 项通过。
第二组公共采集/拟合与发布、O12 姿态解析、来源保护、四型号配置及启动检查共 141 项通过，
耗时 114.53 秒。两组为本次修改的定向回归，没有宣称本次又执行过全包测试。
本次 `colcon build --packages-select linkerhand_calibration --symlink-install` 通过；
安装环境读取四产品配置、校验受保护哈希及公共默认值均通过，`git diff --check` 通过。
这不代表所有平面双解已消除，也不替代下述相机时序和实机重复性验证。
## 仍不能由本轮软件审查保证的事项
### P1：真实采集时间与传输积压
`hikrobot_camera.py::_publish_frame` 使用主机取到帧时的 ROS 时间戳，未将设备曝光时间映射到统一时钟。
SDK 帧结构有设备时间戳和帧号，但设备时钟单位、偏移、漂移与主机时钟同步还没有在当前硬件验证。
`runtime/ros/io.py::_state_callback` 对无时间戳反馈也采用接收时间。
因此，USB/驱动缓存可能把旧图像与新反馈配对；50 ms 的消息时间戳检查和“只取最新帧”不能排除该问题。
本会话已经实测到三台相机共享 480 Mbps Hub 上行，且正面/上方取帧超时。
后续应在恢复 USB 3.x 链路后，核验设备时间戳、帧号、曝光与反馈时序，并用独立运动数据检查配对误差。
不能猜测设备时钟单位、直接给时间戳减一个常数，或放宽质量阈值作为修复。
### P1：重复性和实机/仿真验收尚未完成
前三轮训练、第四轮独立验证、冻结 Tag 安装和双文件重读校验已接入。
但同手多次独立重采、重新摆放和重新贴 Tag、不同保持姿态/多关节组合动作的实际验收仍缺证据。
L6/O6 部分关节由小指参数迁移，不属于所有关节独立实测。
发布报告仍应如实保留 `arbitrary_multiaxis_validated: false`。
外部仿真要加载本次修正 URDF，用同一 SDK 指令话题及配套 JSON 驱动；用实机反馈直接驱动仿真，
只能显示反馈对应姿态，不能证明指令映射正确。桥本身是消息转换器，不是独立精度测量工具。
### P2：数学模块和旧配置仍有维护负担
`core/fitting/spatial.py` 约 3,656 行，包含轴线拟合、基座位姿、零位优化、可观测性和验证，
其中 `solve_urdf_zero_offsets` 从约第 1,921 行开始，嵌套函数及共享局部状态较多。
它是当前生产算法，不是可直接删除的废代码。后续拆分应按这些数学职责进行，
用固定输入的参数、残差、失败原因和产物回归验证等价性，不应复制成各型号独立算法。
当前约 58 个兼容模块仍用于旧导入路径、历史回放/迁移工具和旧回归；不能仅因目录名称或静态引用少就删除。
几行的公开兼容导出不是第二套拟合算法。旧型号诊断、旧 URDF plan 和部分旧 YAML 参数仍需明确退休范围后继续收缩。
受保护的历史配置在本轮保持不变；旧 YAML 中一些已不被在线节点读取的参数，不应当作当前算法的生效开关。
原始 G20/O12 的 mimic/限位冲突和 CAD 保留策略仍需机械资料确认；当前通用 URDF 修正
主要处理零位、限位和线性 mimic，不估计连杆长度、轴位置、mesh 或惯量。
## 按需求核对
| 需求 | 当前判断 |
| --- | --- |
| 多型号共用拟合和 URDF 修正 | 已实现统一生产链；SDK 新协议仍需 Adapter 和 ROS 启动绑定 |
| 新型号只关心 SDK、贴 Tag、避让 | 基本成立，但必须声明关节绑定、可观测性、迁移/保留策略和可修改字段，不能从未知安装的单轴 Tag 自动猜出 CAD 零位 |
| 统一 SDK 指令→rad JSON 与修正 URDF | 正式仅生成统一 v2 指令表；u8 为 256 项，rad 为显式节点；被动表由标准 mimic 推导并验收，详细证据保留在报告 |
| 开始前可移动、开始后固定 | 每次 Start 丢弃预览参考，正式锁定；可见固定基准有漂移监测；遮挡对象是否移动不能实时保证 |
| 正确、可重复 | 有独立验证和来源保护，但仍受采集时序与未完成实机验收限制 |
| 少暂停且原因明确 | 统一策略保留；开始前明确缺失设备，不用短时视觉丢失实时打断扫描 |
| 统一进度且明确型号 | 已修正型号、设备等待原因及方向统计显示 |
| 同源指令实机/仿真验证 | 有统一转换桥；实际动态/组合动作对比尚未完成 |
## 本轮验证
- 审查中完整标定包回归：**544 通过、1 跳过，383.25 秒**。跳过项要求显式提供
`O12_REPLAY_RAW` / `O12_REPLAY_REFERENCE`，没有用合成数据替代历史实测。
- 随后补充的启动清理、单一正式输出及归档改动：启动/runner/正式 Profile 拟合/精简产物定向组
**74 通过，108.78 秒**，包含四型号与虚拟型号的实际公共拟合和发布路径。
- 最后完成回调、暂停竞争、ROS host、等待诊断、实时内参保护：**25 通过，0.98 秒**。
与上一组有重叠，不相加冒充一次全量结果；没有宣称最后所有变更又跑过一次全量。
- `colcon build --packages-select linkerhand_calibration --symlink-install` 通过。
安装入口读取四产品的受保护配置、ROS launch 实参解析均通过；167 个生产/兼容 Python 文件 AST 通过；
`git diff --check` 通过。
- 新 launch 测试实际构造四型号及改名机位的 launch actions，但不执行这些动作。
隔离 ROS 节点/标准加载器测试不启动 SDK 或相机，不代表实机验收。
## 数学核心与 ROS 组合重构的验收记录
本轮以包含公共 `0.03 px` 姿态分支修复的工作区为基线，只拆分职责、明确数据和并发边界。
没有删除基线中仍存在的历史入口，没有修改配置、拟合公式、优化初值/顺序、残差权重或验收阈值。
- `core/fitting/spatial.py` 保留 45 个原定义符号的显式导出；实际计算在 `spatial_solver/`。
`solve.py` 的入口顺序为输入准备、训练求解、统计、独立验证及结果装配。
数据通过 `ZeroProblem`、`TrainingProblem`、`TrainingFit` 和各验收结果类型传递。
训练接口只接收训练观测，训练几何中的掌部姿态按训练轮筛选。
- `core/geometry/pnp.py` 保留 14 个原定义符号；实现拆为 IPPE、单 Tag 跟踪、刚性组和轨迹选择。
跟踪默认值集中在 `tag_pose/parameters.py`，ROS 参数只负责加载和 deg/rad 转换。
- `UnifiedCalibrationNode` 直接继承 ROS `Node`，组合消息 IO 与 `CalibrationCoordinator`。
协调器不加载 ROS 库，复用 `SessionExecution`、采集、运动、固定基准和断点组件。
运行参数、观测输入和快照使用明确类型；阶段只有 `CalibrationSession.phase` 一个来源。
- `FinalizationController` 组合原 worker 与发布器，协调器接收阶段事件并授权提交。
状态锁先于 worker 锁；PnP 在状态锁外计算，提交时检查会话版本、采集/运动对象及稳态边界。
暂停、中止与提交互斥；失败不触发自动恢复运动，成功只提交一次。
SDK 绑定显式接收反馈新鲜度、时钟、健康订阅和发布接口。
固定输入记录在本次工作区快照 `/tmp/calibration_structure_baseline_30qf3ozo/`。
持久化的对比报告及压缩前后输出位于 `calibration_output/structure_refactor_20260910/`，
其中 `comparison.json` 记录输入摘要、差异、产物摘要和拒绝原因，`protected_inputs.json` 保存输入哈希。
这些记录是合成数据的结构等价证据，不是实机精度验收。
| 固定输入 | 比较浮点值数量 | 最大绝对差异 | JSON/拟合结果/阶段事件及 URDF 文本 |
| --- | ---: | ---: | --- |
| G20 | 46,046 | 0 | 一致 |
| L6 | 15,304 | 0 | 一致 |
| O6 | 15,304 | 0 | 一致 |
| O12 | 14,494 | 0 | 一致 |
| 虚拟型号（任务重排） | 15,304 | 0 | 一致 |
对五组输入分别移除第四轮或注入第四轮 Tag 姿态滑移，10 个拒绝原因和阶段事件与基线一致。
167 个配置、Profile、原始 URDF/资源文件的哈希不变。
另一个虚拟型号同时重排 SDK 通道、机位名称和扫描任务，经过真实公共采集、finalizer、
最终 JSON/URDF 验收和发布，再检查同一 SDK 指令经过模拟传输、查表和标准 mimic 图的一致性。
验证结果：
- 整个标定包：581 passed，1 skipped，337.43 s。
- SDK 绑定时钟补充后，协调器、四型号 ROS 构造和安全策略回归：27 passed。
- 跳过项是 `test_o12_recorded_replay.py`：未提供 `O12_REPLAY_RAW` 与独立参考模型；不计作实机通过。
- 改动模块通过 Pyflakes，`git diff --check` 通过。
- `colcon build --packages-select linkerhand_calibration --symlink-install` 成功。
- 安装后的 12 个 console entry 可导入；两个产品 CLI 的 `--help` 正常。
两个 ROS 节点入口均通过四型号受保护 Profile 的转交检查；安装环境下四型号 ROS 构造/Start 回归 4 passed。
本轮不处理相机采集时刻同步，也不替代实机多次标定和同话题实机/仿真运动对照。
src/linkerhand_calibration/README.md
-236
View File
@@ -1,236 +0,0 @@
# LinkerHand 多型号统一标定
G20、L6、O6、O12 使用同一个产品启动器、在线状态机、采集器、拟合器、标准 URDF
验收和发布器。型号差异来自 `config/profiles/*.yaml` 和 SDK Adapter，不再调用型号节点。
本包校准关节零位、有效行程及标准线性 mimic；保留 CAD 连杆尺寸、轴位置、mesh、惯量。
JSON 描述 SDK 输入与修正 URDF 角度的转换，不能补偿错误的 URDF。
软件仿真通过不等于实机精度通过；当前原始 G20/O12 存在需要确认的 mimic/限位冲突。
详见 [执行与验收记录](CALIBRATION_REFACTOR_PROGRESS.md)。
## 启动
```bash
cd /home/lxp/projects/linkerhand_retarget_ros2
source /opt/ros/jazzy/setup.bash
colcon build --packages-select linkerhand_calibration --symlink-install
source install/setup.bash
# 无运动、无 SDK 启动的配置检查
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/o12_right_product.yaml --validate-only
# 正式启动：自动启动 SDK、相机、检测及标定，READY 后自动开始运动
ros2 run linkerhand_calibration calibrate_hand --config \
src/linkerhand_calibration/config/o12_right_product.yaml
```
其他型号替换为 `g20_right_product.yaml`、`l6_right_product.yaml`、`o6_right_product.yaml`。
`calibrate_g20_right` 是同一 runner 的旧命令别名。不要同时运行 GUI、单独 SDK 或其他控制器。
`--commands-disabled` 只预览，不发送运动；`--no-resume` 强制新采集。Ctrl+C 中止，不自动快速张手。
重新开始时先安全回基准，仍必须确保现场没有障碍物。
| Profile | SDK 位置通道 | 扫描任务 | Tag | 输出格式 |
| --- | ---: | ---: | ---: | --- |
| G20/right/g20_right_19/v1 | 20 | 16 | 19 | unified v2，256 项指令查表 |
| L6/right/l6_right_8/v1 | 6 | 3 | 8 | unified v2，256 项指令查表 |
| O6/right/o6_right_8/v1 | 6 | 3 | 8 | unified v2，256 项指令查表 |
| O12/right/o12_right_16/v1 | 12 | 11 | 16 | unified v2，rad 输入节点查表 |
任务数、主动关节数和实测关节数不必相等。一条运动可观测多个主动/被动关节。
L6/O6 未贴 Tag 的同机构关节使用显式迁移，不能把迁移称为独立实测。
每任务四个往返周期：前三轮训练，第四轮独立验证。
每个方向先连续扫描，再单通道访问稳态点。训练每方向默认 9 点，第四轮为交错点加端点；
每点至少 3 个稳定同步图像，轨迹结束后最多等待 2 秒，不能用精确指令/反馈相等判断稳定。
稳态点缺失与连续扫描不足统一在方向结束处理，最多同速重扫一次。
字节拟合不再改写原始 command/feedback，也不将接近端点的读数伪装成 0/255。
## 基准、避让与恢复
- 开始前可以调整手掌、相机和 Tag；预览数据不会成为正式参考。
- 开始后清空预览缓存，先安全回基准，再为每机位固定 Tag 收集至少 10 个有效帧。
缺少基准时保持并显示缺少的 ID，不因为等待时间长暂停。
- 锁定后禁止人工移动手掌、相机、支架、Tag 粘贴位置；程序驱动关节和避让仍是允许的。
- 相机相对位置改变时须重新确认/标定外参；仅重新锁定掌心 Tag 不能替代外参标定。
- 固定 Tag 被避让遮住时可用已锁定参考继续采集；软件不能保证检测完全不可见对象的移动。
运动 Tag 的刚性和几何一致性仍须通过最终独立验证。
- 断点版本为 `unified_engine_v4_dual_mapping`，不复用缺少稳态证据的旧 v1/v2/v3。
新断点检查配置哈希、基准及所复用 Tag 安装关系（5 px、2°、5 mm）。缺失或不一致即放弃复用，
自动完整重采，旧会话不修改。恢复时重新执行准备与避让，不直接跳到旧姿态。
O12 避让：小指和无名指到 Profile 的最大弯曲指令；标定食指前，中指 MCP/PIP 到最大弯曲指令，
中指侧摆为 0 rad。到位采用完成轨迹、正确方向、至少 80% 请求位移和短时稳定，
不要求反馈数值精确等于命令。SDK 指令最大值与实测 URDF 角度不是同一个量。
O12 使用 HCAN device 0/channel 0 的已锁定 vendor Python wheel，不检查 `can0`。
只读反馈桥可在第一次运动命令之前取得状态，不会发送零位命令来“激活回读”。
POSITION 和活动故障由 SDK 确认；无法单独回读温度时使用 SDK 错误位过热保护，不伪造温度。
字节 SDK 当前没有独立硬件故障遥测，状态会明确注明这一能力边界。
## 哪些情况暂停
实时仅因人工中止、竞争控制器、活动硬件故障/模式错误、真实失联、反馈超过 1 秒未更新、
物理越限、明显运动要求下连续 2 秒无推进、已锁固定基准连续 10 帧漂移超过 5 px 而暂停。
开始后相机投影参数变化也会拒绝继续使用混合坐标数据。
短时 Tag 丢失、PnP/同步失败、低检测率/低反馈 Hz、正常跟随滞后、固有耦合和非目标小幅运动
不会实时中断轨迹。无效视觉帧直接丢弃。
所有型号的独立 Tag 姿态筛选共用 `core/geometry/pnp.py` 中的近似同误差阈值，默认 `0.03 px`。
ROS 和离线采集使用相同默认值，型号 YAML 不重复声明该值。`1.5 px` 是图像质量拒绝阈值，
不能用它判断两种姿态是否同样可信；仅在误差差值不超过近似同误差阈值时使用时间连续性。
当另一分支的图像拟合明确更好时允许纠正旧分支。覆盖参数必须有限、非负且小于图像质量拒绝阈值。
这项修复不保证解决所有平面 Tag 双解，真实角点噪声、采集时序和最终运动精度仍需实测验证。
方向结束才检查 ≥40 个有效同步样本、≥32 分箱、内部空白 ≤行程的 1/16、规定的有效行程覆盖，
以及 Profile 声明的端点/双视角观测。数据不足只同速重扫一次；第二次失败暂停。
拟合或最终文件验收失败不重新自动运动、不更新发布指针。
所有型号显示相同进度：阶段、任务/轮次/方向、分机位 Tag ID、命令/反馈/单位/速度、
有效数据、基准、断点来源，以及中文原因、建议和原始诊断。耗时拟合在后台执行，不阻塞反馈与状态。
标题明确显示型号、左右手和序列号。开始前等待状态列出缺少或过期的相机内参、检测消息和 SDK 条件；
空 Tag 检测消息可以证明视觉链路工作，不要求开始前已经识别全部 Tag。
收到开始请求时重新检查设备状态；相机有效内参必须与受保护外参中的指纹一致。
开始前相机消息的新鲜度窗口为 2 秒，该窗口不用于扫描中的实时暂停。
## 唯一生产代码链
```text
linkerhand_calibration/
├── config/profiles/ 唯一型号定义（受保护 YAML）
├── config/*_product.yaml 设备、序列号、相机、路径与哈希
├── urdf/ 原始 CAD/mesh，不覆盖
└── linkerhand_calibration/
├── profiles/ 加载、静态验证、Tag/CAD 观测关系
├── core/domain/ Profile、样本、CalibrationResult、状态
├── core/geometry/ 相机、变换、PnP 兼容导出
│ └── tag_pose/ IPPE、连续跟踪、刚性组、轨迹分支选择
├── core/fitting/ 运动曲线、线性耦合、固定安装
│ └── spatial_solver/ 空间观测、基座、零位、统计与独立验收
├── core/urdf/ 授权修正、FK、范围与最终文件验收
├── runtime/runner.py 唯一产品启动器
├── runtime/coordinator.py 会话协调、采集提交、运动和收尾接线
├── runtime/parameters.py 已解析的运行参数；无 ROS 类型
├── runtime/inputs.py 相机/检测输入、时钟与发布接口
├── runtime/cameras.py 受保护相机身份与消息新鲜度
├── runtime/session.py 唯一在线状态机
├── runtime/execution.py Profile 任务与运动效果调度
├── runtime/motion_execution.py 平滑轨迹与位移到位判定
├── runtime/capture.py 多机位视觉/反馈采集
├── runtime/scan_quality.py 唯一方向数据门
├── runtime/reference_lock.py 开始后固定基准
├── runtime/resume.py 唯一断点证据检查
├── runtime/safety.py 最小实时保护
├── runtime/status.py 统一状态与中文显示
├── runtime/snapshot.py 类型化状态快照与旧状态映射
├── runtime/adapters/ SDK 协议，不包含标定业务
├── runtime/ros/ ROS 参数加载、消息转换、订阅/服务/定时器
├── runtime/artifacts/ 收尾控制、后台 finalizer、serializer、原子发布
└── compat/ 历史格式、布局与离线诊断；不提供旧在线流程
```
`calibrate_hand → runner → runtime/ros/entrypoint → UnifiedCalibrationNode → CalibrationCoordinator → SessionExecution`
是所有型号的生产调用链。节点直接继承 ROS `Node`，组合 `RosCalibrationIO` 和协调器，
不通过父子类业务回调执行标定。SDK 绑定接收明确的反馈新鲜度、时钟、健康订阅和发布接口。
空间入口 `core/fitting/spatial.py` 保留显式兼容导出，计算顺序在
`spatial_solver/solve.py`：输入准备 → 仅训练求解 → 可观测性/轮间统计 → 冻结结果的独立验证 → 结果装配。
`TrainingProblem` 不包含第四轮观测，训练几何中的掌部姿态也按训练轮筛选。
基座策略由 Profile 的几何约束选择；曲线、零位坐标约定、权重、阈值和原始 URDF 授权修正规则不变。
PnP 的原导入路径继续可用。`tag_pose/parameters.py` 集中定义跟踪默认值，
ROS 加载器将角度参数由 deg 转为 rad，离线采集直接使用同一套默认值。
公共分支容差仍为 `0.03 px`，重投影质量、倾角、过滤原因和连续跟踪恢复策略保持一致。
`CalibrationSession.phase` 是流程状态来源。协调器在状态锁内开始、中止、推进和提交观测；
耗时姿态计算在锁外进行，提交时核对会话版本、采集对象、运动对象及稳态采样边界。
Start 更换预览采集对象并重新锁定基准，暂停/中止使尚未完成的旧观测失效。
收尾由 `FinalizationController` 组合 worker 和发布器，worker 只上报阶段事件、准备候选文件；
协调器核实全部验收阶段后才能提交。锁顺序固定为状态锁 → worker 锁，
拟合失败维持当前姿态，中止后禁止提交，成功产物只提交一次。
老的单相机拇指/CMC 在线节点和 launch 已退出安装入口；需要局部测量时应增加经过验证的 Profile，
不能重新启用旧状态机。老版本离线诊断不是新的标准 URDF 发布证据。
## 新增型号
必需文件：
```text
config/profiles/new_hand.yaml
config/new_hand_product.yaml
urdf/new_hand/raw.urdf
urdf/new_hand/meshes/*
```
Profile 声明：
1. Adapter、SDK 通道顺序、单位、正负方向、命令/反馈范围、基准值、独立速度槽布局。
2. Tag ID/尺寸/机位/安装 link，以及每次运动的父子观测关系；不明确的 link 不能靠名称猜。
3. 扫描任务、速度、准备/避让/收尾 waypoint；必要时指定不可见 Tag 的恢复验证姿态。
4. 哪些零位可观测、哪些固定保留 CAD、哪些参数同机构迁移；声明空间观测约束和拟合原语。
5. 每关节允许修正的 `origin.rpy`、`limit.lower/upper`、`mimic.multiplier/offset`。
这仍需要正确的可观测性设计：单轴加一对未知安装角的 Tag 不能自动辨识绝对 CAD 零位。
不能为了减少配置，默认把 SDK 零点或最大指令当成 CAD 零位。可参考最简单的 L6/O6 Profile，
删除不适用关系后重新声明；不要盲目复制另一型号的空间约束。
复用 SDK 不增加 Python；新协议只增加 `runtime/adapters/` 实现及协议工厂注册。
没有型号节点、runner、状态机、质量/断点/发布器；新数学只能增加通用拟合原语。
新型号使用 `artifacts.output_schema_version: 2` 选择统一指令查表格式；产物自身为
`format: unified_calibration_v2, schema_version: 2`。详细双映射保留在独立标定报告中。
旧 generic v1、v4/v6/v7 只保留历史兼容工具，不把反馈表改名为指令表来伪造兼容。
正式 Profile 校验及 finalizer 只接受 `output_schema_version: 2`，不能用旧格式跳过指令映射拟合。
配置错字、错误绑定、未知修正字段在启动前失败；修改受保护文件后应审查并更新对应 SHA256。
## 产物与离线验证
会话目录保存原始样本、冻结 Tag 安装、拟合诊断、标准 URDF 验收、JSON、URDF。
有效发布由 `release_manifest.json` 及产品发布指针共同证明；单独的 `PASS` 摘要或存在 URDF 文件不等于已发布。
manifest 记录 JSON、URDF、标定报告三文件 SHA256、受保护输入和独立 holdout 证据。任一验收失败都不会替换原发布指针。
`measurement_contract.json` 提前列出 Tag/link、零位观测关系、CAD 保留/迁移与原始 mimic 限位冲突。
主 JSON 以原始 URDF 关节名为键，每个关节仅包含 `sdk_channel` 和 `angle_rad`；
O12 等 rad 输入还包含一一对应的 `input_values`（在有效节点间线性插值，不外推）。
字节型号固定 256 项：SDK 指令 `v` 对应 `angle_rad[v]`，只能在训练指令实际覆盖 0–255 时导出。
输出均是修正 URDF 的 q_output，不再加零偏。顶层保留 `format/schema_version/profile_id/model/side/serial_number/input_unit`。
被动关节的查表由导出 URDF 的标准 mimic 递归生成，绑定同一驱动通道，必须与 URDF 完全一致。
单张查表取训练正反向指令曲线的平均值，再用实际导出表重放第四轮视觉数据；回差等因素导致超差时禁止发布，
不通过隐藏方向分支、外推、放宽精度或 JSON 非线性被动联动制造通过结果。
`calibration_report.json` 保存完整的 `command_to_rad` / `feedback_to_rad`、输入域、方向分支、
零位、来源、迁移和训练/holdout 指标。报告中的 `applicability` 记录保持姿态；
未采集多姿态证据不能宣称任意多轴工况已验证。主 JSON 用于指令查表，不能误作反馈转换。
旧 unified v1 会话继续只读兼容，不自动覆盖或转换历史产物；新输出单表必须重新通过最终文件验收。
统一消费桥可直接读取通过验证的 manifest（纯消息转换，不会发送实机命令）：
```bash
ros2 run linkerhand_calibration calibrated_joint_state_bridge --ros-args \
-p calibration_file:=<session/release_manifest.json> -p input_kind:=command \
-p input_topic:=<SDK原始指令话题> -p output_topic:=/calibration/verification/joint_states
```
需要反馈角度显示时显式选择 `input_kind:=feedback`，桥读取配套报告中的反馈映射。
桥检查三文件哈希、拒绝越域输入，
被动角由实际导出 URDF 计算，不加第二次零偏。仿真输出不能与实机反馈话题同名。
MuJoCo 是用户后续使用的外部验收项目，本轮不改动它、不增加 MuJoCo 依赖，自动标定不要求打开 GUI。
外部对比须确认加载的模型来自这份 URDF，指令转换使用配套 JSON；不能用实机反馈直接驱动仿真来证明指令映射。
```bash
# 同一算法离线回放新策略的完整采集；不启动硬件，默认不发布
ros2 run linkerhand_calibration calibrate_hand --config <product.yaml> \
--offline-raw <raw_samples.jsonl> --offline-output <新的输出目录>
# 只比较两份 URDF，不修改，不控制机械手
ros2 run linkerhand_calibration compare_calibration_urdfs \
--reference <参考.urdf> --candidate <本次.urdf> --output <新报告.json>
```
离线回放也检查原始会话策略、SDK 输入域、配置/相机证据、每方向完整数据及最终文件。
旧会话不能自动获得当前哈希或新版本认证。`--publish-offline` 必须显式给出，且不会绕过验证。
算法和坐标规则见 [CALIBRATION_FLOW.md](CALIBRATION_FLOW.md)，批准的原始方案保持在
[CALIBRATION_REFACTOR_PLAN.md](CALIBRATION_REFACTOR_PLAN.md)。
当前代码审查、已修复问题与未完成的精度验收见
[2026-09-10 需求审查](CALIBRATION_CODE_REVIEW_20260910.md)。相机图像目前采用主机取帧时间；
USB/驱动积压情况下，图像与反馈的时间戳接近不能证明曝光时刻同步，不能据此宣称实机精度通过。
src/linkerhand_calibration/config/g20_right_product.yaml
-40
View File
@@ -1,40 +0,0 @@
schema_version: 3
profile_id: G20/right/g20_right_19/v1
profile_config: package://linkerhand_calibration/config/profiles/g20_right_19.yaml
profile_config_sha256: 35cad4432a01b3e1af157f1e914c2054e71522a44c6786db2519e4c2a0966a46
model: G20
side: right
tag_layout: g20_right_19
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: 4927506d787d665c16f0209ee4d052654e648bb25beaa1913ed0d416f449d523
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
-39
View File
@@ -1,39 +0,0 @@
schema_version: 3
profile_id: L6/right/l6_right_8/v1
profile_config: package://linkerhand_calibration/config/profiles/l6_right_8.yaml
profile_config_sha256: b9654ce998b56c10d7094364e8b715663163d2101e1f70e077cb0723fc47f02e
model: L6
side: right
tag_layout: l6_right_8
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: 090d82a5609e8b981c1b8c6ebeaf4c2f477323209ae3db3b9d8713ac6d6169b5
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
-66
View File
@@ -1,66 +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_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: 1
non_target_motion_tolerance_u8: 3.0
fixed_base_maximum_corner_drift_px: 5.0
fixed_base_movement_confirmation_frames: 10
src/linkerhand_calibration/config/o12_right_16_tags.yaml
-41
View File
@@ -1,41 +0,0 @@
/o12_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, 3, 12, 13]
frames: [front_base, thumb_cmc, thumb_mcp, thumb_dip, middle_roll, index_roll]
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
/o12_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: [4, 5, 6, 7, 8, 9, 10, 11]
frames: [side_base, pinky_mcp, pinky_pip, pinky_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]
/o12_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: [14, 15]
frames: [top_base, thumb_yaw]
sizes: [0.016, 0.016]
src/linkerhand_calibration/config/o12_right_product.yaml
-47
View File
@@ -1,47 +0,0 @@
schema_version: 3
profile_id: O12/right/o12_right_16/v1
profile_config: package://linkerhand_calibration/config/profiles/o12_right_16.yaml
profile_config_sha256: 0b5cd27168bc863e5c1aed5382c08e33ab4c02a23359a325dc9925fd30599e5f
model: O12
side: right
tag_layout: o12_right_16
namespace: /o12_calibration
serial_number: O12_RIGHT_001
output_root: calibration_output
sdk:
driver: o12_sdk_bridge
python_package: src/agillink_omnihand_sdk/linux/x64/python/omnihand-1.1.8-cp312-cp312-linux_x86_64.whl
package_sha256: cae7a0d5bce7e7c9d72cc90a0dd15e152f11e2a1ecf04cb6761e8171399ff170
transport: hcan
setup: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/setup.bash
config: src/agillink_omnihand_sdk/linux/x64/ros2/jazzy/share/omnihand_node/config/omnihand_pro_2025_node.yaml
config_sha256: 1e3c0942b32128943fbba27846a69a8483af6da9551d99ed687e45c4321ade9c
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/o12_right/linkerhand_o12_t3_right-0703.urdf
source_urdf_sha256: 75b3c18992a3640d2d7d36719da5ff75428ded477b97d9cf29adbe43a6eb27eb
camera_extrinsics: config/o12_three_camera_extrinsics.yaml
camera_extrinsics_sha256: 5a515d0706f4e67d5e26bfddb348b519817bd72e885ea9e43997e41016176e53
calibration_config: package://linkerhand_calibration/config/o12_three_camera_calibration.yaml
calibration_config_sha256: c2fcdd532e22c15013413e655311de2ca87107c23a96d5c043e8f340f232eebd
tag_config: package://linkerhand_calibration/config/o12_right_16_tags.yaml
tag_config_sha256: 41001c3afba74cc01eb524a75dc58561a37e9364fab029ea12d879156a008dab
release:
required_independent_passes: 1
static_repeatability_deg: 1.0
src/linkerhand_calibration/config/o12_three_camera_calibration.yaml
-53
View File
@@ -1,53 +0,0 @@
o12_calibration:
ros__parameters:
command_topic: /o12/right/joint_cmd
state_topic: /o12/right/joint_states
front_camera_info_topic: /o12_calibration/front/camera/camera_info
front_detections_topic: /o12_calibration/front/apriltag/detections
side_camera_info_topic: /o12_calibration/side/camera/camera_info
side_detections_topic: /o12_calibration/side/apriltag/detections
top_camera_info_topic: /o12_calibration/top/camera/camera_info
top_detections_topic: /o12_calibration/top/apriltag/detections
# O12 standard interface is radians. Scan the complete reviewed SDK range;
# the source CAD limits are outputs to correct, not acquisition limits.
# Raw 0..2000 mixed control is forbidden.
command_rate_hz: 50.0
repetitions: 4
tag_size_m: 0.016
tag_size_override_ids: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
minimum_detection_rate: 0.95
minimum_joint_frame_rate: 0.85
minimum_feedback_hz: 35.0
maximum_state_image_skew_ms: 50.0
minimum_sweep_frames: 40
minimum_state_span_fraction: 0.90
minimum_sweep_bins: 32
maximum_bin_gap: 2
endpoint_tolerance_rad: 0.01
endpoint_hold_seconds: 0.5
motor_stall_timeout_seconds: 2.0
position_timeout_seconds: 60.0
sweep_timeout_seconds: 180.0
automatic_sweep_retry_limit: 1
non_target_motion_tolerance_rad: 0.015
maximum_temperature_c: 70
# 当前 O12 固件返回空温度报告。仍发起查询；5秒无结果后依赖已验证的
# joint_error_states bit1 过热保护，并在会话记录中明确标注降级。
temperature_report_required: false
temperature_fallback_after_seconds: 5.0
# 快速标定档：各任务按4倍请求，但拇指pitch、侧摆、屈曲及避让均有独立硬上限。
motion_speed_scale: 4.0
# 正弦速度加减速时间；中段保持关节限速，避免位置余弦全程低速。
trajectory_ramp_seconds: 0.4
maximum_hamming: 0
minimum_decision_margin: 30.0
minimum_edge_pixels: 30.0
fixed_base_maximum_corner_drift_px: 5.0
fixed_base_movement_confirmation_frames: 10
pnp_maximum_reprojection_error_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
src/linkerhand_calibration/config/o6_right_8_tags.yaml
-59
View File
@@ -1,59 +0,0 @@
/o6_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_ip]
sizes: [0.016, 0.016, 0.016]
/o6_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]
/o6_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_yaw]
sizes: [0.016, 0.016]
src/linkerhand_calibration/config/o6_right_product.yaml
-39
View File
@@ -1,39 +0,0 @@
schema_version: 3
profile_id: O6/right/o6_right_8/v1
profile_config: package://linkerhand_calibration/config/profiles/o6_right_8.yaml
profile_config_sha256: ab1d86cb0c3e514de5322a44c1444e99d9b1f1ce43f8a2451cd032ed9c31e4ff
model: O6
side: right
tag_layout: o6_right_8
namespace: /o6_calibration
serial_number: O6_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/o6_right/linkerhand_o6_right.urdf
source_urdf_sha256: 8f184faad699fbf771e388f109a4e8793b5cb190c33a87b2eba8491a3a37dd62
camera_extrinsics: config/o6_three_camera_extrinsics.yaml
camera_extrinsics_sha256: 29af61f7bf1bad6718cbbaa54b0536f0a471c83f5bb3554f264ab9d292e56ca4
calibration_config: package://linkerhand_calibration/config/o6_three_camera_calibration.yaml
calibration_config_sha256: 15adaec462d0895dc68d19760e8f333c62e57786d124f22cfc481007a3d976a4
tag_config: package://linkerhand_calibration/config/o6_right_8_tags.yaml
tag_config_sha256: 16abe7119b4764f86333dae8264247571d1e0bca45af959d558bef4fb5485f5e
release:
required_independent_passes: 1
static_repeatability_deg: 1.0
src/linkerhand_calibration/config/o6_three_camera_calibration.yaml
-63
View File
@@ -1,63 +0,0 @@
o6_calibration:
ros__parameters:
command_topic: /o6/cb_right_hand_control_cmd
state_topic: /o6/cb_right_hand_state
setting_topic: /o6/cb_hand_setting_cmd
front_camera_info_topic: /o6_calibration/front/camera/camera_info
front_detections_topic: /o6_calibration/front/apriltag/detections
side_camera_info_topic: /o6_calibration/side/camera/camera_info
side_detections_topic: /o6_calibration/side/apriltag/detections
top_camera_info_topic: /o6_calibration/top/camera/camera_info
top_detections_topic: /o6_calibration/top/apriltag/detections
baseline_command_u8: [255, 255, 255, 255, 255, 255]
# O6 has a different speed scale from L6. Motion is still bounded by the
# six-second cosine command trajectory; these values are firmware limits.
baseline_speed_u8: 80
preflight_speed_u8: 60
formal_speed_u8: 40
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
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_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: 2.2
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: 1
non_target_motion_tolerance_u8: 3.0
fixed_base_maximum_corner_drift_px: 5.0
fixed_base_movement_confirmation_frames: 10
src/linkerhand_calibration/config/profiles/g20_right_19.yaml
-1504
View File
File diff suppressed because it is too large Load Diff
src/linkerhand_calibration/config/profiles/l6_right_8.yaml
-372
View File
@@ -1,372 +0,0 @@
schema_version: 1
profile_id: L6/right/l6_right_8/v1
namespace: /l6_calibration
sdk_adapter: legacy_byte_sdk
command:
sdk_to_joint_direction: [-1, -1, -1, -1, -1, -1]
names:
- thumb_cmc_pitch
- thumb_cmc_roll
- index_mcp_pitch
- middle_mcp_pitch
- ring_mcp_pitch
- pinky_mcp_pitch
baseline_u8:
- 255
- 255
- 255
- 255
- 255
- 255
command_index_by_joint:
rh_thumb_cmc_pitch: 0
rh_thumb_cmc_roll: 1
rh_index_mcp_pitch: 2
rh_middle_mcp_pitch: 3
rh_ring_mcp_pitch: 4
rh_pinky_mcp_pitch: 5
disabled_indices: []
urdf_joint_by_joint:
rh_thumb_cmc_pitch: rh_thumb_cmc_pitch
rh_thumb_cmc_roll: rh_thumb_cmc_roll
rh_index_mcp_pitch: rh_index_mcp_pitch
rh_middle_mcp_pitch: rh_middle_mcp_pitch
rh_ring_mcp_pitch: rh_ring_mcp_pitch
rh_pinky_mcp_pitch: rh_pinky_mcp_pitch
feedback_name_aliases:
thumb_cmc_yaw: thumb_cmc_roll
speed_slot_by_command_index:
'0': 0
'1': 1
'2': 2
'3': 3
'4': 4
'5': 5
unit: u8
baseline: []
lower_bounds: []
upper_bounds: []
feedback_lower_bounds: []
feedback_upper_bounds: []
feedback_by_index: false
vision:
views:
- name: front
tags:
- role: front_base
fixed_reference: true
id: 0
- role: thumb_pitch
fixed_reference: false
id: 1
- role: thumb_dip
fixed_reference: false
id: 2
- name: side
tags:
- role: side_base
fixed_reference: true
id: 3
- role: pinky_pitch
fixed_reference: false
id: 4
- role: pinky_dip
fixed_reference: false
id: 5
- name: top
tags:
- role: top_base
fixed_reference: true
id: 6
- role: thumb_roll
fixed_reference: false
id: 7
common_frame: calibration_common
extrinsic_reference_view: front
extrinsics_quality_limits:
reprojection_rms_px: 1.2
maximum_rotation_repeatability_deg: 0.3
maximum_translation_repeatability_m: 0.0015
minimum_capture_counts:
front_side_captures: 15
front_top_captures: 15
motion:
tasks:
- key: thumb_roll_top
view: top
command_index: 1
joints:
- rh_thumb_cmc_roll
auxiliary_commands:
- - 0
- 255
validation_only: false
start_u8: 255
end_u8: 0
preflight_speed_u8: 1
formal_speed_u8: 1
start: null
end: null
preflight_speed: null
formal_speed: null
- key: thumb_pitch_dip_front
view: front
command_index: 0
joints:
- rh_thumb_cmc_pitch
- rh_thumb_dip
auxiliary_commands:
- - 1
- 255
validation_only: false
start_u8: 255
end_u8: 0
preflight_speed_u8: 1
formal_speed_u8: 1
start: null
end: null
preflight_speed: null
formal_speed: null
- key: pinky_pitch_dip_side
view: side
command_index: 5
joints:
- rh_pinky_mcp_pitch
- rh_pinky_dip
auxiliary_commands: []
validation_only: false
start_u8: 255
end_u8: 0
preflight_speed_u8: 1
formal_speed_u8: 1
start: null
end: null
preflight_speed: null
formal_speed: null
preparation_waypoints_u8: []
safe_return_waypoints_u8: []
speed_parameters:
preflight_u8: 1
formal_u8: 1
speed_settle_seconds: 0.2
command_trajectory_full_range_seconds: 6.0
torque_u8: 80
endpoint_hold_seconds: 1.0
stall_timeout_seconds: 2.0
precheck_sweeps: false
steady_command_checkpoints: false
measurement:
measurements:
rh_thumb_cmc_roll:
joint: rh_thumb_cmc_roll
kind: relative_rotation
view: top
parent_role: top_base
child_role: thumb_roll
validation_source: null
pose_axis_line_required: true
rh_thumb_cmc_pitch:
joint: rh_thumb_cmc_pitch
kind: relative_rotation
view: front
parent_role: front_base
child_role: thumb_pitch
validation_source: null
pose_axis_line_required: true
rh_thumb_dip:
joint: rh_thumb_dip
kind: relative_rotation
view: front
parent_role: thumb_pitch
child_role: thumb_dip
validation_source: null
pose_axis_line_required: true
rh_pinky_mcp_pitch:
joint: rh_pinky_mcp_pitch
kind: relative_rotation
view: side
parent_role: side_base
child_role: pinky_pitch
validation_source: null
pose_axis_line_required: true
rh_pinky_dip:
joint: rh_pinky_dip
kind: relative_rotation
view: side
parent_role: pinky_pitch
child_role: pinky_dip
validation_source: null
pose_axis_line_required: true
cross_view_sources: {}
image_curve_joints: []
directional_zero: true
cross_view_roll_curve: false
stable_cross_view_cone_bias: false
zero:
active_joints:
- rh_index_mcp_pitch
- rh_middle_mcp_pitch
- rh_pinky_mcp_pitch
- rh_ring_mcp_pitch
- rh_thumb_cmc_pitch
- rh_thumb_cmc_roll
passive_joints:
- rh_index_dip
- rh_middle_dip
- rh_pinky_dip
- rh_ring_dip
- rh_thumb_dip
direct_zero_joints:
- rh_pinky_mcp_pitch
- rh_thumb_cmc_pitch
- rh_thumb_cmc_roll
axis_joints:
- rh_pinky_dip
- rh_pinky_mcp_pitch
- rh_thumb_cmc_pitch
- rh_thumb_cmc_roll
- rh_thumb_dip
mechanical_endpoint_joints: []
post_solve_endpoint_joints: []
mimic_source_by_joint:
rh_thumb_dip: rh_thumb_cmc_pitch
rh_index_dip: rh_index_mcp_pitch
rh_middle_dip: rh_middle_mcp_pitch
rh_ring_dip: rh_ring_mcp_pitch
rh_pinky_dip: rh_pinky_mcp_pitch
cad_frozen_joints:
- rh_index_dip
- rh_middle_dip
- rh_pinky_dip
- rh_ring_dip
- rh_thumb_dip
endpoint_anchor_by_joint: {}
fitted_mimic_joints:
- rh_pinky_dip
- rh_thumb_dip
coupling_model_by_joint:
rh_thumb_dip: linear_mimic
rh_pinky_dip: linear_mimic
rh_index_dip: linear_mimic
rh_middle_dip: linear_mimic
rh_ring_dip: linear_mimic
transferred_zero_sources: {rh_index_mcp_pitch: rh_pinky_mcp_pitch, rh_middle_mcp_pitch: rh_pinky_mcp_pitch, rh_ring_mcp_pitch: rh_pinky_mcp_pitch}
transferred_mimic_sources: {rh_index_dip: rh_pinky_dip, rh_middle_dip: rh_pinky_dip, rh_ring_dip: rh_pinky_dip}
spatial:
base_pose_strategy: thumb_serial
root_anchor_joints: [rh_thumb_cmc_roll]
orientation_anchor_joint: rh_pinky_mcp_pitch
directed_base_axis_joints: [rh_thumb_cmc_roll, rh_pinky_mcp_pitch]
depth_free_axis_projection: true
axis_order: [rh_thumb_cmc_roll, rh_thumb_cmc_pitch, rh_thumb_dip, rh_pinky_mcp_pitch, rh_pinky_dip]
axis_parent_joint: {rh_thumb_cmc_pitch: rh_thumb_cmc_roll}
phase_parent_joint: {rh_thumb_dip: rh_thumb_cmc_pitch, rh_pinky_dip: rh_pinky_mcp_pitch}
offset_observer_joint: {rh_thumb_cmc_roll: rh_thumb_cmc_pitch, rh_thumb_cmc_pitch: rh_thumb_dip, rh_pinky_mcp_pitch: rh_pinky_dip}
quality:
training_cycles:
- 0
- 1
- 2
holdout_cycle: 3
hard_threshold_keys:
- maximum_mimic_residual_rad
- maximum_state_image_skew_ms
- maximum_validation_error_rad
- minimum_detection_rate
retry_metric_scope: {}
isolated_holdout: true
scope:
calibrate_joints:
partial:
- rh_pinky_mcp_pitch
- rh_thumb_cmc_pitch
- rh_thumb_cmc_roll
frozen_joints:
partial:
- rh_index_mcp_pitch
- rh_middle_mcp_pitch
- rh_ring_mcp_pitch
default_scope: partial
artifacts:
output_schema_version: 2
calibration_filename: l6_right_{serial_number}_partial_calibration.json
corrected_urdf_filename: linkerhand_l6_right_{serial_number}_partial_zero_calibrated.urdf
protected_input_fields:
- calibration_config_sha256
- camera_extrinsics_sha256
- profile_config_sha256
- source_urdf_sha256
- tag_config_sha256
publication_pointer: latest_partial_passed
session_compatibility_tokens:
- feedback_curves_v6
- l6_partial_v1
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v4_dual_mapping
mapping_probe_maximum_rad: 0.0
automatic_rescan_limit: 1
minimum_valid_samples: 40
minimum_bins: 32
maximum_unobserved_fraction: 0.0625
legacy_minimum_span_01: 0.9411764705882353
physical_first_cycle_minimum_span_01: 0.85
physical_repeat_minimum_fraction: 0.9
stall_timeout_seconds: 2.0
feedback_stale_seconds: 1.0
fixed_reference_minimum_frames: 10
fixed_reference_maximum_drift_px: 5.0
fixed_reference_confirmation_frames: 10
urdf:
authorized_fields:
rh_pinky_mcp_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_thumb_cmc_roll:
- limit.lower
- limit.upper
- origin.rpy
rh_index_mcp_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_thumb_cmc_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_middle_mcp_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_ring_mcp_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_ring_dip:
- mimic.multiplier
- mimic.offset
rh_thumb_dip:
- mimic.multiplier
- mimic.offset
rh_index_dip:
- mimic.multiplier
- mimic.offset
rh_pinky_dip:
- mimic.multiplier
- mimic.offset
rh_middle_dip:
- mimic.multiplier
- mimic.offset
joint_coverage:
rh_pinky_mcp_pitch: measured_static_dynamic
rh_thumb_cmc_roll: measured_static_dynamic
rh_thumb_cmc_pitch: measured_static_dynamic
rh_middle_mcp_pitch: transferred_static_dynamic
rh_index_mcp_pitch: transferred_static_dynamic
rh_ring_mcp_pitch: transferred_static_dynamic
rh_pinky_dip: measured_dynamic_cad_static
rh_middle_dip: transferred_dynamic_cad_static
rh_ring_dip: transferred_dynamic_cad_static
rh_thumb_dip: measured_dynamic_cad_static
rh_index_dip: transferred_dynamic_cad_static
src/linkerhand_calibration/config/profiles/o12_right_16.yaml
-926
View File
@@ -1,926 +0,0 @@
schema_version: 1
profile_id: O12/right/o12_right_16/v1
namespace: /o12_calibration
sdk_adapter: o12_hcan_sdk
command:
unit: rad
feedback_by_index: true
sdk_to_joint_direction:
- 1
- -1
- -1
- -1
- -1
- 1
- 1
- -1
- 1
- 1
- 1
- 1
names:
- thumb_roll
- thumb_abad
- thumb_mcp
- thumb_pip
- index_abad
- index_mcp
- index_pip
- middle_abad
- middle_mcp
- middle_pip
- ring_mcp
- pinky_mcp
baseline_u8: []
baseline:
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
- 0
lower_bounds:
- 0
- -1.387536755335492
- -0.8272860654453121
- -1.2915436464758039
- -0.2617993877991494
- 0
- 0
- -0.2617993877991494
- 0
- 0
- 0
- 0
upper_bounds:
- 0.9424777960769379
- 0
- 0
- 0
- 0.2617993877991494
- 1.3526301702956054
- 1.530653753999027
- 0.2617993877991494
- 1.3578661580515883
- 1.8151424220741028
- 1.53588974175501
- 1.53588974175501
feedback_lower_bounds:
- -0.03490658503988659
- -1.4224433403753787
- -0.8621926504851987
- -1.3264502315156905
- -0.296705972839036
- -0.03490658503988659
- -0.03490658503988659
- -0.296705972839036
- -0.03490658503988659
- -0.03490658503988659
- -0.03490658503988659
- -0.03490658503988659
feedback_upper_bounds:
- 0.9773843811168246
- 0.03490658503988659
- 0.03490658503988659
- 0.03490658503988659
- 0.296705972839036
- 1.387536755335492
- 1.5655603390389137
- 0.296705972839036
- 1.392772743091475
- 1.8500490071139892
- 1.5707963267948966
- 1.5707963267948966
command_index_by_joint:
thumb_cmc_roll: 0
thumb_cmc_yaw: 1
thumb_cmc_pitch: 2
thumb_mcp: 3
index_mcp_roll: 4
index_mcp_pitch: 5
index_pip: 6
middle_mcp_roll: 7
middle_mcp_pitch: 8
middle_pip: 9
ring_mcp_pitch: 10
pinky_mcp_pitch: 11
urdf_joint_by_joint:
thumb_cmc_roll: thumb_cmc_roll
thumb_cmc_yaw: thumb_cmc_yaw
thumb_cmc_pitch: thumb_cmc_pitch
thumb_mcp: thumb_mcp
index_mcp_roll: index_mcp_roll
index_mcp_pitch: index_mcp_pitch
index_pip: index_pip
middle_mcp_roll: middle_mcp_roll
middle_mcp_pitch: middle_mcp_pitch
middle_pip: middle_pip
ring_mcp_pitch: ring_mcp_pitch
pinky_mcp_pitch: pinky_mcp_pitch
maximum_velocity:
- 0.16
- 0.16
- 0.1
- 0.32
- 0.12
- 0.32
- 0.32
- 0.12
- 0.32
- 0.32
- 0.32
- 0.32
vision:
common_frame: calibration_common
extrinsic_reference_view: front
extrinsics_quality_limits:
reprojection_rms_px: 2.0
maximum_rotation_repeatability_deg: 0.3
maximum_translation_repeatability_m: 0.0015
minimum_capture_counts:
front_side_captures: 15
front_top_captures: 15
views:
- name: front
tags:
- role: front_base
id: 0
fixed_reference: true
- role: thumb_cmc
id: 1
- role: thumb_mcp
id: 2
- role: thumb_dip
id: 3
- role: middle_roll
id: 12
- role: index_roll
id: 13
- name: side
tags:
- role: side_base
id: 4
fixed_reference: true
- role: pinky_mcp
id: 5
- role: pinky_pip
id: 6
- role: pinky_dip
id: 7
- role: middle_pip
id: 8
- role: middle_dip
id: 9
- role: index_pip
id: 10
- role: index_dip
id: 11
- name: top
tags:
- role: top_base
id: 14
fixed_reference: true
- role: thumb_yaw
id: 15
motion:
speed_parameters:
command_rate_hz: 50
clearance_flex_rad_s: 0.1
clearance_splay_rad_s: 0.04
probe_travel_rad: 0.05235987755982989
probe_speed_rad_s: 0.02
endpoint_hold_seconds: 1.0
stall_timeout_seconds: 2.0
baseline_rad_s: 0.2
trajectory_ramp_seconds: 0.4
tasks:
- key: thumb_pitch_front
view: front
command_index: 2
joints:
- thumb_cmc_pitch
start: 0
end: -0.8272860654453121
preflight_speed: 0.06
formal_speed: 0.1
preparation_groups:
- - 0
- 1
- 3
- - 4
- 7
- - 5
- 6
- 8
- 9
- - 10
- 11
- - 2
- key: thumb_roll_front
view: front
command_index: 0
joints:
- thumb_cmc_roll
start: 0
end: 0.9424777960769379
preflight_speed: 0.06
formal_speed: 0.16
preparation_groups:
- - 1
- 2
- 3
- - 4
- 7
- - 5
- 6
- 8
- 9
- - 10
- 11
- - 0
- key: thumb_mcp_dip_front
view: front
command_index: 3
joints:
- thumb_mcp
- thumb_dip
start: 0
end: -1.2915436464758039
preflight_speed: 0.06
formal_speed: 0.32
preparation_groups:
- - 0
- 1
- 2
- - 4
- 7
- - 5
- 6
- 8
- 9
- - 10
- 11
- - 3
- key: thumb_yaw_top
view: top
command_index: 1
joints:
- thumb_cmc_yaw
start: 0
end: -1.387536755335492
preflight_speed: 0.06
formal_speed: 0.16
preparation_groups:
- - 0
- 2
- 3
- - 4
- 7
- - 5
- 6
- 8
- 9
- - 10
- 11
- - 1
- key: pinky_chain_side
view: side
command_index: 11
joints:
- pinky_mcp_pitch
- pinky_pip
- pinky_dip
start: 0
end: 1.53588974175501
preflight_speed: 0.06
formal_speed: 0.32
preparation_groups:
- - 0
- 1
- 2
- 3
- - 4
- 7
- - 5
- 6
- 8
- 9
- - 10
- - 11
- key: middle_roll_front
view: front
command_index: 7
joints:
- middle_mcp_roll
auxiliary_commands:
- - 4
- -0.17453292519943295
- - 5
- 0.11
- - 10
- 1.53588974175501
- - 11
- 1.53588974175501
- - 8
- 0.17
start: 0.2617993877991494
end: -0.2617993877991494
preflight_speed: 0.06
formal_speed: 0.12
preparation_groups:
- - 10
- 11
- - 5
- 6
- - 4
- - 0
- 1
- 2
- 3
- - 8
- 9
- - 7
- key: middle_mcp_side
view: side
command_index: 8
joints:
- middle_mcp_pitch
auxiliary_commands:
- - 4
- -0.17453292519943295
- - 5
- 0.11
- - 10
- 1.53588974175501
- - 11
- 1.53588974175501
start: 0
end: 1.3578661580515883
preflight_speed: 0.06
formal_speed: 0.32
preparation_groups:
- - 10
- 11
- - 5
- 6
- - 4
- - 0
- 1
- 2
- 3
- - 7
- 9
- - 8
- key: middle_pip_dip_side
view: side
command_index: 9
joints:
- middle_pip
- middle_dip
auxiliary_commands:
- - 4
- -0.17453292519943295
- - 5
- 0.11
- - 10
- 1.53588974175501
- - 11
- 1.53588974175501
start: 0
end: 1.8151424220741028
preflight_speed: 0.06
formal_speed: 0.32
preparation_groups:
- - 10
- 11
- - 5
- 6
- - 4
- - 0
- 1
- 2
- 3
- - 7
- 8
- - 9
- key: index_roll_front
view: front
command_index: 4
joints:
- index_mcp_roll
auxiliary_commands:
- - 7
- 0
- - 8
- 1.3578661580515883
- - 9
- 1.8151424220741028
- - 10
- 1.53588974175501
- - 11
- 1.53588974175501
- - 5
- 0.17
start: 0.2617993877991494
end: -0.2617993877991494
preflight_speed: 0.06
formal_speed: 0.12
entry_waypoints:
- - - 10
- 1.53588974175501
- - 11
- 1.53588974175501
- - - 4
- 0
- - 7
- 0
- - - 8
- 1.3578661580515883
- - 9
- 1.8151424220741028
preparation_groups:
- - 10
- 11
- - 7
- - 8
- 9
- - 0
- 1
- 2
- 3
- - 5
- 6
- - 4
- key: index_mcp_side
view: side
command_index: 5
joints:
- index_mcp_pitch
auxiliary_commands:
- - 7
- 0
- - 8
- 1.3578661580515883
- - 9
- 1.8151424220741028
- - 10
- 1.53588974175501
- - 11
- 1.53588974175501
start: 0
end: 1.3526301702956054
preflight_speed: 0.06
formal_speed: 0.32
entry_waypoints:
- - - 10
- 1.53588974175501
- - 11
- 1.53588974175501
- - - 4
- 0
- - 7
- 0
- - - 8
- 1.3578661580515883
- - 9
- 1.8151424220741028
preparation_groups:
- - 10
- 11
- - 7
- - 8
- 9
- - 0
- 1
- 2
- 3
- - 4
- 6
- - 5
- key: index_pip_dip_side
view: side
command_index: 6
joints:
- index_pip
- index_dip
auxiliary_commands:
- - 7
- 0
- - 8
- 1.3578661580515883
- - 9
- 1.8151424220741028
- - 10
- 1.53588974175501
- - 11
- 1.53588974175501
start: 0
end: 1.530653753999027
preflight_speed: 0.06
formal_speed: 0.32
entry_waypoints:
- - - 10
- 1.53588974175501
- - 11
- 1.53588974175501
- - - 4
- 0
- - 7
- 0
- - - 8
- 1.3578661580515883
- - 9
- 1.8151424220741028
preparation_groups:
- - 10
- 11
- - 7
- - 8
- 9
- - 0
- 1
- 2
- 3
- - 4
- 5
- - 6
return_groups:
- - 4
- 7
- - 8
- 9
- - 10
- 11
- - 0
- 1
- 2
- 3
- 5
- 6
measurement:
candidate_selection_tasks:
- thumb_mcp_dip_front
directional_zero: true
stable_cross_view_cone_bias: true
cross_view_sources:
middle_mcp_roll: middle_mcp_pitch
index_mcp_roll: index_mcp_pitch
measurements:
thumb_cmc_pitch:
kind: relative_rotation
view: front
parent_role: front_base
child_role: thumb_cmc
thumb_cmc_roll:
kind: relative_rotation
view: front
parent_role: front_base
child_role: thumb_cmc
thumb_mcp:
kind: relative_rotation
view: front
parent_role: thumb_cmc
child_role: thumb_mcp
thumb_dip:
kind: relative_rotation
view: front
parent_role: thumb_mcp
child_role: thumb_dip
pose_axis_line_required: false
thumb_cmc_yaw:
kind: relative_rotation
view: top
parent_role: top_base
child_role: thumb_yaw
pinky_mcp_pitch:
kind: relative_rotation
view: side
parent_role: side_base
child_role: pinky_mcp
pinky_pip:
kind: relative_rotation
view: side
parent_role: pinky_mcp
child_role: pinky_pip
pose_axis_line_required: false
pinky_dip:
kind: relative_rotation
view: side
parent_role: pinky_pip
child_role: pinky_dip
pose_axis_line_required: false
middle_mcp_roll:
kind: relative_rotation
view: front
parent_role: front_base
child_role: middle_roll
middle_mcp_pitch:
kind: relative_rotation
view: side
parent_role: side_base
child_role: middle_pip
middle_pip:
kind: relative_rotation
view: side
parent_role: side_base
child_role: middle_pip
middle_dip:
kind: relative_rotation
view: side
parent_role: middle_pip
child_role: middle_dip
pose_axis_line_required: false
index_mcp_roll:
kind: relative_rotation
view: front
parent_role: front_base
child_role: index_roll
index_mcp_pitch:
kind: relative_rotation
view: side
parent_role: side_base
child_role: index_pip
index_pip:
kind: relative_rotation
view: side
parent_role: side_base
child_role: index_pip
index_dip:
kind: relative_rotation
view: side
parent_role: index_pip
child_role: index_dip
pose_axis_line_required: false
zero:
active_joints:
- thumb_cmc_roll
- thumb_cmc_yaw
- thumb_cmc_pitch
- thumb_mcp
- index_mcp_roll
- index_mcp_pitch
- index_pip
- middle_mcp_roll
- middle_mcp_pitch
- middle_pip
- ring_mcp_pitch
- pinky_mcp_pitch
passive_joints:
- thumb_dip
- index_dip
- middle_dip
- ring_pip
- ring_dip
- pinky_pip
- pinky_dip
direct_zero_joints:
- thumb_cmc_roll
- thumb_cmc_yaw
- thumb_cmc_pitch
- index_mcp_roll
- index_mcp_pitch
- index_pip
- middle_mcp_roll
- middle_mcp_pitch
- middle_pip
- pinky_mcp_pitch
axis_joints:
- thumb_cmc_roll
- thumb_cmc_yaw
- thumb_cmc_pitch
- thumb_mcp
- index_mcp_roll
- index_mcp_pitch
- index_pip
- middle_mcp_roll
- middle_mcp_pitch
- middle_pip
- pinky_mcp_pitch
- index_dip
- middle_dip
- pinky_pip
mechanical_endpoint_joints: []
post_solve_endpoint_joints: []
mimic_source_by_joint:
thumb_dip: thumb_mcp
index_dip: index_pip
middle_dip: middle_pip
ring_pip: ring_mcp_pitch
ring_dip: ring_pip
pinky_pip: pinky_mcp_pitch
pinky_dip: pinky_pip
cad_frozen_joints:
- thumb_mcp
- thumb_dip
- index_dip
- middle_dip
- ring_pip
- ring_dip
- pinky_pip
- pinky_dip
fitted_mimic_joints:
- thumb_dip
- index_dip
- middle_dip
- pinky_pip
- pinky_dip
coupling_model_by_joint:
thumb_dip: linear_mimic
index_dip: linear_mimic
middle_dip: linear_mimic
pinky_pip: linear_mimic
pinky_dip: linear_mimic
transferred_zero_sources:
ring_mcp_pitch: pinky_mcp_pitch
spatial:
base_pose_strategy: thumb_serial
root_anchor_joints:
- thumb_cmc_roll
orientation_anchor_joint: pinky_mcp_pitch
directed_base_axis_joints:
- thumb_cmc_roll
- pinky_mcp_pitch
depth_free_axis_projection: true
accept_validated_zero_in_confidence_interval: true
project_axis_gauge_before_image: true
axis_order:
- thumb_cmc_roll
- thumb_cmc_yaw
- thumb_cmc_pitch
- thumb_mcp
- pinky_mcp_pitch
- pinky_pip
- middle_mcp_roll
- middle_mcp_pitch
- middle_pip
- middle_dip
- index_mcp_roll
- index_mcp_pitch
- index_pip
- index_dip
axis_parent_joint:
thumb_cmc_yaw: thumb_cmc_roll
thumb_cmc_pitch: thumb_cmc_yaw
index_mcp_pitch: index_mcp_roll
middle_mcp_pitch: middle_mcp_roll
phase_parent_joint:
thumb_mcp: thumb_cmc_pitch
index_pip: index_mcp_pitch
index_dip: index_pip
middle_pip: middle_mcp_pitch
middle_dip: middle_pip
pinky_pip: pinky_mcp_pitch
offset_observer_joint:
thumb_cmc_roll: thumb_cmc_yaw
thumb_cmc_yaw: thumb_cmc_pitch
thumb_cmc_pitch: thumb_mcp
index_mcp_roll: index_mcp_pitch
index_mcp_pitch: index_pip
index_pip: index_dip
middle_mcp_roll: middle_mcp_pitch
middle_mcp_pitch: middle_pip
middle_pip: middle_dip
pinky_mcp_pitch: pinky_pip
quality:
training_cycles:
- 0
- 1
- 2
holdout_cycle: 3
hard_threshold_keys:
- holdout
isolated_holdout: true
scope:
default_scope: full
calibrate_joints:
full:
- thumb_cmc_roll
- thumb_cmc_yaw
- thumb_cmc_pitch
- thumb_mcp
- index_mcp_roll
- index_mcp_pitch
- index_pip
- middle_mcp_roll
- middle_mcp_pitch
- middle_pip
- ring_mcp_pitch
- pinky_mcp_pitch
frozen_joints:
full: []
artifacts:
output_schema_version: 2
calibration_filename: o12_right_{serial_number}_calibration.json
corrected_urdf_filename: linkerhand_o12_right_{serial_number}_zero_calibrated.urdf
protected_input_fields:
- source_urdf_sha256
- camera_extrinsics_sha256
- calibration_config_sha256
- tag_config_sha256
- sdk_config_sha256
- sdk_package_sha256
- profile_config_sha256
publication_pointer: latest_passed
session_compatibility_tokens:
- o12_right_16_v1
- feedback_rad_v1
- full_sdk_range_v2
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v4_dual_mapping
mapping_probe_maximum_rad: 0.05235987755982989
urdf:
authorized_fields:
thumb_cmc_roll:
- origin.rpy
- limit.lower
- limit.upper
thumb_cmc_yaw:
- origin.rpy
- limit.lower
- limit.upper
thumb_cmc_pitch:
- origin.rpy
- limit.lower
- limit.upper
thumb_mcp:
- origin.rpy
- limit.lower
- limit.upper
index_mcp_roll:
- origin.rpy
- limit.lower
- limit.upper
index_mcp_pitch:
- origin.rpy
- limit.lower
- limit.upper
index_pip:
- origin.rpy
- limit.lower
- limit.upper
middle_mcp_roll:
- origin.rpy
- limit.lower
- limit.upper
middle_mcp_pitch:
- origin.rpy
- limit.lower
- limit.upper
middle_pip:
- origin.rpy
- limit.lower
- limit.upper
ring_mcp_pitch:
- origin.rpy
- limit.lower
- limit.upper
pinky_mcp_pitch:
- origin.rpy
- limit.lower
- limit.upper
thumb_dip:
- mimic.multiplier
- mimic.offset
index_dip:
- mimic.multiplier
- mimic.offset
middle_dip:
- mimic.multiplier
- mimic.offset
pinky_pip:
- mimic.multiplier
- mimic.offset
pinky_dip:
- mimic.multiplier
- mimic.offset
ring_pip:
- mimic.offset
ring_dip:
- mimic.offset
joint_coverage:
thumb_cmc_roll: measured_static_dynamic
thumb_cmc_yaw: measured_static_dynamic
thumb_cmc_pitch: measured_static_dynamic
thumb_mcp: measured_dynamic_cad_static
thumb_dip: measured_dynamic_cad_static
index_mcp_roll: measured_static_dynamic
index_mcp_pitch: measured_static_dynamic
index_pip: measured_static_dynamic
index_dip: measured_dynamic_cad_static
middle_mcp_roll: measured_static_dynamic
middle_mcp_pitch: measured_static_dynamic
middle_pip: measured_static_dynamic
middle_dip: measured_dynamic_cad_static
ring_mcp_pitch: transferred_static_dynamic
ring_pip: mimic_nominal
ring_dip: mimic_nominal
pinky_mcp_pitch: measured_static_dynamic
pinky_pip: measured_dynamic_cad_static
pinky_dip: measured_dynamic_cad_static
src/linkerhand_calibration/config/profiles/o6_right_8.yaml
-368
View File
@@ -1,368 +0,0 @@
schema_version: 1
profile_id: O6/right/o6_right_8/v1
namespace: /o6_calibration
sdk_adapter: legacy_byte_sdk
command:
sdk_to_joint_direction: [-1, -1, -1, -1, -1, -1]
names:
- thumb_cmc_pitch
- thumb_cmc_yaw
- index_mcp_pitch
- middle_mcp_pitch
- ring_mcp_pitch
- pinky_mcp_pitch
baseline_u8:
- 255
- 255
- 255
- 255
- 255
- 255
command_index_by_joint:
rh_thumb_cmc_pitch: 0
rh_thumb_cmc_yaw: 1
rh_index_mcp_pitch: 2
rh_middle_mcp_pitch: 3
rh_ring_mcp_pitch: 4
rh_pinky_mcp_pitch: 5
disabled_indices: []
urdf_joint_by_joint:
rh_thumb_cmc_pitch: rh_thumb_cmc_pitch
rh_thumb_cmc_yaw: rh_thumb_cmc_yaw
rh_index_mcp_pitch: rh_index_mcp_pitch
rh_middle_mcp_pitch: rh_middle_mcp_pitch
rh_ring_mcp_pitch: rh_ring_mcp_pitch
rh_pinky_mcp_pitch: rh_pinky_mcp_pitch
feedback_name_aliases: {}
speed_slot_by_command_index:
'0': 0
'1': 1
'2': 2
'3': 3
'4': 4
'5': 5
unit: u8
baseline: []
lower_bounds: []
upper_bounds: []
feedback_lower_bounds: []
feedback_upper_bounds: []
feedback_by_index: false
vision:
views:
- name: front
tags:
- role: front_base
fixed_reference: true
id: 0
- role: thumb_pitch
fixed_reference: false
id: 1
- role: thumb_ip
fixed_reference: false
id: 2
- name: side
tags:
- role: side_base
fixed_reference: true
id: 3
- role: pinky_pitch
fixed_reference: false
id: 4
- role: pinky_dip
fixed_reference: false
id: 5
- name: top
tags:
- role: top_base
fixed_reference: true
id: 6
- role: thumb_yaw
fixed_reference: false
id: 7
common_frame: calibration_common
extrinsic_reference_view: front
extrinsics_quality_limits:
reprojection_rms_px: 1.5
maximum_rotation_repeatability_deg: 0.3
maximum_translation_repeatability_m: 0.0015
minimum_capture_counts:
front_side_captures: 15
front_top_captures: 15
motion:
tasks:
- key: thumb_yaw_top
view: top
command_index: 1
joints:
- rh_thumb_cmc_yaw
auxiliary_commands: []
validation_only: false
start_u8: 255
end_u8: 0
preflight_speed_u8: 60
formal_speed_u8: 40
start: null
end: null
preflight_speed: null
formal_speed: null
- key: thumb_pitch_ip_front
view: front
command_index: 0
joints:
- rh_thumb_cmc_pitch
- rh_thumb_ip
auxiliary_commands: []
validation_only: false
start_u8: 255
end_u8: 0
preflight_speed_u8: 60
formal_speed_u8: 40
start: null
end: null
preflight_speed: null
formal_speed: null
- key: pinky_pitch_dip_side
view: side
command_index: 5
joints:
- rh_pinky_mcp_pitch
- rh_pinky_dip
auxiliary_commands: []
validation_only: false
start_u8: 255
end_u8: 0
preflight_speed_u8: 60
formal_speed_u8: 40
start: null
end: null
preflight_speed: null
formal_speed: null
preparation_waypoints_u8: []
safe_return_waypoints_u8: []
speed_parameters:
baseline_u8: 80
preflight_u8: 60
formal_u8: 40
speed_settle_seconds: 0.2
command_trajectory_full_range_seconds: 6.0
torque_u8: 80
endpoint_hold_seconds: 1.0
stall_timeout_seconds: 2.0
precheck_sweeps: false
steady_command_checkpoints: false
measurement:
measurements:
rh_thumb_cmc_yaw:
joint: rh_thumb_cmc_yaw
kind: relative_rotation
view: top
parent_role: top_base
child_role: thumb_yaw
validation_source: null
pose_axis_line_required: true
rh_thumb_cmc_pitch:
joint: rh_thumb_cmc_pitch
kind: relative_rotation
view: front
parent_role: front_base
child_role: thumb_pitch
validation_source: null
pose_axis_line_required: true
rh_thumb_ip:
joint: rh_thumb_ip
kind: relative_rotation
view: front
parent_role: thumb_pitch
child_role: thumb_ip
validation_source: null
pose_axis_line_required: true
rh_pinky_mcp_pitch:
joint: rh_pinky_mcp_pitch
kind: relative_rotation
view: side
parent_role: side_base
child_role: pinky_pitch
validation_source: null
pose_axis_line_required: true
rh_pinky_dip:
joint: rh_pinky_dip
kind: relative_rotation
view: side
parent_role: pinky_pitch
child_role: pinky_dip
validation_source: null
pose_axis_line_required: true
cross_view_sources: {}
image_curve_joints: []
directional_zero: true
cross_view_roll_curve: false
stable_cross_view_cone_bias: false
zero:
active_joints:
- rh_index_mcp_pitch
- rh_middle_mcp_pitch
- rh_pinky_mcp_pitch
- rh_ring_mcp_pitch
- rh_thumb_cmc_pitch
- rh_thumb_cmc_yaw
passive_joints:
- rh_index_dip
- rh_middle_dip
- rh_pinky_dip
- rh_ring_dip
- rh_thumb_ip
direct_zero_joints:
- rh_pinky_mcp_pitch
- rh_thumb_cmc_pitch
- rh_thumb_cmc_yaw
axis_joints:
- rh_pinky_dip
- rh_pinky_mcp_pitch
- rh_thumb_cmc_pitch
- rh_thumb_cmc_yaw
- rh_thumb_ip
mechanical_endpoint_joints: []
post_solve_endpoint_joints: []
mimic_source_by_joint:
rh_thumb_ip: rh_thumb_cmc_pitch
rh_index_dip: rh_index_mcp_pitch
rh_middle_dip: rh_middle_mcp_pitch
rh_ring_dip: rh_ring_mcp_pitch
rh_pinky_dip: rh_pinky_mcp_pitch
cad_frozen_joints:
- rh_index_dip
- rh_middle_dip
- rh_pinky_dip
- rh_ring_dip
- rh_thumb_ip
endpoint_anchor_by_joint: {}
fitted_mimic_joints:
- rh_pinky_dip
- rh_thumb_ip
coupling_model_by_joint:
rh_thumb_ip: linear_mimic
rh_index_dip: linear_mimic
rh_middle_dip: linear_mimic
rh_ring_dip: linear_mimic
rh_pinky_dip: linear_mimic
transferred_zero_sources: {rh_index_mcp_pitch: rh_pinky_mcp_pitch, rh_middle_mcp_pitch: rh_pinky_mcp_pitch, rh_ring_mcp_pitch: rh_pinky_mcp_pitch}
transferred_mimic_sources: {rh_index_dip: rh_pinky_dip, rh_middle_dip: rh_pinky_dip, rh_ring_dip: rh_pinky_dip}
spatial:
base_pose_strategy: thumb_serial
root_anchor_joints: [rh_thumb_cmc_yaw]
orientation_anchor_joint: rh_pinky_mcp_pitch
directed_base_axis_joints: [rh_thumb_cmc_yaw, rh_pinky_mcp_pitch]
depth_free_axis_projection: true
axis_order: [rh_thumb_cmc_yaw, rh_thumb_cmc_pitch, rh_thumb_ip, rh_pinky_mcp_pitch, rh_pinky_dip]
axis_parent_joint: {rh_thumb_cmc_pitch: rh_thumb_cmc_yaw}
phase_parent_joint: {rh_thumb_ip: rh_thumb_cmc_pitch, rh_pinky_dip: rh_pinky_mcp_pitch}
offset_observer_joint: {rh_thumb_cmc_yaw: rh_thumb_cmc_pitch, rh_thumb_cmc_pitch: rh_thumb_ip, rh_pinky_mcp_pitch: rh_pinky_dip}
quality:
training_cycles:
- 0
- 1
- 2
holdout_cycle: 3
hard_threshold_keys:
- maximum_mimic_residual_rad
- maximum_state_image_skew_ms
- maximum_validation_error_rad
- minimum_detection_rate
retry_metric_scope: {}
isolated_holdout: true
scope:
calibrate_joints:
partial:
- rh_pinky_mcp_pitch
- rh_thumb_cmc_pitch
- rh_thumb_cmc_yaw
frozen_joints:
partial:
- rh_index_mcp_pitch
- rh_middle_mcp_pitch
- rh_ring_mcp_pitch
default_scope: partial
artifacts:
output_schema_version: 2
calibration_filename: o6_right_{serial_number}_partial_calibration.json
corrected_urdf_filename: linkerhand_o6_right_{serial_number}_partial_zero_calibrated.urdf
protected_input_fields:
- calibration_config_sha256
- camera_extrinsics_sha256
- profile_config_sha256
- source_urdf_sha256
- tag_config_sha256
publication_pointer: latest_partial_passed
session_compatibility_tokens:
- feedback_curves_v6
- o6_partial_v1
publish_corrected_urdf: true
acquisition:
policy_version: unified_engine_v4_dual_mapping
mapping_probe_maximum_rad: 0.0
automatic_rescan_limit: 1
minimum_valid_samples: 40
minimum_bins: 32
maximum_unobserved_fraction: 0.0625
legacy_minimum_span_01: 0.9411764705882353
physical_first_cycle_minimum_span_01: 0.85
physical_repeat_minimum_fraction: 0.9
stall_timeout_seconds: 2.0
feedback_stale_seconds: 1.0
fixed_reference_minimum_frames: 10
fixed_reference_maximum_drift_px: 5.0
fixed_reference_confirmation_frames: 10
urdf:
authorized_fields:
rh_pinky_mcp_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_index_mcp_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_thumb_cmc_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_middle_mcp_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_ring_mcp_pitch:
- limit.lower
- limit.upper
- origin.rpy
rh_thumb_cmc_yaw:
- limit.lower
- limit.upper
- origin.rpy
rh_ring_dip:
- mimic.multiplier
- mimic.offset
rh_index_dip:
- mimic.multiplier
- mimic.offset
rh_pinky_dip:
- mimic.multiplier
- mimic.offset
rh_thumb_ip:
- mimic.multiplier
- mimic.offset
rh_middle_dip:
- mimic.multiplier
- mimic.offset
joint_coverage:
rh_pinky_mcp_pitch: measured_static_dynamic
rh_thumb_cmc_pitch: measured_static_dynamic
rh_middle_mcp_pitch: transferred_static_dynamic
rh_index_mcp_pitch: transferred_static_dynamic
rh_ring_mcp_pitch: transferred_static_dynamic
rh_thumb_cmc_yaw: measured_static_dynamic
rh_pinky_dip: measured_dynamic_cad_static
rh_thumb_ip: measured_dynamic_cad_static
rh_middle_dip: transferred_dynamic_cad_static
rh_ring_dip: transferred_dynamic_cad_static
rh_index_dip: transferred_dynamic_cad_static
src/linkerhand_calibration/config/schemas/hand_profile.schema.yaml
-44
View File
@@ -1,44 +0,0 @@
$schema: https://json-schema.org/draft/2020-12/schema
title: LinkerHand calibration profile
type: object
additionalProperties: false
required:
- schema_version
- profile_id
- namespace
- sdk_adapter
- command
- vision
- motion
- measurement
- zero
- quality
- scope
- artifacts
- urdf
properties:
schema_version: {const: 1}
profile_id: {type: string, pattern: "^[A-Za-z0-9_-]+/(left|right)/[a-z0-9_-]+/v[1-9][0-9]*$"}
namespace: {type: string, pattern: "^/[^/].*[^/]$"}
sdk_adapter: {type: string, minLength: 1}
command: {type: object}
vision: {type: object}
motion: {type: object}
measurement: {type: object}
zero: {type: object}
quality: {type: object}
scope: {type: object}
artifacts: {type: object}
acquisition: {type: object}
urdf:
type: object
required: [authorized_fields]
properties:
authorized_fields:
type: object
additionalProperties:
type: array
uniqueItems: true
items:
enum: [origin.rpy, limit.lower, limit.upper, mimic.multiplier, mimic.offset]
joint_coverage: {type: object}
src/linkerhand_calibration/config/schemas/product.schema.yaml
-14
View File
@@ -1,14 +0,0 @@
$schema: https://json-schema.org/draft/2020-12/schema
title: LinkerHand calibration product
type: object
required: [schema_version, profile_id, serial_number, cameras, artifacts]
properties:
schema_version: {type: integer, minimum: 2}
profile_id: {type: string}
profile_config: {type: string}
serial_number: {type: string, pattern: "^[A-Za-z0-9_.-]+$"}
namespace: {type: string}
sdk: {type: object}
cameras: {type: object, minProperties: 1}
artifacts: {type: object}
release: {type: object}
src/linkerhand_calibration/config/three_camera_calibration.yaml
-191
View File
@@ -1,191 +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_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: 5.0
fixed_base_movement_confirmation_frames: 10
# 仅在拇指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
# unified_engine_v2 不执行每任务全行程预检；保留参数仅兼容旧配置读取。
task_precheck_hold_seconds: 2.0
position_timeout_seconds: 30.0
sweep_timeout_seconds: 90.0
# 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
# 这类硬故障不自动重试。
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: 1
# 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
automatic_fit_retry_limit: 0
automatic_motion_retry_limit: 0
# 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll，
# 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。
cross_view_roll_diagnostic_finger: ""
# 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
provisional_warning_ratio: 1.25
retry_minimum_speed: 3
retry_speed_scales: [1.0]
retry_endpoint_hold_seconds: [0.5]
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/launch/unified_calibration.launch.py
-18
View File
@@ -1,18 +0,0 @@
"""Stable launch name for the profile-driven calibration stack."""
from __future__ import annotations
import importlib.util
from pathlib import Path
def generate_launch_description():
implementation = Path(__file__).with_name("three_camera_calibration.launch.py")
spec = importlib.util.spec_from_file_location(
"linkerhand_unified_calibration_launch", implementation
)
if spec is None or spec.loader is None:
raise RuntimeError("unified calibration launch implementation is missing")
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
return module.generate_launch_description()
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"]

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