新零位相机外参标定方案
This commit is contained in:
@@ -62,6 +62,7 @@ Thumbs.db
|
||||
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
|
||||
/profiles/
|
||||
/calibration_output/
|
||||
/config/g20_three_camera_extrinsics.yaml
|
||||
*.wear_check.json
|
||||
*.checkpoint.json
|
||||
*.verification.json
|
||||
@@ -69,6 +70,7 @@ 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
|
||||
|
||||
# ROS bag / MCAP recordings and CAN captures
|
||||
|
||||
@@ -1,9 +1,10 @@
|
||||
# G20 左手 AprilTag 标定
|
||||
|
||||
## 三机位全手一键标定
|
||||
## 三机位三维关节轴零位标定（schema v4）
|
||||
|
||||
正式全手入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，
|
||||
但只有 `/g20_calibration` 一个节点拥有机械手命令发布权。默认机位绑定为：
|
||||
正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有
|
||||
`/g20_calibration` 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要
|
||||
贴正；相机和Tag在一次标定中必须固定。默认绑定为：
|
||||
|
||||
```text
|
||||
front = DB2163742，Tag 0/1/2/3/10
|
||||
@@ -31,7 +32,7 @@ ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且
|
||||
ID 9 必须在拇指横摆的完整行程中持续可见。
|
||||
贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。
|
||||
|
||||
每台相机必须有独立的内参文件：
|
||||
每台相机必须有独立内参文件：
|
||||
|
||||
```text
|
||||
~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
@@ -39,16 +40,56 @@ 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` 联合优化唯一旋转/平移，自动剔除粗大异常组；两组均得到
|
||||
至少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不超过1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过
|
||||
1.5 mm。文件同时绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存
|
||||
通过结果，正式标定也不会运动。
|
||||
|
||||
### 2. 预检和正式标定
|
||||
|
||||
先使用禁止运动模式检查三个机位、外参、内参和标签：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
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 +100,14 @@ 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 \
|
||||
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
|
||||
```
|
||||
|
||||
@@ -110,28 +126,51 @@ 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`，每项三轮
|
||||
`255→0→255`。轨迹角由父/子Tag完整相对四元数的旋转向量投影到三维拟合轴得到。
|
||||
对正面/侧面近图像平面的浅圆弧，使用姿态旋转轴约束Tag中心三维圆，只由圆轨迹
|
||||
拟合轴线位置和半径，避免单目平面Tag深度噪声改变轴方向；对运动平面充分可观测的
|
||||
斜视轨迹，仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令
|
||||
严格归零：普通通道255，四指侧摆127。中指、无名指、小指继续继承食指曲线；四指
|
||||
`mcp_roll/mcp_pitch/pip` 的静态URDF偏移统一固定为0，不再把单根食指的安装残差复制
|
||||
到四根手指。`thumb_mcp` 的静态URDF偏移同样固定为0，避免IP轴线深度偏差在命令255
|
||||
处产生额外向外伸展；其256项实测轨迹保持不变。
|
||||
|
||||
零位模式锁死原始CAD几何。URDF中确定平行的 `thumb_cmc_pitch→thumb_mcp→thumb_ip`
|
||||
和 `index_mcp_pitch→index_pip→index_dip` 使用同轮上游实测方向，同时由各自Tag中心
|
||||
轨迹独立拟合轴线位置；被动角度曲线仍由对应父/子Tag完整相对姿态生成。这样保留
|
||||
下游轴线对上游零位的约束，同时避免小平面Tag的稳定PnP方向偏差被误认为URDF轴误差。
|
||||
|
||||
前两轮拟合，第三轮强制留出验证；第三轮轨迹MAE必须≤1°、P95≤2°，三轮真实
|
||||
姿态轴方向极差≤0.75°、轴向/径向RMS≤3 mm。零位留出验收只统计能观测对应上游
|
||||
零位的下游轴分量：方向MAE≤1°、P95≤2°，轴线RMS≤3 mm；根轴自身及与当前零位
|
||||
无关的固定CAD/PnP残差不会再把零位推向±20°。独立三维运动平面充分可观测的关节
|
||||
额外要求姿态轴与圆轴夹角≤1°。任一指标失败会暂停并指出失败关节；调用 `resume`
|
||||
只重扫对应电机的6个方向。
|
||||
|
||||
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
|
||||
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
|
||||
不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。
|
||||
|
||||
零位求解联合优化公共手掌六维位姿和3个拇指CMC直接主动关节的
|
||||
`urdf_zero_offset_rad`。依赖链为 yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线
|
||||
约束拇指pitch；`thumb_mcp` 固定为0，不再使用IP轴线修改它的静态零位。食指扫描仍
|
||||
用于生成四指轨迹，但不再用于修改四指静态URDF零位。
|
||||
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
|
||||
保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
|
||||
偏移超过±20°时整次失败。
|
||||
|
||||
生成修正URDF时，四指主动 `mcp_roll/mcp_pitch/pip` 的关节限位会检查对应256项实测
|
||||
轨迹；仅当实测端点越过原始CAD限位时才向外扩展，并额外保留0.5°数值余量。不会
|
||||
收窄原限位，也不会修改拇指或四指被动DIP的限位。
|
||||
|
||||
每个直接测量任务完成 `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` 记录区分，
|
||||
便于调试；最终拟合只使用当前通过尝试的内存数据。
|
||||
|
||||
标定食指 `index_mcp_roll`（电机6）及其随机复测时，为避免中指遮挡ID 10，
|
||||
标定食指 `index_mcp_roll`（电机6）时，为避免中指遮挡ID 10，
|
||||
程序将中指、无名指和小指的侧摆电机7/8/9固定为0；开始采样前会同时确认
|
||||
电机6到达扫描起点且电机7/8/9均已到达0。离开该标定项后恢复统一基准姿态。
|
||||
该项目还会通过SDK设置接口把五指速度临时设为 `[15,5,15,15,15]`，即只把
|
||||
@@ -140,31 +179,34 @@ PnP深度在三轮间不稳定，实测会让三维行程漂移，因此继续
|
||||
五指速度设为 `[15,10,15,15,15]`，即食指屈伸使用第三档速度10；其余直接
|
||||
测量关节保持普通速度15。
|
||||
|
||||
标定 `thumb_cmc_yaw`（电机10）及其随机复测时，程序将
|
||||
标定 `thumb_cmc_yaw`（电机10）时，程序将
|
||||
`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag
|
||||
ID 9的可见性和PnP稳定性。离开该标定项后，电机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`。
|
||||
三机位流程默认 `validation_enabled:=false`，即不增加随机机械动作，但第三轮留出验证
|
||||
始终启用且不能关闭；最终 `quality.validation_mae_rad/p95_rad` 正是第三轮轨迹误差。
|
||||
|
||||
上面机位在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
|
||||
```
|
||||
|
||||
文件包含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)`，绝不叠加旧校准文件或
|
||||
覆盖原文件；除16个主动关节的 `origin.rpy` 外，原URDF文本保持不变。每帧Tag SE(3)、
|
||||
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
|
||||
|
||||
下面保留原有正面拇指独立标定说明和兼容入口。
|
||||
|
||||
@@ -205,7 +247,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 必须在全行程内均可见。需要短时检查标记时，
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -18,7 +18,8 @@ g20_calibration:
|
||||
index_flex_calibration_speed: 10
|
||||
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
|
||||
@@ -34,9 +35,27 @@ g20_calibration:
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
pnp_tracker_reset_seconds: 5.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
|
||||
# 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
|
||||
# 约束三维圆，不让单目平面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
|
||||
zero_maximum_offset_deg: 20.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
# 电机10在命令0时实测会稳定反馈为3；仅其0端使用±3，255端及其他电机仍为±2。
|
||||
thumb_yaw_zero_endpoint_tolerance_u8: 3.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
baseline_hold_seconds: 0.5
|
||||
position_timeout_seconds: 30.0
|
||||
@@ -51,26 +70,22 @@ g20_calibration:
|
||||
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,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)
|
||||
+1411
File diff suppressed because it is too large
Load Diff
@@ -2,7 +2,7 @@
|
||||
|
||||
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,13 @@ 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,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointSpec:
|
||||
name: str
|
||||
@@ -90,11 +96,11 @@ VIEW_TAGS: dict[str, dict[str, int]] = {
|
||||
JOINT_SPECS: dict[str, JointSpec] = {
|
||||
"thumb_cmc_pitch": JointSpec(
|
||||
"thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="projected",
|
||||
zero_kind="urdf_axis_chain",
|
||||
),
|
||||
"index_mcp_pitch": JointSpec(
|
||||
"index_mcp_pitch", 1, True, "side", "side_base", "index_mcp",
|
||||
zero_kind="projected",
|
||||
zero_kind="urdf_axis_chain",
|
||||
),
|
||||
"middle_mcp_pitch": JointSpec(
|
||||
"middle_mcp_pitch", 2, True, None, None, None,
|
||||
@@ -110,11 +116,11 @@ JOINT_SPECS: dict[str, JointSpec] = {
|
||||
),
|
||||
"thumb_cmc_roll": JointSpec(
|
||||
"thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="projected",
|
||||
zero_kind="urdf_axis_chain",
|
||||
),
|
||||
"index_mcp_roll": JointSpec(
|
||||
"index_mcp_roll", 6, True, "front", "front_base", "index_roll",
|
||||
zero_kind="travel_midpoint",
|
||||
zero_kind="urdf_axis_chain",
|
||||
),
|
||||
"middle_mcp_roll": JointSpec(
|
||||
"middle_mcp_roll", 7, True, None, None, None,
|
||||
@@ -130,15 +136,15 @@ JOINT_SPECS: dict[str, JointSpec] = {
|
||||
),
|
||||
"thumb_cmc_yaw": JointSpec(
|
||||
"thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw",
|
||||
zero_kind="projected",
|
||||
zero_kind="urdf_axis_chain",
|
||||
),
|
||||
"thumb_mcp": JointSpec(
|
||||
"thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp",
|
||||
zero_kind="projected",
|
||||
zero_kind="urdf_axis_chain",
|
||||
),
|
||||
"index_pip": JointSpec(
|
||||
"index_pip", 16, True, "side", "index_mcp", "index_pip",
|
||||
zero_kind="projected",
|
||||
zero_kind="urdf_axis_chain",
|
||||
),
|
||||
"middle_pip": JointSpec(
|
||||
"middle_pip", 17, True, None, None, None,
|
||||
@@ -203,8 +209,10 @@ 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.
|
||||
# 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: frozenset[str] = frozenset(
|
||||
{
|
||||
"thumb_cmc_pitch",
|
||||
@@ -244,7 +252,7 @@ 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] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
) -> list[int]:
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 values")
|
||||
@@ -264,7 +272,7 @@ def build_full_hand_command(
|
||||
def build_calibration_motion_command(
|
||||
spec: SweepSpec,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = BASELINE_COMMAND,
|
||||
baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
) -> list[int]:
|
||||
"""Build a sweep command, including any required clearance pose."""
|
||||
result = build_full_hand_command(
|
||||
@@ -612,23 +620,20 @@ 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] = THREE_CAMERA_BASELINE_COMMAND,
|
||||
) -> dict[str, Any]:
|
||||
if set(measured_fits) != set(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 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(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 commands")
|
||||
|
||||
joints: dict[str, dict[str, Any]] = {}
|
||||
for name, spec in JOINT_SPECS.items():
|
||||
@@ -639,32 +644,23 @@ def build_compact_payload(
|
||||
"angle_rad": [round(float(value), 8) for value in fit.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"] = int(baseline[spec.motor_index])
|
||||
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,
|
||||
"schema_version": 4,
|
||||
"model": "G20",
|
||||
"side": "left",
|
||||
"serial_number": str(serial_number),
|
||||
@@ -693,8 +689,8 @@ 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["schema_version"] != 4:
|
||||
raise ValueError("schema_version must be 4")
|
||||
if payload["model"] != "G20" or payload["side"] != "left":
|
||||
raise ValueError("payload must describe a left G20")
|
||||
if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
|
||||
@@ -710,10 +706,10 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None:
|
||||
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:
|
||||
@@ -727,6 +723,20 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None:
|
||||
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:
|
||||
@@ -737,17 +747,8 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None:
|
||||
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")
|
||||
if spec.active and joint["zero_angles"] != source["zero_angles"]:
|
||||
raise ValueError(f"{name} must copy its source zero offset")
|
||||
|
||||
quality = payload["quality"]
|
||||
if set(quality) != {
|
||||
|
||||
@@ -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():
|
||||
|
||||
+53
-4
@@ -2,6 +2,7 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
@@ -92,6 +93,27 @@ def three_camera_reason_zh(
|
||||
)
|
||||
tolerance = sample.get("endpoint_tolerance_u8", "?")
|
||||
|
||||
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 +181,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 +203,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", []):
|
||||
@@ -209,7 +251,7 @@ def three_camera_reason_zh(
|
||||
)
|
||||
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机内参、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "call_start":
|
||||
@@ -226,7 +268,7 @@ 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":
|
||||
@@ -234,7 +276,7 @@ def three_camera_reason_zh(
|
||||
if state == "RETURN_BASELINE":
|
||||
return "正在把已使用的标定电机恢复到统一基准命令。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
return "所有扫描已经完成，正在拟合21个关节的轨迹和零位。", "无需操作。"
|
||||
return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
|
||||
return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
|
||||
|
||||
|
||||
@@ -298,9 +340,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)
|
||||
|
||||
+807
-202
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -180,7 +180,8 @@ def _launch_stack(context):
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
),
|
||||
"startup_torque": 80,
|
||||
"state_poll_rate": 10.0,
|
||||
# Match 30 Hz cameras so state/image p95 skew stays below 50 ms.
|
||||
"state_poll_rate": 30.0,
|
||||
"repeat_position_commands": False,
|
||||
"is_touch": False,
|
||||
}
|
||||
@@ -197,6 +198,17 @@ def _launch_stack(context):
|
||||
{
|
||||
"serial_number": hand_serial,
|
||||
"session_dir": str(session_dir),
|
||||
"camera_extrinsics_file": LaunchConfiguration(
|
||||
"camera_extrinsics_file"
|
||||
),
|
||||
"source_urdf_path": LaunchConfiguration("source_urdf_path"),
|
||||
"corrected_urdf_output_dir": LaunchConfiguration(
|
||||
"corrected_urdf_output_dir"
|
||||
),
|
||||
**{
|
||||
f"{view}_camera_serial": camera_serials[view]
|
||||
for view in VIEWS
|
||||
},
|
||||
"commands_enabled": ParameterValue(
|
||||
LaunchConfiguration("commands_enabled"), value_type=bool
|
||||
),
|
||||
@@ -263,6 +275,25 @@ def generate_launch_description() -> LaunchDescription:
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
info_root = Path.home() / ".ros" / "camera_info"
|
||||
workspace_source_urdf = (
|
||||
Path.cwd()
|
||||
/ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
|
||||
"linker_hand/g20_left/linkerhand_g20_left.urdf"
|
||||
)
|
||||
try:
|
||||
linkerhand_share = Path(
|
||||
get_package_share_directory("linkerhand_retarget")
|
||||
)
|
||||
installed_source_urdf = linkerhand_share / (
|
||||
"assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf"
|
||||
)
|
||||
except Exception:
|
||||
installed_source_urdf = workspace_source_urdf
|
||||
default_source_urdf = (
|
||||
workspace_source_urdf
|
||||
if workspace_source_urdf.is_file()
|
||||
else installed_source_urdf
|
||||
)
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(
|
||||
@@ -315,6 +346,18 @@ def generate_launch_description() -> LaunchDescription:
|
||||
"index_flex_calibration_speed", default_value="10"
|
||||
),
|
||||
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=str(default_source_urdf)
|
||||
),
|
||||
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"),
|
||||
|
||||
@@ -0,0 +1,177 @@
|
||||
"""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,
|
||||
),
|
||||
"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(
|
||||
[
|
||||
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("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),
|
||||
]
|
||||
)
|
||||
@@ -47,6 +47,10 @@ setup(
|
||||
"three_camera_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.three_camera_node:main"
|
||||
),
|
||||
(
|
||||
"three_camera_extrinsics_node = "
|
||||
"g20_thumb_apriltag_calibration.extrinsics_node:main"
|
||||
),
|
||||
(
|
||||
"camera_alignment_view = "
|
||||
"g20_thumb_apriltag_calibration.alignment_view:main"
|
||||
|
||||
@@ -57,6 +57,8 @@ def test_three_camera_tag_ids_and_topics_are_disjoint() -> None:
|
||||
key = f"/g20_calibration/{view}/apriltag/apriltag"
|
||||
parameters = tags[key]["ros__parameters"]
|
||||
assert parameters["tag"]["ids"] == ids
|
||||
assert parameters["size"] == 0.016
|
||||
assert parameters["tag"]["sizes"] == [0.016] * len(ids)
|
||||
assert parameters["qos_profile"] == "sensor_data"
|
||||
assert parameters["detector"]["decimate"] == 1.5
|
||||
all_ids.update(ids)
|
||||
@@ -69,6 +71,8 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
)
|
||||
parameters = config["g20_calibration"]["ros__parameters"]
|
||||
|
||||
assert parameters["tag_size_m"] == 0.016
|
||||
|
||||
assert parameters["baseline_command_u8"] == [
|
||||
255,
|
||||
255,
|
||||
@@ -102,10 +106,18 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
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["thumb_yaw_zero_endpoint_tolerance_u8"] == 3.0
|
||||
assert parameters["minimum_sweep_bins"] >= 32
|
||||
assert parameters["maximum_bin_gap"] <= 16
|
||||
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["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_offset_deg"] <= 20.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,194 @@
|
||||
"""Focused tests for checkerboard frame pairing."""
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from g20_thumb_apriltag_calibration.extrinsics_node import (
|
||||
BoardPose,
|
||||
StereoCapture,
|
||||
_fit_stereo_robust,
|
||||
_minimum_history_skew_ns,
|
||||
_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_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
|
||||
@@ -257,24 +258,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 +303,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 +311,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)
|
||||
|
||||
@@ -174,6 +174,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",
|
||||
|
||||
@@ -1,14 +1,21 @@
|
||||
import json
|
||||
import math
|
||||
from dataclasses import replace
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from g20_thumb_apriltag_calibration.core import (
|
||||
DIRECTION_DECREASING,
|
||||
DIRECTION_INCREASING,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import SWEEP_SPECS
|
||||
from g20_thumb_apriltag_calibration.three_camera_node import (
|
||||
FrameObservation,
|
||||
G20ThreeCameraCalibrationNode,
|
||||
STATE_PREPARE_SWEEP,
|
||||
STATE_SWEEP,
|
||||
SweepItem,
|
||||
)
|
||||
|
||||
@@ -22,23 +29,176 @@ def _sweep_items() -> list[SweepItem]:
|
||||
]
|
||||
|
||||
|
||||
def _image_cycle_records(travels_rad: list[float]) -> list[dict]:
|
||||
def _frame(view: str, state_u8: list[float]) -> FrameObservation:
|
||||
return FrameObservation(
|
||||
stamp_ns=1,
|
||||
received_at=2.0,
|
||||
view=view,
|
||||
state_u8=tuple(state_u8),
|
||||
state_sync_error_ns=0,
|
||||
joint_vectors_xyz_m={},
|
||||
image_vectors_xy_px={},
|
||||
joint_quaternions_xyzw={},
|
||||
parent_poses_common={},
|
||||
child_poses_common={},
|
||||
joint_reprojection_error_px={},
|
||||
)
|
||||
|
||||
|
||||
def test_prepare_sweep_retains_synchronised_start_endpoint_frame() -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
||||
item = SweepItem(spec, 2, DIRECTION_DECREASING)
|
||||
baseline = [255] * 20
|
||||
endpoint_state = list(baseline)
|
||||
endpoint_state[6] = 253.0
|
||||
endpoint_state[7:10] = [0.0, 0.0, 0.0]
|
||||
outside_state = list(endpoint_state)
|
||||
outside_state[6] = 252.0
|
||||
node = SimpleNamespace(
|
||||
state=STATE_PREPARE_SWEEP,
|
||||
active_sweep=item,
|
||||
baseline_command=tuple(baseline),
|
||||
endpoint_tolerance_u8=2.0,
|
||||
preflight_frames=30,
|
||||
latest_state_u8=tuple(endpoint_state),
|
||||
sweep_start_frames=[],
|
||||
)
|
||||
node._motion_command_reached = lambda selected, command, state=None: (
|
||||
G20ThreeCameraCalibrationNode._motion_command_reached(
|
||||
node, selected, command, state
|
||||
)
|
||||
)
|
||||
node._endpoint_tolerance_for_spec = lambda selected, endpoint: (
|
||||
G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
|
||||
node, selected, endpoint
|
||||
)
|
||||
)
|
||||
|
||||
endpoint_frame = _frame("front", endpoint_state)
|
||||
G20ThreeCameraCalibrationNode._accept_frame(node, endpoint_frame)
|
||||
G20ThreeCameraCalibrationNode._accept_frame(
|
||||
node, _frame("front", outside_state)
|
||||
)
|
||||
|
||||
assert node.sweep_start_frames == [endpoint_frame]
|
||||
|
||||
|
||||
def test_begin_sweep_carries_start_endpoint_frame_into_sweep() -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
||||
item = SweepItem(spec, 2, DIRECTION_DECREASING)
|
||||
baseline = [255] * 20
|
||||
endpoint_state = list(baseline)
|
||||
endpoint_state[6] = 253.0
|
||||
endpoint_state[7:10] = [0.0, 0.0, 0.0]
|
||||
endpoint_frame = _frame("front", endpoint_state)
|
||||
published: list[list[int]] = []
|
||||
node = SimpleNamespace(
|
||||
active_sweep=item,
|
||||
baseline_command=tuple(baseline),
|
||||
sweep_frames=[],
|
||||
sweep_start_frames=[endpoint_frame],
|
||||
_publish_command=lambda command: published.append(command),
|
||||
)
|
||||
|
||||
G20ThreeCameraCalibrationNode._begin_active_sweep(node, 10.0)
|
||||
|
||||
assert node.state == STATE_SWEEP
|
||||
assert node.sweep_frames == [endpoint_frame]
|
||||
assert node.sweep_start_frames == []
|
||||
assert node.sweep_last_valid_at == 10.0
|
||||
assert published[0][6] == 0
|
||||
assert published[0][7:10] == [0, 0, 0]
|
||||
|
||||
|
||||
def test_only_thumb_yaw_uses_three_count_endpoint_deadband() -> None:
|
||||
yaw = next(item for item in SWEEP_SPECS if item.motor_index == 10)
|
||||
index_roll = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
||||
node = SimpleNamespace(
|
||||
endpoint_tolerance_u8=2.0,
|
||||
thumb_yaw_zero_endpoint_tolerance_u8=3.0,
|
||||
)
|
||||
|
||||
assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
|
||||
node, yaw, 0
|
||||
) == 3.0
|
||||
assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
|
||||
node, yaw, 255
|
||||
) == 2.0
|
||||
assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
|
||||
node, index_roll, 0
|
||||
) == 2.0
|
||||
|
||||
|
||||
def test_thumb_yaw_command_zero_accepts_feedback_three_only_for_swept_motor() -> None:
|
||||
yaw = next(item for item in SWEEP_SPECS if item.motor_index == 10)
|
||||
state = [255.0] * 20
|
||||
state[10] = 3.0
|
||||
state[5] = 143.0
|
||||
node = SimpleNamespace(
|
||||
latest_state_u8=tuple(state),
|
||||
baseline_command=tuple([255] * 20),
|
||||
endpoint_tolerance_u8=2.0,
|
||||
thumb_yaw_zero_endpoint_tolerance_u8=3.0,
|
||||
)
|
||||
node._endpoint_tolerance_for_spec = lambda spec, endpoint: (
|
||||
G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec(
|
||||
node, spec, endpoint
|
||||
)
|
||||
)
|
||||
|
||||
assert G20ThreeCameraCalibrationNode._motion_command_reached(
|
||||
node, yaw, 0, tuple(state)
|
||||
)
|
||||
state[5] = 142.0
|
||||
assert not G20ThreeCameraCalibrationNode._motion_command_reached(
|
||||
node, yaw, 0, tuple(state)
|
||||
)
|
||||
|
||||
|
||||
def _image_cycle_records(
|
||||
travels_rad: list[float], *, depth_slope: float = 0.0
|
||||
) -> list[dict]:
|
||||
commands = list(range(0, 256, 16)) + [255]
|
||||
records = []
|
||||
for cycle, travel in enumerate(travels_rad):
|
||||
for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING):
|
||||
for command in commands:
|
||||
angle = travel * (255.0 - command) / 255.0
|
||||
x = 0.03 * math.cos(angle)
|
||||
y = 0.03 * math.sin(angle)
|
||||
z = float(depth_slope) * (x - 0.03)
|
||||
records.append(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_translation_xyz_m": [0.01, 0.0, 0.0],
|
||||
"relative_translation_xyz_m": [
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
],
|
||||
"image_relative_xy_px": [
|
||||
100.0 * math.cos(angle),
|
||||
100.0 * math.sin(angle),
|
||||
],
|
||||
"relative_quaternion_xyzw": Rotation.from_rotvec(
|
||||
np.asarray([0.0, 0.0, angle])
|
||||
).as_quat().tolist(),
|
||||
"parent_pose_common": {
|
||||
"translation_xyz_m": [0.0, 0.0, 0.0],
|
||||
"quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
|
||||
},
|
||||
"child_pose_common": {
|
||||
"translation_xyz_m": [
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
],
|
||||
"quaternion_xyzw": Rotation.from_rotvec(
|
||||
np.asarray([0.0, 0.0, angle])
|
||||
).as_quat().tolist(),
|
||||
},
|
||||
"state_u8": [float(command)] + [255.0] * 19,
|
||||
}
|
||||
)
|
||||
return records
|
||||
@@ -61,16 +221,23 @@ def _fit_check_node(records_by_joint: dict) -> SimpleNamespace:
|
||||
maximum_hysteresis_rad=math.radians(5.0),
|
||||
passive_maximum_monotonic_correction_rad=math.radians(3.0),
|
||||
passive_maximum_hysteresis_rad=math.radians(7.5),
|
||||
zero_minimum_radius_px=20.0,
|
||||
zero_maximum_radial_rms_px=2.0,
|
||||
zero_maximum_radial_p95_px=3.5,
|
||||
zero_maximum_round_difference_rad=math.radians(1.0),
|
||||
baseline_command=[255] * 20,
|
||||
axis_maximum_plane_rms_m=0.003,
|
||||
passive_axis_maximum_plane_rms_m=0.004,
|
||||
axis_maximum_radial_rms_m=0.003,
|
||||
axis_maximum_rotation_circle_difference_rad=math.radians(1.0),
|
||||
active_maximum_rotation_orthogonal_rms_rad=math.radians(2.5),
|
||||
passive_maximum_rotation_orthogonal_rms_rad=math.radians(7.5),
|
||||
zero_maximum_axis_cycle_difference_rad=math.radians(0.75),
|
||||
)
|
||||
node._fit_joint_records = lambda name, records, relaxed=False: (
|
||||
G20ThreeCameraCalibrationNode._fit_joint_records(
|
||||
node, name, records, relaxed=relaxed
|
||||
)
|
||||
)
|
||||
node._fit_axis_measurement = lambda name, cycle: (
|
||||
G20ThreeCameraCalibrationNode._fit_axis_measurement(node, name, cycle)
|
||||
)
|
||||
return node
|
||||
|
||||
|
||||
@@ -80,6 +247,7 @@ def test_fit_failure_rewinds_to_failed_specs_first_direction(tmp_path) -> None:
|
||||
sweep_items=_sweep_items(),
|
||||
sweep_attempts={spec.motor_index: 1},
|
||||
retry_sweep_spec=None,
|
||||
retry_resume_index=None,
|
||||
fit_failure={},
|
||||
sweep_index=24,
|
||||
raw_path=tmp_path / "raw_samples.jsonl",
|
||||
@@ -133,7 +301,7 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None:
|
||||
assert records["thumb_ip"] == []
|
||||
assert records["thumb_cmc_pitch"] == [{"keep": True}]
|
||||
assert node.sweep_attempts[15] == 2
|
||||
assert node.retry_sweep_spec is None
|
||||
assert node.retry_sweep_spec == spec
|
||||
assert node.fit_failure == {}
|
||||
event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
|
||||
assert event == {
|
||||
@@ -168,3 +336,127 @@ def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None:
|
||||
assert cycle_failure["joint"] == "thumb_cmc_pitch"
|
||||
assert cycle_failure["actual"] == 8.0
|
||||
assert cycle_failure["limit"] == 3.0
|
||||
|
||||
|
||||
def test_provisional_fit_accepts_constrained_circle_for_image_joint() -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 0)
|
||||
node = _fit_check_node(
|
||||
{
|
||||
"thumb_cmc_pitch": _image_cycle_records(
|
||||
[math.radians(47.0)] * 3,
|
||||
depth_slope=2.00,
|
||||
)
|
||||
}
|
||||
)
|
||||
|
||||
failures = G20ThreeCameraCalibrationNode._provisional_fit_failures(
|
||||
node, spec
|
||||
)
|
||||
|
||||
assert not [
|
||||
failure
|
||||
for failure in failures
|
||||
if failure["metric"]
|
||||
in {
|
||||
"rotation_circle_axis_difference_deg",
|
||||
"axis_cycle_difference_deg",
|
||||
"axis_plane_rms_mm",
|
||||
"axis_radial_rms_mm",
|
||||
}
|
||||
]
|
||||
|
||||
|
||||
def test_thumb_ip_axis_uses_same_cycle_thumb_mcp_direction() -> None:
|
||||
records = _image_cycle_records([math.radians(47.0)] * 3)
|
||||
node = _fit_check_node(
|
||||
{
|
||||
"thumb_cmc_pitch": records,
|
||||
"thumb_mcp": records,
|
||||
"thumb_ip": records,
|
||||
}
|
||||
)
|
||||
|
||||
upstream = node._fit_axis_measurement("thumb_mcp", 0)
|
||||
passive = node._fit_axis_measurement("thumb_ip", 0)
|
||||
|
||||
assert abs(
|
||||
float(
|
||||
np.asarray(upstream.axis_common_xyz)
|
||||
@ np.asarray(passive.axis_common_xyz)
|
||||
)
|
||||
) > math.cos(math.radians(0.01))
|
||||
|
||||
|
||||
def test_provisional_fit_uses_orientation_constrained_circle_for_yaw() -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 10)
|
||||
node = _fit_check_node(
|
||||
{
|
||||
"thumb_cmc_yaw": _image_cycle_records(
|
||||
[math.radians(47.0)] * 3,
|
||||
depth_slope=0.30,
|
||||
)
|
||||
}
|
||||
)
|
||||
|
||||
failures = G20ThreeCameraCalibrationNode._provisional_fit_failures(
|
||||
node, spec
|
||||
)
|
||||
|
||||
assert not [
|
||||
failure
|
||||
for failure in failures
|
||||
if failure["metric"]
|
||||
in {
|
||||
"rotation_circle_axis_difference_deg",
|
||||
"axis_cycle_difference_deg",
|
||||
"axis_plane_rms_mm",
|
||||
"axis_radial_rms_mm",
|
||||
}
|
||||
]
|
||||
|
||||
|
||||
def test_provisional_fit_uses_active_and_passive_axis_model_limits() -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 15)
|
||||
records = _image_cycle_records([math.radians(47.0)] * 3)
|
||||
node = _fit_check_node(
|
||||
{
|
||||
"thumb_mcp": records,
|
||||
"thumb_ip": records,
|
||||
}
|
||||
)
|
||||
residuals_deg = {"thumb_mcp": 2.0, "thumb_ip": 6.0}
|
||||
|
||||
def fitted(name, selected, relaxed=False):
|
||||
fit = G20ThreeCameraCalibrationNode._fit_joint_records(
|
||||
node, name, selected, relaxed=relaxed
|
||||
)
|
||||
quality = dict(fit.quality)
|
||||
quality["rotation_orthogonal_rms_rad"] = math.radians(
|
||||
residuals_deg[name]
|
||||
)
|
||||
return replace(fit, quality=quality)
|
||||
|
||||
node._fit_joint_records = fitted
|
||||
failures = G20ThreeCameraCalibrationNode._provisional_fit_failures(
|
||||
node, spec
|
||||
)
|
||||
assert not [
|
||||
failure
|
||||
for failure in failures
|
||||
if failure["metric"] == "rotation_orthogonal_rms_deg"
|
||||
]
|
||||
|
||||
residuals_deg["thumb_mcp"] = 3.0
|
||||
residuals_deg["thumb_ip"] = 8.0
|
||||
failures = G20ThreeCameraCalibrationNode._provisional_fit_failures(
|
||||
node, spec
|
||||
)
|
||||
axis_failures = [
|
||||
failure
|
||||
for failure in failures
|
||||
if failure["metric"] == "rotation_orthogonal_rms_deg"
|
||||
]
|
||||
assert {(item["joint"], item["limit"]) for item in axis_failures} == {
|
||||
("thumb_mcp", 2.5),
|
||||
("thumb_ip", 7.5),
|
||||
}
|
||||
|
||||
@@ -0,0 +1,550 @@
|
||||
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,
|
||||
write_zero_corrected_urdf,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
ACTIVE_JOINTS,
|
||||
JOINT_SPECS,
|
||||
MEASURED_JOINTS,
|
||||
PASSIVE_JOINTS,
|
||||
JointCurveFit,
|
||||
)
|
||||
|
||||
|
||||
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"
|
||||
)
|
||||
|
||||
|
||||
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 _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)
|
||||
|
||||
|
||||
def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth() -> 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(
|
||||
"thumb_cmc_pitch", 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_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",
|
||||
)
|
||||
|
||||
|
||||
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)
|
||||
|
||||
|
||||
def test_urdf_writer_expands_only_requested_limits_with_margin(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
margin = math.radians(0.5)
|
||||
measured_ranges = {
|
||||
"index_mcp_roll": (-0.25113634, 0.24813698),
|
||||
"index_mcp_pitch": (0.0, 1.22424601),
|
||||
"index_pip": (0.0, 1.74215779),
|
||||
}
|
||||
destination = write_zero_corrected_urdf(
|
||||
source_urdf=SOURCE_URDF,
|
||||
output_directory=tmp_path,
|
||||
serial_number="G20_LEFT_001",
|
||||
offsets_rad={"thumb_cmc_yaw": math.radians(1.0)},
|
||||
timestamp="20260807_181500",
|
||||
joint_angle_ranges_rad=measured_ranges,
|
||||
joint_limit_margin_rad=margin,
|
||||
)
|
||||
|
||||
roll_lower, roll_upper = _joint_limit(destination, "index_mcp_roll")
|
||||
pitch_lower, pitch_upper = _joint_limit(destination, "index_mcp_pitch")
|
||||
pip_lower, pip_upper = _joint_limit(destination, "index_pip")
|
||||
assert roll_lower == pytest.approx(measured_ranges["index_mcp_roll"][0] - margin)
|
||||
assert roll_upper == pytest.approx(measured_ranges["index_mcp_roll"][1] + margin)
|
||||
assert pitch_lower == pytest.approx(_joint_limit(SOURCE_URDF, "index_mcp_pitch")[0])
|
||||
assert pitch_upper == pytest.approx(measured_ranges["index_mcp_pitch"][1] + margin)
|
||||
assert pip_lower == pytest.approx(_joint_limit(SOURCE_URDF, "index_pip")[0])
|
||||
assert pip_upper == pytest.approx(measured_ranges["index_pip"][1] + margin)
|
||||
assert _joint_limit(destination, "thumb_cmc_yaw") == pytest.approx(
|
||||
_joint_limit(SOURCE_URDF, "thumb_cmc_yaw")
|
||||
)
|
||||
assert _joint_limit(destination, "middle_mcp_roll") == pytest.approx(
|
||||
_joint_limit(SOURCE_URDF, "middle_mcp_roll")
|
||||
)
|
||||
|
||||
|
||||
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 test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None:
|
||||
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, 0.0, 0.0, 0.0, 0.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
|
||||
for name, expected in true_offsets.items():
|
||||
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=1.0e-12
|
||||
)
|
||||
for target, source in INHERITED_ZERO_JOINTS.items():
|
||||
assert result.all_active_offsets_rad[target] == pytest.approx(
|
||||
result.direct_offsets_rad[source], abs=1.0e-12
|
||||
)
|
||||
for joint in (
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
):
|
||||
assert result.all_active_offsets_rad[joint] == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
for joint in (
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
):
|
||||
assert result.all_active_offsets_rad[joint] == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
for joint in ("index_pip", "middle_pip", "ring_pip", "pinky_pip"):
|
||||
assert result.all_active_offsets_rad[joint] == pytest.approx(
|
||||
0.0, abs=1.0e-12
|
||||
)
|
||||
Reference in New Issue
Block a user