diff --git a/.gitignore b/.gitignore index f6bcdff..5d3adc9 100644 --- a/.gitignore +++ b/.gitignore @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 9d49964..18ba27b 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -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 必须在全行程内均可见。需要短时检查标记时， diff --git a/src/g20_thumb_apriltag_calibration/config/front_tags.yaml b/src/g20_thumb_apriltag_calibration/config/front_tags.yaml index a469078..7a33f4d 100644 --- a/src/g20_thumb_apriltag_calibration/config/front_tags.yaml +++ b/src/g20_thumb_apriltag_calibration/config/front_tags.yaml @@ -6,7 +6,7 @@ # from back-pressuring image_proc's reliable image publisher. qos_profile: sensor_data family: 36h11 - size: 0.01 + size: 0.016 profile: false max_hamming: 0 detector: @@ -20,11 +20,11 @@ tag: ids: [0, 1, 2, 3] frames: [tag_t0, tag_t3, tag_t4, tag_t5] - sizes: [0.010, 0.010, 0.010, 0.010] + sizes: [0.016, 0.016, 0.016, 0.016] g20_thumb_calibration: ros__parameters: tag_roles: [t0, t3, t4, t5] tag_ids: [0, 1, 2, 3] tag_frames: [tag_t0, tag_t3, tag_t4, tag_t5] - tag_sizes_m: [0.010, 0.010, 0.010, 0.010] + tag_sizes_m: [0.016, 0.016, 0.016, 0.016] diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml index ea727e4..339d411 100644 --- a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml index 5f24344..b7afb31 100644 --- a/src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml @@ -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] diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics.py new file mode 100644 index 0000000..3cea37b --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics.py @@ -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) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics_node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics_node.py new file mode 100644 index 0000000..1fb899d --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/extrinsics_node.py @@ -0,0 +1,1411 @@ +"""Interactive one-time three-camera checkerboard extrinsic calibration.""" + +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass +import math +import os +from pathlib import Path +import time +from typing import Any + +import cv2 +from cv_bridge import CvBridge +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, Image +from std_srvs.srv import Trigger + +from .core import robust_rotation_summary +from .extrinsics import ( + VIEWS, + camera_info_fingerprint, + dump_three_camera_extrinsics, +) +from .hikrobot_camera import configure_fastdds_large_image_transport + + +@dataclass(frozen=True) +class BoardPose: + stamp_ns: int + camera_from_board_candidates: tuple[np.ndarray, ...] + reprojection_rms_px: float + image_points: np.ndarray | None = None + + +@dataclass(frozen=True) +class ImageFrame: + stamp_ns: int + message: Image + + +@dataclass(frozen=True) +class PairAssessment: + other: str + ready: bool + reason_code: str + message: str + front_frame: ImageFrame | None = None + selected_frame: ImageFrame | None = None + front_pose: BoardPose | None = None + selected_pose: BoardPose | None = None + skew_ns: int | None = None + pair_reprojection_rms_px: float | None = None + transform_candidates: tuple[np.ndarray, ...] = () + provisional_rotation_error_rad: float | None = None + provisional_translation_error_m: float | None = None + + +@dataclass(frozen=True) +class StereoCapture: + front_points_px: np.ndarray + other_points_px: np.ndarray + transform_candidates: tuple[np.ndarray, ...] + pair_reprojection_rms_px: float + skew_ns: int + + +@dataclass(frozen=True) +class StereoFitResult: + front_from_other: np.ndarray + stereo_rms_px: float + per_view_rms_px: tuple[float, ...] + inlier_indices: tuple[int, ...] + rejected_indices: tuple[int, ...] + rotation_stability_rad: float + translation_stability_m: float + passed: bool + + +def _select_latest_synchronised_pair( + front_history: list[Any] | deque[Any], + other_history: list[Any] | deque[Any], + maximum_skew_ns: int, +) -> tuple[Any, Any, int] | None: + """Select the newest available pair whose image stamps are close enough. + + Keep image callbacks cheap and pair their timestamps before running the + expensive checkerboard detector. This prevents image-processing latency + from turning into a false camera synchronization failure. + """ + candidates: list[tuple[int, int, Any, Any]] = [] + for front in front_history: + for other in other_history: + skew = abs(int(front.stamp_ns) - int(other.stamp_ns)) + if skew <= int(maximum_skew_ns): + common_stamp = min(int(front.stamp_ns), int(other.stamp_ns)) + candidates.append((common_stamp, -skew, front, other)) + if not candidates: + return None + _, negative_skew, front, other = max( + candidates, key=lambda item: (item[0], item[1]) + ) + return front, other, -negative_skew + + +def _minimum_history_skew_ns( + front_history: list[Any] | deque[Any], + other_history: list[Any] | deque[Any], +) -> int | None: + if not front_history or not other_history: + return None + return min( + abs(int(front.stamp_ns) - int(other.stamp_ns)) + for front in front_history + for other in other_history + ) + + +def _stamp_ns(message: Image) -> int: + return ( + int(message.header.stamp.sec) * 1_000_000_000 + + int(message.header.stamp.nanosec) + ) + + +def _inverse(transform: np.ndarray) -> np.ndarray: + result = np.eye(4) + result[:3, :3] = transform[:3, :3].T + result[:3, 3] = -result[:3, :3] @ transform[:3, 3] + return result + + +def _mean_transform(transforms: list[np.ndarray]) -> np.ndarray: + quaternion = robust_rotation_summary( + [Rotation.from_matrix(value[:3, :3]).as_quat() for value in transforms] + )[0] + result = np.eye(4) + result[:3, :3] = Rotation.from_quat(quaternion).as_matrix() + result[:3, 3] = np.median( + np.asarray([value[:3, 3] for value in transforms]), axis=0 + ) + return result + + +def _transform_residual( + reference: np.ndarray, observed: np.ndarray +) -> tuple[float, float]: + rotation = Rotation.from_matrix(reference[:3, :3]).inv() * Rotation.from_matrix( + observed[:3, :3] + ) + translation = observed[:3, 3] - reference[:3, 3] + return float(rotation.magnitude()), float(np.linalg.norm(translation)) + + +def _select_consistent_transforms( + candidates_by_capture: list[list[np.ndarray]], +) -> tuple[np.ndarray, list[np.ndarray]]: + """Resolve the checkerboard's 180-degree corner-order ambiguity.""" + if not candidates_by_capture or not candidates_by_capture[0]: + raise ValueError("no checkerboard transform candidates") + best_score = float("inf") + best_reference = np.eye(4) + best_selected: list[np.ndarray] = [] + for seed in candidates_by_capture[0]: + reference = seed + selected: list[np.ndarray] = [] + for _ in range(5): + selected = [] + for candidates in candidates_by_capture: + selected.append( + min( + candidates, + key=lambda candidate: ( + _transform_residual(reference, candidate)[0] + / math.radians(0.3) + + _transform_residual(reference, candidate)[1] + / 0.0015 + ), + ) + ) + updated = _mean_transform(selected) + if _transform_residual(reference, updated)[0] < 1.0e-10: + reference = updated + break + reference = updated + score = sum( + (rotation / math.radians(0.3)) ** 2 + + (translation / 0.0015) ** 2 + for rotation, translation in ( + _transform_residual(reference, observed) + for observed in selected + ) + ) + if score < best_score: + best_score = score + best_reference = reference + best_selected = selected + return best_reference, best_selected + + +def _summarize_transform_repeatability( + candidates_by_capture: list[list[np.ndarray]], +) -> tuple[np.ndarray, list[np.ndarray], float, float]: + """Fit one transform and return worst rotation/translation residuals.""" + reference, selected = _select_consistent_transforms(candidates_by_capture) + residuals = [ + _transform_residual(reference, observed) for observed in selected + ] + return ( + reference, + selected, + max(rotation for rotation, _ in residuals), + max(translation for _, translation in residuals), + ) + + +def _stereo_fit_once( + captures: list[StereoCapture], + indices: list[int], + object_points: np.ndarray, + front_matrix: np.ndarray, + other_matrix: np.ndarray, + image_size: tuple[int, int], +) -> tuple[float, np.ndarray, np.ndarray]: + if len(indices) < 3: + raise ValueError("stereo calibration requires at least three views") + objects = [ + np.asarray(object_points, dtype=np.float32).copy() for _ in indices + ] + candidate_sets = [ + list(captures[index].transform_candidates) for index in indices + ] + _, selected_transforms = _select_consistent_transforms(candidate_sets) + front_points: list[np.ndarray] = [] + other_points: list[np.ndarray] = [] + for capture_index, selected_transform in zip( + indices, selected_transforms + ): + capture = captures[capture_index] + candidate_index = min( + range(len(capture.transform_candidates)), + key=lambda index: sum( + _transform_residual( + selected_transform, + capture.transform_candidates[index], + ) + ), + ) + candidates_per_camera = int( + round(math.sqrt(len(capture.transform_candidates))) + ) + if candidates_per_camera * candidates_per_camera != len( + capture.transform_candidates + ): + raise ValueError("checkerboard pose candidate layout is invalid") + front_reversed = candidate_index // candidates_per_camera != 0 + other_reversed = candidate_index % candidates_per_camera != 0 + front = np.asarray(capture.front_points_px, dtype=np.float32) + other = np.asarray(capture.other_points_px, dtype=np.float32) + front_points.append(front[::-1].copy() if front_reversed else front) + other_points.append(other[::-1].copy() if other_reversed else other) + result = cv2.stereoCalibrateExtended( + objects, + front_points, + other_points, + np.asarray(front_matrix, dtype=float).copy(), + np.zeros(5), + np.asarray(other_matrix, dtype=float).copy(), + np.zeros(5), + tuple(int(value) for value in image_size), + np.eye(3), + np.zeros((3, 1)), + flags=cv2.CALIB_FIX_INTRINSIC, + criteria=( + cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, + 100, + 1.0e-7, + ), + ) + rms = float(result[0]) + other_from_front = np.eye(4) + other_from_front[:3, :3] = np.asarray(result[5], dtype=float) + other_from_front[:3, 3] = np.asarray(result[6], dtype=float).reshape(3) + per_camera_errors = np.asarray(result[9], dtype=float).reshape( + len(indices), -1 + ) + per_view_errors = np.sqrt( + np.mean(np.square(per_camera_errors), axis=1) + ) + return rms, _inverse(other_from_front), per_view_errors + + +def _fit_stereo_robust( + captures: list[StereoCapture], + object_points: np.ndarray, + front_matrix: np.ndarray, + other_matrix: np.ndarray, + image_size: tuple[int, int], + *, + minimum_inliers: int, + maximum_rms_px: float, + maximum_rotation_stability_rad: float, + maximum_translation_stability_m: float, +) -> StereoFitResult: + """Jointly fit one stereo transform and remove gross pose outliers.""" + if len(captures) < 3: + raise ValueError("at least three stereo captures are required") + active = list(range(len(captures))) + fit_rms = float("inf") + transform = np.eye(4) + per_view = np.full(len(active), float("inf")) + while len(active) >= 3: + fit_rms, transform, per_view = _stereo_fit_once( + captures, + active, + object_points, + front_matrix, + other_matrix, + image_size, + ) + median = float(np.median(per_view)) + mad = float(np.median(np.abs(per_view - median))) + reprojection_limit = max( + maximum_rms_px * 1.5, + median + 3.0 * 1.4826 * max(mad, 1.0e-6), + ) + rotation_limit = max( + math.radians(1.2), 4.0 * maximum_rotation_stability_rad + ) + translation_limit = max( + 0.008, 4.0 * maximum_translation_stability_m + ) + scores: list[float] = [] + for local_index, capture_index in enumerate(active): + residuals = [ + _transform_residual(transform, candidate) + for candidate in captures[capture_index].transform_candidates + ] + rotation_error, translation_error = min( + residuals, + key=lambda value: ( + value[0] / rotation_limit + + value[1] / translation_limit + ), + ) + scores.append( + max( + float(per_view[local_index]) / reprojection_limit, + rotation_error / rotation_limit, + translation_error / translation_limit, + ) + ) + worst_local = int(np.argmax(scores)) + if scores[worst_local] <= 1.0 or len(active) <= 3: + break + active.pop(worst_local) + + rotation_stability = float("inf") + translation_stability = float("inf") + if len(active) >= 6: + rotation_values: list[float] = [] + translation_values: list[float] = [] + for held_out_fold in range(3): + subset = [ + index + for position, index in enumerate(active) + if position % 3 != held_out_fold + ] + _, subset_transform, _ = _stereo_fit_once( + captures, + subset, + object_points, + front_matrix, + other_matrix, + image_size, + ) + rotation_error, translation_error = _transform_residual( + transform, subset_transform + ) + rotation_values.append(rotation_error) + translation_values.append(translation_error) + rotation_stability = max(rotation_values) + translation_stability = max(translation_values) + + rejected = tuple( + index for index in range(len(captures)) if index not in set(active) + ) + passed = bool( + len(active) >= minimum_inliers + and fit_rms <= maximum_rms_px + and rotation_stability <= maximum_rotation_stability_rad + and translation_stability <= maximum_translation_stability_m + ) + return StereoFitResult( + front_from_other=transform, + stereo_rms_px=fit_rms, + per_view_rms_px=tuple(float(value) for value in per_view), + inlier_indices=tuple(active), + rejected_indices=rejected, + rotation_stability_rad=rotation_stability, + translation_stability_m=translation_stability, + passed=passed, + ) + + +class ThreeCameraExtrinsicsNode(Node): + GUI_WINDOW = "G20 Three-Camera Extrinsics" + + def __init__(self) -> None: + super().__init__("g20_camera_extrinsics") + self._declare_parameters() + self._load_parameters() + self.bridge = CvBridge() + self.camera_matrices: dict[str, np.ndarray] = {} + self.camera_identities: dict[str, dict[str, Any]] = {} + # Do not run expensive 2 MP checkerboard detection in the image + # callbacks. That made a single-threaded executor compare frames that + # were processed at different times. Cache messages cheaply, select a + # synchronized pair first, then detect only those two images on demand. + self.image_history: dict[str, deque[ImageFrame]] = { + view: deque(maxlen=12) for view in VIEWS + } + self.captures: dict[str, list[list[np.ndarray]]] = { + "side": [], + "top": [], + } + self.capture_reprojection: dict[str, list[float]] = { + "side": [], + "top": [], + } + self.stereo_captures: dict[str, list[StereoCapture]] = { + "side": [], + "top": [], + } + self.fit_results: dict[str, StereoFitResult | None] = { + "side": None, + "top": None, + } + self.captured_front_board: dict[str, list[np.ndarray]] = { + "side": [], + "top": [], + } + self.gui_active_other = "side" + self.gui_last_assessment: PairAssessment | None = None + self.gui_feedback = "Move the checkerboard into both views" + self.gui_action: str | None = None + self.gui_auto_capture = self.auto_capture_default + self.gui_auto_pose: np.ndarray | None = None + self.gui_auto_stable_since: float | None = None + self.gui_enabled = bool(self.enable_gui and os.environ.get("DISPLAY")) + for view in VIEWS: + self.create_subscription( + CameraInfo, + self.info_topics[view], + lambda message, selected=view: self._info_callback( + selected, message + ), + qos_profile_sensor_data, + ) + self.create_subscription( + Image, + self.image_topics[view], + lambda message, selected=view: self._image_callback( + selected, message + ), + qos_profile_sensor_data, + ) + self.create_service( + Trigger, "~/capture_front_side", self._capture_front_side + ) + self.create_service( + Trigger, "~/capture_front_top", self._capture_front_top + ) + self.create_service(Trigger, "~/save", self._save) + self.create_service(Trigger, "~/reset", self._reset) + if self.enable_gui and not self.gui_enabled: + self.get_logger().warning( + "Extrinsics GUI requested but DISPLAY is unavailable; GUI disabled" + ) + if self.gui_enabled: + try: + cv2.namedWindow(self.GUI_WINDOW, cv2.WINDOW_NORMAL) + cv2.resizeWindow(self.GUI_WINDOW, 1400, 760) + cv2.setMouseCallback( + self.GUI_WINDOW, self._gui_mouse_callback + ) + self.gui_timer = self.create_timer( + 1.0 / self.gui_refresh_hz, self._gui_timer_callback + ) + except cv2.error as error: + self.gui_enabled = False + self.get_logger().error( + f"Failed to create extrinsics GUI; GUI disabled: {error}" + ) + self.get_logger().info( + "Checkerboard extrinsic calibration ready: " + f"inner corners={self.columns}x{self.rows}, " + f"square={self.square_size_m * 1000.0:.3f} mm" + ) + + def _declare_parameters(self) -> None: + self.declare_parameter("output_file", "camera_extrinsics.yaml") + self.declare_parameter("checkerboard_columns", 8) + self.declare_parameter("checkerboard_rows", 5) + self.declare_parameter("square_size_m", 0.027) + self.declare_parameter("minimum_captures_per_pair", 15) + self.declare_parameter("maximum_pair_skew_ms", 100.0) + self.declare_parameter("maximum_reprojection_rms_px", 1.2) + self.declare_parameter("maximum_rotation_repeatability_deg", 0.3) + self.declare_parameter("maximum_translation_repeatability_m", 0.0015) + self.declare_parameter("enable_gui", False) + self.declare_parameter("gui_refresh_hz", 2.0) + self.declare_parameter("auto_capture_default", False) + self.declare_parameter("auto_capture_stable_seconds", 1.0) + for view in VIEWS: + self.declare_parameter(f"{view}_camera_serial", "") + self.declare_parameter( + f"{view}_image_topic", + f"/g20_extrinsics/{view}/camera/image_rect", + ) + self.declare_parameter( + f"{view}_camera_info_topic", + f"/g20_extrinsics/{view}/camera/camera_info", + ) + + def _load_parameters(self) -> None: + def value(name: str) -> Any: + return self.get_parameter(name).value + + self.output_file = Path(str(value("output_file"))).expanduser().resolve() + self.columns = int(value("checkerboard_columns")) + self.rows = int(value("checkerboard_rows")) + self.square_size_m = float(value("square_size_m")) + self.minimum_captures = int(value("minimum_captures_per_pair")) + self.maximum_pair_skew_ns = int( + float(value("maximum_pair_skew_ms")) * 1_000_000.0 + ) + self.maximum_reprojection_rms_px = float( + value("maximum_reprojection_rms_px") + ) + self.maximum_rotation_repeatability_rad = math.radians( + float(value("maximum_rotation_repeatability_deg")) + ) + self.maximum_translation_repeatability_m = float( + value("maximum_translation_repeatability_m") + ) + self.enable_gui = bool(value("enable_gui")) + self.gui_refresh_hz = float(value("gui_refresh_hz")) + self.auto_capture_default = bool(value("auto_capture_default")) + self.auto_capture_stable_seconds = float( + value("auto_capture_stable_seconds") + ) + self.serials = { + view: str(value(f"{view}_camera_serial")) for view in VIEWS + } + self.image_topics = { + view: str(value(f"{view}_image_topic")) for view in VIEWS + } + self.info_topics = { + view: str(value(f"{view}_camera_info_topic")) for view in VIEWS + } + if self.columns < 3 or self.rows < 3 or self.square_size_m <= 0.0: + raise ValueError("checkerboard geometry is invalid") + if self.minimum_captures < 15: + raise ValueError("minimum_captures_per_pair must be at least 15") + if self.gui_refresh_hz <= 0.0 or self.gui_refresh_hz > 10.0: + raise ValueError("gui_refresh_hz must be in (0, 10]") + if self.auto_capture_stable_seconds < 0.5: + raise ValueError("auto_capture_stable_seconds must be at least 0.5") + if any(not serial for serial in self.serials.values()): + raise ValueError("all three camera serial numbers are required") + + @property + def object_points(self) -> np.ndarray: + points = np.zeros((self.columns * self.rows, 3), dtype=np.float32) + points[:, :2] = np.mgrid[0:self.columns, 0:self.rows].T.reshape(-1, 2) + points[:, :2] *= self.square_size_m + return points + + def _info_callback(self, view: str, message: CameraInfo) -> None: + matrix = np.asarray(message.p, dtype=float).reshape(3, 4)[:, :3] + if ( + int(message.width) <= 0 + or int(message.height) <= 0 + or not np.all(np.isfinite(matrix)) + or matrix[0, 0] <= 0.0 + or matrix[1, 1] <= 0.0 + ): + self.camera_matrices.pop(view, None) + self.camera_identities.pop(view, None) + return + self.camera_matrices[view] = matrix + self.camera_identities[view] = { + "serial_number": self.serials[view], + "width": int(message.width), + "height": int(message.height), + "intrinsics_sha256": camera_info_fingerprint( + width=message.width, + height=message.height, + camera_matrix=message.k, + distortion=message.d, + rectification=message.r, + projection=message.p, + ), + } + + def _image_callback(self, view: str, message: Image) -> None: + stamp_ns = _stamp_ns(message) + if stamp_ns <= 0: + return + self.image_history[view].append( + ImageFrame(stamp_ns=stamp_ns, message=message) + ) + + def _detect_board_pose( + self, view: str, message: Image + ) -> BoardPose | None: + matrix = self.camera_matrices.get(view) + if matrix is None: + return None + image = self.bridge.imgmsg_to_cv2(message, desired_encoding="mono8") + found, corners = cv2.findChessboardCornersSB( + image, + (self.columns, self.rows), + flags=cv2.CALIB_CB_NORMALIZE_IMAGE, + ) + if not found or corners is None: + return None + image_points = np.asarray(corners, dtype=np.float32).reshape(-1, 2) + pose_candidates: list[np.ndarray] = [] + reprojection_values: list[float] = [] + for ordered_points in (image_points, image_points[::-1].copy()): + solved, rotation_vector, translation = cv2.solvePnP( + self.object_points, + ordered_points, + matrix, + np.zeros(5), + flags=cv2.SOLVEPNP_IPPE, + ) + if not solved: + continue + # IPPE supplies a stable planar initial pose. LM refinement then + # minimizes the exact reprojection residual used by the quality + # gate, avoiding an avoidable analytical-solver error floor. + rotation_vector, translation = cv2.solvePnPRefineLM( + self.object_points, + ordered_points, + matrix, + np.zeros(5), + rotation_vector, + translation, + ) + projected, _ = cv2.projectPoints( + self.object_points, + rotation_vector, + translation, + matrix, + np.zeros(5), + ) + residual = projected.reshape(-1, 2) - ordered_points + reprojection_values.append( + float(np.sqrt(np.mean(np.sum(np.square(residual), axis=1)))) + ) + rotation_matrix, _ = cv2.Rodrigues(rotation_vector) + transform = np.eye(4) + transform[:3, :3] = rotation_matrix + transform[:3, 3] = np.asarray(translation).reshape(3) + pose_candidates.append(transform) + if not pose_candidates: + return None + return BoardPose( + stamp_ns=_stamp_ns(message), + camera_from_board_candidates=tuple(pose_candidates), + reprojection_rms_px=min(reprojection_values), + image_points=image_points, + ) + + def _assess_pair(self, other: str) -> PairAssessment: + """Evaluate the newest synchronized pair without recording it.""" + if not self.image_history["front"] or not self.image_history[other]: + return PairAssessment( + other, False, "waiting_for_images", f"front/{other}尚未收到图像" + ) + frame_pair = _select_latest_synchronised_pair( + self.image_history["front"], + self.image_history[other], + self.maximum_pair_skew_ns, + ) + if frame_pair is None: + minimum_skew = _minimum_history_skew_ns( + self.image_history["front"], self.image_history[other] + ) + message = ( + f"front/{other}没有可配对的图像" + if minimum_skew is None + else ( + f"front/{other}最近图像历史的最小时间差" + f"{minimum_skew / 1e6:.1f}ms，超过" + f"{self.maximum_pair_skew_ns / 1e6:.1f}ms门限" + ) + ) + return PairAssessment( + other, False, "image_skew_too_large", message + ) + + front_frame, selected_frame, skew = frame_pair + front = self._detect_board_pose("front", front_frame.message) + selected = self._detect_board_pose(other, selected_frame.message) + missing = [ + view + for view, pose in (("front", front), (other, selected)) + if pose is None + ] + if missing: + return PairAssessment( + other=other, + ready=False, + reason_code="checkerboard_missing", + message=( + f"同步图像已找到（时间差{skew / 1e6:.1f}ms），" + f"但{'/'.join(missing)}没有检测到完整棋盘" + ), + front_frame=front_frame, + selected_frame=selected_frame, + front_pose=front, + selected_pose=selected, + skew_ns=skew, + ) + + assert front is not None and selected is not None + pair_rms = float( + np.sqrt( + np.mean( + np.square( + [ + front.reprojection_rms_px, + selected.reprojection_rms_px, + ] + ) + ) + ) + ) + common = { + "other": other, + "front_frame": front_frame, + "selected_frame": selected_frame, + "front_pose": front, + "selected_pose": selected, + "skew_ns": skew, + "pair_reprojection_rms_px": pair_rms, + } + if pair_rms > self.maximum_reprojection_rms_px: + return PairAssessment( + ready=False, + reason_code="reprojection_rms_too_large", + message=( + f"当前组重投影RMS={pair_rms:.3f}px超过" + f"{self.maximum_reprojection_rms_px:.3f}px门限，未计入。" + f"front={front.reprojection_rms_px:.3f}px，" + f"{other}={selected.reprojection_rms_px:.3f}px；" + f"已保留{len(self.captures[other])}/{self.minimum_captures}组" + ), + **common, + ) + + transform_candidates = tuple( + front_pose @ _inverse(other_pose) + for front_pose in front.camera_from_board_candidates + for other_pose in selected.camera_from_board_candidates + ) + previous = self.captures[other] + board_poses = self.captured_front_board[other] + if board_poses: + board_change_rotation = ( + Rotation.from_matrix(board_poses[-1][:3, :3]).inv() + * Rotation.from_matrix( + front.camera_from_board_candidates[0][:3, :3] + ) + ).magnitude() + board_change_translation = np.linalg.norm( + board_poses[-1][:3, 3] + - front.camera_from_board_candidates[0][:3, 3] + ) + if ( + board_change_rotation < math.radians(2.0) + and board_change_translation < 0.015 + ): + return PairAssessment( + ready=False, + reason_code="board_pose_too_close", + message=( + "棋盘姿态与上次过近，" + "请改变位置或倾角后再采集" + ), + transform_candidates=transform_candidates, + **common, + ) + + current_fit = self.fit_results[other] + rotation_error = ( + None + if current_fit is None + else current_fit.rotation_stability_rad + ) + translation_error = ( + None + if current_fit is None + else current_fit.translation_stability_m + ) + metrics = { + "transform_candidates": transform_candidates, + "provisional_rotation_error_rad": rotation_error, + "provisional_translation_error_m": translation_error, + } + return PairAssessment( + ready=True, + reason_code="ready", + message=( + f"当前姿态合格，可加入候选；" + f"已采集{len(previous)}组" + ), + **common, + **metrics, + ) + + def _update_stereo_fit(self, other: str) -> StereoFitResult | None: + captures = self.stereo_captures[other] + if len(captures) < 3: + self.fit_results[other] = None + return None + identity = self.camera_identities.get("front") + front_matrix = self.camera_matrices.get("front") + other_matrix = self.camera_matrices.get(other) + if identity is None or front_matrix is None or other_matrix is None: + self.fit_results[other] = None + return None + try: + result = _fit_stereo_robust( + captures, + self.object_points, + front_matrix, + other_matrix, + (int(identity["width"]), int(identity["height"])), + minimum_inliers=self.minimum_captures, + maximum_rms_px=self.maximum_reprojection_rms_px, + maximum_rotation_stability_rad=( + self.maximum_rotation_repeatability_rad + ), + maximum_translation_stability_m=( + self.maximum_translation_repeatability_m + ), + ) + except (ValueError, cv2.error) as error: + self.fit_results[other] = None + self.get_logger().warning( + f"front/{other} joint stereo fit unavailable: {error}" + ) + return None + self.fit_results[other] = result + return result + + def _capture_pair( + self, other: str, response: Trigger.Response + ) -> Trigger.Response: + assessment = self._assess_pair(other) + if other == self.gui_active_other: + self.gui_last_assessment = assessment + if not assessment.ready: + response.success = False + response.message = assessment.message + return response + + assert assessment.front_pose is not None + assert assessment.selected_pose is not None + assert assessment.skew_ns is not None + assert assessment.pair_reprojection_rms_px is not None + assert assessment.front_pose.image_points is not None + assert assessment.selected_pose.image_points is not None + previous = self.captures[other] + previous.append(list(assessment.transform_candidates)) + self.stereo_captures[other].append( + StereoCapture( + front_points_px=assessment.front_pose.image_points.copy(), + other_points_px=assessment.selected_pose.image_points.copy(), + transform_candidates=assessment.transform_candidates, + pair_reprojection_rms_px=( + assessment.pair_reprojection_rms_px + ), + skew_ns=assessment.skew_ns, + ) + ) + self.captured_front_board[other].append( + assessment.front_pose.camera_from_board_candidates[0].copy() + ) + self.capture_reprojection[other].extend( + [ + assessment.front_pose.reprojection_rms_px, + assessment.selected_pose.reprojection_rms_px, + ] + ) + fit = self._update_stereo_fit(other) + fit_text = "联合拟合需要至少3组" + if fit is not None: + fit_text = ( + f"联合RMS={fit.stereo_rms_px:.3f}px，" + f"内点={len(fit.inlier_indices)}/{len(previous)}，" + f"稳定性={math.degrees(fit.rotation_stability_rad):.3f}°/" + f"{fit.translation_stability_m * 1000.0:.3f}mm" + ) + response.success = True + response.message = ( + f"front/{other}已加入候选 {len(previous)}组，" + f"时间差{assessment.skew_ns / 1e6:.1f}ms，" + f"重投影RMS={assessment.pair_reprojection_rms_px:.3f}px，" + f"{fit_text}" + ) + return response + + @staticmethod + def _fit_gui_image(image: np.ndarray, width: int, height: int) -> np.ndarray: + canvas = np.full((height, width, 3), 24, dtype=np.uint8) + scale = min(width / image.shape[1], height / image.shape[0]) + resized = cv2.resize( + image, + ( + max(1, int(round(image.shape[1] * scale))), + max(1, int(round(image.shape[0] * scale))), + ), + interpolation=cv2.INTER_AREA, + ) + x = (width - resized.shape[1]) // 2 + y = (height - resized.shape[0]) // 2 + canvas[y : y + resized.shape[0], x : x + resized.shape[1]] = resized + return canvas + + def _annotated_gui_frame( + self, + frame: ImageFrame | None, + pose: BoardPose | None, + label: str, + ) -> np.ndarray: + if frame is None: + image = np.full((480, 640, 3), 24, dtype=np.uint8) + cv2.putText( + image, + f"{label}: waiting for image", + (30, 60), + cv2.FONT_HERSHEY_SIMPLEX, + 0.8, + (0, 180, 255), + 2, + cv2.LINE_AA, + ) + return image + mono = self.bridge.imgmsg_to_cv2( + frame.message, desired_encoding="mono8" + ) + image = cv2.cvtColor(mono, cv2.COLOR_GRAY2BGR) + if pose is not None and pose.image_points is not None: + cv2.drawChessboardCorners( + image, + (self.columns, self.rows), + pose.image_points.reshape(-1, 1, 2), + True, + ) + rms = "--" if pose is None else f"{pose.reprojection_rms_px:.3f}px" + color = ( + (40, 220, 40) + if pose is not None + and pose.reprojection_rms_px <= self.maximum_reprojection_rms_px + else (0, 180, 255) + ) + cv2.putText( + image, + f"{label.upper()} RMS {rms}", + (30, 55), + cv2.FONT_HERSHEY_SIMPLEX, + 1.1, + color, + 3, + cv2.LINE_AA, + ) + return image + + @staticmethod + def _draw_gui_button( + canvas: np.ndarray, + rectangle: tuple[int, int, int, int], + label: str, + color: tuple[int, int, int], + ) -> None: + x1, y1, x2, y2 = rectangle + cv2.rectangle(canvas, (x1, y1), (x2, y2), color, -1) + cv2.rectangle(canvas, (x1, y1), (x2, y2), (220, 220, 220), 1) + size, _ = cv2.getTextSize( + label, cv2.FONT_HERSHEY_SIMPLEX, 0.65, 2 + ) + x = x1 + max(8, (x2 - x1 - size[0]) // 2) + y = y1 + (y2 - y1 + size[1]) // 2 + cv2.putText( + canvas, + label, + (x, y), + cv2.FONT_HERSHEY_SIMPLEX, + 0.65, + (255, 255, 255), + 2, + cv2.LINE_AA, + ) + + def _gui_reason_text(self, assessment: PairAssessment) -> str: + return { + "waiting_for_images": "WAITING FOR IMAGES", + "image_skew_too_large": "IMAGE TIME DIFFERENCE TOO LARGE", + "checkerboard_missing": "FULL CHECKERBOARD NOT FOUND", + "reprojection_rms_too_large": "REPROJECTION RMS TOO HIGH", + "board_pose_too_close": "MOVE BOARD TO A NEW POSE", + "ready": "READY - ADD CANDIDATE", + }.get(assessment.reason_code, assessment.reason_code.upper()) + + def _gui_mouse_callback(self, event, x, y, flags, parameter) -> None: + del flags, parameter + if event != cv2.EVENT_LBUTTONUP: + return + buttons = getattr(self, "gui_buttons", {}) + for name, rectangle in buttons.items(): + x1, y1, x2, y2 = rectangle + if not (x1 <= x <= x2 and y1 <= y <= y2): + continue + if name in ("side", "top"): + self.gui_active_other = name + self.gui_last_assessment = None + self.gui_auto_pose = None + self.gui_auto_stable_since = None + self.gui_feedback = f"Selected FRONT + {name.upper()}" + elif name == "auto": + self.gui_auto_capture = not self.gui_auto_capture + self.gui_auto_pose = None + self.gui_auto_stable_since = None + self.gui_feedback = ( + "Auto capture enabled" + if self.gui_auto_capture + else "Auto capture disabled" + ) + elif name == "capture": + if ( + self.gui_last_assessment is not None + and self.gui_last_assessment.ready + ): + self.gui_action = "capture" + elif name == "save": + if all( + self.fit_results[view] is not None + and bool(self.fit_results[view].passed) + for view in ("side", "top") + ): + self.gui_action = "save" + break + + def _gui_timer_callback(self) -> None: + if not self.gui_enabled: + return + try: + action = self.gui_action + self.gui_action = None + if action == "capture": + response = self._capture_pair( + self.gui_active_other, Trigger.Response() + ) + if response.success: + self.gui_feedback = ( + f"Captured FRONT + {self.gui_active_other.upper()} " + f"{len(self.captures[self.gui_active_other])}/" + f"{self.minimum_captures}" + ) + self.get_logger().info(response.message) + else: + self.gui_feedback = "Capture rejected; see quality status" + self.get_logger().warning(response.message) + elif action == "save": + response = self._save(None, Trigger.Response()) + if response.success: + self.gui_feedback = f"Saved: {self.output_file}" + self.get_logger().info(response.message) + else: + self.gui_feedback = "Save failed; see terminal" + self.get_logger().warning(response.message) + + assessment = self._assess_pair(self.gui_active_other) + self.gui_last_assessment = assessment + active_fit = self.fit_results[self.gui_active_other] + if ( + self.gui_auto_capture + and active_fit is not None + and active_fit.passed + ): + self.gui_auto_capture = False + self.gui_feedback = ( + f"FRONT + {self.gui_active_other.upper()} PASSED; " + "switch pair" + ) + if self.gui_auto_capture and assessment.ready: + assert assessment.front_pose is not None + current_pose = ( + assessment.front_pose.camera_from_board_candidates[0] + ) + now = time.monotonic() + stable = False + if self.gui_auto_pose is not None: + rotation_change, translation_change = _transform_residual( + self.gui_auto_pose, current_pose + ) + stable = bool( + rotation_change <= math.radians(0.5) + and translation_change <= 0.002 + ) + if stable: + if self.gui_auto_stable_since is None: + self.gui_auto_stable_since = now + elif ( + now - self.gui_auto_stable_since + >= self.auto_capture_stable_seconds + ): + response = self._capture_pair( + self.gui_active_other, Trigger.Response() + ) + if response.success: + self.gui_feedback = ( + f"Auto captured FRONT + " + f"{self.gui_active_other.upper()} " + f"{len(self.captures[self.gui_active_other])}" + ) + self.get_logger().info(response.message) + else: + self.gui_feedback = "Auto capture rejected" + self.get_logger().warning(response.message) + self.gui_auto_pose = None + self.gui_auto_stable_since = None + assessment = self._assess_pair( + self.gui_active_other + ) + self.gui_last_assessment = assessment + else: + self.gui_auto_pose = current_pose.copy() + self.gui_auto_stable_since = now + else: + self.gui_auto_pose = None + self.gui_auto_stable_since = None + front_frame = assessment.front_frame + selected_frame = assessment.selected_frame + if front_frame is None and self.image_history["front"]: + front_frame = self.image_history["front"][-1] + if selected_frame is None and self.image_history[self.gui_active_other]: + selected_frame = self.image_history[self.gui_active_other][-1] + front_image = self._annotated_gui_frame( + front_frame, assessment.front_pose, "front" + ) + selected_image = self._annotated_gui_frame( + selected_frame, + assessment.selected_pose, + self.gui_active_other, + ) + view_width, view_height = 640, 488 + left = self._fit_gui_image(front_image, view_width, view_height) + right = self._fit_gui_image( + selected_image, view_width, view_height + ) + canvas = np.full((700, 1300, 3), 18, dtype=np.uint8) + canvas[10 : 10 + view_height, 5 : 5 + view_width] = left + canvas[10 : 10 + view_height, 655 : 655 + view_width] = right + + ready_color = (40, 220, 40) if assessment.ready else (0, 80, 230) + batch_status = ( + " | BATCH PASSED" + if active_fit is not None and active_fit.passed + else "" + ) + cv2.putText( + canvas, + self._gui_reason_text(assessment) + batch_status, + (20, 530), + cv2.FONT_HERSHEY_SIMPLEX, + 0.85, + ready_color, + 2, + cv2.LINE_AA, + ) + pair_rms = ( + "--" + if assessment.pair_reprojection_rms_px is None + else f"{assessment.pair_reprojection_rms_px:.3f}" + ) + skew = ( + "--" + if assessment.skew_ns is None + else f"{assessment.skew_ns / 1e6:.1f}" + ) + rotation = ( + "--" + if assessment.provisional_rotation_error_rad is None + else f"{math.degrees(assessment.provisional_rotation_error_rad):.3f}" + ) + translation = ( + "--" + if assessment.provisional_translation_error_m is None + else f"{assessment.provisional_translation_error_m * 1000.0:.3f}" + ) + cv2.putText( + canvas, + f"Pair RMS {pair_rms}/{self.maximum_reprojection_rms_px:.3f}px " + f"Skew {skew}/{self.maximum_pair_skew_ns / 1e6:.1f}ms " + f"Batch stability {rotation}/" + f"{math.degrees(self.maximum_rotation_repeatability_rad):.3f}deg " + f"{translation}/" + f"{self.maximum_translation_repeatability_m * 1000.0:.3f}mm", + (20, 562), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + (220, 220, 220), + 1, + cv2.LINE_AA, + ) + cv2.putText( + canvas, + f"Candidates SIDE {len(self.stereo_captures['side'])} " + f"(inliers " + f"{0 if self.fit_results['side'] is None else len(self.fit_results['side'].inlier_indices)}) " + f"TOP {len(self.stereo_captures['top'])} (inliers " + f"{0 if self.fit_results['top'] is None else len(self.fit_results['top'].inlier_indices)}) | " + f"{self.gui_feedback}", + (20, 590), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + (200, 200, 200), + 1, + cv2.LINE_AA, + ) + + self.gui_buttons = { + "side": (20, 615, 220, 680), + "top": (235, 615, 435, 680), + "auto": (450, 615, 635, 680), + "capture": (650, 615, 970, 680), + "save": (985, 615, 1280, 680), + } + self._draw_gui_button( + canvas, + self.gui_buttons["side"], + "FRONT + SIDE", + (170, 105, 20) + if self.gui_active_other == "side" + else (75, 75, 75), + ) + self._draw_gui_button( + canvas, + self.gui_buttons["top"], + "FRONT + TOP", + (170, 105, 20) + if self.gui_active_other == "top" + else (75, 75, 75), + ) + self._draw_gui_button( + canvas, + self.gui_buttons["auto"], + "AUTO ON" if self.gui_auto_capture else "AUTO OFF", + (30, 155, 30) if self.gui_auto_capture else (75, 75, 75), + ) + self._draw_gui_button( + canvas, + self.gui_buttons["capture"], + "ADD CANDIDATE" if assessment.ready else "ADD (DISABLED)", + (30, 155, 30) if assessment.ready else (65, 65, 65), + ) + save_ready = all( + self.fit_results[view] is not None + and bool(self.fit_results[view].passed) + for view in ("side", "top") + ) + self._draw_gui_button( + canvas, + self.gui_buttons["save"], + "SAVE" if save_ready else "SAVE (DISABLED)", + (30, 155, 30) if save_ready else (65, 65, 65), + ) + cv2.imshow(self.GUI_WINDOW, canvas) + cv2.waitKey(1) + except cv2.error as error: + self.gui_enabled = False + self.get_logger().error(f"Extrinsics GUI disabled: {error}") + + def _capture_front_side(self, request, response): + del request + return self._capture_pair("side", response) + + def _capture_front_top(self, request, response): + del request + return self._capture_pair("top", response) + + def _save(self, request, response): + del request + if set(self.camera_identities) != set(VIEWS): + response.success = False + response.message = "三台相机内参尚未全部收到" + return response + if any( + len(self.stereo_captures[view]) < self.minimum_captures + for view in ("side", "top") + ): + response.success = False + response.message = ( + f"候选采集不足：side={len(self.stereo_captures['side'])}，" + f"top={len(self.stereo_captures['top'])}" + ) + return response + results: dict[str, StereoFitResult] = {} + for view in ("side", "top"): + result = self._update_stereo_fit(view) + if result is None: + response.success = False + response.message = f"front/{view}联合外参拟合失败" + return response + results[view] = result + if any(not result.passed for result in results.values()): + response.success = False + response.message = "联合外参质量不合格：" + "; ".join( + f"{view}: RMS={result.stereo_rms_px:.3f}px, " + f"内点={len(result.inlier_indices)}/" + f"{len(self.stereo_captures[view])}, " + f"旋转稳定性=" + f"{math.degrees(result.rotation_stability_rad):.3f}°, " + f"平移稳定性=" + f"{result.translation_stability_m * 1000.0:.3f}mm" + for view, result in results.items() + ) + return response + transforms = { + "front": np.eye(4), + "side": results["side"].front_from_other, + "top": results["top"].front_from_other, + } + quality = { + "passed": True, + "reprojection_rms_px": max( + result.stereo_rms_px for result in results.values() + ), + "maximum_rotation_repeatability_deg": math.degrees( + max( + result.rotation_stability_rad + for result in results.values() + ) + ), + "maximum_translation_repeatability_m": max( + result.translation_stability_m for result in results.values() + ), + "front_side_captures": len(results["side"].inlier_indices), + "front_top_captures": len(results["top"].inlier_indices), + "front_side_candidates": len(self.stereo_captures["side"]), + "front_top_candidates": len(self.stereo_captures["top"]), + "front_side_rejected": len(results["side"].rejected_indices), + "front_top_rejected": len(results["top"].rejected_indices), + } + dump_three_camera_extrinsics( + self.output_file, + cameras=self.camera_identities, + front_from_view=transforms, + quality=quality, + ) + response.success = True + response.message = f"外参已保存：{self.output_file}；{quality}" + return response + + def _reset(self, request, response): + del request + for values in self.captures.values(): + values.clear() + for values in self.capture_reprojection.values(): + values.clear() + for values in self.stereo_captures.values(): + values.clear() + for values in self.captured_front_board.values(): + values.clear() + self.fit_results = {"side": None, "top": None} + self.gui_auto_pose = None + self.gui_auto_stable_since = None + response.success = True + response.message = "已清空外参采样" + return response + + +def main(args: list[str] | None = None) -> None: + configure_fastdds_large_image_transport() + rclpy.init(args=args) + node = ThreeCameraExtrinsicsNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node.gui_enabled: + cv2.destroyAllWindows() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py index b21d431..04435c2 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py @@ -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) != { diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py index 6b575df..13f9a36 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/storage.py @@ -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(): diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py index 2869978..77a7b40 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py @@ -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) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py index 6e8acee..d11400b 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py @@ -10,7 +10,7 @@ from pathlib import Path import random import re import time -from typing import Any, Mapping +from typing import Any, Mapping, Sequence import numpy as np import rclpy @@ -31,16 +31,23 @@ from .acquisition import ( update_pnp_reset_watchdog, ) from .core import ( - BASELINE_COMMAND, COMMAND_NAMES, DIRECTION_DECREASING, DIRECTION_INCREASING, + robust_rotation_summary, +) +from .extrinsics import ( + ThreeCameraExtrinsics, + camera_info_fingerprint, + load_three_camera_extrinsics, + matrix_payload, + transform_matrix, ) from .full_hand import ( - IMAGE_TRAJECTORY_JOINTS, JOINT_SPECS, MEASURED_JOINTS, SWEEP_SPECS, + THREE_CAMERA_BASELINE_COMMAND, VIEW_TAGS, JointCurveFit, SweepSpec, @@ -48,10 +55,6 @@ from .full_hand import ( build_calibration_speed_profile, build_compact_payload, calibration_auxiliary_commands, - center_splay_curve, - fit_measured_joint_curve, - fit_projected_zero, - measure_joint_observation, ) from .hikrobot_camera import configure_fastdds_large_image_transport from .pnp import ( @@ -59,8 +62,23 @@ from .pnp import ( SquareTagPose, SquareTagPoseTracker, ) -from .storage import append_jsonl, atomic_write_json +from .storage import append_jsonl, append_jsonl_many, atomic_write_json from .three_camera_diagnostics import render_three_camera_status_text_zh +from .urdf_zero import ( + AXIS_JOINTS, + CONSTRAINED_CIRCLE_JOINTS, + ZERO_AXIS_OBSERVATION_JOINTS, + ZERO_LINE_OBSERVATION_JOINTS, + ZERO_ROOT_ANCHOR_JOINTS, + JointAxisMeasurement, + ZeroSolveResult, + fit_joint_axis_measurement, + fit_rotation_joint_curve, + measure_rotation_joint_observation, + rotation_curve_holdout_errors, + solve_urdf_zero_offsets, + write_zero_corrected_urdf, +) STATE_PREFLIGHT = "PREFLIGHT" @@ -85,6 +103,24 @@ def _safe_name(value: str) -> str: return safe or "UNSET" +def _robust_pose_payload( + poses: Sequence[Mapping[str, Sequence[float]]], +) -> dict[str, list[float]]: + if not poses: + raise ValueError("at least one pose is required") + translation = np.median( + np.asarray([pose["translation_xyz_m"] for pose in poses], dtype=float), + axis=0, + ) + quaternion = robust_rotation_summary( + [pose["quaternion_xyzw"] for pose in poses] + )[0] + return { + "translation_xyz_m": [float(value) for value in translation], + "quaternion_xyzw": [float(value) for value in quaternion], + } + + def _view_pairs(view: str) -> tuple[tuple[str, str], ...]: if view == "front": return ( @@ -113,6 +149,10 @@ class FrameObservation: state_sync_error_ns: int joint_vectors_xyz_m: Mapping[str, tuple[float, float, float]] image_vectors_xy_px: Mapping[str, tuple[float, float]] + joint_quaternions_xyzw: Mapping[str, tuple[float, float, float, float]] + parent_poses_common: Mapping[str, Mapping[str, list[float]]] + child_poses_common: Mapping[str, Mapping[str, list[float]]] + joint_reprojection_error_px: Mapping[str, float] @dataclass(frozen=True) @@ -147,6 +187,10 @@ class ViewRuntime: camera_matrix: np.ndarray | None = None camera_info_valid: bool = False camera_frame: str = "" + image_width: int = 0 + image_height: int = 0 + intrinsics_sha256: str = "" + extrinsics_valid: bool = False valid_flags: deque[bool] = field(default_factory=deque) detection_times: deque[float] = field(default_factory=deque) latest_tag_quality: dict[str, TagQuality] = field(default_factory=dict) @@ -201,6 +245,7 @@ class G20ThreeCameraCalibrationNode(Node): for name in ("front", "side", "top") } self.latest_state_u8: tuple[float, ...] = () + self.last_state_at = 0.0 self.state_history: deque[StateSample] = deque(maxlen=1200) self.latest_hand_info: dict[str, Any] = {} self.commanded_speed_profile: tuple[int, ...] = () @@ -218,10 +263,16 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_index = 0 self.active_sweep: SweepItem | None = None self.sweep_frames: list[FrameObservation] = [] + # Keep synchronised endpoint observations acquired while the motor is + # held at the sweep start. If these frames are discarded and the + # target is commanded immediately, a fast motor can leave the endpoint + # before the next camera/state pair arrives. + self.sweep_start_frames: list[FrameObservation] = [] self.sweep_started_at = 0.0 self.sweep_last_valid_at = 0.0 self.sweep_endpoint_since: float | None = None self.retry_sweep_spec: SweepSpec | None = None + self.retry_resume_index: int | None = None self.fit_failure: dict[str, Any] = {} self.sweep_attempts: dict[int, int] = { spec.motor_index: 1 for spec in SWEEP_SPECS @@ -231,9 +282,9 @@ class G20ThreeCameraCalibrationNode(Node): } self.measured_fits: dict[str, JointCurveFit] = {} - self.projected_zeros_rad: dict[str, float] = {} - self.splay_zero_command_u8 = 0 - self.splay_midpoint_rad = 0.0 + self.axis_measurements: list[JointAxisMeasurement] = [] + self.zero_result: ZeroSolveResult | None = None + self.corrected_urdf_path: Path | None = None self.fit_quality_passed = False self.validation_items: list[ValidationItem] = [] @@ -292,6 +343,9 @@ class G20ThreeCameraCalibrationNode(Node): def _declare_parameters(self) -> None: self.declare_parameter("serial_number", "UNSET") self.declare_parameter("session_dir", "calibration_output/session") + self.declare_parameter("camera_extrinsics_file", "") + self.declare_parameter("source_urdf_path", "") + self.declare_parameter("corrected_urdf_output_dir", "") self.declare_parameter("commands_enabled", True) self.declare_parameter("command_topic", "/g20/cb_left_hand_control_cmd") self.declare_parameter("state_topic", "/g20/cb_left_hand_state") @@ -300,6 +354,7 @@ class G20ThreeCameraCalibrationNode(Node): "setting_topic", "/g20/cb_hand_setting_cmd" ) for view in ("front", "side", "top"): + self.declare_parameter(f"{view}_camera_serial", "") self.declare_parameter( f"{view}_camera_info_topic", f"/g20_calibration/{view}/camera/camera_info", @@ -308,8 +363,10 @@ class G20ThreeCameraCalibrationNode(Node): f"{view}_detections_topic", f"/g20_calibration/{view}/apriltag/detections", ) - self.declare_parameter("tag_size_m", 0.010) - self.declare_parameter("baseline_command_u8", list(BASELINE_COMMAND)) + self.declare_parameter("tag_size_m", 0.016) + self.declare_parameter( + "baseline_command_u8", list(THREE_CAMERA_BASELINE_COMMAND) + ) self.declare_parameter("normal_calibration_speed", 15) self.declare_parameter("index_roll_calibration_speed", 5) self.declare_parameter("index_flex_calibration_speed", 10) @@ -328,8 +385,21 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("pnp_maximum_tag_tilt_deg", 75.0) self.declare_parameter("pnp_tracker_reset_seconds", 5.0) self.declare_parameter("top_pnp_invalid_reset_seconds", 1.0) - self.declare_parameter("maximum_state_image_skew_ms", 150.0) + self.declare_parameter("maximum_state_image_skew_ms", 50.0) + self.declare_parameter("axis_maximum_plane_rms_m", 0.003) + self.declare_parameter("passive_axis_maximum_plane_rms_m", 0.004) + self.declare_parameter("axis_maximum_radial_rms_m", 0.003) + self.declare_parameter("axis_maximum_rotation_circle_difference_deg", 1.0) + self.declare_parameter( + "active_maximum_rotation_orthogonal_rms_deg", 2.5 + ) + self.declare_parameter( + "passive_maximum_rotation_orthogonal_rms_deg", 7.5 + ) + self.declare_parameter("zero_maximum_axis_cycle_difference_deg", 0.75) + self.declare_parameter("zero_maximum_offset_deg", 20.0) self.declare_parameter("endpoint_tolerance_u8", 2.0) + self.declare_parameter("thumb_yaw_zero_endpoint_tolerance_u8", 3.0) self.declare_parameter("endpoint_hold_seconds", 0.5) self.declare_parameter("baseline_hold_seconds", 0.5) self.declare_parameter("position_timeout_seconds", 30.0) @@ -352,10 +422,6 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter( "passive_maximum_cycle_travel_difference_deg", 10.0 ) - self.declare_parameter("zero_minimum_radius_px", 20.0) - self.declare_parameter("zero_maximum_radial_rms_px", 2.0) - self.declare_parameter("zero_maximum_radial_p95_px", 3.5) - self.declare_parameter("zero_maximum_round_difference_deg", 1.0) self.declare_parameter("maximum_monotonic_correction_deg", 2.0) self.declare_parameter("maximum_hysteresis_deg", 5.0) self.declare_parameter("passive_maximum_monotonic_correction_deg", 3.0) @@ -365,8 +431,8 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("validation_frames", 10) self.declare_parameter("validation_seed", 20260804) self.declare_parameter("validation_timeout_seconds", 20.0) - self.declare_parameter("maximum_validation_mae_deg", 2.0) - self.declare_parameter("maximum_validation_p95_deg", 3.0) + self.declare_parameter("maximum_validation_mae_deg", 1.0) + self.declare_parameter("maximum_validation_p95_deg", 2.0) def _load_parameters(self) -> None: def value(name: str) -> Any: @@ -376,6 +442,30 @@ class G20ThreeCameraCalibrationNode(Node): if self.serial_number == "UNSET": raise ValueError("serial_number is required") self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() + self.camera_extrinsics_file = Path( + str(value("camera_extrinsics_file")) + ).expanduser().resolve() + self.source_urdf_path = Path( + str(value("source_urdf_path")) + ).expanduser().resolve() + output_value = str(value("corrected_urdf_output_dir")) + self.corrected_urdf_output_dir = ( + Path(output_value).expanduser().resolve() + if output_value + else self.source_urdf_path.parent + ) + self.camera_serials = { + view: str(value(f"{view}_camera_serial")) + for view in ("front", "side", "top") + } + self.extrinsics: ThreeCameraExtrinsics | None = None + self.extrinsics_error = "" + try: + self.extrinsics = load_three_camera_extrinsics( + self.camera_extrinsics_file + ) + except Exception as error: + self.extrinsics_error = str(error) self.commands_enabled = bool(value("commands_enabled")) self.command_topic = str(value("command_topic")) self.state_topic = str(value("state_topic")) @@ -430,7 +520,34 @@ class G20ThreeCameraCalibrationNode(Node): self.maximum_state_image_skew_ns = int( float(value("maximum_state_image_skew_ms")) * 1_000_000.0 ) + self.axis_maximum_plane_rms_m = float( + value("axis_maximum_plane_rms_m") + ) + self.passive_axis_maximum_plane_rms_m = float( + value("passive_axis_maximum_plane_rms_m") + ) + self.axis_maximum_radial_rms_m = float( + value("axis_maximum_radial_rms_m") + ) + self.axis_maximum_rotation_circle_difference_rad = math.radians( + float(value("axis_maximum_rotation_circle_difference_deg")) + ) + self.active_maximum_rotation_orthogonal_rms_rad = math.radians( + float(value("active_maximum_rotation_orthogonal_rms_deg")) + ) + self.passive_maximum_rotation_orthogonal_rms_rad = math.radians( + float(value("passive_maximum_rotation_orthogonal_rms_deg")) + ) + self.zero_maximum_axis_cycle_difference_rad = math.radians( + float(value("zero_maximum_axis_cycle_difference_deg")) + ) + self.zero_maximum_offset_rad = math.radians( + float(value("zero_maximum_offset_deg")) + ) self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) + self.thumb_yaw_zero_endpoint_tolerance_u8 = float( + value("thumb_yaw_zero_endpoint_tolerance_u8") + ) self.endpoint_hold_seconds = float(value("endpoint_hold_seconds")) self.baseline_hold_seconds = float(value("baseline_hold_seconds")) self.position_timeout_seconds = float(value("position_timeout_seconds")) @@ -467,16 +584,6 @@ class G20ThreeCameraCalibrationNode(Node): self.passive_maximum_cycle_travel_difference_rad = math.radians( float(value("passive_maximum_cycle_travel_difference_deg")) ) - self.zero_minimum_radius_px = float(value("zero_minimum_radius_px")) - self.zero_maximum_radial_rms_px = float( - value("zero_maximum_radial_rms_px") - ) - self.zero_maximum_radial_p95_px = float( - value("zero_maximum_radial_p95_px") - ) - self.zero_maximum_round_difference_rad = math.radians( - float(value("zero_maximum_round_difference_deg")) - ) self.maximum_monotonic_correction_rad = math.radians( float(value("maximum_monotonic_correction_deg")) ) @@ -506,6 +613,26 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError("baseline_command_u8 must contain exactly 20 values") if any(value < 0 or value > 255 for value in self.baseline_command): raise ValueError("baseline_command_u8 values must be in [0, 255]") + if self.baseline_command != THREE_CAMERA_BASELINE_COMMAND: + raise ValueError( + "schema v4 requires the fixed G20 three-camera baseline" + ) + if any(not serial for serial in self.camera_serials.values()): + raise ValueError("front/side/top camera serial parameters are required") + if not self.source_urdf_path.is_file(): + raise ValueError( + f"source_urdf_path does not exist: {self.source_urdf_path}" + ) + if ( + "zero_calibrated" in self.source_urdf_path.stem.lower() + or re.search( + r"calibrated_20\d{6}", + self.source_urdf_path.stem.lower(), + ) + ): + raise ValueError( + "source_urdf_path must be the original CAD URDF" + ) if not 0 <= self.normal_calibration_speed <= 255: raise ValueError("normal_calibration_speed must be in [0, 255]") if not 0 <= self.index_roll_calibration_speed <= 255: @@ -514,6 +641,17 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError("index_flex_calibration_speed must be in [0, 255]") if self.speed_setting_settle_seconds < 0.0: raise ValueError("speed_setting_settle_seconds must be non-negative") + if not 0.0 <= self.endpoint_tolerance_u8 <= 10.0: + raise ValueError("endpoint_tolerance_u8 must be in [0, 10]") + if not ( + self.endpoint_tolerance_u8 + <= self.thumb_yaw_zero_endpoint_tolerance_u8 + <= 10.0 + ): + raise ValueError( + "thumb_yaw_zero_endpoint_tolerance_u8 must be between the " + "default endpoint tolerance and 10" + ) if self.top_pnp_invalid_reset_seconds <= 0.0: raise ValueError("top_pnp_invalid_reset_seconds must be positive") if not all( @@ -522,6 +660,10 @@ class G20ThreeCameraCalibrationNode(Node): self.image_trajectory_maximum_radial_rms_px, self.image_trajectory_maximum_radial_p95_px, self.image_trajectory_minimum_radius_px, + self.axis_maximum_plane_rms_m, + self.passive_axis_maximum_plane_rms_m, + self.active_maximum_rotation_orthogonal_rms_rad, + self.passive_maximum_rotation_orthogonal_rms_rad, self.trajectory_maximum_cycle_travel_difference_rad, self.passive_maximum_cycle_travel_difference_rad, self.passive_maximum_monotonic_correction_rad, @@ -593,6 +735,29 @@ class G20ThreeCameraCalibrationNode(Node): runtime.camera_info_valid = valid runtime.camera_matrix = matrix if valid else None runtime.camera_frame = str(message.header.frame_id) + runtime.image_width = int(message.width) + runtime.image_height = int(message.height) + runtime.intrinsics_sha256 = ( + "" + if matrix is None + else camera_info_fingerprint( + width=message.width, + height=message.height, + camera_matrix=message.k, + distortion=message.d, + rectification=message.r, + projection=message.p, + ) + ) + runtime.extrinsics_valid = False + if valid and self.extrinsics is not None: + runtime.extrinsics_valid = self.extrinsics.camera_matches( + view, + serial_number=self.camera_serials[view], + width=runtime.image_width, + height=runtime.image_height, + intrinsics_sha256=runtime.intrinsics_sha256, + ) def _state_callback(self, message: JointState) -> None: if len(message.position) != 20: @@ -606,6 +771,7 @@ class G20ThreeCameraCalibrationNode(Node): if stamp <= 0: stamp = int(self.get_clock().now().nanoseconds) self.latest_state_u8 = state + self.last_state_at = time.monotonic() if not self.state_history or stamp > self.state_history[-1].stamp_ns: self.state_history.append(StateSample(stamp, state)) @@ -734,6 +900,13 @@ class G20ThreeCameraCalibrationNode(Node): state_u8, sync_error_ns = matched joint_vectors: dict[str, tuple[float, float, float]] = {} image_vectors: dict[str, tuple[float, float]] = {} + joint_quaternions: dict[str, tuple[float, float, float, float]] = {} + parent_poses_common: dict[str, Mapping[str, list[float]]] = {} + child_poses_common: dict[str, Mapping[str, list[float]]] = {} + joint_reprojection: dict[str, float] = {} + if self.extrinsics is None or not runtime.extrinsics_valid: + return + front_from_view = self.extrinsics.transform(view) for name, spec in JOINT_SPECS.items(): if not spec.measured or spec.view != view: continue @@ -749,8 +922,26 @@ class G20ThreeCameraCalibrationNode(Node): centres_by_role[spec.child_role] - centres_by_role[spec.parent_role] ) + parent_matrix = front_from_view @ transform_matrix( + parent.translation_xyz_m, parent.quaternion_xyzw + ) + child_matrix = front_from_view @ transform_matrix( + child.translation_xyz_m, child.quaternion_xyzw + ) + relative_rotation = Rotation.from_matrix( + parent_matrix[:3, :3] + ).inv() * Rotation.from_matrix(child_matrix[:3, :3]) joint_vectors[name] = tuple(float(value) for value in relative) image_vectors[name] = tuple(float(value) for value in image_relative) + joint_quaternions[name] = tuple( + float(value) for value in relative_rotation.as_quat() + ) + parent_poses_common[name] = matrix_payload(parent_matrix) + child_poses_common[name] = matrix_payload(child_matrix) + joint_reprojection[name] = max( + float(parent.reprojection_error_px), + float(child.reprojection_error_px), + ) observation = FrameObservation( stamp_ns=stamp, received_at=now, @@ -759,10 +950,30 @@ class G20ThreeCameraCalibrationNode(Node): state_sync_error_ns=int(sync_error_ns), joint_vectors_xyz_m=joint_vectors, image_vectors_xy_px=image_vectors, + joint_quaternions_xyzw=joint_quaternions, + parent_poses_common=parent_poses_common, + child_poses_common=child_poses_common, + joint_reprojection_error_px=joint_reprojection, ) self._accept_frame(observation) def _accept_frame(self, observation: FrameObservation) -> None: + if self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: + if observation.view != self.active_sweep.spec.view: + return + if not self._motion_command_reached( + self.active_sweep.spec, + self.active_sweep.start_u8, + observation.state_u8, + ): + return + self.sweep_start_frames.append(observation) + maximum_start_frames = max( + 1, int(getattr(self, "preflight_frames", 30)) + ) + if len(self.sweep_start_frames) > maximum_start_frames: + del self.sweep_start_frames[:-maximum_start_frames] + return if self.state == STATE_SWEEP and self.active_sweep is not None: if observation.view != self.active_sweep.spec.view: return @@ -795,6 +1006,7 @@ class G20ThreeCameraCalibrationNode(Node): minimum_frames = min(30, self.preflight_frames) return bool( runtime.camera_info_valid + and runtime.extrinsics_valid and len(runtime.valid_flags) >= minimum_frames and runtime.valid_rate >= self.minimum_detection_rate and runtime.detection_hz >= self.minimum_detection_hz @@ -803,7 +1015,9 @@ class G20ThreeCameraCalibrationNode(Node): def _all_preflight_ready(self, now: float) -> bool: return bool( - len(self.latest_state_u8) == 20 + self.extrinsics is not None + and len(self.latest_state_u8) == 20 + and now - self.last_state_at <= 1.0 and all(self._view_ready(runtime, now) for runtime in self.views.values()) ) @@ -817,7 +1031,10 @@ class G20ThreeCameraCalibrationNode(Node): return None def _resume_preflight_ready(self, now: float) -> bool: - if len(self.latest_state_u8) != 20: + if ( + len(self.latest_state_u8) != 20 + or now - self.last_state_at > 1.0 + ): return False active_view = self._active_view_for_resume() return all( @@ -846,7 +1063,6 @@ class G20ThreeCameraCalibrationNode(Node): "reason": self.paused_reason, }, ) - self.retry_sweep_spec = None self.fit_failure = {} return spec @@ -883,11 +1099,32 @@ class G20ThreeCameraCalibrationNode(Node): ] self.sweep_index = 0 self.retry_sweep_spec = None + self.retry_resume_index = None self.fit_failure = {} self.sweep_attempts = { spec.motor_index: 1 for spec in SWEEP_SPECS } self.records_by_joint = {name: [] for name in MEASURED_JOINTS} + self.axis_measurements.clear() + self.zero_result = None + self.corrected_urdf_path = None + self.validation_errors_rad.clear() + self.completed_payload = None + self.sweep_frames.clear() + self.sweep_start_frames.clear() + append_jsonl( + self.raw_path, + { + "kind": "session_start", + "tag_family": "36h11", + "tag_size_m": float(self.tag_size_m), + "baseline_command_u8": [ + int(value) for value in self.baseline_command + ], + "camera_extrinsics_file": str(self.camera_extrinsics_file), + "source_urdf_path": str(self.source_urdf_path), + }, + ) self._begin_return_baseline("next_sweep") response.success = True response.message = "three-camera calibration started" @@ -929,6 +1166,7 @@ class G20ThreeCameraCalibrationNode(Node): runtime.group_tracker.reset() runtime.pnp_invalid_since = None self.sweep_frames.clear() + self.sweep_start_frames.clear() self.validation_frames_buffer.clear() if self.retry_sweep_spec is not None: self._prepare_failed_sweep_retry() @@ -1048,10 +1286,28 @@ class G20ThreeCameraCalibrationNode(Node): indices.update(calibration_auxiliary_commands(spec)) return all( abs(float(state[index]) - expected[index]) - <= self.endpoint_tolerance_u8 + <= ( + self._endpoint_tolerance_for_spec(spec, command_u8) + if index == spec.motor_index + else self.endpoint_tolerance_u8 + ) for index in indices ) + def _endpoint_tolerance_for_spec( + self, spec: SweepSpec, endpoint_u8: int + ) -> float: + """Return the measured feedback deadband for one sweep endpoint.""" + if spec.motor_index == 10 and int(endpoint_u8) == 0: + return float( + getattr( + self, + "thumb_yaw_zero_endpoint_tolerance_u8", + self.endpoint_tolerance_u8, + ) + ) + return float(self.endpoint_tolerance_u8) + def _start_next_sweep(self) -> None: if self.sweep_index >= len(self.sweep_items): self.active_sweep = None @@ -1059,6 +1315,8 @@ class G20ThreeCameraCalibrationNode(Node): return self.active_sweep = self.sweep_items[self.sweep_index] item = self.active_sweep + self.sweep_frames.clear() + self.sweep_start_frames.clear() self.position_hold_since = None self.motion_stage_started_at = time.monotonic() self.state = STATE_PREPARE_SWEEP @@ -1078,6 +1336,8 @@ class G20ThreeCameraCalibrationNode(Node): def _begin_active_sweep(self, now: float) -> None: assert self.active_sweep is not None self.sweep_frames.clear() + self.sweep_frames.extend(self.sweep_start_frames) + self.sweep_start_frames.clear() self.sweep_started_at = now self.sweep_last_valid_at = now self.sweep_endpoint_since = None @@ -1105,52 +1365,96 @@ class G20ThreeCameraCalibrationNode(Node): *, relaxed: bool = False, ) -> JointCurveFit: - return fit_measured_joint_curve( + del relaxed + zero_command = int( + self.baseline_command[JOINT_SPECS[joint_name].motor_index] + ) + return fit_rotation_joint_curve( + records, zero_command_u8=zero_command + ) + + def _fit_axis_measurement( + self, joint_name: str, cycle: int + ) -> JointAxisMeasurement: + constraint: Sequence[float] | None = None + upstream_joint = { + # These neighbouring axes are parallel in the fixed source URDF. + # A small planar Tag's monocular PnP orientation can have a stable + # field-dependent bias, so use the well-observed upstream axis + # direction and let each Tag-centre trajectory independently fit + # its physical axis line. This changes no URDF geometry and is + # valid only because the source-URDF axes are deliberately locked. + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + "index_pip": "index_mcp_pitch", + "index_dip": "index_pip", + }.get(joint_name) + if upstream_joint is not None: + # Resolve recursively so thumb_ip receives the already constrained + # thumb_mcp direction (and index_dip the constrained index_pip + # direction), rather than reintroducing the raw PnP orientation at + # the last passive joint. + upstream = self._fit_axis_measurement(upstream_joint, cycle) + constraint = upstream.axis_common_xyz + spec = JOINT_SPECS[joint_name] + return fit_joint_axis_measurement( joint_name, - records, - maximum_plane_rms_m=( - float("inf") - if relaxed - else self.trajectory_maximum_plane_rms_m - ), - maximum_radial_rms_m=( - float("inf") - if relaxed - else self.trajectory_maximum_radial_rms_m - ), - minimum_radius_m=( - 0.0 if relaxed else self.trajectory_minimum_radius_m - ), - minimum_arc_rad=( - 0.0 if relaxed else self.trajectory_minimum_arc_rad - ), - image_maximum_radial_rms_px=( - float("inf") - if relaxed - else self.image_trajectory_maximum_radial_rms_px - ), - image_maximum_radial_p95_px=( - float("inf") - if relaxed - else self.image_trajectory_maximum_radial_p95_px - ), - image_minimum_radius_px=( - 0.0 if relaxed else self.image_trajectory_minimum_radius_px + self.records_by_joint[joint_name], + cycle=cycle, + zero_command_u8=int( + self.baseline_command[spec.motor_index] ), + axis_common_constraint=constraint, ) def _provisional_fit_failures( self, spec: SweepSpec ) -> list[dict[str, Any]]: - """Check geometry, repeatability and zero after one six-way task.""" + """Check full-pose curve and 3-D axis after one six-way task.""" failures: list[dict[str, Any]] = [] - fits: dict[str, JointCurveFit] = {} + if hasattr(self, "views"): + valid_rate = float(self.views[spec.view].valid_rate) + if valid_rate < self.minimum_detection_rate: + failures.append( + { + "joint": spec.joints[0], + "metric": "tag_valid_rate_percent", + "actual": round(100.0 * valid_rate, 3), + "limit": round( + 100.0 * self.minimum_detection_rate, 3 + ), + "comparison": "minimum", + } + ) for joint_name in spec.joints: + records = self.records_by_joint[joint_name] + sync_p95 = float( + np.percentile( + [ + float(record.get("state_image_sync_error_ms", 0.0)) + for record in records + ], + 95.0, + ) + ) + sync_limit_ms = ( + getattr(self, "maximum_state_image_skew_ns", 50_000_000) + / 1_000_000.0 + ) + if sync_p95 > sync_limit_ms: + failures.append( + { + "joint": joint_name, + "metric": "state_image_sync_p95_ms", + "actual": round(sync_p95, 6), + "limit": round(sync_limit_ms, 6), + "comparison": "maximum", + } + ) try: fit = self._fit_joint_records( joint_name, - self.records_by_joint[joint_name], - relaxed=True, + records, ) except Exception as error: failures.append( @@ -1161,62 +1465,26 @@ class G20ThreeCameraCalibrationNode(Node): } ) continue - fits[joint_name] = fit - quality = fit.quality - if joint_name in IMAGE_TRAJECTORY_JOINTS: - checks = ( - ( - "image_radial_rms_px", - float(quality["radial_rms_px"]), - self.image_trajectory_maximum_radial_rms_px, - "maximum", + checks = ( + ( + "rotation_orthogonal_rms_deg", + math.degrees( + float(fit.quality["rotation_orthogonal_rms_rad"]) ), - ( - "image_radial_p95_px", - float(quality["radial_p95_px"]), - self.image_trajectory_maximum_radial_p95_px, - "maximum", + math.degrees( + self.active_maximum_rotation_orthogonal_rms_rad + if JOINT_SPECS[joint_name].active + else self.passive_maximum_rotation_orthogonal_rms_rad ), - ( - "image_radius_px", - float(quality["radius_px"]), - self.image_trajectory_minimum_radius_px, - "minimum", - ), - ( - "arc_deg", - math.degrees(float(quality["arc_rad"])), - math.degrees(self.trajectory_minimum_arc_rad), - "minimum", - ), - ) - else: - checks = ( - ( - "plane_rms_mm", - 1000.0 * float(quality["plane_rms_m"]), - 1000.0 * self.trajectory_maximum_plane_rms_m, - "maximum", - ), - ( - "radial_rms_mm", - 1000.0 * float(quality["radial_rms_m"]), - 1000.0 * self.trajectory_maximum_radial_rms_m, - "maximum", - ), - ( - "radius_mm", - 1000.0 * float(quality["radius_m"]), - 1000.0 * self.trajectory_minimum_radius_m, - "minimum", - ), - ( - "arc_deg", - math.degrees(float(quality["arc_rad"])), - math.degrees(self.trajectory_minimum_arc_rad), - "minimum", - ), - ) + "maximum", + ), + ( + "arc_deg", + math.degrees(float(fit.quality["arc_rad"])), + math.degrees(self.trajectory_minimum_arc_rad), + "minimum", + ), + ) joint_spec = JOINT_SPECS[joint_name] monotonic_limit = ( self.maximum_monotonic_correction_rad @@ -1228,7 +1496,7 @@ class G20ThreeCameraCalibrationNode(Node): if joint_spec.active else self.passive_maximum_hysteresis_rad ) - checks += ( + checks = checks + ( ( "monotonic_correction_deg", math.degrees(fit.maximum_monotonic_correction_rad), @@ -1260,6 +1528,7 @@ class G20ThreeCameraCalibrationNode(Node): ) cycle_travels: list[float] = [] + cycle_axes: list[np.ndarray] = [] for cycle in range(self.repetitions): cycle_records = [ record @@ -1286,6 +1555,66 @@ class G20ThreeCameraCalibrationNode(Node): - float(cycle_fit.angle_rad[255]) ) ) + try: + axis = self._fit_axis_measurement(joint_name, cycle) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "axis_fit", + "cycle": cycle + 1, + "reason": str(error), + } + ) + continue + cycle_axes.append(np.asarray(axis.axis_common_xyz, dtype=float)) + plane_limit = ( + self.axis_maximum_plane_rms_m + if joint_spec.active + else self.passive_axis_maximum_plane_rms_m + ) + axis_checks = [ + ( + "axis_radial_rms_mm", + 1000.0 * axis.radial_rms_m, + 1000.0 * self.axis_maximum_radial_rms_m, + ), + ] + if joint_name not in CONSTRAINED_CIRCLE_JOINTS: + axis_checks.extend( + [ + ( + "axis_plane_rms_mm", + 1000.0 * axis.plane_rms_m, + 1000.0 * plane_limit, + ), + ( + "rotation_circle_axis_difference_deg", + math.degrees( + axis.rotation_circle_axis_difference_rad + ), + math.degrees( + self.axis_maximum_rotation_circle_difference_rad + ), + ), + ] + ) + # With a trusted orientation-constrained axis, plane_rms is + # scatter *along* that infinite axis. It cannot change the + # axis line or joint zero, so only the perpendicular/radial + # error is an admissibility check for constrained circles. + for metric, actual, limit in axis_checks: + if actual > limit: + failures.append( + { + "joint": joint_name, + "metric": metric, + "cycle": cycle + 1, + "actual": round(float(actual), 6), + "limit": round(float(limit), 6), + "comparison": "maximum", + } + ) if len(cycle_travels) == self.repetitions: travel_range = max(cycle_travels) - min(cycle_travels) cycle_limit = ( @@ -1307,41 +1636,29 @@ class G20ThreeCameraCalibrationNode(Node): ], } ) - - failed_joints = {str(item["joint"]) for item in failures} - for joint_name in spec.joints: - if joint_name in failed_joints or joint_name not in fits: - continue - joint_spec = JOINT_SPECS[joint_name] - if joint_spec.zero_kind == "projected": - try: - fit_projected_zero( - self.records_by_joint[joint_name], - minimum_radius_px=self.zero_minimum_radius_px, - minimum_arc_rad=self.trajectory_minimum_arc_rad, - maximum_radial_rms_px=self.zero_maximum_radial_rms_px, - maximum_radial_p95_px=self.zero_maximum_radial_p95_px, - maximum_round_difference_rad=( - self.zero_maximum_round_difference_rad - ), - ) - except Exception as error: + if len(cycle_axes) == self.repetitions: + maximum_difference = 0.0 + for left in cycle_axes: + for right in cycle_axes: + maximum_difference = max( + maximum_difference, + math.acos( + abs(float(np.clip(left @ right, -1.0, 1.0))) + ), + ) + if maximum_difference > self.zero_maximum_axis_cycle_difference_rad: failures.append( { "joint": joint_name, - "metric": "projected_zero", - "reason": str(error), - } - ) - elif joint_spec.zero_kind == "travel_midpoint": - try: - center_splay_curve(fits[joint_name]) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "travel_midpoint", - "reason": str(error), + "metric": "axis_cycle_difference_deg", + "actual": round(math.degrees(maximum_difference), 6), + "limit": round( + math.degrees( + self.zero_maximum_axis_cycle_difference_rad + ), + 6, + ), + "comparison": "maximum", } ) return failures @@ -1351,6 +1668,7 @@ class G20ThreeCameraCalibrationNode(Node): ) -> None: start_index = self._sweep_spec_start_index(spec) attempt = self.sweep_attempts.get(spec.motor_index, 1) + self.retry_resume_index = self.sweep_index self.retry_sweep_spec = spec self.sweep_index = start_index self.fit_failure = { @@ -1378,11 +1696,17 @@ class G20ThreeCameraCalibrationNode(Node): states = np.asarray( [float(frame.state_u8[motor]) for frame in self.sweep_frames] ) + start_tolerance = self._endpoint_tolerance_for_spec( + item.spec, item.start_u8 + ) + target_tolerance = self._endpoint_tolerance_for_spec( + item.spec, item.target_u8 + ) bins: dict[int, list[FrameObservation]] = {} for frame, state in zip(self.sweep_frames, states): - if abs(state - item.start_u8) <= self.endpoint_tolerance_u8: + if abs(state - item.start_u8) <= start_tolerance: command = item.start_u8 - elif abs(state - item.target_u8) <= self.endpoint_tolerance_u8: + elif abs(state - item.target_u8) <= target_tolerance: command = item.target_u8 else: command = int(np.clip(np.rint(state), 0, 255)) @@ -1398,6 +1722,45 @@ class G20ThreeCameraCalibrationNode(Node): self._pause("sweep_bin_gap_too_large") return + append_jsonl_many( + self.raw_path, + ( + { + "kind": "synchronised_frame", + "attempt": self.sweep_attempts.get(motor, 1), + "view": item.spec.view, + "motor_index": motor, + "joints": list(item.spec.joints), + "cycle": item.cycle, + "direction": item.direction, + "image_stamp_ns": int(frame.stamp_ns), + "actual_state_u8": [ + float(value) for value in frame.state_u8 + ], + "state_image_sync_error_ms": round( + abs(frame.state_sync_error_ns) / 1_000_000.0, 6 + ), + "relative_quaternion_xyzw": { + name: list(frame.joint_quaternions_xyzw[name]) + for name in item.spec.joints + }, + "parent_pose_common": { + name: frame.parent_poses_common[name] + for name in item.spec.joints + }, + "child_pose_common": { + name: frame.child_poses_common[name] + for name in item.spec.joints + }, + "pnp_reprojection_error_px": { + name: frame.joint_reprojection_error_px[name] + for name in item.spec.joints + }, + } + for frame in self.sweep_frames + ), + ) + for joint_name in item.spec.joints: for command in commands: frames = bins[command] @@ -1413,6 +1776,28 @@ class G20ThreeCameraCalibrationNode(Node): ), axis=0, ) + relative_quaternion = robust_rotation_summary( + [ + frame.joint_quaternions_xyzw[joint_name] + for frame in frames + ] + )[0] + parent_pose = _robust_pose_payload( + [ + frame.parent_poses_common[joint_name] + for frame in frames + ] + ) + child_pose = _robust_pose_payload( + [ + frame.child_poses_common[joint_name] + for frame in frames + ] + ) + state = np.median( + np.asarray([frame.state_u8 for frame in frames], dtype=float), + axis=0, + ) record = { "kind": "sample", "attempt": self.sweep_attempts.get(motor, 1), @@ -1428,6 +1813,37 @@ class G20ThreeCameraCalibrationNode(Node): "image_relative_xy_px": [ float(value) for value in image_vector ], + "relative_quaternion_xyzw": [ + float(value) for value in relative_quaternion + ], + "parent_pose_common": parent_pose, + "child_pose_common": child_pose, + "state_u8": [float(value) for value in state], + "state_image_sync_error_ms": round( + float( + np.percentile( + [ + abs(frame.state_sync_error_ns) + for frame in frames + ], + 95.0, + ) + / 1_000_000.0 + ), + 6, + ), + "pnp_reprojection_error_px": round( + float( + np.percentile( + [ + frame.joint_reprojection_error_px[joint_name] + for frame in frames + ], + 95.0, + ) + ), + 6, + ), "valid_frames": len(frames), } self.records_by_joint[joint_name].append(record) @@ -1448,7 +1864,11 @@ class G20ThreeCameraCalibrationNode(Node): ) return if self.retry_sweep_spec == previous_spec: + resume_index = self.retry_resume_index self.retry_sweep_spec = None + self.retry_resume_index = None + if resume_index is not None: + self.sweep_index = resume_index self.fit_failure = {} if self.sweep_index >= len(self.sweep_items): self._begin_return_baseline("fit") @@ -1459,34 +1879,69 @@ class G20ThreeCameraCalibrationNode(Node): def _fit_all_curves(self) -> None: self.state = STATE_FITTING - self.reason = "fitting_21_joint_compact_calibration" + self.reason = "fitting_3d_axes_and_urdf_zero_offsets" + training_fits: dict[str, JointCurveFit] = {} + holdout_by_joint: dict[str, tuple[float, ...]] = {} measured: dict[str, JointCurveFit] = {} for name in MEASURED_JOINTS: + training_records = [ + record + for record in self.records_by_joint[name] + if int(record["cycle"]) in {0, 1} + ] + holdout_records = [ + record + for record in self.records_by_joint[name] + if int(record["cycle"]) == 2 + ] + zero_command = int( + self.baseline_command[JOINT_SPECS[name].motor_index] + ) + training_fits[name] = self._fit_joint_records( + name, training_records + ) + holdout_by_joint[name] = rotation_curve_holdout_errors( + training_fits[name], + holdout_records, + zero_command_u8=zero_command, + ) measured[name] = self._fit_joint_records( name, self.records_by_joint[name] ) - centred, zero_command, midpoint = center_splay_curve( - measured["index_mcp_roll"] + axes: list[JointAxisMeasurement] = [] + for name in AXIS_JOINTS: + for cycle in range(self.repetitions): + axes.append(self._fit_axis_measurement(name, cycle)) + + motor_by_joint = { + name: int(spec.motor_index) for name, spec in JOINT_SPECS.items() + } + holdout_zero_result = solve_urdf_zero_offsets( + source_urdf=self.source_urdf_path, + measurements=axes, + curves=training_fits, + motor_by_joint=motor_by_joint, + maximum_offset_rad=self.zero_maximum_offset_rad, + maximum_validation_mae_rad=self.maximum_validation_mae_rad, + maximum_validation_p95_rad=self.maximum_validation_p95_rad, ) - measured["index_mcp_roll"] = centred - projected: dict[str, float] = {} - for name, spec in JOINT_SPECS.items(): - if spec.zero_kind != "projected": - continue - projected[name] = fit_projected_zero( - self.records_by_joint[name], - minimum_radius_px=self.zero_minimum_radius_px, - minimum_arc_rad=self.trajectory_minimum_arc_rad, - maximum_radial_rms_px=self.zero_maximum_radial_rms_px, - maximum_radial_p95_px=self.zero_maximum_radial_p95_px, - maximum_round_difference_rad=( - self.zero_maximum_round_difference_rad - ), + holdout_errors = np.abs( + np.asarray( + [ + value + for values in holdout_by_joint.values() + for value in values + ], + dtype=float, ) - self.measured_fits = measured - self.projected_zeros_rad = projected - self.splay_zero_command_u8 = zero_command - self.splay_midpoint_rad = midpoint + ) + trajectory_holdout_passed = bool( + holdout_errors.size + and float(np.mean(holdout_errors)) + <= self.maximum_validation_mae_rad + and float(np.percentile(holdout_errors, 95.0)) + <= self.maximum_validation_p95_rad + ) self.fit_quality_passed = all( fit.maximum_monotonic_correction_rad <= ( @@ -1502,13 +1957,107 @@ class G20ThreeCameraCalibrationNode(Node): ) for name, fit in measured.items() ) + if not trajectory_holdout_passed or not holdout_zero_result.passed: + trajectory_score = { + name: float(np.percentile(np.abs(values), 95.0)) + for name, values in holdout_by_joint.items() + } + zero_axis_errors = { + name: error + for name, error in ( + holdout_zero_result.validation_error_by_joint_rad.items() + ) + if name in ZERO_AXIS_OBSERVATION_JOINTS + and name not in ZERO_ROOT_ANCHOR_JOINTS + } + zero_line_errors = { + name: error + for name, error in ( + holdout_zero_result.validation_line_error_by_joint_m.items() + ) + if name in ZERO_LINE_OBSERVATION_JOINTS + and name not in ZERO_ROOT_ANCHOR_JOINTS + } + if not trajectory_holdout_passed: + worst_joint = max(trajectory_score, key=trajectory_score.get) + elif zero_axis_errors and max(zero_axis_errors.values()) > ( + self.maximum_validation_p95_rad + ): + worst_joint = max( + zero_axis_errors, key=zero_axis_errors.get + ) + elif zero_line_errors: + worst_joint = max(zero_line_errors, key=zero_line_errors.get) + else: + worst_joint = "thumb_cmc_yaw" + failed_spec = next( + spec for spec in SWEEP_SPECS if worst_joint in spec.joints + ) + failures: list[dict[str, Any]] = [ + { + "joint": name, + "metric": "third_cycle_trajectory_p95_deg", + "actual": round(math.degrees(error), 6), + "limit": math.degrees( + self.maximum_validation_p95_rad + ), + "comparison": "maximum", + } + for name, error in trajectory_score.items() + if error > self.maximum_validation_p95_rad + ] + failures.extend( + { + "joint": name, + "metric": "third_cycle_axis_holdout_deg", + "actual": round(math.degrees(error), 6), + "limit": math.degrees( + self.maximum_validation_p95_rad + ), + "comparison": "maximum", + } + for name, error in zero_axis_errors.items() + if error > self.maximum_validation_p95_rad + ) + if holdout_zero_result.axis_line_rms_m > 0.003: + failures.append( + { + "joint": worst_joint, + "metric": "third_cycle_axis_line_rms_mm", + "actual": round( + 1000.0 * holdout_zero_result.axis_line_rms_m, 6 + ), + "limit": 3.0, + "comparison": "maximum", + } + ) + self._pause_for_provisional_fit_failure(failed_spec, failures) + return + zero_result = solve_urdf_zero_offsets( + source_urdf=self.source_urdf_path, + measurements=axes, + curves=measured, + motor_by_joint=motor_by_joint, + maximum_offset_rad=self.zero_maximum_offset_rad, + maximum_validation_mae_rad=self.maximum_validation_mae_rad, + maximum_validation_p95_rad=self.maximum_validation_p95_rad, + ) + if not zero_result.passed: + raise RuntimeError("final_all_cycle_zero_refit_failed") + self.measured_fits = measured + self.axis_measurements = axes + self.zero_result = zero_result + self.validation_errors_rad = [ + float(value) + for values in holdout_by_joint.values() + for value in values + ] if self.validation_enabled: self._build_validation_items() self._start_next_validation() else: self.validation_items.clear() self.validation_index = 0 - self.validation_errors_rad.clear() self._begin_return_baseline("finalize") def _build_validation_items(self) -> None: @@ -1522,7 +2071,6 @@ class G20ThreeCameraCalibrationNode(Node): ValidationItem(spec, command) for command in commands ) self.validation_index = 0 - self.validation_errors_rad.clear() def _start_next_validation(self) -> None: if self.validation_index >= len(self.validation_items): @@ -1555,19 +2103,15 @@ class G20ThreeCameraCalibrationNode(Node): item = self.active_validation command = item.command_u8 for name in item.spec.joints: - vectors = np.asarray( - [frame.joint_vectors_xyz_m[name] for frame in self.validation_frames_buffer] - ) - image_vectors = np.asarray( + quaternion = robust_rotation_summary( [ - frame.image_vectors_xy_px[name] + frame.joint_quaternions_xyzw[name] for frame in self.validation_frames_buffer ] - ) - observed = measure_joint_observation( + )[0] + observed = measure_rotation_joint_observation( self.measured_fits[name], - vector_xyz_m=np.median(vectors, axis=0), - image_vector_xy_px=np.median(image_vectors, axis=0), + quaternion, ) expected = float(self.measured_fits[name].angle_rad[command]) self.validation_errors_rad.append(observed - expected) @@ -1582,23 +2126,60 @@ class G20ThreeCameraCalibrationNode(Node): self._start_next_validation() def _finalize(self) -> None: + if self.zero_result is None: + raise RuntimeError("URDF zero solution is missing") errors = np.abs(np.asarray(self.validation_errors_rad, dtype=float)) validation_passed = bool( - not self.validation_enabled - or ( - errors.size > 0 - and float(np.mean(errors)) <= self.maximum_validation_mae_rad - and float(np.percentile(errors, 95.0)) - <= self.maximum_validation_p95_rad + self.zero_result.passed + and ( + not self.validation_enabled + or ( + errors.size > 0 + and float(np.mean(errors)) + <= self.maximum_validation_mae_rad + and float(np.percentile(errors, 95.0)) + <= self.maximum_validation_p95_rad + ) ) ) passed = bool(self.fit_quality_passed and validation_passed) + if not passed: + raise RuntimeError("final_calibration_quality_failed") + stamp = ( + self.session_dir.name + if re.fullmatch(r"\d{8}_\d{6}", self.session_dir.name) + else None + ) + # The four-finger active trajectories are intentionally kept as + # measured (the untagged fingers inherit the index curves). Ensure + # the generated URDF cannot clip a measured endpoint. This is kept + # separate from the thumb so thumb limits and calibration data remain + # byte-for-byte governed by the existing solution. + four_finger_angle_ranges_rad: dict[str, tuple[float, ...]] = {} + for name, joint_spec in JOINT_SPECS.items(): + if not joint_spec.active: + continue + if not name.startswith(("index_", "middle_", "ring_", "pinky_")): + continue + if not name.endswith(("_mcp_roll", "_mcp_pitch", "_pip")): + continue + source_name = joint_spec.source_joint or name + four_finger_angle_ranges_rad[name] = tuple( + float(value) for value in self.measured_fits[source_name].angle_rad + ) + self.corrected_urdf_path = write_zero_corrected_urdf( + source_urdf=self.source_urdf_path, + output_directory=self.corrected_urdf_output_dir, + serial_number=self.serial_number, + offsets_rad=self.zero_result.all_active_offsets_rad, + timestamp=stamp, + joint_angle_ranges_rad=four_finger_angle_ranges_rad, + joint_limit_margin_rad=math.radians(0.5), + ) payload = build_compact_payload( serial_number=self.serial_number, measured_fits=self.measured_fits, - projected_zeros_rad=self.projected_zeros_rad, - splay_zero_command_u8=self.splay_zero_command_u8, - splay_midpoint_rad=self.splay_midpoint_rad, + urdf_zero_offsets_rad=self.zero_result.all_active_offsets_rad, validation_errors_rad=self.validation_errors_rad, passed=passed, baseline=self.baseline_command, @@ -1674,7 +2255,10 @@ class G20ThreeCameraCalibrationNode(Node): now - self.speed_commanded_at >= self.speed_setting_settle_seconds ) - if reached and speed_ready: + # Do not leave the start endpoint until at least one complete, + # timestamp-synchronised Tag/state observation has been retained. + # The retained frame becomes the strict 0/255 endpoint bin. + if reached and speed_ready and self.sweep_start_frames: if self.position_hold_since is None: self.position_hold_since = now elif now - self.position_hold_since >= self.endpoint_hold_seconds: @@ -1748,6 +2332,7 @@ class G20ThreeCameraCalibrationNode(Node): name: { "ready": self._view_ready(runtime, now), "camera_info_valid": runtime.camera_info_valid, + "camera_extrinsics_valid": runtime.extrinsics_valid, "detection_hz": round(runtime.detection_hz, 2), "valid_rate": round(runtime.valid_rate, 4), "required_tag_ids": sorted(runtime.role_by_id), @@ -1775,6 +2360,12 @@ class G20ThreeCameraCalibrationNode(Node): active: dict[str, Any] = {} if self.active_sweep is not None: motor = self.active_sweep.spec.motor_index + start_tolerance = self._endpoint_tolerance_for_spec( + self.active_sweep.spec, self.active_sweep.start_u8 + ) + target_tolerance = self._endpoint_tolerance_for_spec( + self.active_sweep.spec, self.active_sweep.target_u8 + ) values = [ float(frame.state_u8[motor]) for frame in self.sweep_frames ] @@ -1783,10 +2374,10 @@ class G20ThreeCameraCalibrationNode(Node): ( self.active_sweep.start_u8 if abs(value - self.active_sweep.start_u8) - <= self.endpoint_tolerance_u8 + <= start_tolerance else self.active_sweep.target_u8 if abs(value - self.active_sweep.target_u8) - <= self.endpoint_tolerance_u8 + <= target_tolerance else int(np.clip(np.rint(value), 0, 255)) ) for value in values @@ -1831,7 +2422,11 @@ class G20ThreeCameraCalibrationNode(Node): "maximum_bin_gap": int(max(np.diff(bins), default=0)), "allowed_maximum_bin_gap": self.maximum_bin_gap, "missing_endpoint_u8": missing_endpoints, - "endpoint_tolerance_u8": self.endpoint_tolerance_u8, + "endpoint_tolerance_u8": max( + start_tolerance, target_tolerance + ), + "start_endpoint_tolerance_u8": start_tolerance, + "target_endpoint_tolerance_u8": target_tolerance, }, "auxiliary_motors": [ { @@ -1907,11 +2502,21 @@ class G20ThreeCameraCalibrationNode(Node): "completed_sweeps": self.sweep_index, "total_sweeps": len(self.sweep_items), "baseline_command_u8": list(self.baseline_command), + "hand_state_fresh": bool( + len(self.latest_state_u8) == 20 + and now - self.last_state_at <= 1.0 + ), "views": views, "active": active, "result_path": ( str(self.final_path) if self.completed_payload is not None else "" ), + "corrected_urdf_path": ( + "" + if self.corrected_urdf_path is None + else str(self.corrected_urdf_path) + ), + "camera_extrinsics_error": self.extrinsics_error, "quality": ( {} if self.completed_payload is None else self.completed_payload["quality"] ), diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py new file mode 100644 index 0000000..f5f26b8 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/urdf_zero.py @@ -0,0 +1,1133 @@ +"""Three-dimensional joint-axis fitting and URDF zero correction.""" + +from __future__ import annotations + +from dataclasses import dataclass +from datetime import datetime +import math +import os +from pathlib import Path +import re +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation + +from .core import fit_rotation_axis, robust_rotation_summary +from .full_hand import IMAGE_TRAJECTORY_JOINTS, JointCurveFit +from .trajectory import _fit_circle_with_axis, _fit_joint_curve, _fit_plane_axis + + +CONSTRAINED_CIRCLE_JOINTS: frozenset[str] = ( + IMAGE_TRAJECTORY_JOINTS | {"index_dip", "thumb_cmc_yaw"} +) + + +DIRECT_ZERO_JOINTS: tuple[str, ...] = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "index_mcp_roll", + "index_mcp_pitch", + "index_pip", +) + +# The four-finger splay, root flexion and PIP curves already use mechanically +# defined baseline commands (127 for splay, 255 for flexion). With only the +# index finger tagged, fitting an absolute palm-to-URDF static correction and +# inheriting it across four fingers introduced a common lean and incorrect +# root/PIP flexion. The thumb MCP axis-line observation likewise produced a +# -6.86 degree extension at command 255 although its measured trajectory is +# already zero there. Keep these joints in the public 7-joint result, but lock +# their static URDF corrections to zero. Their measured angle curves are +# deliberately left unchanged. +FIXED_DIRECT_ZERO_OFFSETS_RAD: dict[str, float] = { + "thumb_mcp": 0.0, + "index_mcp_roll": 0.0, + "index_mcp_pitch": 0.0, + "index_pip": 0.0, +} +OPTIMIZED_ZERO_JOINTS: tuple[str, ...] = tuple( + name for name in DIRECT_ZERO_JOINTS + if name not in FIXED_DIRECT_ZERO_OFFSETS_RAD +) + +AXIS_JOINTS: tuple[str, ...] = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + "index_mcp_roll", + "index_mcp_pitch", + "index_pip", + "index_dip", +) + +INHERITED_ZERO_JOINTS: dict[str, str] = { + "middle_mcp_roll": "index_mcp_roll", + "ring_mcp_roll": "index_mcp_roll", + "pinky_mcp_roll": "index_mcp_roll", + "middle_mcp_pitch": "index_mcp_pitch", + "ring_mcp_pitch": "index_mcp_pitch", + "pinky_mcp_pitch": "index_mcp_pitch", + "middle_pip": "index_pip", + "ring_pip": "index_pip", + "pinky_pip": "index_pip", +} + +ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 = 16 + +# A joint's own screw axis is invariant to its own encoder-zero offset. Only +# the downstream axis selected by the kinematic chain can observe that zero. +# Feeding every axis-direction and axis-line residual into the optimizer lets +# fixed CAD/PnP geometry residuals push unrelated zero offsets to their bounds. +# Keep the two root lines as the palm-pose anchor, then use only the component +# that actually observes the preceding zero in each serial chain. +ZERO_AXIS_OBSERVATION_JOINTS: frozenset[str] = frozenset( + { + "thumb_cmc_roll", + "index_mcp_roll", + "thumb_cmc_yaw", # observes thumb_cmc_roll + "thumb_cmc_pitch", # observes thumb_cmc_yaw + } +) +ZERO_LINE_OBSERVATION_JOINTS: frozenset[str] = frozenset( + { + "thumb_cmc_roll", + "index_mcp_roll", + "thumb_mcp", # observes thumb_cmc_pitch + } +) +ZERO_ROOT_ANCHOR_JOINTS: frozenset[str] = frozenset( + {"thumb_cmc_roll", "index_mcp_roll"} +) +ZERO_AXIS_PARENT_JOINT: dict[str, str] = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", +} + + +def _zero_sensitive_axis_error_rad( + predicted_axis: Sequence[float], + observed_axis: Sequence[float], + parent_axis: Sequence[float], +) -> float: + """Return only the axis error that a parent-joint zero can change. + + Rotating a downstream axis about its parent preserves their mutual cone + angle. The component normal to their plane is therefore the observable + encoder-zero error; cone-angle mismatch belongs to fixed geometry/PnP and + must not push a zero offset or fail its holdout validation. + """ + predicted = _vector(predicted_axis, 3, name="predicted axis") + predicted /= np.linalg.norm(predicted) + observed = _vector(observed_axis, 3, name="observed axis") + observed /= np.linalg.norm(observed) + parent = _vector(parent_axis, 3, name="parent axis") + parent /= np.linalg.norm(parent) + if float(predicted @ observed) < 0.0: + observed = -observed + sensitivity = np.cross(parent, predicted) + magnitude = float(np.linalg.norm(sensitivity)) + if magnitude < 1.0e-9: + raise ValueError("parent and downstream axes are parallel") + sensitivity /= magnitude + return math.asin(float(np.clip(observed @ sensitivity, -1.0, 1.0))) +def _vector(value: Sequence[float], size: int, *, name: str) -> np.ndarray: + result = np.asarray(value, dtype=float) + if result.shape != (size,) or not np.all(np.isfinite(result)): + raise ValueError(f"{name} must contain {size} finite values") + return result + + +def _pose_matrix(payload: Mapping[str, Any]) -> np.ndarray: + translation = _vector(payload["translation_xyz_m"], 3, name="translation") + quaternion = _vector(payload["quaternion_xyzw"], 4, name="quaternion") + quaternion /= np.linalg.norm(quaternion) + result = np.eye(4) + result[:3, :3] = Rotation.from_quat(quaternion).as_matrix() + result[:3, 3] = translation + return result + + +def _relative_rotation(record: Mapping[str, Any]) -> np.ndarray: + quaternion = _vector( + record["relative_quaternion_xyzw"], 4, name="relative quaternion" + ) + return quaternion / np.linalg.norm(quaternion) + + +def _reference_group_key(record: Mapping[str, Any]) -> tuple[Any, Any]: + return record.get("cycle"), record.get("direction") + + +def _interpolate_reference_rotation( + records: Sequence[Mapping[str, Any]], zero_command_u8: int +) -> Rotation | None: + by_command: dict[int, list[np.ndarray]] = {} + for record in records: + command = int(record["command_u8"]) + by_command.setdefault(command, []).append(_relative_rotation(record)) + if not by_command: + return None + + rotations = { + command: Rotation.from_quat(robust_rotation_summary(values)[0]) + for command, values in by_command.items() + } + zero = int(zero_command_u8) + if zero in rotations: + return rotations[zero] + + lower = [command for command in rotations if command < zero] + upper = [command for command in rotations if command > zero] + lower_command = max(lower) if lower else None + upper_command = min(upper) if upper else None + if lower_command is not None and upper_command is not None: + lower_distance = zero - lower_command + upper_distance = upper_command - zero + if max(lower_distance, upper_distance) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + lower_rotation = rotations[lower_command] + upper_rotation = rotations[upper_command] + fraction = lower_distance / (upper_command - lower_command) + delta = (lower_rotation.inv() * upper_rotation).as_rotvec() + return lower_rotation * Rotation.from_rotvec(delta * fraction) + + nearest_command = min(rotations, key=lambda command: abs(command - zero)) + if abs(nearest_command - zero) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + return rotations[nearest_command] + return None + + +def _near_zero_records( + records: Sequence[Mapping[str, Any]], zero_command_u8: int +) -> list[Mapping[str, Any]]: + groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} + for record in records: + groups.setdefault(_reference_group_key(record), []).append(record) + selected: list[Mapping[str, Any]] = [] + zero = int(zero_command_u8) + for group in groups.values(): + distance = min(abs(int(record["command_u8"]) - zero) for record in group) + if distance > ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + continue + selected.extend( + record + for record in group + if abs(int(record["command_u8"]) - zero) == distance + ) + return selected + + +def _baseline_reference( + records: Sequence[Mapping[str, Any]], zero_command_u8: int +) -> tuple[float, float, float, float]: + groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} + for record in records: + groups.setdefault(_reference_group_key(record), []).append(record) + values = [ + rotation.as_quat() + for group in groups.values() + if ( + rotation := _interpolate_reference_rotation( + group, zero_command_u8 + ) + ) + is not None + ] + if not values: + raise ValueError( + "joint records have no samples within " + f"{ZERO_REFERENCE_MAXIMUM_DISTANCE_U8} commands of zero " + f"{zero_command_u8}" + ) + return robust_rotation_summary(values)[0] + + +def fit_rotation_joint_curve( + records: Sequence[Mapping[str, Any]], *, zero_command_u8: int +) -> JointCurveFit: + """Fit a command curve from full parent-to-child tag orientations.""" + samples = [dict(record) for record in records] + if len(samples) < 12: + raise ValueError("rotation trajectory requires at least 12 samples") + vectors: list[np.ndarray] = [] + commands: list[int] = [] + values_by_record: list[float] = [] + references: dict[int, Rotation] = {} + for cycle in sorted({int(record["cycle"]) for record in samples}): + cycle_records = [ + record for record in samples if int(record["cycle"]) == cycle + ] + references[cycle] = Rotation.from_quat( + _baseline_reference(cycle_records, zero_command_u8) + ) + for record in samples: + observed = Rotation.from_quat(_relative_rotation(record)) + vector = ( + references[int(record["cycle"])].inv() * observed + ).as_rotvec() + vectors.append(vector) + commands.append(int(record["command_u8"])) + axis = fit_rotation_axis(vectors, commands) + values_by_record = [float(vector @ axis) for vector in vectors] + curves, correction, hysteresis = _fit_joint_curve(samples, values_by_record) + for key in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(curves[key], dtype=float) + values -= float(values[int(zero_command_u8)]) + curves[key] = [round(float(value), 8) for value in values] + orthogonal = [ + float(np.linalg.norm(vector - float(vector @ axis) * axis)) + for vector in vectors + ] + return JointCurveFit( + angle_rad=tuple(float(value) for value in curves["angle_rad"]), + decreasing_rad=tuple(float(value) for value in curves["decreasing_rad"]), + increasing_rad=tuple(float(value) for value in curves["increasing_rad"]), + circle={ + "space": "relative_rotation_3d", + "axis_xyz": [float(value) for value in axis], + "zero_command_u8": int(zero_command_u8), + "reference_quaternion_xyzw": [ + float(value) + for value in _baseline_reference(samples, zero_command_u8) + ], + }, + maximum_monotonic_correction_rad=float(correction), + maximum_hysteresis_rad=float(hysteresis), + quality={ + "rotation_orthogonal_rms_rad": float( + np.sqrt(np.mean(np.square(orthogonal))) + ), + "arc_rad": float( + max(curves["angle_rad"]) - min(curves["angle_rad"]) + ), + }, + ) + + +def measure_rotation_joint_observation( + fit: JointCurveFit, quaternion_xyzw: Sequence[float] +) -> float: + """Measure one parent-to-child orientation with a fitted 3-D curve.""" + if fit.circle.get("space") != "relative_rotation_3d": + raise ValueError("joint fit is not a relative-rotation curve") + reference = Rotation.from_quat( + _vector( + fit.circle["reference_quaternion_xyzw"], + 4, + name="reference quaternion", + ) + ) + observed = Rotation.from_quat( + _vector(quaternion_xyzw, 4, name="observed quaternion") + ) + axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") + axis /= np.linalg.norm(axis) + return float((reference.inv() * observed).as_rotvec() @ axis) + + +def rotation_curve_holdout_errors( + fit: JointCurveFit, + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, +) -> tuple[float, ...]: + """Validate a fitted curve on an untouched scan cycle.""" + samples = [dict(record) for record in records] + if not samples: + raise ValueError("holdout records are empty") + reference = Rotation.from_quat( + _baseline_reference(samples, zero_command_u8) + ) + axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") + axis /= np.linalg.norm(axis) + errors: list[float] = [] + for record in samples: + observed = Rotation.from_quat(_relative_rotation(record)) + angle = float((reference.inv() * observed).as_rotvec() @ axis) + command = int(record["command_u8"]) + direction = str(record["direction"]) + expected_curve = ( + fit.decreasing_rad + if direction == "decreasing" + else fit.increasing_rad + ) + errors.append(angle - float(expected_curve[command])) + return tuple(errors) + + +@dataclass(frozen=True) +class JointAxisMeasurement: + joint: str + cycle: int + axis_common_xyz: tuple[float, float, float] + point_common_xyz_m: tuple[float, float, float] + condition_state_u8: tuple[float, ...] + plane_rms_m: float + radial_rms_m: float + rotation_circle_axis_difference_rad: float + + +def fit_joint_axis_measurement( + joint: str, + records: Sequence[Mapping[str, Any]], + *, + cycle: int, + zero_command_u8: int, + axis_common_constraint: Sequence[float] | None = None, +) -> JointAxisMeasurement: + """Fit one physical screw axis from one complete scan cycle.""" + samples = [ + dict(record) for record in records if int(record["cycle"]) == int(cycle) + ] + if len(samples) < 12: + raise ValueError(f"{joint} cycle {cycle + 1} has too few samples") + points = np.asarray( + [record["relative_translation_xyz_m"] for record in samples], dtype=float + ) + free_circle_axis, free_plane_rms = _fit_plane_axis([points]) + + reference = Rotation.from_quat( + _baseline_reference(samples, zero_command_u8) + ) + rotation_vectors = [ + (reference.inv() * Rotation.from_quat(_relative_rotation(record))).as_rotvec() + for record in samples + ] + commands = [int(record["command_u8"]) for record in samples] + rotation_axis_child = fit_rotation_axis(rotation_vectors, commands) + # reference maps the child Tag frame at baseline into the parent Tag + # frame. The quaternion delta axis is expressed in that child frame, + # while the fitted centre circle is expressed in the parent frame. This + # conversion is what makes arbitrary Tag mounting rotations harmless. + rotation_axis = reference.apply(rotation_axis_child) + if float(rotation_axis @ free_circle_axis) < 0.0: + free_circle_axis = -free_circle_axis + disagreement = math.acos( + float(np.clip(rotation_axis @ free_circle_axis, -1.0, 1.0)) + ) + + zero_records = _near_zero_records(samples, zero_command_u8) + if not zero_records: + raise ValueError( + f"{joint} cycle has no record near baseline {zero_command_u8}" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_translation = np.median(parent_poses[:, :3, 3], axis=0) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + parent_rotation = Rotation.from_quat(parent_quaternion) + + if axis_common_constraint is not None: + common_axis = _vector( + axis_common_constraint, 3, name="common axis constraint" + ) + common_axis /= np.linalg.norm(common_axis) + fitted_axis = parent_rotation.inv().apply(common_axis) + if float(fitted_axis @ rotation_axis) < 0.0: + fitted_axis = -fitted_axis + circle = _fit_circle_with_axis(points, fitted_axis) + plane_rms = float(circle["plane_rms_m"]) + elif str(joint) in CONSTRAINED_CIRCLE_JOINTS: + # For a monocular view of a shallow arc, the Tag centre samples are + # nearly collinear in 3-D. Planar-PnP depth noise can consequently + # rotate an unconstrained best-fit plane by tens of degrees even when + # the full Tag orientations repeat to a fraction of a degree. The + # orientation trajectory observes the screw-axis direction directly; + # use it to constrain the 3-D circle and estimate only the axis line. + # This remains a full-SE(3) fit and is independent of camera level and + # arbitrary Tag mounting rotation. + fitted_axis = rotation_axis + circle = _fit_circle_with_axis(points, fitted_axis) + plane_rms = float(circle["plane_rms_m"]) + else: + # Oblique trajectories with an observable 3-D motion plane retain the + # independent rotation/centre cross-check and fuse both estimates. + free_circle = _fit_circle_with_axis(points, free_circle_axis) + fitted_axis = rotation_axis + free_circle_axis + if float(np.linalg.norm(fitted_axis)) < 1.0e-9: + raise ValueError(f"{joint} rotation and centre axes are opposed") + fitted_axis /= np.linalg.norm(fitted_axis) + circle = free_circle + plane_rms = max( + float(free_plane_rms), float(circle["plane_rms_m"]) + ) + axis_common = parent_rotation.apply(fitted_axis) + point_common = parent_rotation.apply( + np.asarray(circle["center_xyz_m"], dtype=float) + ) + parent_translation + state = np.median( + np.asarray([record["state_u8"] for record in zero_records], dtype=float), + axis=0, + ) + return JointAxisMeasurement( + joint=str(joint), + cycle=int(cycle), + axis_common_xyz=tuple(float(value) for value in axis_common), + point_common_xyz_m=tuple(float(value) for value in point_common), + condition_state_u8=tuple(float(value) for value in state), + plane_rms_m=float(plane_rms), + radial_rms_m=float(circle["radial_rms_m"]), + rotation_circle_axis_difference_rad=float(disagreement), + ) + + +@dataclass(frozen=True) +class _UrdfJoint: + name: str + parent: str + child: str + origin: np.ndarray + axis: np.ndarray + mimic_joint: str | None + mimic_multiplier: float + mimic_offset: float + + +class UrdfKinematicModel: + def __init__(self, source: str | Path) -> None: + self.source = Path(source).expanduser().resolve() + if not self.source.is_file(): + raise ValueError(f"source URDF does not exist: {self.source}") + root = ET.parse(self.source).getroot() + self.joints: dict[str, _UrdfJoint] = {} + self.parent_joint_by_child: dict[str, str] = {} + for element in root.findall("joint"): + if element.get("type") == "fixed": + pass + name = str(element.get("name")) + parent = element.find("parent") + child = element.find("child") + if parent is None or child is None: + continue + origin_node = element.find("origin") + xyz = _parse_triplet( + "0 0 0" if origin_node is None else origin_node.get("xyz", "0 0 0") + ) + rpy = _parse_triplet( + "0 0 0" if origin_node is None else origin_node.get("rpy", "0 0 0") + ) + origin = np.eye(4) + origin[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() + origin[:3, 3] = xyz + axis_node = element.find("axis") + axis = _parse_triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + axis /= np.linalg.norm(axis) + mimic = element.find("mimic") + model = _UrdfJoint( + name=name, + parent=str(parent.get("link")), + child=str(child.get("link")), + origin=origin, + axis=axis, + mimic_joint=None if mimic is None else str(mimic.get("joint")), + mimic_multiplier=( + 1.0 if mimic is None else float(mimic.get("multiplier", "1")) + ), + mimic_offset=( + 0.0 if mimic is None else float(mimic.get("offset", "0")) + ), + ) + self.joints[name] = model + self.parent_joint_by_child[model.child] = name + + def _chain(self, target_joint: str) -> list[_UrdfJoint]: + if target_joint not in self.joints: + raise ValueError(f"URDF is missing joint {target_joint}") + result: list[_UrdfJoint] = [] + current = self.joints[target_joint] + while True: + result.append(current) + parent_joint = self.parent_joint_by_child.get(current.parent) + if parent_joint is None: + break + current = self.joints[parent_joint] + result.reverse() + return result + + def axis_line( + self, + target_joint: str, + *, + zero_offsets: Mapping[str, float], + joint_angles: Mapping[str, float], + ) -> tuple[np.ndarray, np.ndarray]: + transform = np.eye(4) + resolved_angles = dict(joint_angles) + for joint in self._chain(target_joint): + corrected_origin = joint.origin @ _axis_rotation( + joint.axis, float(zero_offsets.get(joint.name, 0.0)) + ) + joint_frame = transform @ corrected_origin + if joint.name == target_joint: + return ( + joint_frame[:3, :3] @ joint.axis, + joint_frame[:3, 3].copy(), + ) + if joint.mimic_joint is None: + angle = float(resolved_angles.get(joint.name, 0.0)) + else: + angle = ( + joint.mimic_multiplier + * float(resolved_angles.get(joint.mimic_joint, 0.0)) + + joint.mimic_offset + ) + resolved_angles[joint.name] = angle + transform = joint_frame @ _axis_rotation(joint.axis, angle) + raise RuntimeError(f"could not resolve axis line for {target_joint}") + + +def _parse_triplet(value: str) -> np.ndarray: + result = np.asarray([float(item) for item in str(value).split()], dtype=float) + if result.shape != (3,) or not np.all(np.isfinite(result)): + raise ValueError(f"invalid URDF vector: {value}") + return result + + +def _axis_rotation(axis: Sequence[float], angle: float) -> np.ndarray: + result = np.eye(4) + result[:3, :3] = Rotation.from_rotvec( + _vector(axis, 3, name="joint axis") * float(angle) + ).as_matrix() + return result + + +@dataclass(frozen=True) +class ZeroSolveResult: + direct_offsets_rad: Mapping[str, float] + all_active_offsets_rad: Mapping[str, float] + base_translation_xyz_m: tuple[float, float, float] + base_quaternion_xyzw: tuple[float, float, float, float] + validation_errors_rad: tuple[float, ...] + validation_error_by_joint_rad: Mapping[str, float] + validation_line_error_by_joint_m: Mapping[str, float] + axis_line_rms_m: float + passed: bool + + +def _angles_from_state( + state_u8: Sequence[float], + *, + curves: Mapping[str, JointCurveFit], + motor_by_joint: Mapping[str, int], +) -> dict[str, float]: + state = np.asarray(state_u8, dtype=float) + result: dict[str, float] = {} + for joint, motor in motor_by_joint.items(): + source = INHERITED_ZERO_JOINTS.get(joint, joint) + if source not in curves: + continue + command = int(np.clip(np.rint(state[int(motor)]), 0, 255)) + result[joint] = float(curves[source].angle_rad[command]) + return result + + +def solve_urdf_zero_offsets( + *, + source_urdf: str | Path, + measurements: Sequence[JointAxisMeasurement], + curves: Mapping[str, JointCurveFit], + motor_by_joint: Mapping[str, int], + training_cycles: Sequence[int] = (0, 1), + validation_cycle: int = 2, + maximum_offset_rad: float = math.radians(20.0), + maximum_validation_mae_rad: float = math.radians(1.0), + maximum_validation_p95_rad: float = math.radians(2.0), +) -> ZeroSolveResult: + model = UrdfKinematicModel(source_urdf) + training = [m for m in measurements if m.cycle in set(training_cycles)] + validation = [m for m in measurements if m.cycle == int(validation_cycle)] + expected = set(AXIS_JOINTS) + if {m.joint for m in training} != expected or {m.joint for m in validation} != expected: + raise ValueError("axis measurements do not contain all required joints/cycles") + + def predicted_local( + measurement: JointAxisMeasurement, + offsets: Mapping[str, float], + ) -> tuple[np.ndarray, np.ndarray]: + angles = _angles_from_state( + measurement.condition_state_u8, + curves=curves, + motor_by_joint=motor_by_joint, + ) + return model.axis_line( + measurement.joint, + zero_offsets=offsets, + joint_angles=angles, + ) + + zero_offsets = {name: 0.0 for name in DIRECT_ZERO_JOINTS} + urdf_axes = [] + measured_axes = [] + urdf_points = [] + measured_points = [] + for measurement in training: + axis, point = predicted_local(measurement, zero_offsets) + urdf_axes.append(axis) + urdf_points.append(point) + measured_axes.append(np.asarray(measurement.axis_common_xyz)) + measured_points.append(np.asarray(measurement.point_common_xyz_m)) + urdf_axes_array = np.asarray(urdf_axes) + measured_axes_array = np.asarray(measured_axes) + urdf_points_array = np.asarray(urdf_points) + measured_points_array = np.asarray(measured_points) + + # A fitted axis is geometrically an unoriented line. Generate base-pose + # starts with iteratively resolved signs so a mixture of motor/URDF axis + # conventions cannot trap the optimizer in a wrong 180-degree basin. + generator = np.random.default_rng(20260806) + rotation_seeds = [Rotation.identity()] + rotation_seeds.extend(Rotation.random(11, random_state=generator)) + rotation_starts: list[Rotation] = [] + for seed in rotation_seeds: + candidate = seed + for _ in range(8): + predicted = candidate.apply(urdf_axes_array) + signs = np.where( + np.sum(predicted * measured_axes_array, axis=1) < 0.0, + -1.0, + 1.0, + ) + candidate, _ = Rotation.align_vectors( + measured_axes_array * signs[:, None], urdf_axes_array + ) + if not any( + (existing.inv() * candidate).magnitude() < math.radians(0.1) + for existing in rotation_starts + ): + rotation_starts.append(candidate) + lower = np.concatenate( + ( + -np.ones(3) * math.pi, + -np.ones(3) * 2.0, + -np.ones(len(OPTIMIZED_ZERO_JOINTS)) * maximum_offset_rad, + ) + ) + upper = np.concatenate( + ( + np.ones(3) * math.pi, + np.ones(3) * 2.0, + np.ones(len(OPTIMIZED_ZERO_JOINTS)) * maximum_offset_rad, + ) + ) + + def offsets_from_parameters(parameters: np.ndarray) -> dict[str, float]: + offsets = { + name: float(value) + for name, value in FIXED_DIRECT_ZERO_OFFSETS_RAD.items() + } + offsets.update( + { + name: float(value) + for name, value in zip( + OPTIMIZED_ZERO_JOINTS, parameters[6:] + ) + } + ) + return offsets + + def residual(parameters: np.ndarray, selected: Sequence[JointAxisMeasurement]) -> np.ndarray: + base_rotation = Rotation.from_rotvec(parameters[:3]) + base_translation = parameters[3:6] + offsets = offsets_from_parameters(parameters) + values: list[float] = [] + for measurement in selected: + axis, point = predicted_local(measurement, offsets) + axis = base_rotation.apply(axis) + point = base_rotation.apply(point) + base_translation + observed_axis = np.asarray(measurement.axis_common_xyz, dtype=float) + observed_point = np.asarray(measurement.point_common_xyz_m, dtype=float) + if float(axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + if measurement.joint in ZERO_ROOT_ANCHOR_JOINTS: + values.extend( + ((axis - observed_axis) / math.radians(1.0)).tolist() + ) + elif measurement.joint in ZERO_AXIS_PARENT_JOINT: + parent_axis, _ = model.axis_line( + ZERO_AXIS_PARENT_JOINT[measurement.joint], + zero_offsets=offsets, + joint_angles=_angles_from_state( + measurement.condition_state_u8, + curves=curves, + motor_by_joint=motor_by_joint, + ), + ) + parent_axis = base_rotation.apply(parent_axis) + values.append( + _zero_sensitive_axis_error_rad( + axis, observed_axis, parent_axis + ) + / math.radians(1.0) + ) + if measurement.joint in ZERO_LINE_OBSERVATION_JOINTS: + values.extend( + ( + np.cross(point - observed_point, observed_axis) / 0.003 + ).tolist() + ) + return np.asarray(values, dtype=float) + + starts: list[np.ndarray] = [] + for rotation_start in rotation_starts: + translation_start = np.median( + measured_points_array - rotation_start.apply(urdf_points_array), + axis=0, + ) + candidate = np.concatenate( + ( + rotation_start.as_rotvec(), + translation_start, + generator.uniform( + -math.radians(3.0), + math.radians(3.0), + size=len(OPTIMIZED_ZERO_JOINTS), + ), + ) + ) + starts.append( + np.clip(candidate, lower + 1.0e-9, upper - 1.0e-9) + ) + + def optimise( + selected: Sequence[JointAxisMeasurement], + candidates: Sequence[np.ndarray], + ) -> Any: + solutions = [ + least_squares( + residual, + start, + args=(selected,), + bounds=(lower, upper), + loss="soft_l1", + f_scale=1.0, + max_nfev=5000, + ) + for start in candidates + ] + return min( + solutions, key=lambda item: float(np.sum(np.square(item.fun))) + ) + + solution = optimise(training, starts) + if not solution.success: + raise ValueError(f"URDF zero optimization failed: {solution.message}") + offsets = offsets_from_parameters(solution.x) + bound_hits = { + name: math.degrees(value) + for name, value in offsets.items() + if abs(value) >= maximum_offset_rad - math.radians(0.01) + } + if bound_hits: + details = ", ".join( + f"{name}={value:+.3f}deg" + for name, value in sorted(bound_hits.items()) + ) + all_details = ", ".join( + f"{name}={math.degrees(value):+.3f}deg" + for name, value in sorted(offsets.items()) + ) + raise ValueError( + "URDF zero offset reached the configured " + f"{math.degrees(maximum_offset_rad):.3f} degree bound: {details}; " + f"all_offsets: {all_details}" + ) + + base_rotation = Rotation.from_rotvec(solution.x[:3]) + base_translation = solution.x[3:6] + validation_errors: list[float] = [] + line_errors: list[float] = [] + validation_error_by_joint: dict[str, float] = {} + validation_line_error_by_joint: dict[str, float] = {} + for measurement in validation: + axis, point = predicted_local(measurement, offsets) + axis = base_rotation.apply(axis) + point = base_rotation.apply(point) + base_translation + observed_axis = np.asarray(measurement.axis_common_xyz, dtype=float) + observed_point = np.asarray(measurement.point_common_xyz_m, dtype=float) + if float(axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + if measurement.joint in ZERO_AXIS_PARENT_JOINT: + parent_axis, _ = model.axis_line( + ZERO_AXIS_PARENT_JOINT[measurement.joint], + zero_offsets=offsets, + joint_angles=_angles_from_state( + measurement.condition_state_u8, + curves=curves, + motor_by_joint=motor_by_joint, + ), + ) + parent_axis = base_rotation.apply(parent_axis) + axis_error = abs( + _zero_sensitive_axis_error_rad( + axis, observed_axis, parent_axis + ) + ) + else: + dot = abs(float(np.clip(axis @ observed_axis, -1.0, 1.0))) + axis_error = math.acos(dot) + line_error = float( + np.linalg.norm(np.cross(point - observed_point, observed_axis)) + ) + validation_errors.append(axis_error) + line_errors.append(line_error) + validation_error_by_joint[measurement.joint] = axis_error + validation_line_error_by_joint[measurement.joint] = line_error + zero_axis_errors = np.asarray( + [ + error + for measurement, error in zip(validation, validation_errors) + if measurement.joint in ZERO_AXIS_OBSERVATION_JOINTS + and measurement.joint not in ZERO_ROOT_ANCHOR_JOINTS + ], + dtype=float, + ) + zero_line_errors = np.asarray( + [ + error + for measurement, error in zip(validation, line_errors) + if measurement.joint in ZERO_LINE_OBSERVATION_JOINTS + and measurement.joint not in ZERO_ROOT_ANCHOR_JOINTS + ], + dtype=float, + ) + zero_line_rms = float( + np.sqrt(np.mean(np.square(zero_line_errors))) + ) + passed = bool( + zero_axis_errors.size + and zero_line_errors.size + and float(np.mean(zero_axis_errors)) <= maximum_validation_mae_rad + and float(np.percentile(zero_axis_errors, 95.0)) + <= maximum_validation_p95_rad + and zero_line_rms <= 0.003 + ) + + # The untouched third cycle is used only to decide whether the result is + # admissible. Once it passes, refit with all three cycles so the delivered + # offset uses every valid observation without weakening holdout validation. + if passed: + all_solution = optimise(list(measurements), [solution.x]) + if not all_solution.success: + raise ValueError( + f"final URDF zero optimization failed: {all_solution.message}" + ) + solution = all_solution + offsets = offsets_from_parameters(solution.x) + final_bound_hits = { + name: math.degrees(value) + for name, value in offsets.items() + if abs(value) >= maximum_offset_rad - math.radians(0.01) + } + if final_bound_hits: + details = ", ".join( + f"{name}={value:+.3f}deg" + for name, value in sorted(final_bound_hits.items()) + ) + all_details = ", ".join( + f"{name}={math.degrees(value):+.3f}deg" + for name, value in sorted(offsets.items()) + ) + raise ValueError( + "final URDF zero offset reached the configured " + f"{math.degrees(maximum_offset_rad):.3f} degree bound: " + f"{details}; all_offsets: {all_details}" + ) + base_rotation = Rotation.from_rotvec(solution.x[:3]) + base_translation = solution.x[3:6] + all_offsets = dict(offsets) + for target, source in INHERITED_ZERO_JOINTS.items(): + all_offsets[target] = offsets[source] + return ZeroSolveResult( + direct_offsets_rad=offsets, + all_active_offsets_rad=all_offsets, + base_translation_xyz_m=tuple(float(value) for value in base_translation), + base_quaternion_xyzw=tuple(float(value) for value in base_rotation.as_quat()), + validation_errors_rad=tuple(float(value) for value in validation_errors), + validation_error_by_joint_rad=validation_error_by_joint, + validation_line_error_by_joint_m=validation_line_error_by_joint, + # Report only the line components that are observable by, and used + # to validate, the encoder-zero solve. Other measured line residuals + # describe fixed CAD/PnP geometry and must not be presented as zero + # calibration error. + axis_line_rms_m=zero_line_rms, + passed=passed, + ) + + +def write_zero_corrected_urdf( + *, + source_urdf: str | Path, + output_directory: str | Path, + serial_number: str, + offsets_rad: Mapping[str, float], + timestamp: str | None = None, + joint_angle_ranges_rad: Mapping[str, Sequence[float]] | None = None, + joint_limit_margin_rad: float = math.radians(0.5), +) -> Path: + source = Path(source_urdf).expanduser().resolve() + output = Path(output_directory).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"source URDF does not exist: {source}") + if ( + "zero_calibrated" in source.stem.lower() + or re.search(r"calibrated_20\d{6}", source.stem.lower()) + ): + raise ValueError( + "source_urdf must be the original CAD URDF, not a calibrated URDF" + ) + output.mkdir(parents=True, exist_ok=True) + stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") + safe_serial = "".join( + character if character.isalnum() or character in "_.-" else "_" + for character in str(serial_number) + ) + destination = output / f"{source.stem}_zero_calibrated_{safe_serial}_{stamp}.urdf" + if destination == source or destination.exists(): + raise ValueError(f"refusing to overwrite URDF: {destination}") + tree = ET.parse(source) + root = tree.getroot() + original_text = source.read_text(encoding="utf-8") + replacement_rpy: dict[str, str] = {} + replacement_limits: dict[str, tuple[str, str]] = {} + found: set[str] = set() + range_targets = dict(joint_angle_ranges_rad or {}) + limit_margin = float(joint_limit_margin_rad) + if not math.isfinite(limit_margin) or limit_margin < 0.0: + raise ValueError("joint_limit_margin_rad must be finite and non-negative") + for joint in root.findall("joint"): + name = str(joint.get("name")) + if name not in offsets_rad and name not in range_targets: + continue + if name in offsets_rad: + axis_node = joint.find("axis") + axis = _parse_triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + axis /= np.linalg.norm(axis) + origin_node = joint.find("origin") + if origin_node is None: + raise ValueError( + f"joint {name} has no origin; refusing a non-minimal rewrite" + ) + rpy = _parse_triplet(origin_node.get("rpy", "0 0 0")) + original = Rotation.from_euler("xyz", rpy).as_matrix() + corrected = original @ Rotation.from_rotvec( + axis * float(offsets_rad[name]) + ).as_matrix() + corrected_rpy = Rotation.from_matrix(corrected).as_euler("xyz") + replacement_rpy[name] = " ".join( + f"{float(value):.15g}" for value in corrected_rpy + ) + if name in range_targets: + values = np.asarray(range_targets[name], dtype=float).reshape(-1) + if values.size == 0 or not np.all(np.isfinite(values)): + raise ValueError(f"joint {name} angle range must be finite and non-empty") + limit_node = joint.find("limit") + if ( + limit_node is None + or limit_node.get("lower") is None + or limit_node.get("upper") is None + ): + raise ValueError(f"joint {name} has no finite lower/upper limit") + lower = float(limit_node.get("lower")) + upper = float(limit_node.get("upper")) + if not math.isfinite(lower) or not math.isfinite(upper): + raise ValueError(f"joint {name} has a non-finite URDF limit") + measured_lower = float(np.min(values)) + measured_upper = float(np.max(values)) + expanded_lower = ( + measured_lower - limit_margin + if measured_lower < lower + else lower + ) + expanded_upper = ( + measured_upper + limit_margin + if measured_upper > upper + else upper + ) + if expanded_lower >= expanded_upper: + raise ValueError(f"joint {name} has an invalid expanded URDF limit") + replacement_limits[name] = ( + f"{expanded_lower:.15g}", + f"{expanded_upper:.15g}", + ) + found.add(name) + missing = sorted((set(offsets_rad) | set(range_targets)) - found) + if missing: + raise ValueError("source URDF is missing target joints: " + ",".join(missing)) + joint_pattern = re.compile( + r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>" + r".*?", + re.DOTALL, + ) + edits: list[tuple[int, int, str]] = [] + for match in joint_pattern.finditer(original_text): + name = match.group("name") + if name not in replacement_rpy and name not in replacement_limits: + continue + block = match.group(0) + if name in replacement_rpy: + origin_match = re.search( + r"]*\brpy\s*=\s*([\"'])(?P[^\"']*)\1", + block, + re.DOTALL, + ) + if origin_match is None: + raise ValueError(f"joint {name} origin has no rpy attribute") + start = match.start() + origin_match.start("rpy") + end = match.start() + origin_match.end("rpy") + edits.append((start, end, replacement_rpy[name])) + if name in replacement_limits: + limit_match = re.search(r"]*/?>", block, re.DOTALL) + if limit_match is None: + raise ValueError(f"joint {name} limit could not be located") + limit_text = limit_match.group(0) + for attribute, value in zip( + ("lower", "upper"), replacement_limits[name] + ): + attribute_match = re.search( + rf"\b{attribute}\s*=\s*([\"'])(?P[^\"']*)\1", + limit_text, + ) + if attribute_match is None: + raise ValueError( + f"joint {name} limit has no {attribute} attribute" + ) + start = ( + match.start() + + limit_match.start() + + attribute_match.start("value") + ) + end = ( + match.start() + + limit_match.start() + + attribute_match.end("value") + ) + edits.append((start, end, value)) + expected_edit_count = len(replacement_rpy) + 2 * len(replacement_limits) + if len(edits) != expected_edit_count: + raise ValueError("could not locate every target joint field in source URDF text") + corrected_text = original_text + for start, end, value in reversed(edits): + corrected_text = corrected_text[:start] + value + corrected_text[end:] + temporary = destination.with_suffix(".urdf.tmp") + with temporary.open("w", encoding="utf-8") as stream: + stream.write(corrected_text) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, destination) + return destination diff --git a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py index d976c67..eb11bb1 100644 --- a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py @@ -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"), diff --git a/src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py b/src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py new file mode 100644 index 0000000..503eb57 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py @@ -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), + ] + ) diff --git a/src/g20_thumb_apriltag_calibration/setup.py b/src/g20_thumb_apriltag_calibration/setup.py index ab423d8..f70e788 100644 --- a/src/g20_thumb_apriltag_calibration/setup.py +++ b/src/g20_thumb_apriltag_calibration/setup.py @@ -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" diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index 4b9797c..0038144 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/src/g20_thumb_apriltag_calibration/test/test_config.py @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py b/src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py new file mode 100644 index 0000000..4352d5a --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py index 0c78b75..b2d6e13 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py +++ b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py @@ -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) diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py index 08caa10..9604b90 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py @@ -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", diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py index ca933e7..b89b50c 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py @@ -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), + } diff --git a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py new file mode 100644 index 0000000..9c62507 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py @@ -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 + )