Compare commits
3 Commits
4107da4c22
...
o30
| Author | SHA1 | Date | |
|---|---|---|---|
 admin
|
57babb966b | ||
|
admin
|
a609d521a0 | ||
|
admin
|
41ff4a61a9 |
@@ -62,6 +62,7 @@ Thumbs.db
|
||||
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
|
||||
/profiles/
|
||||
/calibration_output/
|
||||
/config/*_three_camera_extrinsics.yaml
|
||||
*.wear_check.json
|
||||
*.checkpoint.json
|
||||
*.verification.json
|
||||
@@ -69,7 +70,12 @@ Thumbs.db
|
||||
|
||||
# Device-specific robot descriptions derived from local calibration runs
|
||||
/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
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_cmc_pitch_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_calibrated_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_zero_calibrated_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_G20_RIGHT_tag.urdf
|
||||
|
||||
# ROS bag / MCAP recordings and CAN captures
|
||||
rosbag2_*/
|
||||
|
||||
@@ -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；
|
||||
- [ ] 仿真关节话题只有一个发布者；
|
||||
- [ ] 使用典型张开、握拳和拇指—食指捏合姿态与实机复核。
|
||||
|
||||
@@ -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`。
|
||||
@@ -1,9 +1,14 @@
|
||||
# G20 左手 AprilTag 标定
|
||||
# G20 左右手与 O30 右手 AprilTag 标定
|
||||
|
||||
## 三机位全手一键标定
|
||||
O30 右手使用相同的三相机/11-Tag 几何采集框架，但采用独立的 20 电机
|
||||
profile、8 项扫描任务和主动关节零位策略。完整映射、固定基准命令和启动方法见
|
||||
[O30 右手操作说明](../../docs/O30右手三相机标定与URDF修正操作说明.md)。
|
||||
|
||||
正式全手入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，
|
||||
但只有 `/g20_calibration` 一个节点拥有机械手命令发布权。默认机位绑定为：
|
||||
## 三机位三维关节轴零位标定（schema v4）
|
||||
|
||||
正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有
|
||||
`/g20_calibration` 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要
|
||||
贴正；相机和Tag在一次标定中必须固定。默认绑定为：
|
||||
|
||||
```text
|
||||
front = DB2163742，Tag 0/1/2/3/10
|
||||
@@ -16,22 +21,22 @@ top = DB2163739，Tag 8/9
|
||||
| ID | 机位 | 固定位置/运动件 |
|
||||
|---:|---|---|
|
||||
| 0 | 正面 | 正面掌壳固定基准 |
|
||||
| 1 | 正面 | 拇指 CMC 后连杆 |
|
||||
| 1 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 |
|
||||
| 2 | 正面 | 拇指 MCP 后连杆 |
|
||||
| 3 | 正面 | 拇指 IP 后末节 |
|
||||
| 4 | 侧面 | 掌壳侧面固定基准（最底下） |
|
||||
| 5 | 侧面 | 食指 MCP 后连杆 |
|
||||
| 6 | 侧面 | 食指 PIP 后连杆 |
|
||||
| 7 | 侧面 | 食指 DIP 后末节 |
|
||||
| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
|
||||
| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
|
||||
| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
|
||||
| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 |
|
||||
| 9 | 上面 | 拇指 CMC yaw 运动件 |
|
||||
| 10 | 正面 | 食指根部侧摆运动件（index_mcp_roll） |
|
||||
| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
|
||||
|
||||
ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见，
|
||||
ID 9 必须在拇指横摆的完整行程中持续可见。
|
||||
贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。
|
||||
|
||||
每台相机必须有独立的内参文件：
|
||||
每台相机必须有独立内参文件：
|
||||
|
||||
```text
|
||||
~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
@@ -39,16 +44,64 @@ ID 9 必须在拇指横摆的完整行程中持续可见。
|
||||
~/.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 \
|
||||
@@ -59,39 +112,26 @@ ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_calibration/top/camera/image_rect
|
||||
```
|
||||
|
||||
安装相机时可以按机位启动红/蓝线对准辅助节点。红线是画面理想水平线，
|
||||
蓝线是在画面下部检测到的桌边、底座边或临时刚性直尺；两线夹角不超过
|
||||
`±0.5°` 且上下构图偏差不超过 `±12 px` 时显示 `ALIGNED`。Tag只画绿色
|
||||
识别框，其角点方向完全不参与红/蓝线角度计算，因此Tag无需为了相机对准而贴正。
|
||||
|
||||
下面以正面机位为例，先启动辅助节点：
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration camera_alignment_view \
|
||||
--ros-args -p view:=front
|
||||
```
|
||||
|
||||
再打开它发布的叠加画面：
|
||||
|
||||
```bash
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_camera_alignment_view/image
|
||||
```
|
||||
|
||||
侧面和上面分别把 `view:=front` 改为 `view:=side`、`view:=top`。建议一次只开
|
||||
一个机位完成调整；侧面或上面没有合适长边时，临时放置与目标机械轴平行的刚性
|
||||
直尺。调整完成后退出辅助节点和
|
||||
`image_view`，再进行正式标定，以免额外的200万像素图像订阅影响采集帧率。
|
||||
这组红/蓝线只检查图像平面滚转角，不检查相机距离、俯仰、偏航，也不会阻止
|
||||
`/g20_calibration/start`。
|
||||
|
||||
确认所有目标关节的 `0～255` 行程安全、MVS 客户端已关闭且没有其他命令发布者后，
|
||||
重新启动正式流程：
|
||||
确认全行程安全、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
|
||||
```
|
||||
|
||||
@@ -110,64 +150,180 @@ ros2 service call /g20_calibration/start std_srvs/srv/Trigger {}
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
```
|
||||
|
||||
程序依次完成正面电机 `0/5/15/6`、侧面电机 `1/16`、上面电机 `10` 的三轮
|
||||
往返扫描。中指、无名指、小指复制食指模板。四指侧摆先以命令255
|
||||
为原始0角测出总行程，再减去总行程的一半；最终满足命令0为正、命令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，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。
|
||||
|
||||
每个直接测量任务完成 `3轮×2方向=6个扫描方向` 后，程序立即试拟合
|
||||
该电机对应的所有主动/被动关节。`thumb_cmc_pitch`、`thumb_cmc_roll`、
|
||||
`thumb_mcp`、`thumb_ip`、`index_mcp_roll`、`index_mcp_pitch` 和 `index_pip` 使用图像平面的
|
||||
连杆相对中心圆相位，避免小尺寸平面 Tag 的PnP深度双解把稳定的二维圆轨迹扭曲成
|
||||
错误三维轨迹。上面斜视的 `thumb_cmc_yaw` 和侧面的被动 `index_dip` 继续使用
|
||||
父Tag坐标系下的三维相对圆。`thumb_ip` 理论上也适合相对三维，但当前正面小Tag的
|
||||
PnP深度在三轮间不稳定，实测会让三维行程漂移，因此继续采用可重复的二维投影轨迹。
|
||||
程序分别检查二维圆残差/半径或三维平面RMS/圆残差/半径，并统一检查最小圆弧、
|
||||
单调修正量、正反程回差、三轮行程一致性和直接零位拟合。主动关节三轮行程最大差
|
||||
默认不超过3°；被动耦合关节允许不超过10°，但仍必须通过其余质量门限。
|
||||
任一指标失败时会立即暂停，中文状态显示关节名、实测值和阈值，不再等到42个方向
|
||||
全部结束。修正现场问题后调用 `resume`，
|
||||
程序只清除该电机任务的内存样本并重扫它的6个方向；前面已通过的关节保留。
|
||||
失败样本不从 `raw_samples.jsonl` 删除，而是使用 `attempt` 和 `retry` 记录区分，
|
||||
便于调试；最终拟合只使用当前通过尝试的内存数据。
|
||||
零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定
|
||||
掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免
|
||||
稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节
|
||||
进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游
|
||||
轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差
|
||||
超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴
|
||||
Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过
|
||||
1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°；
|
||||
小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。
|
||||
|
||||
标定食指 `index_mcp_roll`（电机6）及其随机复测时，为避免中指遮挡ID 10，
|
||||
程序将中指、无名指和小指的侧摆电机7/8/9固定为0；开始采样前会同时确认
|
||||
电机6到达扫描起点且电机7/8/9均已到达0。离开该标定项后恢复统一基准姿态。
|
||||
该项目还会通过SDK设置接口把五指速度临时设为 `[15,5,15,15,15]`，即只把
|
||||
食指速度从15降为5；离开该项目后恢复 `[15,15,15,15,15]`。
|
||||
侧面标定 `index_mcp_pitch`（电机1）和 `index_pip/index_dip`（电机16）时，
|
||||
五指速度设为 `[15,10,15,15,15]`，即食指屈伸使用第三档速度10；其余直接
|
||||
测量关节保持普通速度15。
|
||||
`thumb_mcp` 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过
|
||||
被动 `thumb_ip` 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴
|
||||
偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手
|
||||
`thumb_mcp` 都保留原始CAD零位0。该保护只冻结静态 `origin.rpy`，不会冻结或复制其
|
||||
`angle_rad[256]` 实测轨迹。
|
||||
|
||||
标定 `thumb_cmc_yaw`（电机10）及其随机复测时，程序将
|
||||
前两轮拟合，第三轮强制留出验证；轨迹与零位角度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稳定性。离开该标定项后，电机5恢复基准值255；
|
||||
最终JSON的 `baseline_command_u8` 不变。
|
||||
ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 `resume` 都保持
|
||||
电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才
|
||||
恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员
|
||||
暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的
|
||||
`baseline_command_u8` 不变。
|
||||
|
||||
三机位流程默认设置 `validation_enabled:=false`，因此拟合完成后会直接
|
||||
恢复基准姿态并生成JSON，不再进入 `VALIDATION_MOVE/VALIDATION_CAPTURE`。
|
||||
此时 `quality.passed` 只由轨迹与零位拟合质量决定，`validation_mae_rad` 和
|
||||
`validation_p95_rad` 为 `null`。需要恢复随机复测时，启动参数加
|
||||
`validation_enabled:=true`。
|
||||
右手标定小指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`、
|
||||
5个被动标记、模板来源和总体质量。相机、Tag、正反程及每轮质量只进入状态、日志和
|
||||
`raw_samples.jsonl`，不写入最终运行时 JSON。
|
||||
`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`。默认使用单终点连续模式：每个方向只发送一次
|
||||
@@ -205,7 +361,8 @@ PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维
|
||||
`T5` 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。
|
||||
- 当前实物使用 `tag36h11` 的 ID `0/1/2/3`，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改
|
||||
`config/front_tags.yaml` 里检测节点和标定节点的两组数组。
|
||||
- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前配置为 `0.010`。
|
||||
- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前实物黑色正方形实测为
|
||||
`16 mm`，因此配置为 `0.016`。
|
||||
测量检测角点所围成的正方形边长，不包含外围白色留边。
|
||||
- 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由
|
||||
静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时，
|
||||
@@ -244,9 +401,9 @@ source install/setup.bash
|
||||
顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。
|
||||
|
||||
`1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。
|
||||
相机节点和三个标定 launch 会自动加载 `config/fastdds_large_images.xml`，使用 64 MB
|
||||
共享内存段；否则相机内部虽为 30 Hz，大图订阅端通常只能收到约 1～4 Hz。修改配置后
|
||||
必须重启相关 ROS 进程才能生效。
|
||||
三相机标定 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；文件缺失时
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
# from back-pressuring image_proc's reliable image publisher.
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.01
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
@@ -20,11 +20,11 @@
|
||||
tag:
|
||||
ids: [0, 1, 2, 3]
|
||||
frames: [tag_t0, tag_t3, tag_t4, tag_t5]
|
||||
sizes: [0.010, 0.010, 0.010, 0.010]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
g20_thumb_calibration:
|
||||
ros__parameters:
|
||||
tag_roles: [t0, t3, t4, t5]
|
||||
tag_ids: [0, 1, 2, 3]
|
||||
tag_frames: [tag_t0, tag_t3, tag_t4, tag_t5]
|
||||
tag_sizes_m: [0.010, 0.010, 0.010, 0.010]
|
||||
tag_sizes_m: [0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
g20_calibration:
|
||||
/**:
|
||||
ros__parameters:
|
||||
command_topic: /g20/cb_left_hand_control_cmd
|
||||
state_topic: /g20/cb_left_hand_state
|
||||
@@ -16,9 +16,14 @@ g20_calibration:
|
||||
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
|
||||
|
||||
tag_size_m: 0.010
|
||||
# tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。
|
||||
tag_size_m: 0.016
|
||||
repetitions: 3
|
||||
preflight_frames: 60
|
||||
minimum_detection_rate: 0.95
|
||||
@@ -33,44 +38,95 @@ g20_calibration:
|
||||
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
|
||||
maximum_state_image_skew_ms: 150.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
|
||||
# 正面拇指pitch/MCP/IP使用二维圆相位，避免平面Tag的PnP深度歧义。
|
||||
# 以下二维参数只供旧轨迹工具兼容，三机位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
|
||||
zero_minimum_radius_px: 20.0
|
||||
zero_maximum_radial_rms_px: 2.0
|
||||
zero_maximum_radial_p95_px: 3.5
|
||||
zero_maximum_round_difference_deg: 1.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
|
||||
|
||||
# 默认跳过耗时的随机复测；需要验收精度时可在launch中设为true。
|
||||
# 默认无额外随机动作；第三轮扫描始终作为不可关闭的留出验证。
|
||||
validation_enabled: false
|
||||
validation_command_count: 3
|
||||
validation_frames: 10
|
||||
validation_seed: 20260804
|
||||
validation_timeout_seconds: 20.0
|
||||
maximum_validation_mae_deg: 2.0
|
||||
maximum_validation_p95_deg: 3.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.010
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
@@ -17,14 +17,14 @@
|
||||
tag:
|
||||
ids: [0, 1, 2, 3, 10]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, index_roll]
|
||||
sizes: [0.010, 0.010, 0.010, 0.010, 0.010]
|
||||
sizes: [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.010
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
@@ -38,14 +38,14 @@
|
||||
tag:
|
||||
ids: [4, 5, 6, 7]
|
||||
frames: [side_base, index_mcp, index_pip, index_dip]
|
||||
sizes: [0.010, 0.010, 0.010, 0.010]
|
||||
sizes: [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.010
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
@@ -59,4 +59,4 @@
|
||||
tag:
|
||||
ids: [8, 9]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.010, 0.010]
|
||||
sizes: [0.016, 0.016]
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
/o30_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]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/o30_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 7]
|
||||
frames: [side_base, pinky_mcp, pinky_pip, pinky_dip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/o30_calibration/top/apriltag/apriltag:
|
||||
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]
|
||||
+288
@@ -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()
|
||||
@@ -0,0 +1,227 @@
|
||||
"""Camera-extrinsic data model shared by calibration and runtime nodes."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
VIEWS: tuple[str, ...] = ("front", "side", "top")
|
||||
|
||||
|
||||
def camera_info_fingerprint(
|
||||
*,
|
||||
width: int,
|
||||
height: int,
|
||||
camera_matrix: Sequence[Sequence[float]] | Sequence[float],
|
||||
distortion: Sequence[float] = (),
|
||||
rectification: Sequence[float] = (),
|
||||
projection: Sequence[float] = (),
|
||||
) -> str:
|
||||
"""Return a stable fingerprint for rectified image geometry."""
|
||||
matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3)
|
||||
payload = {
|
||||
"width": int(width),
|
||||
"height": int(height),
|
||||
"camera_matrix": [round(float(value), 12) for value in matrix.flat],
|
||||
"distortion": [round(float(value), 12) for value in distortion],
|
||||
"rectification": [round(float(value), 12) for value in rectification],
|
||||
"projection": [round(float(value), 12) for value in projection],
|
||||
}
|
||||
encoded = json.dumps(
|
||||
payload, sort_keys=True, separators=(",", ":")
|
||||
).encode("utf-8")
|
||||
return hashlib.sha256(encoded).hexdigest()
|
||||
|
||||
|
||||
def transform_matrix(
|
||||
translation_xyz_m: Sequence[float],
|
||||
quaternion_xyzw: Sequence[float],
|
||||
) -> np.ndarray:
|
||||
translation = np.asarray(translation_xyz_m, dtype=float)
|
||||
quaternion = np.asarray(quaternion_xyzw, dtype=float)
|
||||
if translation.shape != (3,) or not np.all(np.isfinite(translation)):
|
||||
raise ValueError("translation_xyz_m must contain three finite values")
|
||||
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
|
||||
raise ValueError("quaternion_xyzw must contain four finite values")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if norm < 1.0e-12:
|
||||
raise ValueError("quaternion_xyzw has zero norm")
|
||||
result = np.eye(4, dtype=float)
|
||||
result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix()
|
||||
result[:3, 3] = translation
|
||||
return result
|
||||
|
||||
|
||||
def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]:
|
||||
value = np.asarray(matrix, dtype=float)
|
||||
if value.shape != (4, 4) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("transform must be a finite 4x4 matrix")
|
||||
return {
|
||||
"translation_xyz_m": [float(item) for item in value[:3, 3]],
|
||||
"quaternion_xyzw": [
|
||||
float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat()
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraCalibrationIdentity:
|
||||
serial_number: str
|
||||
width: int
|
||||
height: int
|
||||
intrinsics_sha256: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ThreeCameraExtrinsics:
|
||||
"""Transforms points from each camera optical frame into front optical."""
|
||||
|
||||
cameras: Mapping[str, CameraCalibrationIdentity]
|
||||
front_from_view: Mapping[str, np.ndarray]
|
||||
quality: Mapping[str, float]
|
||||
|
||||
def transform(self, view: str) -> np.ndarray:
|
||||
if view not in self.front_from_view:
|
||||
raise KeyError(f"extrinsics do not contain view {view}")
|
||||
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_extrinsics_payload(payload: Mapping[str, Any]) -> None:
|
||||
if int(payload.get("schema_version", -1)) != 1:
|
||||
raise ValueError("camera extrinsics schema_version must be 1")
|
||||
if payload.get("reference_view") != "front":
|
||||
raise ValueError("camera extrinsics reference_view must be front")
|
||||
cameras = payload.get("cameras")
|
||||
transforms = payload.get("front_from_view")
|
||||
quality = payload.get("quality")
|
||||
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")
|
||||
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 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")
|
||||
if not str(identity.get("serial_number", "")):
|
||||
raise ValueError(f"{view} camera serial_number is missing")
|
||||
if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0:
|
||||
raise ValueError(f"{view} camera image dimensions are invalid")
|
||||
fingerprint = str(identity.get("intrinsics_sha256", ""))
|
||||
if len(fingerprint) != 64:
|
||||
raise ValueError(f"{view} camera intrinsics fingerprint is invalid")
|
||||
transform = transforms[view]
|
||||
if not isinstance(transform, Mapping):
|
||||
raise ValueError(f"{view} transform must be an object")
|
||||
matrix = transform_matrix(
|
||||
transform.get("translation_xyz_m", ()),
|
||||
transform.get("quaternion_xyzw", ()),
|
||||
)
|
||||
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_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}")
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
payload = yaml.safe_load(stream)
|
||||
if not isinstance(payload, Mapping):
|
||||
raise ValueError("camera extrinsics file must contain an object")
|
||||
validate_extrinsics_payload(payload)
|
||||
cameras = {
|
||||
view: CameraCalibrationIdentity(
|
||||
serial_number=str(payload["cameras"][view]["serial_number"]),
|
||||
width=int(payload["cameras"][view]["width"]),
|
||||
height=int(payload["cameras"][view]["height"]),
|
||||
intrinsics_sha256=str(
|
||||
payload["cameras"][view]["intrinsics_sha256"]
|
||||
),
|
||||
)
|
||||
for view in VIEWS
|
||||
}
|
||||
transforms = {
|
||||
view: transform_matrix(
|
||||
payload["front_from_view"][view]["translation_xyz_m"],
|
||||
payload["front_from_view"][view]["quaternion_xyzw"],
|
||||
)
|
||||
for view in VIEWS
|
||||
}
|
||||
return ThreeCameraExtrinsics(
|
||||
cameras=cameras,
|
||||
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)
|
||||
+1569
File diff suppressed because it is too large
Load Diff
@@ -1,8 +1,8 @@
|
||||
"""Pure three-camera G20 calibration model and compact runtime schema.
|
||||
"""Pure three-camera hand calibration models and compact runtime schema.
|
||||
|
||||
The hardware node records one parent/child AprilTag trajectory for each
|
||||
directly observable joint. This module deliberately contains no ROS imports:
|
||||
curve fitting, four-finger splay centring, inheritance, and schema validation
|
||||
curve fitting, baseline centring, inheritance, and schema validation
|
||||
remain deterministic and unit-testable without connected cameras or a hand.
|
||||
"""
|
||||
|
||||
@@ -14,7 +14,6 @@ from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .core import BASELINE_COMMAND
|
||||
from .trajectory import (
|
||||
_angle_for_circle,
|
||||
_fit_circle_with_axis,
|
||||
@@ -31,6 +30,66 @@ from .zero_calibration import (
|
||||
)
|
||||
|
||||
|
||||
THREE_CAMERA_BASELINE_COMMAND: tuple[int, ...] = (
|
||||
255, 255, 255, 255, 255, 255,
|
||||
127, 127, 127, 127,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
|
||||
)
|
||||
|
||||
O30_RIGHT_BASELINE_COMMAND: tuple[int, ...] = (
|
||||
0, 0, 255, 205, 165, 20,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
)
|
||||
|
||||
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",
|
||||
)
|
||||
|
||||
# Names published by linker_hand_o30_ros2_sdk. The SDK ignores command
|
||||
# names, but using its names here lets state messages be safely reordered.
|
||||
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",
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointSpec:
|
||||
name: str
|
||||
@@ -41,6 +100,7 @@ class JointSpec:
|
||||
child_role: str | None
|
||||
source_joint: str | None = None
|
||||
zero_kind: str | None = None
|
||||
command_increasing: bool = False
|
||||
|
||||
@property
|
||||
def measured(self) -> bool:
|
||||
@@ -66,132 +126,375 @@ class JointCurveFit:
|
||||
zero_offset_rad: float = 0.0
|
||||
|
||||
|
||||
VIEW_TAGS: dict[str, dict[str, int]] = {
|
||||
"front": {
|
||||
"front_base": 0,
|
||||
"thumb_cmc": 1,
|
||||
"thumb_mcp": 2,
|
||||
"thumb_ip": 3,
|
||||
"index_roll": 10,
|
||||
},
|
||||
"side": {
|
||||
"side_base": 4,
|
||||
"index_mcp": 5,
|
||||
"index_pip": 6,
|
||||
"index_dip": 7,
|
||||
},
|
||||
"top": {
|
||||
"top_base": 8,
|
||||
"thumb_yaw": 9,
|
||||
},
|
||||
@dataclass(frozen=True)
|
||||
class HandCalibrationProfile:
|
||||
model: str
|
||||
side: str
|
||||
command_names: tuple[str, ...]
|
||||
baseline_command: tuple[int, ...]
|
||||
reference_finger: str
|
||||
view_tags: Mapping[str, Mapping[str, int]]
|
||||
preflight_view_roles: Mapping[str, tuple[str, ...]]
|
||||
joint_specs: Mapping[str, JointSpec]
|
||||
sweep_specs: tuple[SweepSpec, ...]
|
||||
image_trajectory_joints: frozenset[str]
|
||||
roll_clearance_commands: Mapping[int, int]
|
||||
thumb_pitch_clearance_commands: Mapping[int, int]
|
||||
|
||||
@property
|
||||
def measured_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if spec.measured
|
||||
)
|
||||
|
||||
@property
|
||||
def active_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if spec.active
|
||||
)
|
||||
|
||||
@property
|
||||
def passive_joints(self) -> tuple[str, ...]:
|
||||
return tuple(
|
||||
name for name, spec in self.joint_specs.items() if not spec.active
|
||||
)
|
||||
|
||||
@property
|
||||
def reference_roll_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_mcp_roll"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_pitch_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_mcp_pitch"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_pip_motor(self) -> int:
|
||||
return int(self.joint_specs[f"{self.reference_finger}_pip"].motor_index)
|
||||
|
||||
@property
|
||||
def reference_speed_slot(self) -> int:
|
||||
return {"index": 1, "middle": 2, "ring": 3, "pinky": 4}[
|
||||
self.reference_finger
|
||||
]
|
||||
|
||||
|
||||
_FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky")
|
||||
_MOTOR_BY_JOINT: dict[str, int] = {
|
||||
**{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)},
|
||||
**{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)},
|
||||
**{f"{finger}_pip": index + 16 for index, finger in enumerate(_FINGERS)},
|
||||
}
|
||||
|
||||
|
||||
JOINT_SPECS: dict[str, JointSpec] = {
|
||||
"thumb_cmc_pitch": JointSpec(
|
||||
"thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"index_mcp_pitch": JointSpec(
|
||||
"index_mcp_pitch", 1, True, "side", "side_base", "index_mcp",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"middle_mcp_pitch": JointSpec(
|
||||
"middle_mcp_pitch", 2, True, None, None, None,
|
||||
source_joint="index_mcp_pitch", zero_kind="inherited",
|
||||
),
|
||||
"ring_mcp_pitch": JointSpec(
|
||||
"ring_mcp_pitch", 3, True, None, None, None,
|
||||
source_joint="index_mcp_pitch", zero_kind="inherited",
|
||||
),
|
||||
"pinky_mcp_pitch": JointSpec(
|
||||
"pinky_mcp_pitch", 4, True, None, None, None,
|
||||
source_joint="index_mcp_pitch", zero_kind="inherited",
|
||||
),
|
||||
"thumb_cmc_roll": JointSpec(
|
||||
def _build_g20_hand_profile(side: str) -> HandCalibrationProfile:
|
||||
hand_side = str(side).lower()
|
||||
if hand_side not in {"left", "right"}:
|
||||
raise ValueError("hand side must be left or right")
|
||||
reference = "index" if hand_side == "left" else "pinky"
|
||||
roll_role = f"{reference}_roll"
|
||||
mcp_role = f"{reference}_mcp"
|
||||
pip_role = f"{reference}_pip"
|
||||
dip_role = f"{reference}_dip"
|
||||
view_tags: dict[str, dict[str, int]] = {
|
||||
"front": {
|
||||
"front_base": 0,
|
||||
"thumb_cmc": 1,
|
||||
"thumb_mcp": 2,
|
||||
"thumb_ip": 3,
|
||||
roll_role: 10,
|
||||
},
|
||||
"side": {
|
||||
"side_base": 4,
|
||||
mcp_role: 5,
|
||||
pip_role: 6,
|
||||
dip_role: 7,
|
||||
},
|
||||
"top": {"top_base": 8, "thumb_yaw": 9},
|
||||
}
|
||||
preflight_view_roles = {
|
||||
view: tuple(tags) for view, tags in view_tags.items()
|
||||
}
|
||||
joint_specs: dict[str, JointSpec] = {}
|
||||
# Both hands use the front base/Tag 1 pair for motor 0. Keeping the
|
||||
# complete thumb root chain in one calibrated view avoids introducing a
|
||||
# cross-camera axis-line phase into the URDF zero solve.
|
||||
thumb_pitch_view = "front"
|
||||
thumb_pitch_parent = "front_base"
|
||||
joint_specs["thumb_cmc_pitch"] = JointSpec(
|
||||
"thumb_cmc_pitch",
|
||||
0,
|
||||
True,
|
||||
thumb_pitch_view,
|
||||
thumb_pitch_parent,
|
||||
"thumb_cmc",
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_mcp_pitch"
|
||||
source = None if finger == reference else f"{reference}_mcp_pitch"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"side" if source is None else None,
|
||||
"side_base" if source is None else None,
|
||||
mcp_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_cmc_roll"] = JointSpec(
|
||||
"thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"index_mcp_roll": JointSpec(
|
||||
"index_mcp_roll", 6, True, "front", "front_base", "index_roll",
|
||||
zero_kind="travel_midpoint",
|
||||
),
|
||||
"middle_mcp_roll": JointSpec(
|
||||
"middle_mcp_roll", 7, True, None, None, None,
|
||||
source_joint="index_mcp_roll", zero_kind="inherited",
|
||||
),
|
||||
"ring_mcp_roll": JointSpec(
|
||||
"ring_mcp_roll", 8, True, None, None, None,
|
||||
source_joint="index_mcp_roll", zero_kind="inherited",
|
||||
),
|
||||
"pinky_mcp_roll": JointSpec(
|
||||
"pinky_mcp_roll", 9, True, None, None, None,
|
||||
source_joint="index_mcp_roll", zero_kind="inherited",
|
||||
),
|
||||
"thumb_cmc_yaw": JointSpec(
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_mcp_roll"
|
||||
source = None if finger == reference else f"{reference}_mcp_roll"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"front" if source is None else None,
|
||||
"front_base" if source is None else None,
|
||||
roll_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_cmc_yaw"] = JointSpec(
|
||||
"thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"thumb_mcp": JointSpec(
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
joint_specs["thumb_mcp"] = JointSpec(
|
||||
"thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"index_pip": JointSpec(
|
||||
"index_pip", 16, True, "side", "index_mcp", "index_pip",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"middle_pip": JointSpec(
|
||||
"middle_pip", 17, True, None, None, None,
|
||||
source_joint="index_pip", zero_kind="inherited",
|
||||
),
|
||||
"ring_pip": JointSpec(
|
||||
"ring_pip", 18, True, None, None, None,
|
||||
source_joint="index_pip", zero_kind="inherited",
|
||||
),
|
||||
"pinky_pip": JointSpec(
|
||||
"pinky_pip", 19, True, None, None, None,
|
||||
source_joint="index_pip", zero_kind="inherited",
|
||||
),
|
||||
"thumb_ip": JointSpec(
|
||||
zero_kind="urdf_axis_chain",
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_pip"
|
||||
source = None if finger == reference else f"{reference}_pip"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[name],
|
||||
True,
|
||||
"side" if source is None else None,
|
||||
mcp_role if source is None else None,
|
||||
pip_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
)
|
||||
joint_specs["thumb_ip"] = JointSpec(
|
||||
"thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip",
|
||||
),
|
||||
"index_dip": JointSpec(
|
||||
"index_dip", 16, False, "side", "index_pip", "index_dip",
|
||||
),
|
||||
"middle_dip": JointSpec(
|
||||
"middle_dip", 17, False, None, None, None,
|
||||
source_joint="index_dip",
|
||||
),
|
||||
"ring_dip": JointSpec(
|
||||
"ring_dip", 18, False, None, None, None,
|
||||
source_joint="index_dip",
|
||||
),
|
||||
"pinky_dip": JointSpec(
|
||||
"pinky_dip", 19, False, None, None, None,
|
||||
source_joint="index_dip",
|
||||
),
|
||||
}
|
||||
)
|
||||
for finger in _FINGERS:
|
||||
name = f"{finger}_dip"
|
||||
source = None if finger == reference else f"{reference}_dip"
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
_MOTOR_BY_JOINT[f"{finger}_pip"],
|
||||
False,
|
||||
"side" if source is None else None,
|
||||
pip_role if source is None else None,
|
||||
dip_role if source is None else None,
|
||||
source_joint=source,
|
||||
)
|
||||
sweep_specs = (
|
||||
SweepSpec(thumb_pitch_view, 0, ("thumb_cmc_pitch",)),
|
||||
SweepSpec("front", 5, ("thumb_cmc_roll",)),
|
||||
SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")),
|
||||
SweepSpec("front", _MOTOR_BY_JOINT[f"{reference}_mcp_roll"], (
|
||||
f"{reference}_mcp_roll",
|
||||
)),
|
||||
SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_mcp_pitch"], (
|
||||
f"{reference}_mcp_pitch",
|
||||
)),
|
||||
SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_pip"], (
|
||||
f"{reference}_pip", f"{reference}_dip",
|
||||
)),
|
||||
SweepSpec("top", 10, ("thumb_cmc_yaw",)),
|
||||
)
|
||||
image_joints = frozenset(
|
||||
{
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
f"{reference}_mcp_roll",
|
||||
f"{reference}_mcp_pitch",
|
||||
f"{reference}_pip",
|
||||
}
|
||||
)
|
||||
measured_roll = _MOTOR_BY_JOINT[f"{reference}_mcp_roll"]
|
||||
# The roll channels are mirrored mechanically between hands. Command 0
|
||||
# moves the three non-reference fingers to the camera-right clearance pose
|
||||
# on the left hand, while the same physical pose is command 255 on the
|
||||
# right hand. Using 0 for both sides made the right index/middle/ring
|
||||
# fingers lean left and could obstruct or load the pinky roll sweep.
|
||||
roll_clearance_command = 0 if hand_side == "left" else 255
|
||||
clearance = {
|
||||
_MOTOR_BY_JOINT[f"{finger}_mcp_roll"]: roll_clearance_command
|
||||
for finger in _FINGERS
|
||||
if finger != reference
|
||||
}
|
||||
assert measured_roll not in clearance
|
||||
# Motor 0 is observed from the front with both other thumb-root channels
|
||||
# fully open. Store the commands explicitly for the right hand so a
|
||||
# caller-provided baseline cannot silently restore the obsolete 50/120
|
||||
# pose and hide Tag 1 from the front camera.
|
||||
thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {}
|
||||
return HandCalibrationProfile(
|
||||
model="G20",
|
||||
side=hand_side,
|
||||
command_names=G20_COMMAND_NAMES,
|
||||
baseline_command=THREE_CAMERA_BASELINE_COMMAND,
|
||||
reference_finger=reference,
|
||||
view_tags=view_tags,
|
||||
preflight_view_roles=preflight_view_roles,
|
||||
joint_specs=joint_specs,
|
||||
sweep_specs=sweep_specs,
|
||||
image_trajectory_joints=image_joints,
|
||||
roll_clearance_commands=clearance,
|
||||
thumb_pitch_clearance_commands=thumb_pitch_clearance,
|
||||
)
|
||||
|
||||
|
||||
SWEEP_SPECS: tuple[SweepSpec, ...] = (
|
||||
SweepSpec("front", 0, ("thumb_cmc_pitch",)),
|
||||
SweepSpec("front", 5, ("thumb_cmc_roll",)),
|
||||
SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")),
|
||||
SweepSpec("front", 6, ("index_mcp_roll",)),
|
||||
SweepSpec("side", 1, ("index_mcp_pitch",)),
|
||||
SweepSpec("side", 16, ("index_pip", "index_dip")),
|
||||
SweepSpec("top", 10, ("thumb_cmc_yaw",)),
|
||||
)
|
||||
def _build_o30_right_profile() -> HandCalibrationProfile:
|
||||
reference = "pinky"
|
||||
roll_role = f"{reference}_roll"
|
||||
mcp_role = f"{reference}_mcp"
|
||||
pip_role = f"{reference}_pip"
|
||||
dip_role = f"{reference}_dip"
|
||||
view_tags: dict[str, dict[str, int]] = {
|
||||
"front": {
|
||||
"front_base": 0,
|
||||
"thumb_cmc": 1,
|
||||
"thumb_mcp": 2,
|
||||
"thumb_ip": 3,
|
||||
roll_role: 10,
|
||||
},
|
||||
"side": {
|
||||
"side_base": 4,
|
||||
mcp_role: 5,
|
||||
pip_role: 6,
|
||||
dip_role: 7,
|
||||
},
|
||||
"top": {"top_base": 8, "thumb_yaw": 9},
|
||||
}
|
||||
joint_specs: dict[str, JointSpec] = {
|
||||
"thumb_cmc_roll": JointSpec(
|
||||
"thumb_cmc_roll", 0, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="urdf_axis_chain",
|
||||
command_increasing=True,
|
||||
),
|
||||
"thumb_cmc_yaw": JointSpec(
|
||||
"thumb_cmc_yaw", 1, True, "top", "top_base", "thumb_yaw",
|
||||
zero_kind="urdf_axis_chain",
|
||||
command_increasing=True,
|
||||
),
|
||||
"thumb_mcp": JointSpec(
|
||||
"thumb_mcp", 6, True, "front", "thumb_cmc", "thumb_mcp",
|
||||
zero_kind="urdf_axis_chain",
|
||||
command_increasing=True,
|
||||
),
|
||||
"thumb_ip": JointSpec(
|
||||
"thumb_ip", 15, True, "front", "thumb_mcp", "thumb_ip",
|
||||
zero_kind="urdf_axis_chain",
|
||||
command_increasing=True,
|
||||
),
|
||||
}
|
||||
motor_groups = {
|
||||
"mcp_roll": (2, 3, 4, 5),
|
||||
"mcp_pitch": (7, 8, 9, 10),
|
||||
"pip": (11, 12, 13, 14),
|
||||
"dip": (16, 17, 18, 19),
|
||||
}
|
||||
role_pairs = {
|
||||
"mcp_roll": ("front", "front_base", roll_role),
|
||||
"mcp_pitch": ("side", "side_base", mcp_role),
|
||||
"pip": ("side", mcp_role, pip_role),
|
||||
"dip": ("side", pip_role, dip_role),
|
||||
}
|
||||
for suffix, motors in motor_groups.items():
|
||||
for finger, motor in zip(_FINGERS, motors):
|
||||
name = f"{finger}_{suffix}"
|
||||
source = None if finger == reference else f"{reference}_{suffix}"
|
||||
view, parent_role, child_role = role_pairs[suffix]
|
||||
joint_specs[name] = JointSpec(
|
||||
name,
|
||||
motor,
|
||||
True,
|
||||
view if source is None else None,
|
||||
parent_role if source is None else None,
|
||||
child_role if source is None else None,
|
||||
source_joint=source,
|
||||
zero_kind="urdf_axis_chain" if source is None else "inherited",
|
||||
command_increasing=True,
|
||||
)
|
||||
sweep_specs = (
|
||||
SweepSpec("front", 0, ("thumb_cmc_roll",)),
|
||||
SweepSpec("front", 6, ("thumb_mcp",)),
|
||||
SweepSpec("front", 15, ("thumb_ip",)),
|
||||
SweepSpec("front", 5, ("pinky_mcp_roll",)),
|
||||
SweepSpec("side", 10, ("pinky_mcp_pitch",)),
|
||||
SweepSpec("side", 14, ("pinky_pip",)),
|
||||
SweepSpec("side", 19, ("pinky_dip",)),
|
||||
SweepSpec("top", 1, ("thumb_cmc_yaw",)),
|
||||
)
|
||||
image_joints = frozenset(
|
||||
{
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
"pinky_mcp_roll",
|
||||
"pinky_mcp_pitch",
|
||||
"pinky_pip",
|
||||
}
|
||||
)
|
||||
return HandCalibrationProfile(
|
||||
model="O30",
|
||||
side="right",
|
||||
command_names=O30_COMMAND_NAMES,
|
||||
baseline_command=O30_RIGHT_BASELINE_COMMAND,
|
||||
reference_finger=reference,
|
||||
view_tags=view_tags,
|
||||
preflight_view_roles={view: tuple(tags) for view, tags in view_tags.items()},
|
||||
joint_specs=joint_specs,
|
||||
sweep_specs=sweep_specs,
|
||||
image_trajectory_joints=image_joints,
|
||||
# The right-hand camera clearance convention matches the G20 right
|
||||
# hand: move the three untagged splay motors to the far endpoint.
|
||||
roll_clearance_commands={2: 255, 3: 255, 4: 255},
|
||||
thumb_pitch_clearance_commands={},
|
||||
)
|
||||
|
||||
MEASURED_JOINTS: tuple[str, ...] = tuple(
|
||||
name for name, spec in JOINT_SPECS.items() if spec.measured
|
||||
)
|
||||
ACTIVE_JOINTS: tuple[str, ...] = tuple(
|
||||
name for name, spec in JOINT_SPECS.items() if spec.active
|
||||
)
|
||||
PASSIVE_JOINTS: tuple[str, ...] = tuple(
|
||||
name for name, spec in JOINT_SPECS.items() if not spec.active
|
||||
)
|
||||
|
||||
LEFT_HAND_PROFILE = _build_g20_hand_profile("left")
|
||||
RIGHT_HAND_PROFILE = _build_g20_hand_profile("right")
|
||||
O30_RIGHT_HAND_PROFILE = _build_o30_right_profile()
|
||||
|
||||
|
||||
def get_hand_calibration_profile(
|
||||
side: str, model: str = "G20"
|
||||
) -> HandCalibrationProfile:
|
||||
hand_model = str(model).upper()
|
||||
value = str(side).lower()
|
||||
if hand_model == "O30":
|
||||
if value != "right":
|
||||
raise ValueError("O30 calibration currently supports only the right hand")
|
||||
return O30_RIGHT_HAND_PROFILE
|
||||
if hand_model != "G20":
|
||||
raise ValueError("hand model must be G20 or O30")
|
||||
if value == "left":
|
||||
return LEFT_HAND_PROFILE
|
||||
if value == "right":
|
||||
return RIGHT_HAND_PROFILE
|
||||
raise ValueError("hand side must be left or right")
|
||||
|
||||
|
||||
# Backwards-compatible aliases keep the existing left-hand API stable.
|
||||
VIEW_TAGS = LEFT_HAND_PROFILE.view_tags
|
||||
JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs
|
||||
SWEEP_SPECS = LEFT_HAND_PROFILE.sweep_specs
|
||||
MEASURED_JOINTS = LEFT_HAND_PROFILE.measured_joints
|
||||
ACTIVE_JOINTS = LEFT_HAND_PROFILE.active_joints
|
||||
PASSIVE_JOINTS = LEFT_HAND_PROFILE.passive_joints
|
||||
SPLAY_JOINTS: tuple[str, ...] = (
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
@@ -203,27 +506,15 @@ SPLAY_JOINTS: tuple[str, ...] = (
|
||||
# and a motion plane that is close to the active camera's image plane. Their
|
||||
# projected circles are substantially more repeatable than the difference of
|
||||
# two independently estimated planar-Tag PnP depths. thumb_ip remains projected
|
||||
# because its front-view PnP depth is not repeatable enough for a 3-D fit; the
|
||||
# side-view index_dip and oblique top-view yaw remain parent-relative 3-D.
|
||||
IMAGE_TRAJECTORY_JOINTS: frozenset[str] = frozenset(
|
||||
{
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
"index_mcp_roll",
|
||||
"index_mcp_pitch",
|
||||
"index_pip",
|
||||
}
|
||||
)
|
||||
# because its front-view PnP depth is not repeatable enough for a 3-D fit. The
|
||||
# side-view index_dip and oblique top-view yaw remain parent-relative full
|
||||
# SE(3) measurements; their circle directions are constrained by the more
|
||||
# repeatable relative-orientation screw axis in urdf_zero.py.
|
||||
IMAGE_TRAJECTORY_JOINTS = LEFT_HAND_PROFILE.image_trajectory_joints
|
||||
|
||||
# Keep the three unmeasured finger-roll motors away from the front camera's
|
||||
# line of sight while index_mcp_roll is measured.
|
||||
INDEX_ROLL_CLEARANCE_COMMANDS: dict[int, int] = {
|
||||
7: 0,
|
||||
8: 0,
|
||||
9: 0,
|
||||
}
|
||||
INDEX_ROLL_CLEARANCE_COMMANDS = LEFT_HAND_PROFILE.roll_clearance_commands
|
||||
|
||||
# Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is
|
||||
# measured. This keeps the moving top-view tag sufficiently front-facing.
|
||||
@@ -232,11 +523,17 @@ THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = {
|
||||
}
|
||||
|
||||
|
||||
def calibration_auxiliary_commands(spec: SweepSpec) -> dict[int, int]:
|
||||
def calibration_auxiliary_commands(
|
||||
spec: SweepSpec,
|
||||
*,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> dict[int, int]:
|
||||
"""Return motors that must remain fixed throughout one calibration task."""
|
||||
if spec.motor_index == 6:
|
||||
return dict(INDEX_ROLL_CLEARANCE_COMMANDS)
|
||||
if spec.motor_index == 10:
|
||||
if spec.motor_index == 0:
|
||||
return dict(profile.thumb_pitch_clearance_commands)
|
||||
if spec.motor_index == profile.reference_roll_motor:
|
||||
return dict(profile.roll_clearance_commands)
|
||||
if profile.model == "G20" and spec.motor_index == 10:
|
||||
return dict(THUMB_YAW_CLEARANCE_COMMANDS)
|
||||
return {}
|
||||
|
||||
@@ -244,17 +541,19 @@ def calibration_auxiliary_commands(spec: SweepSpec) -> dict[int, int]:
|
||||
def build_full_hand_command(
|
||||
motor_index: int,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = BASELINE_COMMAND,
|
||||
baseline: Sequence[int] | None = None,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
if len(baseline) != 20:
|
||||
selected_baseline = profile.baseline_command if baseline is None else baseline
|
||||
if len(selected_baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 values")
|
||||
motor = int(motor_index)
|
||||
if motor not in {spec.motor_index for spec in JOINT_SPECS.values()}:
|
||||
raise ValueError("motor_index is not a controlled G20 calibration channel")
|
||||
if motor not in {spec.motor_index for spec in profile.joint_specs.values()}:
|
||||
raise ValueError("motor_index is not a controlled calibration channel")
|
||||
command = int(command_u8)
|
||||
if not 0 <= command <= 255:
|
||||
raise ValueError("command_u8 must be in [0, 255]")
|
||||
result = [int(value) for value in baseline]
|
||||
result = [int(value) for value in selected_baseline]
|
||||
if any(not 0 <= value <= 255 for value in result):
|
||||
raise ValueError("baseline values must be in [0, 255]")
|
||||
result[motor] = command
|
||||
@@ -264,16 +563,18 @@ def build_full_hand_command(
|
||||
def build_calibration_motion_command(
|
||||
spec: SweepSpec,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = BASELINE_COMMAND,
|
||||
baseline: Sequence[int] | None = None,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
"""Build a sweep command, including any required clearance pose."""
|
||||
result = build_full_hand_command(
|
||||
spec.motor_index,
|
||||
command_u8,
|
||||
baseline=baseline,
|
||||
profile=profile,
|
||||
)
|
||||
for motor_index, auxiliary_command in calibration_auxiliary_commands(
|
||||
spec
|
||||
spec, profile=profile
|
||||
).items():
|
||||
result[motor_index] = auxiliary_command
|
||||
return result
|
||||
@@ -285,8 +586,9 @@ def build_calibration_speed_profile(
|
||||
normal_speed: int,
|
||||
index_roll_speed: int,
|
||||
index_flex_speed: int,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> list[int]:
|
||||
"""Return G20's five per-finger speeds for one calibration task."""
|
||||
"""Return the five-value speed command accepted by the selected SDK."""
|
||||
normal = int(normal_speed)
|
||||
index_roll = int(index_roll_speed)
|
||||
index_flex = int(index_flex_speed)
|
||||
@@ -295,10 +597,29 @@ def build_calibration_speed_profile(
|
||||
):
|
||||
raise ValueError("calibration speeds must be in [0, 255]")
|
||||
speeds = [normal] * 5
|
||||
if spec.motor_index == 6:
|
||||
speeds[1] = index_roll
|
||||
elif spec.motor_index in {1, 16}:
|
||||
speeds[1] = index_flex
|
||||
if profile.model == "O30":
|
||||
# The current O30 ROS SDK accepts the common five-value command and
|
||||
# broadcasts its first entry to all 20 motors. Only one motor moves
|
||||
# during calibration, so broadcast the active task's intended speed.
|
||||
task_speed = (
|
||||
index_roll
|
||||
if spec.motor_index == profile.reference_roll_motor
|
||||
else index_flex
|
||||
if spec.motor_index in {
|
||||
profile.reference_pitch_motor,
|
||||
profile.reference_pip_motor,
|
||||
profile.joint_specs[f"{profile.reference_finger}_dip"].motor_index,
|
||||
}
|
||||
else normal
|
||||
)
|
||||
return [task_speed] * 5
|
||||
if spec.motor_index == profile.reference_roll_motor:
|
||||
speeds[profile.reference_speed_slot] = index_roll
|
||||
elif spec.motor_index in {
|
||||
profile.reference_pitch_motor,
|
||||
profile.reference_pip_motor,
|
||||
}:
|
||||
speeds[profile.reference_speed_slot] = index_flex
|
||||
return speeds
|
||||
|
||||
|
||||
@@ -479,9 +800,10 @@ def fit_measured_joint_curve(
|
||||
image_maximum_radial_rms_px: float = 2.0,
|
||||
image_maximum_radial_p95_px: float = 3.5,
|
||||
image_minimum_radius_px: float = 20.0,
|
||||
profile: HandCalibrationProfile = LEFT_HAND_PROFILE,
|
||||
) -> JointCurveFit:
|
||||
"""Select the stable trajectory representation for one measured joint."""
|
||||
if str(joint_name) in IMAGE_TRAJECTORY_JOINTS:
|
||||
if str(joint_name) in profile.image_trajectory_joints:
|
||||
return fit_joint_image_curve(
|
||||
records,
|
||||
maximum_radial_rms_px=image_maximum_radial_rms_px,
|
||||
@@ -608,69 +930,106 @@ def fit_projected_zero(
|
||||
return round(float(circular_median_rad(angles)), 8)
|
||||
|
||||
|
||||
def remap_inherited_curve(
|
||||
values: Sequence[float],
|
||||
*,
|
||||
source_zero_command_u8: int,
|
||||
target_zero_command_u8: int,
|
||||
) -> list[float]:
|
||||
"""Re-centre a shared curve at the target motor's neutral command.
|
||||
|
||||
O30 finger-yaw motors have different straight-hand commands. A literal
|
||||
copy of the reference pinky's curve would therefore make the index,
|
||||
middle, and ring fingers non-zero at their own baseline. Preserve the
|
||||
measured command scale and subtract its value at the target neutral; this
|
||||
changes only the dynamic angle datum and does not invent a static URDF
|
||||
correction for an unobserved motor.
|
||||
"""
|
||||
curve = np.asarray(values, dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError("inherited curve must contain 256 finite values")
|
||||
source_zero = int(source_zero_command_u8)
|
||||
target_zero = int(target_zero_command_u8)
|
||||
if not 0 <= source_zero <= 255 or not 0 <= target_zero <= 255:
|
||||
raise ValueError("curve zero commands must be in [0, 255]")
|
||||
if abs(float(curve[source_zero])) > 1.0e-6:
|
||||
raise ValueError("source curve must be zero at source_zero_command_u8")
|
||||
result = curve.copy()
|
||||
result -= float(result[target_zero])
|
||||
return [round(float(value), 8) for value in result]
|
||||
|
||||
|
||||
def build_compact_payload(
|
||||
*,
|
||||
serial_number: str,
|
||||
measured_fits: Mapping[str, JointCurveFit],
|
||||
projected_zeros_rad: Mapping[str, float],
|
||||
splay_zero_command_u8: int,
|
||||
splay_midpoint_rad: float,
|
||||
urdf_zero_offsets_rad: Mapping[str, float],
|
||||
validation_errors_rad: Sequence[float],
|
||||
passed: bool,
|
||||
baseline: Sequence[int] = BASELINE_COMMAND,
|
||||
baseline: Sequence[int] | None = None,
|
||||
side: str = "left",
|
||||
model: str = "G20",
|
||||
) -> dict[str, Any]:
|
||||
if set(measured_fits) != set(MEASURED_JOINTS):
|
||||
profile = get_hand_calibration_profile(side, model)
|
||||
selected_baseline = profile.baseline_command if baseline is None else baseline
|
||||
if set(measured_fits) != set(profile.measured_joints):
|
||||
raise ValueError("measured_fits must contain all directly measured joints")
|
||||
expected_projected = {
|
||||
name for name, spec in JOINT_SPECS.items()
|
||||
if spec.zero_kind == "projected"
|
||||
expected_active = {
|
||||
name for name, spec in profile.joint_specs.items() if spec.active
|
||||
}
|
||||
if set(projected_zeros_rad) != expected_projected:
|
||||
raise ValueError("projected_zeros_rad has the wrong joint set")
|
||||
if not 0 <= int(splay_zero_command_u8) <= 255:
|
||||
raise ValueError("splay zero command must be in [0, 255]")
|
||||
if set(urdf_zero_offsets_rad) != expected_active:
|
||||
raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set")
|
||||
if len(selected_baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 commands")
|
||||
|
||||
measured_curves = {
|
||||
name: [round(float(value), 8) for value in fit.angle_rad]
|
||||
for name, fit in measured_fits.items()
|
||||
}
|
||||
joints: dict[str, dict[str, Any]] = {}
|
||||
for name, spec in JOINT_SPECS.items():
|
||||
for name, spec in profile.joint_specs.items():
|
||||
source_name = spec.source_joint or name
|
||||
fit = measured_fits[source_name]
|
||||
source_spec = profile.joint_specs[source_name]
|
||||
source_zero = int(selected_baseline[source_spec.motor_index])
|
||||
target_zero = int(selected_baseline[spec.motor_index])
|
||||
angle_rad = (
|
||||
measured_curves[source_name]
|
||||
if spec.source_joint is None
|
||||
else remap_inherited_curve(
|
||||
measured_curves[source_name],
|
||||
source_zero_command_u8=source_zero,
|
||||
target_zero_command_u8=target_zero,
|
||||
)
|
||||
)
|
||||
joint: dict[str, Any] = {
|
||||
"motor_index": int(spec.motor_index),
|
||||
"angle_rad": [round(float(value), 8) for value in fit.angle_rad],
|
||||
"angle_rad": angle_rad,
|
||||
}
|
||||
if spec.active:
|
||||
joint["zero_command_u8"] = (
|
||||
int(splay_zero_command_u8)
|
||||
if name in SPLAY_JOINTS
|
||||
else 255
|
||||
)
|
||||
joint["zero_command_u8"] = target_zero
|
||||
joint["zero_angles"] = {
|
||||
"urdf_zero_offset_rad": round(
|
||||
float(urdf_zero_offsets_rad[name]), 8
|
||||
)
|
||||
}
|
||||
else:
|
||||
joint["passive"] = True
|
||||
if spec.source_joint is not None:
|
||||
joint["source_joint"] = spec.source_joint
|
||||
elif spec.zero_kind == "projected":
|
||||
joint["zero_angles"] = {
|
||||
"table_projected_zero_rad": round(
|
||||
float(projected_zeros_rad[name]), 8
|
||||
)
|
||||
}
|
||||
elif spec.zero_kind == "travel_midpoint":
|
||||
joint["zero_angles"] = {
|
||||
"travel_midpoint_rad": round(float(splay_midpoint_rad), 8)
|
||||
}
|
||||
joints[name] = joint
|
||||
|
||||
errors = np.abs(np.asarray(validation_errors_rad, dtype=float))
|
||||
mae = float(np.mean(errors)) if errors.size else float("nan")
|
||||
p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan")
|
||||
payload = {
|
||||
"schema_version": 3,
|
||||
"model": "G20",
|
||||
"side": "left",
|
||||
"schema_version": 4,
|
||||
"model": profile.model,
|
||||
"side": profile.side,
|
||||
"serial_number": str(serial_number),
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": [int(value) for value in baseline],
|
||||
"baseline_command_u8": [int(value) for value in selected_baseline],
|
||||
"joints": joints,
|
||||
"quality": {
|
||||
"passed": bool(passed),
|
||||
@@ -693,27 +1052,31 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None:
|
||||
}
|
||||
if set(payload) != expected_top:
|
||||
raise ValueError("compact calibration has unexpected top-level fields")
|
||||
if payload["schema_version"] != 3:
|
||||
raise ValueError("schema_version must be 3")
|
||||
if payload["model"] != "G20" or payload["side"] != "left":
|
||||
raise ValueError("payload must describe a left G20")
|
||||
if payload["schema_version"] != 4:
|
||||
raise ValueError("schema_version must be 4")
|
||||
model = str(payload["model"]).upper()
|
||||
side = str(payload["side"]).lower()
|
||||
profile = get_hand_calibration_profile(side, model)
|
||||
if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
|
||||
raise ValueError("payload angle or command units are invalid")
|
||||
baseline = payload["baseline_command_u8"]
|
||||
if not isinstance(baseline, list) or len(baseline) != 20:
|
||||
raise ValueError("baseline_command_u8 must contain 20 values")
|
||||
joints = payload["joints"]
|
||||
if not isinstance(joints, Mapping) or set(joints) != set(JOINT_SPECS):
|
||||
raise ValueError("payload must contain exactly 21 G20 joints")
|
||||
if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs):
|
||||
raise ValueError(
|
||||
f"payload must contain exactly {len(profile.joint_specs)} "
|
||||
f"{profile.model} joints"
|
||||
)
|
||||
|
||||
for name, spec in JOINT_SPECS.items():
|
||||
for name, spec in profile.joint_specs.items():
|
||||
joint = joints[name]
|
||||
allowed = {"motor_index", "angle_rad"}
|
||||
allowed.add("zero_command_u8" if spec.active else "passive")
|
||||
if spec.active:
|
||||
allowed.add("zero_angles")
|
||||
if spec.source_joint is not None:
|
||||
allowed.add("source_joint")
|
||||
elif spec.zero_kind in {"projected", "travel_midpoint"}:
|
||||
allowed.add("zero_angles")
|
||||
if set(joint) != allowed:
|
||||
raise ValueError(f"{name} has unexpected fields")
|
||||
if int(joint["motor_index"]) != spec.motor_index:
|
||||
@@ -721,33 +1084,52 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None:
|
||||
curve = np.asarray(joint["angle_rad"], dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError(f"{name}.angle_rad must contain 256 finite values")
|
||||
if np.any(np.diff(curve) > 1.0e-7):
|
||||
raise ValueError(f"{name}.angle_rad must be non-increasing")
|
||||
differences = np.diff(curve)
|
||||
if (
|
||||
np.any(differences < -1.0e-7)
|
||||
if spec.command_increasing
|
||||
else np.any(differences > 1.0e-7)
|
||||
):
|
||||
direction = "non-decreasing" if spec.command_increasing else "non-increasing"
|
||||
raise ValueError(f"{name}.angle_rad must be {direction}")
|
||||
if spec.active:
|
||||
zero = joint["zero_command_u8"]
|
||||
if not isinstance(zero, int) or not 0 <= zero <= 255:
|
||||
raise ValueError(f"{name}.zero_command_u8 is invalid")
|
||||
zero_angles = joint.get("zero_angles")
|
||||
if not isinstance(zero_angles, Mapping) or set(zero_angles) != {
|
||||
"urdf_zero_offset_rad"
|
||||
}:
|
||||
raise ValueError(
|
||||
f"{name}.zero_angles must contain urdf_zero_offset_rad"
|
||||
)
|
||||
offset = float(zero_angles["urdf_zero_offset_rad"])
|
||||
if not math.isfinite(offset):
|
||||
raise ValueError(f"{name}.urdf_zero_offset_rad is invalid")
|
||||
if abs(float(curve[zero])) > 1.0e-6:
|
||||
raise ValueError(
|
||||
f"{name}.angle_rad must be zero at zero_command_u8"
|
||||
)
|
||||
elif joint.get("passive") is not True:
|
||||
raise ValueError(f"{name} must be marked passive")
|
||||
if spec.source_joint is not None:
|
||||
if joint.get("source_joint") != spec.source_joint:
|
||||
raise ValueError(f"{name} has the wrong source_joint")
|
||||
source = joints[spec.source_joint]
|
||||
if joint["angle_rad"] != source["angle_rad"]:
|
||||
raise ValueError(f"{name} must copy its source curve exactly")
|
||||
if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]:
|
||||
raise ValueError(f"{name} must copy its source zero command")
|
||||
|
||||
index_roll = np.asarray(joints["index_mcp_roll"]["angle_rad"], dtype=float)
|
||||
if not index_roll[0] > 0.0 or not index_roll[255] < 0.0:
|
||||
raise ValueError("index_mcp_roll endpoints must be positive then negative")
|
||||
if abs(float(index_roll[0] + index_roll[255])) > 1.0e-7:
|
||||
raise ValueError("index_mcp_roll endpoints must be symmetric")
|
||||
for name, spec in JOINT_SPECS.items():
|
||||
if name not in SPLAY_JOINTS and spec.source_joint != "index_mcp_roll":
|
||||
curve = np.asarray(joints[name]["angle_rad"], dtype=float)
|
||||
if abs(float(curve[255])) > 1.0e-6:
|
||||
raise ValueError(f"{name}.angle_rad[255] must be zero")
|
||||
expected_curve = (
|
||||
remap_inherited_curve(
|
||||
source["angle_rad"],
|
||||
source_zero_command_u8=int(source["zero_command_u8"]),
|
||||
target_zero_command_u8=int(joint["zero_command_u8"]),
|
||||
)
|
||||
if spec.active
|
||||
else source["angle_rad"]
|
||||
)
|
||||
if joint["angle_rad"] != expected_curve:
|
||||
raise ValueError(f"{name} must remap its source curve exactly")
|
||||
# source_joint denotes a dynamic command-angle curve source. The
|
||||
# absolute URDF zero belongs to an independent motor/assembly and
|
||||
# must remain per-joint unless it was independently observed.
|
||||
|
||||
quality = payload["quality"]
|
||||
if set(quality) != {
|
||||
|
||||
@@ -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()
|
||||
@@ -41,6 +41,19 @@ def _relative_pose(
|
||||
return relative_rotation, relative_translation
|
||||
|
||||
|
||||
def _normal_alignment_rad(first: SquareTagPose, second: SquareTagPose) -> float:
|
||||
"""Return the undirected angle between two observed Tag face normals."""
|
||||
first_normal = Rotation.from_quat(first.quaternion_xyzw).apply(
|
||||
[0.0, 0.0, 1.0]
|
||||
)
|
||||
second_normal = Rotation.from_quat(second.quaternion_xyzw).apply(
|
||||
[0.0, 0.0, 1.0]
|
||||
)
|
||||
return math.acos(
|
||||
abs(float(np.clip(first_normal @ second_normal, -1.0, 1.0)))
|
||||
)
|
||||
|
||||
|
||||
def select_rigid_group_trajectory(
|
||||
frames: Sequence[Mapping[str, Sequence[SquareTagPose]]],
|
||||
*,
|
||||
@@ -50,6 +63,8 @@ def select_rigid_group_trajectory(
|
||||
rotation_scale_rad: float,
|
||||
translation_scale_m: float,
|
||||
pair_geometry: str = "pose",
|
||||
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
|
||||
normal_alignment_scale_rad: float | None = None,
|
||||
) -> tuple[
|
||||
list[dict[str, SquareTagPose]],
|
||||
dict[str, float | str],
|
||||
@@ -66,20 +81,34 @@ def select_rigid_group_trajectory(
|
||||
"""
|
||||
role_names = tuple(str(role) for role in roles)
|
||||
pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs)
|
||||
normal_pair_names = tuple(
|
||||
(str(first), str(second))
|
||||
for first, second in normal_alignment_pairs
|
||||
)
|
||||
if not frames:
|
||||
raise ValueError("at least one PnP frame is required")
|
||||
if len(set(role_names)) != len(role_names) or not role_names:
|
||||
raise ValueError("roles must be non-empty and unique")
|
||||
if any(
|
||||
parent not in role_names or child not in role_names
|
||||
for parent, child in pair_names
|
||||
for parent, child in (*pair_names, *normal_pair_names)
|
||||
):
|
||||
raise ValueError("fixed_pairs must reference roles")
|
||||
raise ValueError("geometry pairs must reference roles")
|
||||
reprojection_scale = float(reprojection_scale_px)
|
||||
rotation_scale = float(rotation_scale_rad)
|
||||
translation_scale = float(translation_scale_m)
|
||||
geometry_mode = str(pair_geometry)
|
||||
if min(reprojection_scale, rotation_scale, translation_scale) <= 0.0:
|
||||
normal_scale = (
|
||||
rotation_scale
|
||||
if normal_alignment_scale_rad is None
|
||||
else float(normal_alignment_scale_rad)
|
||||
)
|
||||
if min(
|
||||
reprojection_scale,
|
||||
rotation_scale,
|
||||
translation_scale,
|
||||
normal_scale,
|
||||
) <= 0.0:
|
||||
raise ValueError("trajectory selection scales must be positive")
|
||||
if geometry_mode not in {"pose", "distance"}:
|
||||
raise ValueError("pair_geometry must be pose or distance")
|
||||
@@ -110,6 +139,12 @@ def select_rigid_group_trajectory(
|
||||
pose.reprojection_error_px
|
||||
for pose in combination.values()
|
||||
) / reprojection_scale
|
||||
normal_alignment_cost = sum(
|
||||
_normal_alignment_rad(
|
||||
combination[first], combination[second]
|
||||
)
|
||||
for first, second in normal_pair_names
|
||||
) / normal_scale
|
||||
rotation_drifts: list[float] = []
|
||||
translation_drifts: list[float] = []
|
||||
distance_drifts: list[float] = []
|
||||
@@ -156,7 +191,7 @@ def select_rigid_group_trajectory(
|
||||
+ sum(translation_drifts) / translation_scale
|
||||
)
|
||||
return (
|
||||
reprojection_cost + geometry_cost,
|
||||
reprojection_cost + geometry_cost + normal_alignment_cost,
|
||||
max(rotation_drifts, default=0.0),
|
||||
(
|
||||
max(distance_drifts, default=0.0)
|
||||
@@ -384,9 +419,25 @@ def select_rigid_group_trajectory(
|
||||
translation_drifts_by_frame, dtype=float
|
||||
)
|
||||
distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float)
|
||||
normal_alignments = np.asarray(
|
||||
[
|
||||
max(
|
||||
(
|
||||
_normal_alignment_rad(frame[first], frame[second])
|
||||
for first, second in normal_pair_names
|
||||
),
|
||||
default=0.0,
|
||||
)
|
||||
for frame in best_path
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
return best_path, {
|
||||
"total_cost": float(best_total),
|
||||
"pair_geometry": geometry_mode,
|
||||
"maximum_normal_alignment_rad": float(
|
||||
np.max(normal_alignments, initial=0.0)
|
||||
),
|
||||
"maximum_pair_rotation_drift_rad": float(
|
||||
np.max(rotation_drifts, initial=0.0)
|
||||
),
|
||||
@@ -414,6 +465,172 @@ def select_rigid_group_trajectory(
|
||||
}
|
||||
|
||||
|
||||
def select_static_rigid_group_initialization(
|
||||
frames: Sequence[Mapping[str, Sequence[SquareTagPose]]],
|
||||
*,
|
||||
roles: Sequence[str],
|
||||
fixed_pairs: Sequence[tuple[str, str]],
|
||||
reprojection_scale_px: float,
|
||||
maximum_pose_jump_rad: float,
|
||||
maximum_translation_jump_m: float,
|
||||
relative_rotation_scale_rad: float,
|
||||
relative_translation_scale_m: float,
|
||||
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
|
||||
normal_alignment_scale_rad: float = math.radians(5.0),
|
||||
) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]:
|
||||
"""Select a static multi-Tag IPPE branch path in bounded time.
|
||||
|
||||
Group initialization is performed while the hand is held at an endpoint,
|
||||
so every frame should describe the same camera and relative Tag poses.
|
||||
Enumerating a full Viterbi transition matrix for every possible first-frame
|
||||
branch is therefore unnecessary: with four two-branch Tags and eight
|
||||
frames it performs more than one hundred thousand scipy rotations and can
|
||||
block the live ROS callback for several seconds.
|
||||
|
||||
Instead, treat every first-frame combination as a possible static
|
||||
reference and independently select the closest combination in each later
|
||||
frame. This preserves the multi-frame rigidity and normal-alignment
|
||||
evidence while changing the search from O(F*C^2*C0) to O(F*C*C0).
|
||||
"""
|
||||
role_names = tuple(str(role) for role in roles)
|
||||
pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs)
|
||||
normal_pair_names = tuple(
|
||||
(str(first), str(second)) for first, second in normal_alignment_pairs
|
||||
)
|
||||
if not frames:
|
||||
raise ValueError("at least one PnP frame is required")
|
||||
if not role_names or len(set(role_names)) != len(role_names):
|
||||
raise ValueError("roles must be non-empty and unique")
|
||||
if any(
|
||||
parent not in role_names or child not in role_names
|
||||
for parent, child in (*pair_names, *normal_pair_names)
|
||||
):
|
||||
raise ValueError("geometry pairs must reference roles")
|
||||
scales = (
|
||||
float(reprojection_scale_px),
|
||||
float(maximum_pose_jump_rad),
|
||||
float(maximum_translation_jump_m),
|
||||
float(relative_rotation_scale_rad),
|
||||
float(relative_translation_scale_m),
|
||||
float(normal_alignment_scale_rad),
|
||||
)
|
||||
if min(scales) <= 0.0:
|
||||
raise ValueError("static initialization scales must be positive")
|
||||
(
|
||||
reprojection_scale,
|
||||
pose_scale,
|
||||
translation_scale,
|
||||
relative_rotation_scale,
|
||||
relative_translation_scale,
|
||||
normal_scale,
|
||||
) = scales
|
||||
|
||||
combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
|
||||
for frame in frames:
|
||||
candidate_lists = [tuple(frame.get(role, ())) for role in role_names]
|
||||
if any(not candidates for candidates in candidate_lists):
|
||||
raise ValueError("every frame must contain every requested role")
|
||||
combinations_by_frame.append(
|
||||
[
|
||||
dict(zip(role_names, combination))
|
||||
for combination in product(*candidate_lists)
|
||||
]
|
||||
)
|
||||
|
||||
def score_against_reference(
|
||||
reference: Mapping[str, SquareTagPose],
|
||||
reference_pairs: Mapping[
|
||||
tuple[str, str], tuple[Rotation, np.ndarray]
|
||||
],
|
||||
combination: Mapping[str, SquareTagPose],
|
||||
) -> float:
|
||||
score = sum(
|
||||
pose.reprojection_error_px for pose in combination.values()
|
||||
) / reprojection_scale
|
||||
score += sum(
|
||||
_normal_alignment_rad(combination[first], combination[second])
|
||||
for first, second in normal_pair_names
|
||||
) / normal_scale
|
||||
score += sum(
|
||||
rotation_distance_rad(
|
||||
reference[role].quaternion_xyzw,
|
||||
combination[role].quaternion_xyzw,
|
||||
)
|
||||
/ pose_scale
|
||||
+ float(
|
||||
np.linalg.norm(
|
||||
np.asarray(combination[role].translation_xyz_m, dtype=float)
|
||||
- np.asarray(reference[role].translation_xyz_m, dtype=float)
|
||||
)
|
||||
)
|
||||
/ translation_scale
|
||||
for role in role_names
|
||||
)
|
||||
for pair, (reference_rotation, reference_translation) in (
|
||||
reference_pairs.items()
|
||||
):
|
||||
rotation, translation = _relative_pose(
|
||||
combination[pair[0]], combination[pair[1]]
|
||||
)
|
||||
score += (
|
||||
float((reference_rotation.inv() * rotation).magnitude())
|
||||
/ relative_rotation_scale
|
||||
+ float(np.linalg.norm(translation - reference_translation))
|
||||
/ relative_translation_scale
|
||||
)
|
||||
return float(score)
|
||||
|
||||
best_total = float("inf")
|
||||
best_path: list[dict[str, SquareTagPose]] | None = None
|
||||
for reference in combinations_by_frame[0]:
|
||||
reference_pairs = {
|
||||
pair: _relative_pose(reference[pair[0]], reference[pair[1]])
|
||||
for pair in pair_names
|
||||
}
|
||||
path = [reference]
|
||||
total = score_against_reference(reference, reference_pairs, reference)
|
||||
for combinations in combinations_by_frame[1:]:
|
||||
scored = [
|
||||
(
|
||||
score_against_reference(
|
||||
reference, reference_pairs, combination
|
||||
),
|
||||
combination,
|
||||
)
|
||||
for combination in combinations
|
||||
]
|
||||
cost, selected = min(scored, key=lambda item: item[0])
|
||||
total += cost
|
||||
path.append(selected)
|
||||
if total < best_total:
|
||||
best_total = total
|
||||
best_path = path
|
||||
|
||||
if best_path is None:
|
||||
raise RuntimeError("static group initialization produced no path")
|
||||
|
||||
# Reuse the complete quality calculation with exactly one chosen branch
|
||||
# per role and frame. This retains all existing quality fields without
|
||||
# reintroducing the combinatorial branch search.
|
||||
reduced_frames = [
|
||||
{role: (frame[role],) for role in role_names} for frame in best_path
|
||||
]
|
||||
selected_path, quality = select_rigid_group_trajectory(
|
||||
reduced_frames,
|
||||
roles=role_names,
|
||||
fixed_pairs=pair_names,
|
||||
reprojection_scale_px=reprojection_scale,
|
||||
rotation_scale_rad=relative_rotation_scale,
|
||||
translation_scale_m=relative_translation_scale,
|
||||
normal_alignment_pairs=normal_pair_names,
|
||||
normal_alignment_scale_rad=normal_scale,
|
||||
)
|
||||
quality = dict(quality)
|
||||
quality["total_cost"] = float(best_total)
|
||||
quality["initialization_search"] = "static_reference"
|
||||
return selected_path, quality
|
||||
|
||||
|
||||
def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray:
|
||||
matrix = np.asarray(camera_matrix, dtype=np.float64)
|
||||
if matrix.shape != (3, 3):
|
||||
@@ -517,9 +734,26 @@ def rotation_distance_rad(
|
||||
first_xyzw: Sequence[float],
|
||||
second_xyzw: Sequence[float],
|
||||
) -> float:
|
||||
first = Rotation.from_quat(np.asarray(first_xyzw, dtype=float))
|
||||
second = Rotation.from_quat(np.asarray(second_xyzw, dtype=float))
|
||||
return float((first.inv() * second).magnitude())
|
||||
first = np.asarray(first_xyzw, dtype=float)
|
||||
second = np.asarray(second_xyzw, dtype=float)
|
||||
if first.shape != (4,) or second.shape != (4,):
|
||||
raise ValueError("quaternions must contain four values")
|
||||
first_norm = float(np.linalg.norm(first))
|
||||
second_norm = float(np.linalg.norm(second))
|
||||
if (
|
||||
not np.all(np.isfinite(first))
|
||||
or not np.all(np.isfinite(second))
|
||||
or first_norm <= 0.0
|
||||
or second_norm <= 0.0
|
||||
):
|
||||
raise ValueError("quaternions must be finite and non-zero")
|
||||
# q and -q represent the same rotation. The absolute dot-product gives
|
||||
# the geodesic SO(3) distance without constructing two scipy Rotation
|
||||
# objects for every branch comparison in the live tracker.
|
||||
cosine_half_angle = abs(
|
||||
float(np.dot(first / first_norm, second / second_norm))
|
||||
)
|
||||
return 2.0 * math.acos(float(np.clip(cosine_half_angle, 0.0, 1.0)))
|
||||
|
||||
|
||||
def select_continuous_pose(
|
||||
@@ -775,19 +1009,30 @@ class SquareTagGroupPoseTracker:
|
||||
reprojection_scale_px: float,
|
||||
reprojection_weight: float,
|
||||
reset_after_seconds: float,
|
||||
initialization_frames: int = 1,
|
||||
normal_alignment_pairs: Sequence[tuple[str, str]] = (),
|
||||
normal_alignment_scale_rad: float = math.radians(5.0),
|
||||
maximum_normal_alignment_rad: float | None = None,
|
||||
) -> None:
|
||||
self.roles = tuple(str(role) for role in roles)
|
||||
self.adjacent_pairs = tuple(
|
||||
(str(parent), str(child))
|
||||
for parent, child in adjacent_pairs
|
||||
)
|
||||
self.normal_alignment_pairs = tuple(
|
||||
(str(first), str(second))
|
||||
for first, second in normal_alignment_pairs
|
||||
)
|
||||
if not self.roles or len(set(self.roles)) != len(self.roles):
|
||||
raise ValueError("roles must be non-empty and unique")
|
||||
if any(
|
||||
parent not in self.roles or child not in self.roles
|
||||
for parent, child in self.adjacent_pairs
|
||||
for parent, child in (
|
||||
*self.adjacent_pairs,
|
||||
*self.normal_alignment_pairs,
|
||||
)
|
||||
):
|
||||
raise ValueError("adjacent_pairs must reference roles")
|
||||
raise ValueError("group geometry pairs must reference roles")
|
||||
self.maximum_pose_jump_rad = float(maximum_pose_jump_rad)
|
||||
self.maximum_translation_jump_m = float(
|
||||
maximum_translation_jump_m
|
||||
@@ -800,6 +1045,15 @@ class SquareTagGroupPoseTracker:
|
||||
)
|
||||
self.reprojection_scale_px = float(reprojection_scale_px)
|
||||
self.reprojection_weight = float(reprojection_weight)
|
||||
self.initialization_frames = int(initialization_frames)
|
||||
self.normal_alignment_scale_rad = float(
|
||||
normal_alignment_scale_rad
|
||||
)
|
||||
self.maximum_normal_alignment_rad = (
|
||||
None
|
||||
if maximum_normal_alignment_rad is None
|
||||
else float(maximum_normal_alignment_rad)
|
||||
)
|
||||
reset_seconds = float(reset_after_seconds)
|
||||
if min(
|
||||
self.maximum_pose_jump_rad,
|
||||
@@ -807,19 +1061,35 @@ class SquareTagGroupPoseTracker:
|
||||
self.relative_rotation_scale_rad,
|
||||
self.relative_translation_scale_m,
|
||||
self.reprojection_scale_px,
|
||||
self.normal_alignment_scale_rad,
|
||||
reset_seconds,
|
||||
) <= 0.0:
|
||||
raise ValueError("group tracking scales must be positive")
|
||||
if self.reprojection_weight < 0.0:
|
||||
raise ValueError("reprojection_weight must be non-negative")
|
||||
if self.initialization_frames < 1:
|
||||
raise ValueError("initialization_frames must be positive")
|
||||
if (
|
||||
self.maximum_normal_alignment_rad is not None
|
||||
and self.maximum_normal_alignment_rad <= 0.0
|
||||
):
|
||||
raise ValueError("maximum normal alignment must be positive")
|
||||
self.reset_after_ns = int(reset_seconds * 1_000_000_000)
|
||||
self._previous: dict[str, SquareTagPose] = {}
|
||||
self._previous_stamp_ns: int | None = None
|
||||
self._initial_candidates: list[
|
||||
dict[str, tuple[SquareTagPose, ...]]
|
||||
] = []
|
||||
self._initial_stamps_ns: list[int] = []
|
||||
self.last_initialization_quality: dict[str, float | str] = {}
|
||||
self.branch_correction_counts: dict[str, int] = {}
|
||||
|
||||
def reset(self) -> None:
|
||||
self._previous.clear()
|
||||
self._previous_stamp_ns = None
|
||||
self._initial_candidates.clear()
|
||||
self._initial_stamps_ns.clear()
|
||||
self.last_initialization_quality.clear()
|
||||
self.branch_correction_counts.clear()
|
||||
|
||||
def select(
|
||||
@@ -856,13 +1126,110 @@ class SquareTagGroupPoseTracker:
|
||||
)
|
||||
|
||||
if not previous_is_fresh:
|
||||
selected = min(
|
||||
combinations,
|
||||
key=lambda combination: sum(
|
||||
pose.reprojection_error_px
|
||||
for pose in combination.values()
|
||||
),
|
||||
)
|
||||
if self._previous_stamp_ns is not None:
|
||||
self._previous.clear()
|
||||
self._previous_stamp_ns = None
|
||||
self._initial_candidates.clear()
|
||||
self._initial_stamps_ns.clear()
|
||||
self.last_initialization_quality.clear()
|
||||
if self.initialization_frames > 1:
|
||||
if self._initial_stamps_ns and not (
|
||||
0 <= stamp - self._initial_stamps_ns[-1]
|
||||
<= self.reset_after_ns
|
||||
):
|
||||
self._initial_candidates.clear()
|
||||
self._initial_stamps_ns.clear()
|
||||
self._initial_candidates.append(
|
||||
{
|
||||
role: tuple(candidates_by_role.get(role, ()))
|
||||
for role in self.roles
|
||||
}
|
||||
)
|
||||
self._initial_stamps_ns.append(stamp)
|
||||
if len(self._initial_candidates) < self.initialization_frames:
|
||||
return (
|
||||
None,
|
||||
"group_initializing:"
|
||||
f"{len(self._initial_candidates)}/"
|
||||
f"{self.initialization_frames}",
|
||||
)
|
||||
selected_path, initialization_quality = (
|
||||
select_static_rigid_group_initialization(
|
||||
self._initial_candidates,
|
||||
roles=self.roles,
|
||||
fixed_pairs=self.adjacent_pairs,
|
||||
reprojection_scale_px=self.reprojection_scale_px,
|
||||
maximum_pose_jump_rad=self.maximum_pose_jump_rad,
|
||||
maximum_translation_jump_m=(
|
||||
self.maximum_translation_jump_m
|
||||
),
|
||||
relative_rotation_scale_rad=(
|
||||
self.relative_rotation_scale_rad
|
||||
),
|
||||
relative_translation_scale_m=(
|
||||
self.relative_translation_scale_m
|
||||
),
|
||||
normal_alignment_pairs=(
|
||||
self.normal_alignment_pairs
|
||||
),
|
||||
normal_alignment_scale_rad=(
|
||||
self.normal_alignment_scale_rad
|
||||
),
|
||||
)
|
||||
)
|
||||
selected = selected_path[-1]
|
||||
stamp = self._initial_stamps_ns[-1]
|
||||
self.last_initialization_quality = dict(
|
||||
initialization_quality
|
||||
)
|
||||
self._initial_candidates.clear()
|
||||
self._initial_stamps_ns.clear()
|
||||
if (
|
||||
self.maximum_normal_alignment_rad is not None
|
||||
and float(
|
||||
initialization_quality[
|
||||
"maximum_normal_alignment_rad"
|
||||
]
|
||||
)
|
||||
> self.maximum_normal_alignment_rad
|
||||
):
|
||||
return None, "group_normal_alignment"
|
||||
else:
|
||||
selected = min(
|
||||
combinations,
|
||||
key=lambda combination: (
|
||||
sum(
|
||||
pose.reprojection_error_px
|
||||
for pose in combination.values()
|
||||
)
|
||||
/ self.reprojection_scale_px
|
||||
+ sum(
|
||||
_normal_alignment_rad(
|
||||
combination[first], combination[second]
|
||||
)
|
||||
for first, second in self.normal_alignment_pairs
|
||||
)
|
||||
/ self.normal_alignment_scale_rad
|
||||
),
|
||||
)
|
||||
maximum_alignment = max(
|
||||
(
|
||||
_normal_alignment_rad(
|
||||
selected[first], selected[second]
|
||||
)
|
||||
for first, second in self.normal_alignment_pairs
|
||||
),
|
||||
default=0.0,
|
||||
)
|
||||
self.last_initialization_quality = {
|
||||
"maximum_normal_alignment_rad": maximum_alignment
|
||||
}
|
||||
if (
|
||||
self.maximum_normal_alignment_rad is not None
|
||||
and maximum_alignment
|
||||
> self.maximum_normal_alignment_rad
|
||||
):
|
||||
return None, "group_normal_alignment"
|
||||
else:
|
||||
previous_relative = {
|
||||
pair: _relative_pose(
|
||||
|
||||
@@ -30,6 +30,24 @@ def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
|
||||
os.fsync(stream.fileno())
|
||||
|
||||
|
||||
def append_jsonl_many(
|
||||
path: str | Path, payloads: Iterable[Mapping[str, Any]]
|
||||
) -> None:
|
||||
"""Durably append a batch while paying the fsync cost only once."""
|
||||
destination = Path(path)
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
lines = [
|
||||
json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
||||
for payload in payloads
|
||||
]
|
||||
if not lines:
|
||||
return
|
||||
with destination.open("a", encoding="utf-8") as stream:
|
||||
stream.write("\n".join(lines) + "\n")
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
|
||||
|
||||
def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
|
||||
source = Path(path)
|
||||
if not source.exists():
|
||||
|
||||
+211
-13
@@ -2,13 +2,14 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
STATE_NAMES_ZH = {
|
||||
"PREFLIGHT": "设备和标签预检",
|
||||
"WAIT_START": "等待开始标定",
|
||||
"RETURN_BASELINE": "正在返回基准姿态",
|
||||
"RETURN_BASELINE": "正在恢复目标姿态",
|
||||
"PREPARE_SWEEP": "正在到达扫描起点",
|
||||
"SWEEP": "正在采集轨迹",
|
||||
"FITTING": "正在拟合轨迹和零位",
|
||||
@@ -34,6 +35,18 @@ JOINT_NAMES_ZH = {
|
||||
"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侧摆",
|
||||
}
|
||||
|
||||
@@ -58,6 +71,21 @@ def _task_text(active: Mapping[str, Any]) -> str:
|
||||
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')}，"
|
||||
@@ -71,10 +99,24 @@ def _task_text(active: Mapping[str, Any]) -> str:
|
||||
target = active.get("target_u8")
|
||||
cycle = active.get("cycle", "?")
|
||||
repetitions = active.get("repetitions", "?")
|
||||
return (
|
||||
f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
|
||||
f"第{cycle}/{repetitions}轮，{start}→{target}"
|
||||
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(
|
||||
@@ -92,6 +134,50 @@ def three_camera_reason_zh(
|
||||
)
|
||||
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 (
|
||||
@@ -159,6 +245,16 @@ def three_camera_reason_zh(
|
||||
"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",
|
||||
@@ -171,6 +267,16 @@ def three_camera_reason_zh(
|
||||
"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", []):
|
||||
@@ -207,9 +313,48 @@ def three_camera_reason_zh(
|
||||
"/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以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "call_start":
|
||||
@@ -226,15 +371,15 @@ def three_camera_reason_zh(
|
||||
if reason == "capturing_random_validation_pose":
|
||||
return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。"
|
||||
if reason in {"calibration_passed", "calibration_complete"}:
|
||||
return "轨迹、零位和随机复测已经完成。", "检查结果路径和quality.passed。"
|
||||
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 "正在把已使用的标定电机恢复到统一基准命令。", "无需操作。"
|
||||
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
return "所有扫描已经完成，正在拟合21个关节的轨迹和零位。", "无需操作。"
|
||||
return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
|
||||
return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
|
||||
|
||||
|
||||
@@ -245,21 +390,57 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
reason_zh, action_zh = three_camera_reason_zh(
|
||||
state, str(payload.get("reason", "")), active
|
||||
)
|
||||
service_prefix = str(payload.get("service_prefix", "/g20_calibration"))
|
||||
if service_prefix != "/g20_calibration":
|
||||
reason_zh = reason_zh.replace("/g20_calibration", service_prefix)
|
||||
action_zh = action_zh.replace("/g20_calibration", service_prefix)
|
||||
progress = float(payload.get("progress", 0.0))
|
||||
completed = payload.get("completed_sweeps", 0)
|
||||
total = payload.get("total_sweeps", 0)
|
||||
executed = int(payload.get("executed_sweep_directions", completed))
|
||||
scan_progress = float(
|
||||
payload.get(
|
||||
"scan_progress",
|
||||
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"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，"
|
||||
f"扫描进度{scan_progress:.1%}）",
|
||||
f"当前任务：{_task_text(active)}",
|
||||
]
|
||||
if state == "RETURN_BASELINE":
|
||||
if executed > int(completed):
|
||||
lines.append(
|
||||
f"正在确认基准姿态：{payload.get('baseline_command_u8', [])}"
|
||||
f"实际采集：已启动{executed}个方向（含自动重扫）；"
|
||||
f"计划进度只统计{total}个唯一方向，重扫不会重复增加计划进度"
|
||||
)
|
||||
if active and active.get("kind") != "fit_failure":
|
||||
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 = (
|
||||
@@ -291,6 +472,16 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
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", [])
|
||||
@@ -298,9 +489,16 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
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}"
|
||||
)
|
||||
lines.append(f"结果：{payload.get('result_path') or '尚未生成'}")
|
||||
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)
|
||||
|
||||
+2547
-312
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,36 @@
|
||||
"""Publish calibrated URDF joint angles from raw u8 commands."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
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="g20_thumb_apriltag_calibration",
|
||||
executable="calibrated_joint_state_bridge",
|
||||
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"
|
||||
),
|
||||
"input_topic": LaunchConfiguration("input_topic"),
|
||||
"output_topic": LaunchConfiguration("output_topic"),
|
||||
}
|
||||
],
|
||||
),
|
||||
]
|
||||
)
|
||||
@@ -1,4 +1,4 @@
|
||||
"""Launch three Hikrobot views and one complete-G20 calibration owner."""
|
||||
"""Launch three Hikrobot views and one supported-hand calibration owner."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -21,11 +21,73 @@ 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):
|
||||
hand_model = LaunchConfiguration("hand_model").perform(context).upper()
|
||||
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
|
||||
try:
|
||||
profile = get_hand_calibration_profile(hand_type, hand_model)
|
||||
except ValueError as error:
|
||||
raise RuntimeError(str(error)) from error
|
||||
model_key = hand_model.lower()
|
||||
calibration_namespace = f"/{model_key}_calibration"
|
||||
if hand_model == "O30":
|
||||
command_topic = f"/cb_{hand_type}_hand_control_cmd"
|
||||
state_topic = f"/cb_{hand_type}_hand_state"
|
||||
hand_info_topic = f"/cb_{hand_type}_hand_info"
|
||||
setting_topic = "/cb_hand_setting_cmd"
|
||||
tag_config = LaunchConfiguration("o30_tag_config")
|
||||
else:
|
||||
command_topic = f"/g20/cb_{hand_type}_hand_control_cmd"
|
||||
state_topic = f"/g20/cb_{hand_type}_hand_state"
|
||||
hand_info_topic = f"/g20/cb_{hand_type}_hand_info"
|
||||
setting_topic = "/g20/cb_hand_setting_cmd"
|
||||
tag_config = LaunchConfiguration("tag_config")
|
||||
requested_source = LaunchConfiguration("source_urdf_path").perform(context)
|
||||
source_urdf = (
|
||||
Path(requested_source).expanduser().resolve()
|
||||
if requested_source
|
||||
else _default_source_urdf(hand_model, hand_type).resolve()
|
||||
)
|
||||
if not source_urdf.is_file():
|
||||
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
|
||||
|
||||
hand_serial = LaunchConfiguration("serial_number").perform(context)
|
||||
if (
|
||||
not hand_serial
|
||||
@@ -64,14 +126,14 @@ def _launch_stack(context):
|
||||
info_topics = []
|
||||
detection_topics = []
|
||||
for view in VIEWS:
|
||||
namespace = f"/g20_calibration/{view}/camera"
|
||||
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"/g20_calibration/{view}/apriltag"
|
||||
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(
|
||||
@@ -91,7 +153,9 @@ def _launch_stack(context):
|
||||
"camera_name": LaunchConfiguration(
|
||||
f"{view}_camera_name"
|
||||
),
|
||||
"frame_id": f"g20_calibration_{view}_optical_frame",
|
||||
"frame_id": (
|
||||
f"{model_key}_calibration_{view}_optical_frame"
|
||||
),
|
||||
"image_width": 1624,
|
||||
"image_height": 1240,
|
||||
"frame_rate": ParameterValue(
|
||||
@@ -124,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}],
|
||||
@@ -136,7 +200,7 @@ def _launch_stack(context):
|
||||
name="apriltag",
|
||||
namespace=detector_namespace,
|
||||
parameters=[
|
||||
LaunchConfiguration("tag_config"),
|
||||
tag_config,
|
||||
{
|
||||
"detector.decimate": ParameterValue(
|
||||
LaunchConfiguration("apriltag_decimate"),
|
||||
@@ -146,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}],
|
||||
),
|
||||
@@ -154,7 +218,7 @@ def _launch_stack(context):
|
||||
)
|
||||
|
||||
vision = ComposableNodeContainer(
|
||||
name="g20_three_camera_vision",
|
||||
name=f"{model_key}_three_camera_vision",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
@@ -162,41 +226,103 @@ def _launch_stack(context):
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
)
|
||||
sdk = Node(
|
||||
package="linker_hand_ros2_sdk",
|
||||
executable="linker_hand_sdk",
|
||||
name="linker_hand_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": "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,
|
||||
}
|
||||
],
|
||||
)
|
||||
if hand_model == "O30":
|
||||
sdk = Node(
|
||||
package="linker_hand_o30_ros2_sdk",
|
||||
executable="linker_hand_o30_ros2_sdk",
|
||||
name="linker_hand_o30_ros2_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": hand_type,
|
||||
"hand_joint": "O30",
|
||||
"is_touch": False,
|
||||
"canfd_device": ParameterValue(
|
||||
LaunchConfiguration("canfd_device"), value_type=int
|
||||
),
|
||||
"comm_type": LaunchConfiguration("o30_comm_type"),
|
||||
"channel": LaunchConfiguration("can_interface"),
|
||||
"bitrate": ParameterValue(
|
||||
LaunchConfiguration("o30_bitrate"), value_type=int
|
||||
),
|
||||
"dbitrate": ParameterValue(
|
||||
LaunchConfiguration("o30_dbitrate"), value_type=int
|
||||
),
|
||||
"auto_setup": ParameterValue(
|
||||
LaunchConfiguration("o30_auto_setup"), value_type=bool
|
||||
),
|
||||
}
|
||||
],
|
||||
)
|
||||
else:
|
||||
sdk = Node(
|
||||
package="linker_hand_ros2_sdk",
|
||||
executable="linker_hand_sdk",
|
||||
name="linker_hand_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": hand_type,
|
||||
"hand_joint": "G20",
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": "/g20",
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
),
|
||||
"startup_torque": 80,
|
||||
"state_poll_rate": 30.0,
|
||||
"velocity_poll_rate": 1.0,
|
||||
"defer_state_reads_while_commanding": False,
|
||||
"repeat_position_commands": False,
|
||||
"is_touch": False,
|
||||
}
|
||||
],
|
||||
)
|
||||
calibration = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="three_camera_calibration_node",
|
||||
name="g20_calibration",
|
||||
name=f"{model_key}_calibration",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
LaunchConfiguration("calibration_config"),
|
||||
{
|
||||
"serial_number": hand_serial,
|
||||
"hand_model": hand_model,
|
||||
"hand_type": hand_type,
|
||||
"session_dir": str(session_dir),
|
||||
# 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),
|
||||
"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
|
||||
),
|
||||
@@ -211,6 +337,13 @@ def _launch_stack(context):
|
||||
LaunchConfiguration("index_flex_calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"o30_internal_speed_u8": ParameterValue(
|
||||
LaunchConfiguration("o30_internal_speed_u8"), value_type=int
|
||||
),
|
||||
"o30_command_full_range_seconds": ParameterValue(
|
||||
LaunchConfiguration("o30_command_full_range_seconds"),
|
||||
value_type=float,
|
||||
),
|
||||
"validation_enabled": ParameterValue(
|
||||
LaunchConfiguration("validation_enabled"), value_type=bool
|
||||
),
|
||||
@@ -234,15 +367,20 @@ def _launch_stack(context):
|
||||
*raw_topics,
|
||||
*info_topics,
|
||||
*detection_topics,
|
||||
"/g20/cb_left_hand_control_cmd",
|
||||
"/g20/cb_left_hand_state",
|
||||
"/g20/cb_left_hand_info",
|
||||
"/g20_calibration/status",
|
||||
command_topic,
|
||||
state_topic,
|
||||
hand_info_topic,
|
||||
f"{calibration_namespace}/status",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
return [
|
||||
LogInfo(msg=f"G20 three-camera session: {session_dir}"),
|
||||
LogInfo(
|
||||
msg=(
|
||||
f"{hand_model} {hand_type} three-camera session: {session_dir}; "
|
||||
f"source_urdf={source_urdf}"
|
||||
)
|
||||
),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"Camera mapping: front="
|
||||
@@ -265,10 +403,23 @@ def generate_launch_description() -> LaunchDescription:
|
||||
info_root = Path.home() / ".ros" / "camera_info"
|
||||
return LaunchDescription(
|
||||
[
|
||||
# Camera processes publish ~2 MB frames across DDS. Force the
|
||||
# matching RMW and provide both current and legacy profile names
|
||||
# so the configured 64 MB shared-memory segment is actually used.
|
||||
SetEnvironmentVariable(
|
||||
name="RMW_IMPLEMENTATION",
|
||||
value="rmw_fastrtps_cpp",
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTDDS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
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"
|
||||
@@ -307,6 +458,11 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
|
||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||
DeclareLaunchArgument("canfd_device", default_value="0"),
|
||||
DeclareLaunchArgument("o30_comm_type", default_value="socketcan"),
|
||||
DeclareLaunchArgument("o30_bitrate", default_value="1000000"),
|
||||
DeclareLaunchArgument("o30_dbitrate", default_value="5000000"),
|
||||
DeclareLaunchArgument("o30_auto_setup", default_value="true"),
|
||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||
DeclareLaunchArgument(
|
||||
"index_roll_calibration_speed", default_value="5"
|
||||
@@ -314,7 +470,23 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument(
|
||||
"index_flex_calibration_speed", default_value="10"
|
||||
),
|
||||
DeclareLaunchArgument("o30_internal_speed_u8", default_value="0"),
|
||||
DeclareLaunchArgument(
|
||||
"o30_command_full_range_seconds", default_value="6.0"
|
||||
),
|
||||
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(
|
||||
"corrected_urdf_output_dir", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
||||
DeclareLaunchArgument("start_cameras", default_value="true"),
|
||||
DeclareLaunchArgument("start_sdk", default_value="true"),
|
||||
@@ -336,6 +508,12 @@ def generate_launch_description() -> LaunchDescription:
|
||||
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),
|
||||
]
|
||||
)
|
||||
|
||||
@@ -0,0 +1,199 @@
|
||||
"""Launch three Hikrobot cameras for one-time checkerboard extrinsics."""
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
|
||||
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
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
def _launch(context):
|
||||
cameras = []
|
||||
rectifiers = []
|
||||
serials = {}
|
||||
for view in VIEWS:
|
||||
serial = LaunchConfiguration(f"{view}_camera_serial").perform(context)
|
||||
if not serial:
|
||||
raise RuntimeError(f"{view}_camera_serial is required")
|
||||
serials[view] = serial
|
||||
namespace = f"/g20_extrinsics/{view}/camera"
|
||||
cameras.append(
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=namespace,
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": serial,
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration(
|
||||
f"{view}_camera_name"
|
||||
),
|
||||
"frame_id": f"g20_extrinsics_{view}_optical_frame",
|
||||
"image_width": 1624,
|
||||
"image_height": 1240,
|
||||
"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": False,
|
||||
"camera_info_url": LaunchConfiguration(
|
||||
f"{view}_camera_info_url"
|
||||
),
|
||||
}
|
||||
],
|
||||
)
|
||||
)
|
||||
rectifiers.append(
|
||||
ComposableNode(
|
||||
package="image_proc",
|
||||
plugin="image_proc::RectifyNode",
|
||||
name=f"rectify_{view}",
|
||||
namespace=namespace,
|
||||
remappings=[
|
||||
("image", f"{namespace}/image_raw"),
|
||||
("camera_info", f"{namespace}/camera_info"),
|
||||
("image_rect", f"{namespace}/image_rect"),
|
||||
],
|
||||
parameters=[{"queue_size": 1}],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
)
|
||||
)
|
||||
container = ComposableNodeContainer(
|
||||
name="g20_extrinsics_vision",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
composable_node_descriptions=rectifiers,
|
||||
output="screen",
|
||||
)
|
||||
solver = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="three_camera_extrinsics_node",
|
||||
name="g20_camera_extrinsics",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"output_file": LaunchConfiguration("output_file"),
|
||||
"checkerboard_columns": ParameterValue(
|
||||
LaunchConfiguration("checkerboard_columns"), value_type=int
|
||||
),
|
||||
"checkerboard_rows": ParameterValue(
|
||||
LaunchConfiguration("checkerboard_rows"), value_type=int
|
||||
),
|
||||
"square_size_m": ParameterValue(
|
||||
LaunchConfiguration("square_size_m"), value_type=float
|
||||
),
|
||||
"enable_gui": ParameterValue(
|
||||
LaunchConfiguration("enable_gui"), value_type=bool
|
||||
),
|
||||
"gui_refresh_hz": ParameterValue(
|
||||
LaunchConfiguration("gui_refresh_hz"), value_type=float
|
||||
),
|
||||
"maximum_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration("maximum_reprojection_rms_px"),
|
||||
value_type=float,
|
||||
),
|
||||
"maximum_single_camera_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration(
|
||||
"maximum_single_camera_reprojection_rms_px"
|
||||
),
|
||||
value_type=float,
|
||||
),
|
||||
"auto_capture_default": ParameterValue(
|
||||
LaunchConfiguration("auto_capture_default"),
|
||||
value_type=bool,
|
||||
),
|
||||
"auto_capture_stable_seconds": ParameterValue(
|
||||
LaunchConfiguration("auto_capture_stable_seconds"),
|
||||
value_type=float,
|
||||
),
|
||||
**{
|
||||
f"{view}_camera_serial": serials[view]
|
||||
for view in VIEWS
|
||||
},
|
||||
}
|
||||
],
|
||||
)
|
||||
return [*cameras, container, solver]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
camera_info = Path.home() / ".ros" / "camera_info"
|
||||
return LaunchDescription(
|
||||
[
|
||||
# Keep the large-image transport deterministic even when the
|
||||
# calling shell selected another ROS 2 RMW implementation.
|
||||
SetEnvironmentVariable(
|
||||
name="RMW_IMPLEMENTATION",
|
||||
value="rmw_fastrtps_cpp",
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTDDS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
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(camera_info / "hikrobot_DB2163742.yaml")),
|
||||
DeclareLaunchArgument("side_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163749.yaml")),
|
||||
DeclareLaunchArgument("top_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163739.yaml")),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("checkerboard_columns", default_value="8"),
|
||||
DeclareLaunchArgument("checkerboard_rows", default_value="5"),
|
||||
DeclareLaunchArgument("square_size_m", default_value="0.027"),
|
||||
DeclareLaunchArgument("enable_gui", default_value="true"),
|
||||
DeclareLaunchArgument("gui_refresh_hz", default_value="2.0"),
|
||||
DeclareLaunchArgument(
|
||||
"maximum_reprojection_rms_px", default_value="1.2"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"maximum_single_camera_reprojection_rms_px",
|
||||
default_value="1.5",
|
||||
),
|
||||
DeclareLaunchArgument("auto_capture_default", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"auto_capture_stable_seconds", default_value="1.0"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"output_file",
|
||||
default_value=str(Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"),
|
||||
),
|
||||
OpaqueFunction(function=_launch),
|
||||
]
|
||||
)
|
||||
@@ -3,7 +3,7 @@
|
||||
<package format="3">
|
||||
<name>g20_thumb_apriltag_calibration</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Three-view Hikrobot AprilTag calibration for the complete left G20 hand.</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>
|
||||
|
||||
@@ -26,7 +26,7 @@ setup(
|
||||
zip_safe=True,
|
||||
maintainer="lxp",
|
||||
maintainer_email="support@linker-robotics.com",
|
||||
description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand",
|
||||
description="Three-view AprilTag calibration for G20 and O30 hands",
|
||||
license="MIT",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
@@ -47,10 +47,23 @@ setup(
|
||||
"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"
|
||||
),
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
import copy
|
||||
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
|
||||
G20_COMMAND_NAMES,
|
||||
G20_URDF_JOINT_NAMES,
|
||||
O30_COMMAND_NAMES,
|
||||
O30_URDF_JOINT_NAMES,
|
||||
CalibratedCommandMapper,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
JointCurveFit,
|
||||
build_compact_payload,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
def _payload(side: str = "right") -> dict:
|
||||
profile = get_hand_calibration_profile(side)
|
||||
curve = [round((255 - command) * 0.001, 8) for command in range(256)]
|
||||
joints = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
joint = {
|
||||
"motor_index": spec.motor_index,
|
||||
"angle_rad": list(curve),
|
||||
}
|
||||
if spec.active:
|
||||
joint["zero_command_u8"] = 255
|
||||
joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0}
|
||||
else:
|
||||
joint["passive"] = True
|
||||
if spec.source_joint is not None:
|
||||
joint["source_joint"] = spec.source_joint
|
||||
joints[name] = joint
|
||||
return {
|
||||
"schema_version": 4,
|
||||
"model": "G20",
|
||||
"side": side,
|
||||
"serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT",
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": [255] * 20,
|
||||
"joints": joints,
|
||||
"quality": {
|
||||
"passed": True,
|
||||
"validation_mae_rad": 0.01,
|
||||
"validation_p95_rad": 0.02,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
command = list(range(20))
|
||||
result = dict(
|
||||
zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))
|
||||
)
|
||||
|
||||
assert result["thumb_cmc_pitch"] == pytest.approx(0.255)
|
||||
assert result["thumb_cmc_roll"] == pytest.approx(0.250)
|
||||
assert result["thumb_cmc_yaw"] == pytest.approx(0.245)
|
||||
assert result["thumb_mcp"] == pytest.approx(0.240)
|
||||
assert result["thumb_ip"] == pytest.approx(0.240)
|
||||
assert result["pinky_pip"] == pytest.approx(0.236)
|
||||
assert result["pinky_dip"] == pytest.approx(0.236)
|
||||
|
||||
|
||||
def test_mapper_uses_names_instead_of_message_order() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
command = list(range(20))
|
||||
names = list(reversed(G20_COMMAND_NAMES))
|
||||
positions = list(reversed(command))
|
||||
|
||||
assert mapper.map_positions(positions, names) == mapper.map_positions(command)
|
||||
|
||||
|
||||
def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None:
|
||||
with pytest.raises(ValueError, match="does not match"):
|
||||
CalibratedCommandMapper(_payload("left"), expected_side="right")
|
||||
|
||||
payload = copy.deepcopy(_payload())
|
||||
payload["quality"]["passed"] = False
|
||||
with pytest.raises(ValueError, match="quality.passed"):
|
||||
CalibratedCommandMapper(payload, expected_side="right")
|
||||
|
||||
|
||||
def test_mapper_rejects_incomplete_named_command() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
with pytest.raises(ValueError, match="missing named channels"):
|
||||
mapper.map_positions([255.0], ["thumb_cmc_pitch"])
|
||||
|
||||
|
||||
def test_o30_mapper_uses_sdk_names_and_twenty_active_urdf_joints() -> None:
|
||||
profile = get_hand_calibration_profile("right", "O30")
|
||||
|
||||
def fit_for(name: str) -> JointCurveFit:
|
||||
zero = profile.baseline_command[profile.joint_specs[name].motor_index]
|
||||
curve = tuple((command - zero) * 0.001 for command in range(256))
|
||||
return JointCurveFit(
|
||||
angle_rad=curve,
|
||||
decreasing_rad=curve,
|
||||
increasing_rad=curve,
|
||||
circle={},
|
||||
maximum_monotonic_correction_rad=0.0,
|
||||
maximum_hysteresis_rad=0.0,
|
||||
quality={},
|
||||
)
|
||||
|
||||
payload = build_compact_payload(
|
||||
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,
|
||||
side="right",
|
||||
model="O30",
|
||||
)
|
||||
mapper = CalibratedCommandMapper(payload, expected_side="right")
|
||||
|
||||
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)
|
||||
@@ -25,6 +25,18 @@ def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None:
|
||||
assert participant.attrib["is_default_profile"] == "true"
|
||||
|
||||
|
||||
def test_three_camera_launches_force_fastdds_large_image_transport() -> None:
|
||||
for launch_name in (
|
||||
"three_camera_calibration.launch.py",
|
||||
"three_camera_extrinsics.launch.py",
|
||||
):
|
||||
launch_text = (PACKAGE_ROOT / "launch" / launch_name).read_text()
|
||||
assert 'name="RMW_IMPLEMENTATION"' in launch_text
|
||||
assert 'value="rmw_fastrtps_cpp"' in launch_text
|
||||
assert 'name="FASTDDS_DEFAULT_PROFILES_FILE"' in launch_text
|
||||
assert 'name="FASTRTPS_DEFAULT_PROFILES_FILE"' in launch_text
|
||||
|
||||
|
||||
def test_front_tag_parameters_match_namespaced_detector() -> None:
|
||||
config = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "front_tags.yaml").read_text()
|
||||
@@ -43,31 +55,53 @@ def test_front_tag_parameters_match_namespaced_detector() -> None:
|
||||
assert detector["detector"]["debug"] is False
|
||||
|
||||
|
||||
def test_three_camera_tag_ids_and_topics_are_disjoint() -> None:
|
||||
tags = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text()
|
||||
)
|
||||
def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None:
|
||||
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["qos_profile"] == "sensor_data"
|
||||
assert parameters["detector"]["decimate"] == 1.5
|
||||
all_ids.update(ids)
|
||||
assert all_ids == set(range(11))
|
||||
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_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["baseline_command_u8"] == [
|
||||
255,
|
||||
@@ -95,17 +129,51 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
assert parameters["normal_calibration_speed"] == 15
|
||||
assert parameters["index_roll_calibration_speed"] == 5
|
||||
assert parameters["index_flex_calibration_speed"] == 10
|
||||
assert parameters["o30_internal_speed_u8"] == 0
|
||||
assert parameters["o30_command_full_range_seconds"] == 6.0
|
||||
assert parameters["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["repetitions"] == 3
|
||||
assert parameters["validation_enabled"] is False
|
||||
assert parameters["minimum_detection_rate"] == 0.95
|
||||
assert parameters["minimum_detection_hz"] == 15.0
|
||||
assert parameters["minimum_state_span_u8"] >= 240.0
|
||||
assert parameters["endpoint_tolerance_u8"] == 2.0
|
||||
assert parameters["o30_endpoint_tolerance_u8"] == 4.0
|
||||
assert (
|
||||
parameters["o30_thumb_cmc_roll_255_endpoint_tolerance_u8"] == 9.0
|
||||
)
|
||||
assert (
|
||||
parameters["o30_index_mcp_roll_255_endpoint_tolerance_u8"] == 8.0
|
||||
)
|
||||
assert parameters["o30_thumb_mcp_zero_endpoint_tolerance_u8"] == 8.0
|
||||
assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0
|
||||
assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
|
||||
assert parameters["minimum_sweep_bins"] >= 32
|
||||
assert parameters["maximum_bin_gap"] <= 16
|
||||
assert parameters["automatic_sweep_retry_limit"] == 3
|
||||
assert parameters["automatic_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"] == 2
|
||||
assert parameters["provisional_warning_ratio"] == 1.25
|
||||
assert parameters["retry_speed_scales"] == [0.8, 0.6, 0.5]
|
||||
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25]
|
||||
assert parameters["position_timeout_seconds"] >= 20.0
|
||||
assert parameters["zero_maximum_round_difference_deg"] <= 1.0
|
||||
assert parameters["maximum_state_image_skew_ms"] <= 50.0
|
||||
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
|
||||
assert parameters["axis_maximum_pose_line_rms_m"] <= 0.001
|
||||
assert parameters["passive_axis_maximum_plane_rms_m"] == 0.004
|
||||
assert parameters["active_maximum_rotation_orthogonal_rms_deg"] == 2.5
|
||||
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_offset_deg"] <= 20.0
|
||||
assert parameters["zero_finger_maximum_offset_deg"] <= 3.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
|
||||
|
||||
@@ -0,0 +1,326 @@
|
||||
"""Focused tests for checkerboard frame pairing."""
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
import g20_thumb_apriltag_calibration.extrinsics_node as extrinsics_node
|
||||
from g20_thumb_apriltag_calibration.extrinsics_node import (
|
||||
BoardPose,
|
||||
StereoCapture,
|
||||
_fit_stereo_robust,
|
||||
_individual_reprojection_passes,
|
||||
_minimum_history_skew_ns,
|
||||
_pose_is_novel,
|
||||
_select_latest_synchronised_pair,
|
||||
_summarize_transform_repeatability,
|
||||
)
|
||||
|
||||
|
||||
def _pose(stamp_ns: int) -> BoardPose:
|
||||
return BoardPose(
|
||||
stamp_ns=stamp_ns,
|
||||
camera_from_board_candidates=(),
|
||||
reprojection_rms_px=0.1,
|
||||
)
|
||||
|
||||
|
||||
def test_pairing_uses_recent_history_instead_of_mismatched_latest_frames():
|
||||
front = [_pose(1_000_000_000), _pose(1_200_000_000)]
|
||||
side = [_pose(1_030_000_000), _pose(1_370_000_000)]
|
||||
|
||||
pair = _select_latest_synchronised_pair(front, side, 100_000_000)
|
||||
|
||||
assert pair is not None
|
||||
selected_front, selected_side, skew = pair
|
||||
assert selected_front.stamp_ns == 1_000_000_000
|
||||
assert selected_side.stamp_ns == 1_030_000_000
|
||||
assert skew == 30_000_000
|
||||
|
||||
|
||||
def test_pairing_prefers_newest_valid_common_pair():
|
||||
front = [_pose(1_000_000_000), _pose(1_200_000_000)]
|
||||
side = [_pose(1_010_000_000), _pose(1_240_000_000)]
|
||||
|
||||
pair = _select_latest_synchronised_pair(front, side, 100_000_000)
|
||||
|
||||
assert pair is not None
|
||||
selected_front, selected_side, skew = pair
|
||||
assert selected_front.stamp_ns == 1_200_000_000
|
||||
assert selected_side.stamp_ns == 1_240_000_000
|
||||
assert skew == 40_000_000
|
||||
|
||||
|
||||
def test_pairing_reports_minimum_skew_when_no_pair_passes():
|
||||
front = [_pose(1_000_000_000), _pose(1_200_000_000)]
|
||||
side = [_pose(1_370_000_000)]
|
||||
|
||||
assert _select_latest_synchronised_pair(
|
||||
front, side, 100_000_000
|
||||
) is None
|
||||
assert _minimum_history_skew_ns(front, side) == 170_000_000
|
||||
|
||||
|
||||
def _transform(rotation_deg: float, translation_m: float) -> np.ndarray:
|
||||
value = np.eye(4)
|
||||
value[:3, :3] = Rotation.from_euler(
|
||||
"z", rotation_deg, degrees=True
|
||||
).as_matrix()
|
||||
value[0, 3] = translation_m
|
||||
return value
|
||||
|
||||
|
||||
def test_repeatability_summary_accepts_consistent_capture_set():
|
||||
captures = [
|
||||
[_transform(-0.1, -0.0005)],
|
||||
[_transform(0.0, 0.0)],
|
||||
[_transform(0.1, 0.0005)],
|
||||
]
|
||||
|
||||
_, selected, rotation_error, translation_error = (
|
||||
_summarize_transform_repeatability(captures)
|
||||
)
|
||||
|
||||
assert len(selected) == 3
|
||||
np.testing.assert_allclose(
|
||||
np.degrees(rotation_error), 0.1, atol=1.0e-6
|
||||
)
|
||||
np.testing.assert_allclose(translation_error, 0.0005, atol=1.0e-9)
|
||||
|
||||
|
||||
def test_repeatability_summary_exposes_current_capture_outlier():
|
||||
captures = [
|
||||
[_transform(0.0, 0.0)],
|
||||
[_transform(0.1, 0.0005)],
|
||||
[_transform(1.0, 0.008)],
|
||||
]
|
||||
|
||||
_, _, rotation_error, translation_error = (
|
||||
_summarize_transform_repeatability(captures)
|
||||
)
|
||||
|
||||
assert np.degrees(rotation_error) > 0.3
|
||||
assert translation_error > 0.0015
|
||||
|
||||
|
||||
def test_each_camera_must_pass_reprojection_gate_independently():
|
||||
assert _individual_reprojection_passes(0.8, 1.1, 1.2)
|
||||
assert not _individual_reprojection_passes(0.5, 1.4, 1.2)
|
||||
assert _individual_reprojection_passes(0.5, 1.4, 1.5)
|
||||
assert not _individual_reprojection_passes(0.5, 1.6, 1.5)
|
||||
assert not _individual_reprojection_passes(float("nan"), 0.5, 1.2)
|
||||
|
||||
|
||||
def test_pose_novelty_is_checked_against_all_previous_captures():
|
||||
previous = [_transform(0.0, 0.0), _transform(10.0, 0.10)]
|
||||
|
||||
assert not _pose_is_novel(
|
||||
_transform(10.5, 0.105),
|
||||
previous,
|
||||
minimum_rotation_rad=np.deg2rad(2.0),
|
||||
minimum_translation_m=0.015,
|
||||
)
|
||||
assert _pose_is_novel(
|
||||
_transform(14.0, 0.13),
|
||||
previous,
|
||||
minimum_rotation_rad=np.deg2rad(2.0),
|
||||
minimum_translation_m=0.015,
|
||||
)
|
||||
|
||||
|
||||
def test_joint_fit_trims_moderate_bad_views_without_relaxing_gate(
|
||||
monkeypatch,
|
||||
):
|
||||
captures = [
|
||||
StereoCapture(
|
||||
front_points_px=np.zeros((40, 2), dtype=np.float32),
|
||||
other_points_px=np.zeros((40, 2), dtype=np.float32),
|
||||
transform_candidates=(np.eye(4),),
|
||||
pair_reprojection_rms_px=1.0,
|
||||
skew_ns=1,
|
||||
)
|
||||
for _ in range(20)
|
||||
]
|
||||
|
||||
def fake_fit_once(
|
||||
captures_arg,
|
||||
indices,
|
||||
object_points,
|
||||
front_matrix,
|
||||
other_matrix,
|
||||
image_size,
|
||||
):
|
||||
del (
|
||||
captures_arg,
|
||||
object_points,
|
||||
front_matrix,
|
||||
other_matrix,
|
||||
image_size,
|
||||
)
|
||||
errors = np.asarray(
|
||||
[1.1 if index < 15 else 1.5 for index in indices],
|
||||
dtype=float,
|
||||
)
|
||||
return float(np.sqrt(np.mean(np.square(errors)))), np.eye(4), errors
|
||||
|
||||
monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once)
|
||||
result = _fit_stereo_robust(
|
||||
captures,
|
||||
np.zeros((40, 3), dtype=np.float32),
|
||||
np.eye(3),
|
||||
np.eye(3),
|
||||
(640, 480),
|
||||
minimum_inliers=15,
|
||||
maximum_rms_px=1.2,
|
||||
maximum_rotation_stability_rad=np.deg2rad(0.3),
|
||||
maximum_translation_stability_m=0.0015,
|
||||
)
|
||||
|
||||
assert result.passed
|
||||
assert result.stereo_rms_px <= 1.2
|
||||
assert len(result.inlier_indices) >= 15
|
||||
assert result.rejected_indices
|
||||
assert set(result.rejected_indices) <= set(range(15, 20))
|
||||
|
||||
|
||||
def test_joint_fit_reports_finite_provisional_metrics_before_minimum(
|
||||
monkeypatch,
|
||||
):
|
||||
captures = [
|
||||
StereoCapture(
|
||||
front_points_px=np.zeros((40, 2), dtype=np.float32),
|
||||
other_points_px=np.zeros((40, 2), dtype=np.float32),
|
||||
transform_candidates=(np.eye(4),),
|
||||
pair_reprojection_rms_px=0.8,
|
||||
skew_ns=1,
|
||||
)
|
||||
for _ in range(10)
|
||||
]
|
||||
|
||||
def fake_fit_once(
|
||||
captures_arg,
|
||||
indices,
|
||||
object_points,
|
||||
front_matrix,
|
||||
other_matrix,
|
||||
image_size,
|
||||
):
|
||||
del (
|
||||
captures_arg,
|
||||
object_points,
|
||||
front_matrix,
|
||||
other_matrix,
|
||||
image_size,
|
||||
)
|
||||
return 0.8, np.eye(4), np.full(len(indices), 0.8)
|
||||
|
||||
monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once)
|
||||
result = _fit_stereo_robust(
|
||||
captures,
|
||||
np.zeros((40, 3), dtype=np.float32),
|
||||
np.eye(3),
|
||||
np.eye(3),
|
||||
(640, 480),
|
||||
minimum_inliers=15,
|
||||
maximum_rms_px=1.2,
|
||||
maximum_rotation_stability_rad=np.deg2rad(0.3),
|
||||
maximum_translation_stability_m=0.0015,
|
||||
)
|
||||
|
||||
assert not result.passed
|
||||
assert result.stereo_rms_px == 0.8
|
||||
assert len(result.inlier_indices) == 10
|
||||
|
||||
|
||||
def test_joint_stereo_fit_recovers_transform_and_rejects_bad_view():
|
||||
random = np.random.default_rng(7)
|
||||
object_points = np.zeros((40, 3), dtype=np.float32)
|
||||
object_points[:, :2] = (
|
||||
np.mgrid[0:8, 0:5].T.reshape(-1, 2) * 0.027
|
||||
)
|
||||
matrix = np.asarray(
|
||||
[[1800.0, 0.0, 812.0], [0.0, 1795.0, 620.0], [0.0, 0.0, 1.0]]
|
||||
)
|
||||
other_from_front = np.eye(4)
|
||||
other_from_front[:3, :3] = Rotation.from_euler(
|
||||
"xyz", [2.0, 18.0, -1.0], degrees=True
|
||||
).as_matrix()
|
||||
other_from_front[:3, 3] = [0.20, -0.01, 0.04]
|
||||
expected_front_from_other = np.linalg.inv(other_from_front)
|
||||
captures = []
|
||||
for index in range(21):
|
||||
front_from_board = np.eye(4)
|
||||
front_from_board[:3, :3] = Rotation.from_euler(
|
||||
"xyz",
|
||||
[
|
||||
-8.0 + index * 0.7,
|
||||
5.0 + (index % 5) * 2.0,
|
||||
-5.0 + (index % 4) * 3.0,
|
||||
],
|
||||
degrees=True,
|
||||
).as_matrix()
|
||||
front_from_board[:3, 3] = [
|
||||
-0.08 + (index % 5) * 0.035,
|
||||
-0.04 + (index % 4) * 0.025,
|
||||
0.75 + (index % 3) * 0.08,
|
||||
]
|
||||
other_from_board = other_from_front @ front_from_board
|
||||
front_rvec = Rotation.from_matrix(
|
||||
front_from_board[:3, :3]
|
||||
).as_rotvec()
|
||||
other_rvec = Rotation.from_matrix(
|
||||
other_from_board[:3, :3]
|
||||
).as_rotvec()
|
||||
front_points, _ = cv2.projectPoints(
|
||||
object_points,
|
||||
front_rvec,
|
||||
front_from_board[:3, 3],
|
||||
matrix,
|
||||
np.zeros(5),
|
||||
)
|
||||
other_points, _ = cv2.projectPoints(
|
||||
object_points,
|
||||
other_rvec,
|
||||
other_from_board[:3, 3],
|
||||
matrix,
|
||||
np.zeros(5),
|
||||
)
|
||||
front_points = front_points.reshape(-1, 2)
|
||||
other_points = other_points.reshape(-1, 2)
|
||||
front_points += random.normal(0.0, 0.12, front_points.shape)
|
||||
other_points += random.normal(0.0, 0.12, other_points.shape)
|
||||
if index == 20:
|
||||
other_points += random.normal(0.0, 4.0, other_points.shape)
|
||||
captures.append(
|
||||
StereoCapture(
|
||||
front_points_px=front_points.astype(np.float32),
|
||||
other_points_px=other_points.astype(np.float32),
|
||||
transform_candidates=(expected_front_from_other.copy(),),
|
||||
pair_reprojection_rms_px=0.2,
|
||||
skew_ns=10_000_000,
|
||||
)
|
||||
)
|
||||
|
||||
result = _fit_stereo_robust(
|
||||
captures,
|
||||
object_points,
|
||||
matrix,
|
||||
matrix,
|
||||
(1624, 1240),
|
||||
minimum_inliers=15,
|
||||
maximum_rms_px=1.2,
|
||||
maximum_rotation_stability_rad=np.deg2rad(0.3),
|
||||
maximum_translation_stability_m=0.0015,
|
||||
)
|
||||
|
||||
assert result.passed
|
||||
assert 20 in result.rejected_indices
|
||||
assert len(result.inlier_indices) >= 15
|
||||
rotation_error = Rotation.from_matrix(
|
||||
result.front_from_other[:3, :3]
|
||||
).inv() * Rotation.from_matrix(expected_front_from_other[:3, :3])
|
||||
assert np.degrees(rotation_error.magnitude()) < 0.05
|
||||
assert np.linalg.norm(
|
||||
result.front_from_other[:3, 3]
|
||||
- expected_front_from_other[:3, 3]
|
||||
) < 0.001
|
||||
@@ -1,4 +1,5 @@
|
||||
import math
|
||||
from dataclasses import replace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
@@ -7,8 +8,10 @@ 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,
|
||||
@@ -21,6 +24,7 @@ from g20_thumb_apriltag_calibration.full_hand import (
|
||||
fit_joint_image_curve,
|
||||
fit_measured_joint_curve,
|
||||
fit_projected_zero,
|
||||
get_hand_calibration_profile,
|
||||
measure_joint_observation,
|
||||
validate_compact_payload,
|
||||
)
|
||||
@@ -69,6 +73,55 @@ 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)
|
||||
)
|
||||
@@ -121,6 +174,20 @@ def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
|
||||
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]
|
||||
@@ -132,6 +199,33 @@ def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
|
||||
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]
|
||||
@@ -257,24 +351,28 @@ def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None:
|
||||
|
||||
def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, midpoint = center_splay_curve(base)
|
||||
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
|
||||
}
|
||||
projected = {
|
||||
name: 0.01 * index
|
||||
for index, (name, spec) in enumerate(JOINT_SPECS.items())
|
||||
if spec.zero_kind == "projected"
|
||||
}
|
||||
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,
|
||||
projected_zeros_rad=projected,
|
||||
splay_zero_command_u8=zero_command,
|
||||
splay_midpoint_rad=midpoint,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[0.01, -0.02],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
)
|
||||
validate_compact_payload(payload)
|
||||
assert set(payload) == {
|
||||
@@ -298,7 +396,7 @@ def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
|
||||
== payload["joints"]["index_mcp_roll"]["angle_rad"]
|
||||
)
|
||||
assert payload["joints"]["index_mcp_roll"]["zero_angles"] == {
|
||||
"travel_midpoint_rad": pytest.approx(0.35, abs=1.0e-4)
|
||||
"urdf_zero_offset_rad": pytest.approx(0.01)
|
||||
}
|
||||
for name in SPLAY_JOINTS:
|
||||
assert payload["joints"][name]["zero_command_u8"] == zero_command
|
||||
@@ -306,25 +404,29 @@ def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
|
||||
|
||||
def test_compact_payload_allows_skipped_random_validation() -> None:
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, midpoint = center_splay_curve(base)
|
||||
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
|
||||
}
|
||||
projected = {
|
||||
name: 0.0
|
||||
for name, spec in JOINT_SPECS.items()
|
||||
if spec.zero_kind == "projected"
|
||||
}
|
||||
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,
|
||||
projected_zeros_rad=projected,
|
||||
splay_zero_command_u8=zero_command,
|
||||
splay_midpoint_rad=midpoint,
|
||||
urdf_zero_offsets_rad=offsets,
|
||||
validation_errors_rad=[],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
)
|
||||
|
||||
validate_compact_payload(payload)
|
||||
@@ -333,3 +435,66 @@ def test_compact_payload_allows_skipped_random_validation() -> None:
|
||||
"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}
|
||||
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -324,6 +324,125 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
|
||||
assert selected == {"t4": return_t4, "t5": return_t5}
|
||||
|
||||
|
||||
def test_group_tracker_initializes_from_multiple_static_frames() -> 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)
|
||||
true_child = _pose(20.0, 0.03, 0.10)
|
||||
|
||||
for index in range(7):
|
||||
false_child = _pose(5.0 * index, 0.03, 0.05)
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (false_child, true_child),
|
||||
},
|
||||
stamp_ns=1_000_000_000 + index * 33_000_000,
|
||||
)
|
||||
assert selected is None
|
||||
assert reason == f"group_initializing:{index + 1}/8"
|
||||
|
||||
false_child = _pose(35.0, 0.03, 0.05)
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (false_child, true_child),
|
||||
},
|
||||
stamp_ns=1_231_000_000,
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": true_child}
|
||||
assert (
|
||||
tracker.last_initialization_quality["initialization_search"]
|
||||
== "static_reference"
|
||||
)
|
||||
assert tracker.last_initialization_quality[
|
||||
"p95_pair_rotation_drift_rad"
|
||||
] < np.deg2rad(1.0)
|
||||
|
||||
|
||||
def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("mcp", "pip", "dip"),
|
||||
adjacent_pairs=(("mcp", "pip"), ("pip", "dip")),
|
||||
normal_alignment_pairs=(("mcp", "pip"), ("pip", "dip")),
|
||||
normal_alignment_scale_rad=np.deg2rad(5.0),
|
||||
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,
|
||||
maximum_normal_alignment_rad=np.deg2rad(15.0),
|
||||
)
|
||||
mcp = _pose(12.0, 0.00, 0.05)
|
||||
correct_pip = _pose(14.0, 0.03, 0.10)
|
||||
mirror_pip = _pose(50.0, 0.03, 0.05)
|
||||
dip = _pose(15.0, 0.06, 0.05)
|
||||
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"mcp": (mcp,),
|
||||
"pip": (mirror_pip, correct_pip),
|
||||
"dip": (dip,),
|
||||
},
|
||||
stamp_ns=1_000_000_000 + index * 33_000_000,
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected == {
|
||||
"mcp": mcp,
|
||||
"pip": correct_pip,
|
||||
"dip": dip,
|
||||
}
|
||||
assert tracker.last_initialization_quality[
|
||||
"maximum_normal_alignment_rad"
|
||||
] < np.deg2rad(3.0)
|
||||
|
||||
|
||||
def test_static_group_rejects_when_no_aligned_branch_exists() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("base", "moving"),
|
||||
adjacent_pairs=(("base", "moving"),),
|
||||
normal_alignment_pairs=(("base", "moving"),),
|
||||
normal_alignment_scale_rad=np.deg2rad(5.0),
|
||||
maximum_normal_alignment_rad=np.deg2rad(15.0),
|
||||
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,
|
||||
)
|
||||
base = _pose(0.0, 0.00, 0.05)
|
||||
mirror = _pose(38.0, 0.03, 0.05)
|
||||
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{"base": (base,), "moving": (mirror,)},
|
||||
stamp_ns=1_000_000_000 + index * 33_000_000,
|
||||
)
|
||||
|
||||
assert selected is None
|
||||
assert reason == "group_normal_alignment"
|
||||
|
||||
|
||||
def test_whole_trajectory_recovers_rigid_group_from_mirror_drift() -> None:
|
||||
roles = ("t3", "t4", "t5")
|
||||
mount_rotations = {
|
||||
|
||||
@@ -3,7 +3,9 @@ import json
|
||||
import numpy as np
|
||||
|
||||
from g20_thumb_apriltag_calibration.three_camera_diagnostics import (
|
||||
_task_text,
|
||||
render_three_camera_status_text_zh,
|
||||
three_camera_reason_zh,
|
||||
)
|
||||
|
||||
|
||||
@@ -64,7 +66,7 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
|
||||
assert "缺少电机端点255" in text
|
||||
assert "实际电机范围为0.4～248.2" in text
|
||||
assert "第1/3轮，255→0" in text
|
||||
assert "已完成6/42个扫描方向" in text
|
||||
assert "计划扫描6/42个方向，扫描进度14.3%" in text
|
||||
assert "当前缺失Tag=2" in text
|
||||
assert "group_pose_jump" not in text
|
||||
assert "PnP拒绝" not in text
|
||||
@@ -135,6 +137,140 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
|
||||
assert "运动采样：" not in text
|
||||
|
||||
|
||||
def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "zero_model_validation_failed",
|
||||
"progress": 0.92,
|
||||
"scan_progress": 1.0,
|
||||
"completed_sweeps": 42,
|
||||
"total_sweeps": 42,
|
||||
"active": {
|
||||
"kind": "zero_model_failure",
|
||||
"view": "front",
|
||||
"motor_index": 15,
|
||||
"joints": ["thumb_mcp", "thumb_ip"],
|
||||
"directions_to_rescan": 0,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "thumb_mcp",
|
||||
"metric": "zero_guard",
|
||||
"reason": "zero_offset_exceeds_configured_limit",
|
||||
"actual_deg": -39.81,
|
||||
"limit_deg": 20.0,
|
||||
}
|
||||
],
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "拇指MCP：零位估计超过安全范围" in text
|
||||
assert "估计-39.81°，允许±20.00°" in text
|
||||
assert "不会自动重扫" in text
|
||||
assert "总体进度：92.0%" in text
|
||||
assert "扫描进度100.0%" in text
|
||||
assert "运动采样：" not in text
|
||||
|
||||
|
||||
def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None:
|
||||
active = {
|
||||
"kind": "sweep",
|
||||
"view": "front",
|
||||
"motor_index": 15,
|
||||
"joints": ["thumb_mcp", "thumb_ip"],
|
||||
"cycle": 1,
|
||||
"repetitions": 3,
|
||||
"start_u8": 255,
|
||||
"target_u8": 0,
|
||||
"fit_attempt": 2,
|
||||
"fit_attempt_limit": 3,
|
||||
}
|
||||
|
||||
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:
|
||||
reason, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"motor_state_stalled:sweep_motor_19:error_u8=5.000",
|
||||
{},
|
||||
)
|
||||
|
||||
assert "距目标5.000个u8" in reason
|
||||
assert "机械端点" in action
|
||||
assert "不要反复调用resume" in action
|
||||
|
||||
|
||||
def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": (
|
||||
"motor_state_stalled:return_baseline:motor_index=10:"
|
||||
"target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:"
|
||||
"error_u8=5.000"
|
||||
),
|
||||
"progress": 0.0,
|
||||
"scan_progress": 0.0,
|
||||
"completed_sweeps": 0,
|
||||
"total_sweeps": 42,
|
||||
"active": {
|
||||
"kind": "motion_stall",
|
||||
"stage": "return_baseline",
|
||||
"motor_index": 10,
|
||||
"target_u8": 255.0,
|
||||
"actual_u8": 250.0,
|
||||
"error_u8": 5.0,
|
||||
"tolerance_u8": 4.0,
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
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
|
||||
assert "运动采样：" not in text
|
||||
|
||||
|
||||
def test_return_baseline_prints_the_exact_command() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
@@ -152,10 +288,34 @@ def test_return_baseline_prints_the_exact_command() -> None:
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "正在返回基准姿态（RETURN_BASELINE）" in text
|
||||
assert "正在恢复目标姿态（RETURN_BASELINE）" in text
|
||||
assert f"正在确认基准姿态：{baseline}" in text
|
||||
|
||||
|
||||
def test_return_recovery_prints_thumb_yaw_start_and_clearance_pose() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
recovery = list(baseline)
|
||||
recovery[5] = 145
|
||||
recovery[10] = 0
|
||||
payload = {
|
||||
"state": "RETURN_BASELINE",
|
||||
"reason": "return_baseline_before_retry_sweep",
|
||||
"progress": 0.5,
|
||||
"completed_sweeps": 36,
|
||||
"total_sweeps": 42,
|
||||
"baseline_command_u8": baseline,
|
||||
"return_command_u8": recovery,
|
||||
"active": {},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert f"正在确认恢复姿态：{recovery}" in text
|
||||
|
||||
|
||||
def test_status_numeric_diagnostics_are_json_serializable() -> None:
|
||||
bins = [0, 16, 255]
|
||||
payload = {
|
||||
@@ -174,6 +334,32 @@ def test_status_numeric_diagnostics_are_json_serializable() -> None:
|
||||
assert '"maximum_bin_gap": 239' in encoded
|
||||
|
||||
|
||||
def test_urdf_zero_bound_has_specific_chinese_scale_guidance() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": (
|
||||
"URDF zero offset reached the configured 20.000 degree bound: "
|
||||
"thumb_cmc_roll=+20.000deg, index_pip=-20.000deg; "
|
||||
"all_offsets: thumb_cmc_roll=+20.000deg"
|
||||
),
|
||||
"progress": 1.0,
|
||||
"completed_sweeps": 42,
|
||||
"total_sweeps": 42,
|
||||
"active": {},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "触及±20.000°安全边界" in text
|
||||
assert "拇指CMC滚转=+20.000deg" in text
|
||||
assert "食指PIP=-20.000deg" in text
|
||||
assert "不要调用resume" in text
|
||||
assert "Tag有效黑框边长" in text
|
||||
assert "未分类原因码" not in text
|
||||
|
||||
|
||||
def test_index_roll_status_prints_clearance_motor_feedback() -> None:
|
||||
payload = {
|
||||
"state": "SWEEP",
|
||||
@@ -188,6 +374,8 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None:
|
||||
"joints": ["index_mcp_roll"],
|
||||
"cycle": 1,
|
||||
"repetitions": 3,
|
||||
"automatic_retry_count": 1,
|
||||
"automatic_retry_limit": 2,
|
||||
"start_u8": 255,
|
||||
"target_u8": 0,
|
||||
"actual_u8": 44.0,
|
||||
@@ -215,3 +403,4 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None:
|
||||
assert "电机9目标0、实际0.0" in text
|
||||
assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text
|
||||
assert "SDK报告[15, 5, 15, 15, 15]" in text
|
||||
assert "自动重试：当前方向已自动重扫1/2次" in text
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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)
|
||||
@@ -75,10 +75,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
|
||||
"握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
|
||||
"张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
|
||||
"拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255],
|
||||
"拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255],
|
||||
"拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100],
|
||||
"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],
|
||||
|
||||
Reference in New Issue
Block a user