From a609d521a089705fef3a1f9072750018a48920f3 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Tue, 11 Aug 2026 15:51:36 +0800 Subject: [PATCH] =?UTF-8?q?g20=E5=8F=B3=E6=89=8B=E6=A0=87=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 4 + src/g20_thumb_apriltag_calibration/README.md | 224 +- .../config/three_camera_calibration.yaml | 33 +- .../calibrated_joint_state_bridge.py | 218 ++ .../extrinsics_node.py | 258 ++- .../full_hand.py | 417 ++-- .../offline_replay.py | 846 ++++++++ .../g20_thumb_apriltag_calibration/pnp.py | 399 +++- .../three_camera_diagnostics.py | 142 +- .../three_camera_node.py | 1698 ++++++++++++++-- .../urdf_zero.py | 1811 ++++++++++++----- .../calibrated_joint_state_bridge.launch.py | 37 + .../launch/three_camera_calibration.launch.py | 94 +- .../launch/three_camera_extrinsics.launch.py | 22 + src/g20_thumb_apriltag_calibration/setup.py | 9 + .../test_calibrated_joint_state_bridge.py | 87 + .../test/test_config.py | 35 +- .../test/test_extrinsics_node.py | 132 ++ .../test/test_full_hand.py | 156 ++ .../test/test_offline_replay.py | 44 + .../test/test_pnp.py | 119 ++ .../test/test_three_camera_diagnostics.py | 134 +- .../test/test_three_camera_retry.py | 586 +++++- .../test/test_urdf_zero.py | 549 ++++- .../gui_control/config/constants.py | 8 +- 25 files changed, 6996 insertions(+), 1066 deletions(-) create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py create mode 100644 src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_offline_replay.py diff --git a/.gitignore b/.gitignore index 5d3adc9..11b5c74 100644 --- a/.gitignore +++ b/.gitignore @@ -72,6 +72,10 @@ Thumbs.db /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_calibrated_*.urdf /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf +/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_cmc_pitch_*.urdf +/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_calibrated_*.urdf +/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_zero_calibrated_*.urdf +/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_G20_RIGHT_tag.urdf # ROS bag / MCAP recordings and CAN captures rosbag2_*/ diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 18ba27b..324e28c 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -1,4 +1,4 @@ -# G20 左手 AprilTag 标定 +# G20 左右手 AprilTag 标定 ## 三机位三维关节轴零位标定（schema v4） @@ -17,16 +17,16 @@ top = DB2163739，Tag 8/9 | ID | 机位 | 固定位置/运动件 | |---:|---|---| | 0 | 正面 | 正面掌壳固定基准 | -| 1 | 正面 | 拇指 CMC 后连杆 | +| 1 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 | | 2 | 正面 | 拇指 MCP 后连杆 | | 3 | 正面 | 拇指 IP 后末节 | | 4 | 侧面 | 掌壳侧面固定基准（最底下） | -| 5 | 侧面 | 食指 MCP 后连杆 | -| 6 | 侧面 | 食指 PIP 后连杆 | -| 7 | 侧面 | 食指 DIP 后末节 | +| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 | +| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 | +| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 | | 8 | 上面 | 上面相机可见的掌壳/底座固定基准 | | 9 | 上面 | 拇指 CMC yaw 运动件 | -| 10 | 正面 | 食指根部侧摆运动件（index_mcp_roll） | +| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 | ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见， ID 9 必须在拇指横摆的完整行程中持续可见。 @@ -62,7 +62,12 @@ PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒 外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加 一个候选；随后以相同方法采集正面/上面。程序使用固定内参的 -`stereoCalibrate` 联合优化唯一旋转/平移，自动剔除粗大异常组；两组均得到 +`stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和 +配对相机的单帧RMS分别不得超过1.5 px，同时组合RMS不得超过1.2 px；界面中 +单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。 +新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组， +再在不低于15个内点的前提下有界裁剪联合误差最高的候选，最终1.2 px门限不会被 +放宽。两组均得到 至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。 ```bash @@ -71,10 +76,12 @@ 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。文件同时绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存 -通过结果，正式标定也不会运动。 +采集时可分别查看 `/g20_extrinsics/{front,side,top}/camera/image_rect`。界面始终 +显示当前配对的整批RMS、旋转稳定性和平移稳定性；`BATCH FAIL` 后会直接列出 +`INLIERS`、`RMS`、`ROT` 或 `TRANS` 失败项。保存门限为：联合重投影RMS不超过 +1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过1.5 mm。文件同时 +绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存通过结果，正式 +标定也不会运动。 ### 2. 预检和正式标定 @@ -83,6 +90,7 @@ ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {} ```bash ros2 launch g20_thumb_apriltag_calibration \ three_camera_calibration.launch.py \ + hand_type:=left \ serial_number:=G20_LEFT_001 \ camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \ @@ -105,12 +113,24 @@ ros2 run image_view image_view --ros-args \ ```bash ros2 launch g20_thumb_apriltag_calibration \ three_camera_calibration.launch.py \ + hand_type:=left \ serial_number:=G20_LEFT_001 \ camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \ can_interface:=can0 ``` +右手使用同一入口；默认自动选择右手SDK话题和原始URDF： + +```bash +ros2 launch g20_thumb_apriltag_calibration \ + three_camera_calibration.launch.py \ + hand_type:=right \ + serial_number:=G20_RIGHT_001 \ + camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ + can_interface:=can0 +``` + 状态显示三个机位均“就绪”后只调用一次： ```bash @@ -126,66 +146,121 @@ 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→255`。轨迹角由父/子Tag完整相对四元数的旋转向量投影到三维拟合轴得到。 -对正面/侧面近图像平面的浅圆弧，使用姿态旋转轴约束Tag中心三维圆，只由圆轨迹 -拟合轴线位置和半径，避免单目平面Tag深度噪声改变轴方向；对运动平面充分可观测的 -斜视轨迹，仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令 -严格归零：普通通道255，四指侧摆127。中指、无名指、小指继续继承食指曲线；四指 -`mcp_roll/mcp_pitch/pip` 的静态URDF偏移统一固定为0，不再把单根食指的安装残差复制 -到四根手指。`thumb_mcp` 的静态URDF偏移同样固定为0，避免IP轴线深度偏差在命令255 -处产生额外向外伸展；其256项实测轨迹保持不变。 +左手依次扫描电机 `0/5/15/6/1/16/10`，右手依次扫描 +`0/5/15/9/4/19/10`，每项三轮 `255→0→255`。轨迹角由父/子Tag完整相对 +四元数的旋转向量投影到三维拟合轴得到。 +轴方向使用相对姿态旋转轴和可信上游轴约束；仅对斜视、非约束关节将中心圆作为独立 +交叉检查并参与融合。轴线上一点则由整段 +相对SE(3)轨迹的 `(I-R)p=t` 方程鲁棒拟合，不再把单目Tag中心自由三维圆的圆心直接 +当成机械轴心。正面/侧面端视关节只使用图像平面内可观分量，丢弃无法由单目确定的 +光轴深度；斜视轨迹仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令 +严格归零：普通通道255，四指侧摆127。左手将食指动态轨迹、右手将小指动态轨迹继承 +给其余三指。11-Tag布局只能可靠恢复参考指的动态命令—角度曲线，不能证明四根独立 +电机的绝对装配相位相同；因此四指全部MCP侧摆、MCP屈伸和PIP静态URDF零偏都保留 +原始CAD的0，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。 -零位模式锁死原始CAD几何。URDF中确定平行的 `thumb_cmc_pitch→thumb_mcp→thumb_ip` -和 `index_mcp_pitch→index_pip→index_dip` 使用同轮上游实测方向，同时由各自Tag中心 -轨迹独立拟合轴线位置；被动角度曲线仍由对应父/子Tag完整相对姿态生成。这样保留 -下游轴线对上游零位的约束，同时避免小平面Tag的稳定PnP方向偏差被误认为URDF轴误差。 +零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定 +掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免 +稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节 +进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游 +轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差 +超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴 +Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过 +1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°； +小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。 -前两轮拟合，第三轮强制留出验证；第三轮轨迹MAE必须≤1°、P95≤2°，三轮真实 -姿态轴方向极差≤0.75°、轴向/径向RMS≤3 mm。零位留出验收只统计能观测对应上游 -零位的下游轴分量：方向MAE≤1°、P95≤2°，轴线RMS≤3 mm；根轴自身及与当前零位 -无关的固定CAD/PnP残差不会再把零位推向±20°。独立三维运动平面充分可观测的关节 -额外要求姿态轴与圆轴夹角≤1°。任一指标失败会暂停并指出失败关节；调用 `resume` -只重扫对应电机的6个方向。 +`thumb_mcp` 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过 +被动 `thumb_ip` 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴 +偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手 +`thumb_mcp` 都保留原始CAD零位0。该保护只冻结静态 `origin.rpy`，不会冻结或复制其 +`angle_rad[256]` 实测轨迹。 + +前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位 +差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的 +95% bootstrap改善置信检查。最终门限不会因自动重试而放宽。 单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动 耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差， 不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。 -零位求解联合优化公共手掌六维位姿和3个拇指CMC直接主动关节的 -`urdf_zero_offset_rad`。依赖链为 yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线 -约束拇指pitch；`thumb_mcp` 固定为0，不再使用IP轴线修改它的静态零位。食指扫描仍 -用于生成四指轨迹，但不再用于修改四指静态URDF零位。 +四指参考源的MCP pitch虽有约70°大行程，但侧面机位接近沿转轴观察，单目PnP深度偏差 +仍可能把低残差的Tag中心圆平面稳定地倾斜。因此MCP pitch与其他端视关节一样，始终用 +完整相对姿态确定轴方向，Tag中心轨迹只参与轴线位置拟合；不再按10°分界在两种轴模型 +之间切换。固定Tag安装旋转会在相对旋转中抵消，不需要中心圆回退。被动PIP/DIP继承 +上游轴方向时不重复报告同一项跨轮轴失败。 + +四指MCP侧摆的动态曲线仍由参考指三轮实测并继承，但绝对静态侧摆零位固定使用原始CAD +的0。仅凭下游pitch轴相对CAD掌坐标反推roll相位，会把稳定的跨视角/固定几何偏差写成 +约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。 + +四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断， +但拟合出的绝对相位不写入任何一根四指的 `origin.rpy`。拇指CMC roll/yaw/pitch的非零 +修正只能来自当前会话的三轮轨迹求解并通过第三轮留出验证；代码和配置中不保存任何 +按左右手或序列号写死的拇指零位角。电机5的256点动态曲线也使用本机三轮实测结果。 + +7个直接零位依赖链为：yaw轴约束拇指roll、pitch轴约束拇指yaw、MCP轴线相位约束 +拇指pitch；IP轴线相位仅作诊断，不能覆盖拇指MCP的原始CAD零位。参考指MCP pitch轴 +约束roll，PIP/DIP轴线相位只用于参考指机构诊断，不再覆盖四指CAD静态零位。 原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5 保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。 -偏移超过±20°时整次失败。 +偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL +会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索 +不会放宽正式结果的硬门限。 -生成修正URDF时，四指主动 `mcp_roll/mcp_pitch/pip` 的关节限位会检查对应256项实测 -轨迹；仅当实测端点越过原始CAD限位时才向外扩展，并额外保留0.5°数值余量。不会 -收窄原限位，也不会修改拇指或四指被动DIP的限位。 +生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的 +`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终 +JSON；实测曲线即使略微越过CAD限位，也不能自动扩大URDF限位。 -任一指标失败时会立即暂停，中文状态显示关节名、实测值和阈值，不再等到42个方向 -全部结束。修正现场问题后调用 `resume`， -程序只清除该电机任务的内存样本并重扫它的6个方向；前面已通过的关节保留。 -失败样本不从 `raw_samples.jsonl` 删除，而是使用 `attempt` 和 `retry` 记录区分， -便于调试；最终拟合只使用当前通过尝试的内存数据。 +坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、 +重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%， +端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时 +连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并 +暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫 +完整关节，每关节最多自动重采2轮。过程指标在最终门限的1.25倍内只发黄色预警，最终 +拟合仍按原硬门限验收。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序 +只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复 +失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试 +仍保存在 `raw_samples.jsonl`。 -标定食指 `index_mcp_roll`（电机6）时，为避免中指遮挡ID 10， -程序将中指、无名指和小指的侧摆电机7/8/9固定为0；开始采样前会同时确认 -电机6到达扫描起点且电机7/8/9均已到达0。离开该标定项后恢复统一基准姿态。 -该项目还会通过SDK设置接口把五指速度临时设为 `[15,5,15,15,15]`，即只把 -食指速度从15降为5；离开该项目后恢复 `[15,15,15,15,15]`。 -侧面标定 `index_mcp_pitch`（电机1）和 `index_pip/index_dip`（电机16）时， -五指速度设为 `[15,10,15,15,15]`，即食指屈伸使用第三档速度10；其余直接 -测量关节保持普通速度15。 +每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。 +程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组 +分支；侧面Tag 4/5/6/7贴面在该端点应近似平行，初始化还会比较相邻Tag法向，避免 +错误镜像分支虽然8帧稳定且重投影很小仍被选中。每轮 `255→0` 前都会在静止端点独立 +重置并重新选择分支，使第三轮同时成为PnP初始化留出，而不是三轮共享同一错误分支。 +再开始正式轨迹采集。初始化帧不写入轨迹；最终单轴、跨轮和留出门限不变。 + +左手测食指roll时将电机7/8/9固定到0；右手测小指roll时因左右手侧摆机构镜像， +将电机6/7/8固定到255。两者均为相机画面向右的物理避挡方向，速度分别为 +`[15,5,15,15,15]` 和 `[15,15,15,15,5]`。参考指MCP pitch/PIP扫描分别使用 +电机1/16（左）或4/19（右），参考指速度10。 + +右手扫描拇指CMC俯仰（电机0）前，程序将拇指横摆电机10和拇指侧摆电机5都固定到 +255，确认两个辅助关节到位后才允许电机0执行全行程。该关节由正面机位使用掌部 +Tag 0和运动Tag 1同帧测量。每帧都会保留两个辅助关节 +的实测条件值，轴线经外参转换到公共坐标系，零位求解按URDF上游关节链补偿；左手 +仍沿用原有正面机位和基准姿态。 标定 `thumb_cmc_yaw`（电机10）时，程序将 `thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag -ID 9的可见性和PnP稳定性。离开该标定项后，电机5恢复基准值255； -最终JSON的 `baseline_command_u8` 不变。 +ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 `resume` 都保持 +电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才 +恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员 +暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的 +`baseline_command_u8` 不变。 + +右手标定小指PIP（电机19）时，命令0对应的固件反馈可能稳定饱和在5。只有电机19 +的0端使用±5反馈容差，并将该实测机械端点归入命令0端点分箱；255端和其他电机仍 +使用默认±2。轨迹仍须覆盖至少240个u8并通过完整拟合门限，所以中途卡滞不会被误判 +为端点到达。 + +右手拇指横摆电机10在命令255时多次实测稳定饱和在250，因此仅右手电机10的255端 +使用±5反馈容差；其命令0端实测反馈为4，仍使用±4，其他电机和中间位置不放宽。基准姿态、作为 +电机0辅助避挡姿态以及电机10自身扫描端点都使用同一条专用判定。 三机位流程默认 `validation_enabled:=false`，即不增加随机机械动作，但第三轮留出验证 始终启用且不能关闭；最终 `quality.validation_mae_rad/p95_rad` 正是第三轮轨迹误差。 +状态中的扫描进度和总体进度分开显示：42/42只表示计划轨迹已采完，总体进度在拟合和 +验证完成、正式JSON与URDF成功写入之前不会显示100%。 上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag 和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求 @@ -200,16 +275,51 @@ calibration_output/G20_LEFT_001/<时间戳>/ g20_left_G20_LEFT_001_calibration.json src/.../g20_left/ linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf + +calibration_output/G20_RIGHT_001/<时间戳>/ + g20_right_G20_RIGHT_001_calibration.json +src/.../g20_right/ + linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf ``` 文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、 `zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从 指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或 -覆盖原文件；除16个主动关节的 `origin.rpy` 外，原URDF文本保持不变。每帧Tag SE(3)、 +覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能 +改变，未观测关节和其他URDF文本保持不变。每帧Tag SE(3)、 图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。 +完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机 +命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖： + +```bash +python3 -m g20_thumb_apriltag_calibration.offline_replay \ + calibration_output/G20_RIGHT_001/20260811_120146 \ + --output-tag AXIS_FRAME_V3 \ + --write +``` + 下面保留原有正面拇指独立标定说明和兼容入口。 +### 4. 修正URDF的运行时关节映射 + +修正URDF已经把 `zero_angles.urdf_zero_offset_rad` 写入关节 +`origin.rpy`。仿真运行时只能再使用同一台、同一侧机械手JSON中的256点 +`angle_rad` 动态曲线，不能把 `urdf_zero_offset_rad` 再加一次，也不能把左手曲线 +用于右手URDF。可用桥接节点将GUI的20通道u8命令转换为完整21关节 +`JointState`（包括5个被动关节）： + +```bash +ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \ + hand_type:=right \ + calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json +``` + +默认订阅 `/cb_right_hand_control_cmd`，发布 +`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个 +发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令， +因此不会静默退回旧标定。 + 该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`apriltag_ros`、 Linker Hand SDK 和标定状态机， 只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次 @@ -287,9 +397,9 @@ source install/setup.bash 顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。 `1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。 -相机节点和三个标定 launch 会自动加载 `config/fastdds_large_images.xml`，使用 64 MB -共享内存段；否则相机内部虽为 30 Hz，大图订阅端通常只能收到约 1～4 Hz。修改配置后 -必须重启相关 ROS 进程才能生效。 +三相机标定 launch 会固定使用 `rmw_fastrtps_cpp`，并通过新旧两个 Fast DDS 环境变量 +加载 `config/fastdds_large_images.xml`，使用 64 MB 共享内存段；否则相机内部虽为 30 Hz， +大图订阅端通常只能收到约 1～4 Hz。修改配置后必须重启相关 ROS 进程才能生效。 首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从 `~/.ros/camera_info/hikrobot_DB2163742.yaml` 加载标准 ROS CameraInfo YAML；文件缺失时 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 339d411..dea3733 100644 --- a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml @@ -34,6 +34,11 @@ g20_calibration: pnp_maximum_translation_jump_m: 0.04 pnp_maximum_tag_tilt_deg: 75.0 pnp_tracker_reset_seconds: 5.0 + # 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。 + pnp_group_initialization_frames: 8 + # 侧面Tag 4/5/6/7在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。 + pnp_group_normal_alignment_scale_deg: 5.0 + pnp_group_maximum_normal_alignment_deg: 15.0 top_pnp_invalid_reset_seconds: 1.0 # 三维位姿必须与实测20通道状态严格按时间戳配对。 maximum_state_image_skew_ms: 50.0 @@ -43,6 +48,8 @@ g20_calibration: # 轴线一致性仍沿用严格检查。 passive_axis_maximum_plane_rms_m: 0.004 axis_maximum_radial_rms_m: 0.003 + # 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。 + axis_maximum_pose_line_rms_m: 0.001 # 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴 # 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。 axis_maximum_rotation_circle_difference_deg: 1.0 @@ -51,20 +58,42 @@ g20_calibration: active_maximum_rotation_orthogonal_rms_deg: 2.5 passive_maximum_rotation_orthogonal_rms_deg: 7.5 zero_maximum_axis_cycle_difference_deg: 0.75 + # 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。 + zero_maximum_axis_cone_mismatch_deg: 5.0 zero_maximum_offset_deg: 20.0 + # 四指绝对静态零偏默认保护范围。MCP侧摆只保留实测动态曲线，静态零位固定为CAD 0。 + zero_finger_maximum_offset_deg: 3.0 endpoint_tolerance_u8: 2.0 - # 电机10在命令0时实测会稳定反馈为3；仅其0端使用±3，255端及其他电机仍为±2。 - thumb_yaw_zero_endpoint_tolerance_u8: 3.0 + # 电机10在命令0时实测会稳定反馈为4；该0端使用±4。 + thumb_yaw_zero_endpoint_tolerance_u8: 4.0 + # 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。 + right_thumb_yaw_255_endpoint_tolerance_u8: 5.0 + # 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。 + pinky_pip_zero_endpoint_tolerance_u8: 5.0 endpoint_hold_seconds: 0.5 baseline_hold_seconds: 0.5 position_timeout_seconds: 30.0 sweep_timeout_seconds: 90.0 + # 反馈在远离目标时连续8秒没有至少1个u8的进展，按机械卡滞立即暂停； + # 这类故障不进入遮挡/超时的三次自动重扫。 + motor_stall_timeout_seconds: 8.0 + motor_stall_minimum_progress_u8: 1.0 invalid_timeout_seconds: 3.0 minimum_sweep_frames: 40 minimum_state_span_u8: 240.0 minimum_sweep_bins: 32 maximum_bin_gap: 16 + # 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。 + automatic_sweep_retry_limit: 3 + # 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。 + automatic_fit_retry_limit: 2 + automatic_motion_retry_limit: 2 + # 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。 + provisional_warning_ratio: 1.25 + retry_minimum_speed: 3 + retry_speed_scales: [0.8, 0.6, 0.5] + retry_endpoint_hold_seconds: [0.75, 1.0, 1.25] trajectory_maximum_plane_rms_m: 0.004 trajectory_maximum_radial_rms_m: 0.004 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py new file mode 100644 index 0000000..f227b07 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/calibrated_joint_state_bridge.py @@ -0,0 +1,218 @@ +"""Map G20 u8 commands to URDF joint angles using one calibration JSON. + +The static encoder-zero corrections in ``zero_angles`` are already baked into +the corrected URDF joint origins. This bridge therefore publishes only the +dynamic ``angle_rad`` values and never adds the static offsets a second time. +""" + +from __future__ import annotations + +import json +import math +from pathlib import Path +from typing import Any, Mapping, Sequence + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState + +from .full_hand import get_hand_calibration_profile, validate_compact_payload + + +G20_COMMAND_NAMES: tuple[str, ...] = ( + "thumb_cmc_pitch", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", + "thumb_cmc_roll", + "index_mcp_roll", + "middle_mcp_roll", + "ring_mcp_roll", + "pinky_mcp_roll", + "thumb_cmc_yaw", + "reserved_11", + "reserved_12", + "reserved_13", + "reserved_14", + "thumb_mcp", + "index_pip", + "middle_pip", + "ring_pip", + "pinky_pip", +) + +# Match the stable ordering used by the existing MuJoCo bridge. JointState +# consumers must use names, but retaining the ordering also keeps logs and +# direct comparisons deterministic. +G20_URDF_JOINT_NAMES: tuple[str, ...] = ( + "index_dip", + "index_mcp_pitch", + "index_mcp_roll", + "index_pip", + "middle_dip", + "middle_mcp_pitch", + "middle_mcp_roll", + "middle_pip", + "pinky_dip", + "pinky_mcp_pitch", + "pinky_mcp_roll", + "pinky_pip", + "ring_dip", + "ring_mcp_pitch", + "ring_mcp_roll", + "ring_pip", + "thumb_cmc_pitch", + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_ip", + "thumb_mcp", +) + + +class CalibratedCommandMapper: + """Validated, side-specific lookup from a G20 command to URDF radians.""" + + def __init__( + self, payload: Mapping[str, Any], *, expected_side: str | None = None + ) -> None: + validate_compact_payload(payload) + side = str(payload["side"]).lower() + if expected_side is not None and side != str(expected_side).lower(): + raise ValueError( + f"calibration side {side!r} does not match requested side " + f"{str(expected_side).lower()!r}" + ) + quality = payload["quality"] + if quality.get("passed") is not True: + raise ValueError("calibration quality.passed must be true") + profile = get_hand_calibration_profile(side) + self.side = side + self.serial_number = str(payload["serial_number"]) + self._motor_by_joint = { + name: int(profile.joint_specs[name].motor_index) + for name in G20_URDF_JOINT_NAMES + } + self._curves = { + name: tuple( + float(value) + for value in payload["joints"][name]["angle_rad"] + ) + for name in G20_URDF_JOINT_NAMES + } + + @staticmethod + def _command_index(value: float) -> int: + command = float(value) + if not math.isfinite(command): + raise ValueError("G20 command positions must be finite") + return max(0, min(255, int(math.floor(command + 0.5)))) + + def map_positions( + self, positions: Sequence[float], names: Sequence[str] = () + ) -> tuple[float, ...]: + values = tuple(float(value) for value in positions) + if names: + if len(names) != len(values): + raise ValueError( + "JointState names and positions must have equal length" + ) + if len(set(names)) != len(names): + raise ValueError("JointState names must be unique") + by_name = dict(zip((str(name) for name in names), values)) + missing = [name for name in G20_COMMAND_NAMES if name not in by_name] + if missing: + raise ValueError( + "G20 command is missing named channels: " + ",".join(missing) + ) + command = tuple(by_name[name] for name in G20_COMMAND_NAMES) + else: + if len(values) != len(G20_COMMAND_NAMES): + raise ValueError( + "unnamed G20 command must contain exactly 20 positions" + ) + command = values + indices = tuple(self._command_index(value) for value in command) + return tuple( + self._curves[name][indices[self._motor_by_joint[name]]] + for name in G20_URDF_JOINT_NAMES + ) + + +def load_calibrated_command_mapper( + calibration_file: str | Path, *, expected_side: str | None = None +) -> CalibratedCommandMapper: + path = Path(calibration_file).expanduser().resolve() + if not path.is_file(): + raise ValueError(f"calibration JSON does not exist: {path}") + payload = json.loads(path.read_text(encoding="utf-8")) + return CalibratedCommandMapper(payload, expected_side=expected_side) + + +class CalibratedJointStateBridge(Node): + def __init__(self) -> None: + super().__init__("g20_calibrated_joint_state_bridge") + self.declare_parameter("hand_type", "right") + self.declare_parameter("calibration_file", "") + self.declare_parameter("input_topic", "") + self.declare_parameter("output_topic", "") + + hand_type = str(self.get_parameter("hand_type").value).lower() + if hand_type not in {"left", "right"}: + raise ValueError("hand_type must be left or right") + calibration_file = str(self.get_parameter("calibration_file").value) + if not calibration_file: + raise ValueError("calibration_file is required") + self.mapper = load_calibrated_command_mapper( + calibration_file, expected_side=hand_type + ) + input_topic = str(self.get_parameter("input_topic").value).strip() + output_topic = str(self.get_parameter("output_topic").value).strip() + self.input_topic = input_topic or f"/cb_{hand_type}_hand_control_cmd" + self.output_topic = ( + output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state" + ) + self.publisher = self.create_publisher(JointState, self.output_topic, 10) + self.subscription = self.create_subscription( + JointState, self.input_topic, self._command_callback, 10 + ) + self._last_error = "" + self.get_logger().info( + f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: " + f"{self.input_topic} -> {self.output_topic}" + ) + + def _command_callback(self, command: JointState) -> None: + try: + positions = self.mapper.map_positions(command.position, command.name) + except ValueError as error: + message = str(error) + if message != self._last_error: + self.get_logger().error(message) + self._last_error = message + return + self._last_error = "" + result = JointState() + result.header = command.header + result.name = list(G20_URDF_JOINT_NAMES) + result.position = list(positions) + self.publisher.publish(result) + + +def main(args: Sequence[str] | None = None) -> None: + rclpy.init(args=args) + node: CalibratedJointStateBridge | None = None + try: + node = CalibratedJointStateBridge() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 index 1fb899d..1d05330 100644 --- 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 @@ -67,6 +67,8 @@ class StereoCapture: transform_candidates: tuple[np.ndarray, ...] pair_reprojection_rms_px: float skew_ns: int + front_reprojection_rms_px: float = float("nan") + other_reprojection_rms_px: float = float("nan") @dataclass(frozen=True) @@ -81,6 +83,64 @@ class StereoFitResult: passed: bool +def _individual_reprojection_passes( + front_rms_px: float, + other_rms_px: float, + maximum_rms_px: float, +) -> bool: + """Do not let one sharp camera hide a poor observation in the other.""" + values = np.asarray([front_rms_px, other_rms_px], dtype=float) + return bool( + np.all(np.isfinite(values)) + and np.all(values >= 0.0) + and np.all(values <= maximum_rms_px) + ) + + +def _pose_is_novel( + pose: np.ndarray, + previous_poses: list[np.ndarray], + *, + minimum_rotation_rad: float, + minimum_translation_m: float, +) -> bool: + """Require a new board pose to differ from every accepted pose. + + Comparing only with the most recent capture lets an operator alternate + between two almost identical poses and create a large but poorly + conditioned data set. A pose is new when it differs in rotation or + translation from every pose already retained. + """ + return all( + rotation >= minimum_rotation_rad + or translation >= minimum_translation_m + for rotation, translation in ( + _transform_residual(previous, pose) for previous in previous_poses + ) + ) + + +def _stereo_fit_failures( + result: StereoFitResult, + *, + minimum_inliers: int, + maximum_rms_px: float, + maximum_rotation_stability_rad: float, + maximum_translation_stability_m: float, +) -> tuple[str, ...]: + """Return stable, user-facing names for failed final quality gates.""" + failures: list[str] = [] + if len(result.inlier_indices) < minimum_inliers: + failures.append("INLIERS") + if result.stereo_rms_px > maximum_rms_px: + failures.append("RMS") + if result.rotation_stability_rad > maximum_rotation_stability_rad: + failures.append("ROT") + if result.translation_stability_m > maximum_translation_stability_m: + failures.append("TRANS") + return tuple(failures) + + def _select_latest_synchronised_pair( front_history: list[Any] | deque[Any], other_history: list[Any] | deque[Any], @@ -313,6 +373,10 @@ def _fit_stereo_robust( fit_rms = float("inf") transform = np.eye(4) per_view = np.full(len(active), float("inf")) + # Always compute a provisional fit from three captures onward so the GUI + # can guide the operator before the final count is reached. Remove only + # unmistakable geometric/reprojection outliers, and never trim below the + # required final inlier count. while len(active) >= 3: fit_rms, transform, per_view = _stereo_fit_once( captures, @@ -355,10 +419,30 @@ def _fit_stereo_robust( ) ) worst_local = int(np.argmax(scores)) - if scores[worst_local] <= 1.0 or len(active) <= 3: + if scores[worst_local] <= 1.0 or len(active) <= minimum_inliers: break active.pop(worst_local) + # Moderate bad views (typically 1.2--1.8 px) are not gross outliers, but + # accumulating them can keep the batch RMS just above the hard gate + # forever. Trim only a view whose own joint stereo error exceeds the + # unchanged final RMS limit, refitting after every removal. This is + # bounded by minimum_inliers and cannot turn an intrinsically poor data set + # into a formal result merely by relaxing the threshold. + while len(active) > minimum_inliers and fit_rms > maximum_rms_px: + worst_local = int(np.argmax(per_view)) + if float(per_view[worst_local]) <= maximum_rms_px: + break + active.pop(worst_local) + fit_rms, transform, per_view = _stereo_fit_once( + captures, + active, + object_points, + front_matrix, + other_matrix, + image_size, + ) + rotation_stability = float("inf") translation_stability = float("inf") if len(active) >= 6: @@ -389,13 +473,7 @@ def _fit_stereo_robust( 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( + provisional = StereoFitResult( front_from_other=transform, stereo_rms_px=fit_rms, per_view_rms_px=tuple(float(value) for value in per_view), @@ -403,7 +481,24 @@ def _fit_stereo_robust( rejected_indices=rejected, rotation_stability_rad=rotation_stability, translation_stability_m=translation_stability, - passed=passed, + passed=False, + ) + failures = _stereo_fit_failures( + provisional, + minimum_inliers=minimum_inliers, + maximum_rms_px=maximum_rms_px, + maximum_rotation_stability_rad=maximum_rotation_stability_rad, + maximum_translation_stability_m=maximum_translation_stability_m, + ) + return StereoFitResult( + front_from_other=provisional.front_from_other, + stereo_rms_px=provisional.stereo_rms_px, + per_view_rms_px=provisional.per_view_rms_px, + inlier_indices=provisional.inlier_indices, + rejected_indices=provisional.rejected_indices, + rotation_stability_rad=provisional.rotation_stability_rad, + translation_stability_m=provisional.translation_stability_m, + passed=not failures, ) @@ -510,6 +605,9 @@ class ThreeCameraExtrinsicsNode(Node): self.declare_parameter("minimum_captures_per_pair", 15) self.declare_parameter("maximum_pair_skew_ms", 100.0) self.declare_parameter("maximum_reprojection_rms_px", 1.2) + self.declare_parameter( + "maximum_single_camera_reprojection_rms_px", 1.5 + ) self.declare_parameter("maximum_rotation_repeatability_deg", 0.3) self.declare_parameter("maximum_translation_repeatability_m", 0.0015) self.declare_parameter("enable_gui", False) @@ -542,6 +640,9 @@ class ThreeCameraExtrinsicsNode(Node): self.maximum_reprojection_rms_px = float( value("maximum_reprojection_rms_px") ) + self.maximum_single_camera_reprojection_rms_px = float( + value("maximum_single_camera_reprojection_rms_px") + ) self.maximum_rotation_repeatability_rad = math.radians( float(value("maximum_rotation_repeatability_deg")) ) @@ -567,6 +668,16 @@ class ThreeCameraExtrinsicsNode(Node): raise ValueError("checkerboard geometry is invalid") if self.minimum_captures < 15: raise ValueError("minimum_captures_per_pair must be at least 15") + if self.maximum_reprojection_rms_px <= 0.0: + raise ValueError("maximum_reprojection_rms_px must be positive") + if ( + self.maximum_single_camera_reprojection_rms_px + < self.maximum_reprojection_rms_px + ): + raise ValueError( + "maximum_single_camera_reprojection_rms_px must be at " + "least maximum_reprojection_rms_px" + ) if self.gui_refresh_hz <= 0.0 or self.gui_refresh_hz > 10.0: raise ValueError("gui_refresh_hz must be in (0, 10]") if self.auto_capture_stable_seconds < 0.5: @@ -753,13 +864,20 @@ class ThreeCameraExtrinsicsNode(Node): "skew_ns": skew, "pair_reprojection_rms_px": pair_rms, } - if pair_rms > self.maximum_reprojection_rms_px: + camera_rms_failed = not _individual_reprojection_passes( + front.reprojection_rms_px, + selected.reprojection_rms_px, + self.maximum_single_camera_reprojection_rms_px, + ) + if camera_rms_failed or pair_rms > self.maximum_reprojection_rms_px: return PairAssessment( ready=False, reason_code="reprojection_rms_too_large", message=( - f"当前组重投影RMS={pair_rms:.3f}px超过" - f"{self.maximum_reprojection_rms_px:.3f}px门限，未计入。" + f"当前组未通过采集门限，未计入。单相机上限=" + f"{self.maximum_single_camera_reprojection_rms_px:.3f}px，" + f"组合上限={self.maximum_reprojection_rms_px:.3f}px；" + f"组合RMS={pair_rms:.3f}px，" f"front={front.reprojection_rms_px:.3f}px，" f"{other}={selected.reprojection_rms_px:.3f}px；" f"已保留{len(self.captures[other])}/{self.minimum_captures}组" @@ -775,19 +893,11 @@ class ThreeCameraExtrinsicsNode(Node): 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 + if not _pose_is_novel( + front.camera_from_board_candidates[0], + board_poses, + minimum_rotation_rad=math.radians(2.0), + minimum_translation_m=0.015, ): return PairAssessment( ready=False, @@ -891,6 +1001,12 @@ class ThreeCameraExtrinsicsNode(Node): assessment.pair_reprojection_rms_px ), skew_ns=assessment.skew_ns, + front_reprojection_rms_px=( + assessment.front_pose.reprojection_rms_px + ), + other_reprojection_rms_px=( + assessment.selected_pose.reprojection_rms_px + ), ) ) self.captured_front_board[other].append( @@ -968,12 +1084,17 @@ class ThreeCameraExtrinsicsNode(Node): 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) - ) + color = (0, 180, 255) + if pose is not None: + if pose.reprojection_rms_px <= self.maximum_reprojection_rms_px: + color = (40, 220, 40) + elif ( + pose.reprojection_rms_px + <= self.maximum_single_camera_reprojection_rms_px + ): + color = (0, 210, 255) + else: + color = (0, 0, 255) cv2.putText( image, f"{label.upper()} RMS {rms}", @@ -1150,6 +1271,10 @@ class ThreeCameraExtrinsicsNode(Node): else: self.gui_auto_pose = None self.gui_auto_stable_since = None + # Auto capture can update the fit during this callback. Render + # the new result immediately instead of showing stale metrics for + # one GUI period. + active_fit = self.fit_results[self.gui_active_other] front_frame = assessment.front_frame selected_frame = assessment.selected_frame if front_frame is None and self.image_history["front"]: @@ -1174,11 +1299,24 @@ class ThreeCameraExtrinsicsNode(Node): 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 "" - ) + batch_status = "" + if active_fit is not None: + failures = _stereo_fit_failures( + active_fit, + 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 + ), + ) + batch_status = ( + " | BATCH PASSED" + if not failures + else " | BATCH FAIL: " + ",".join(failures) + ) cv2.putText( canvas, self._gui_reason_text(assessment) + batch_status, @@ -1199,27 +1337,31 @@ class ThreeCameraExtrinsicsNode(Node): if assessment.skew_ns is None else f"{assessment.skew_ns / 1e6:.1f}" ) + batch_rms = ( + "--" if active_fit is None else f"{active_fit.stereo_rms_px:.3f}" + ) rotation = ( "--" - if assessment.provisional_rotation_error_rad is None - else f"{math.degrees(assessment.provisional_rotation_error_rad):.3f}" + if active_fit is None + else f"{math.degrees(active_fit.rotation_stability_rad):.3f}" ) translation = ( "--" - if assessment.provisional_translation_error_m is None - else f"{assessment.provisional_translation_error_m * 1000.0:.3f}" + if active_fit is None + else f"{active_fit.translation_stability_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"Batch RMS {batch_rms}/{self.maximum_reprojection_rms_px:.3f}px " + f"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, + 0.52, (220, 220, 220), 1, cv2.LINE_AA, @@ -1326,15 +1468,31 @@ class ThreeCameraExtrinsicsNode(Node): results[view] = result if any(not result.passed for result in results.values()): response.success = False + diagnostics: list[str] = [] + for view, result in results.items(): + failures = _stereo_fit_failures( + result, + 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 + ), + ) + diagnostics.append( + 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, " + f"未通过={','.join(failures) or '无'}" + ) 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() + diagnostics ) return response transforms = { 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 04435c2..63c9e70 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 @@ -72,132 +72,249 @@ class JointCurveFit: zero_offset_rad: float = 0.0 -VIEW_TAGS: dict[str, dict[str, int]] = { - "front": { - "front_base": 0, - "thumb_cmc": 1, - "thumb_mcp": 2, - "thumb_ip": 3, - "index_roll": 10, - }, - "side": { - "side_base": 4, - "index_mcp": 5, - "index_pip": 6, - "index_dip": 7, - }, - "top": { - "top_base": 8, - "thumb_yaw": 9, - }, +@dataclass(frozen=True) +class HandCalibrationProfile: + side: str + reference_finger: str + view_tags: Mapping[str, Mapping[str, int]] + preflight_view_roles: Mapping[str, tuple[str, ...]] + joint_specs: Mapping[str, JointSpec] + sweep_specs: tuple[SweepSpec, ...] + image_trajectory_joints: frozenset[str] + roll_clearance_commands: Mapping[int, int] + thumb_pitch_clearance_commands: Mapping[int, int] + + @property + def measured_joints(self) -> tuple[str, ...]: + return tuple( + name for name, spec in self.joint_specs.items() if spec.measured + ) + + @property + def active_joints(self) -> tuple[str, ...]: + return tuple( + name for name, spec in self.joint_specs.items() if spec.active + ) + + @property + def passive_joints(self) -> tuple[str, ...]: + return tuple( + name for name, spec in self.joint_specs.items() if not spec.active + ) + + @property + def reference_roll_motor(self) -> int: + return int(self.joint_specs[f"{self.reference_finger}_mcp_roll"].motor_index) + + @property + def reference_pitch_motor(self) -> int: + return int(self.joint_specs[f"{self.reference_finger}_mcp_pitch"].motor_index) + + @property + def reference_pip_motor(self) -> int: + return int(self.joint_specs[f"{self.reference_finger}_pip"].motor_index) + + @property + def reference_speed_slot(self) -> int: + return {"index": 1, "middle": 2, "ring": 3, "pinky": 4}[ + self.reference_finger + ] + + +_FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky") +_MOTOR_BY_JOINT: dict[str, int] = { + **{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)}, + **{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)}, + **{f"{finger}_pip": index + 16 for index, finger in enumerate(_FINGERS)}, } -JOINT_SPECS: dict[str, JointSpec] = { - "thumb_cmc_pitch": JointSpec( - "thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc", +def _build_hand_profile(side: str) -> HandCalibrationProfile: + hand_side = str(side).lower() + if hand_side not in {"left", "right"}: + raise ValueError("hand side must be left or right") + reference = "index" if hand_side == "left" else "pinky" + roll_role = f"{reference}_roll" + mcp_role = f"{reference}_mcp" + pip_role = f"{reference}_pip" + dip_role = f"{reference}_dip" + view_tags: dict[str, dict[str, int]] = { + "front": { + "front_base": 0, + "thumb_cmc": 1, + "thumb_mcp": 2, + "thumb_ip": 3, + roll_role: 10, + }, + "side": { + "side_base": 4, + mcp_role: 5, + pip_role: 6, + dip_role: 7, + }, + "top": {"top_base": 8, "thumb_yaw": 9}, + } + preflight_view_roles = { + view: tuple(tags) for view, tags in view_tags.items() + } + joint_specs: dict[str, JointSpec] = {} + # Both hands use the front base/Tag 1 pair for motor 0. Keeping the + # complete thumb root chain in one calibrated view avoids introducing a + # cross-camera axis-line phase into the URDF zero solve. + thumb_pitch_view = "front" + thumb_pitch_parent = "front_base" + joint_specs["thumb_cmc_pitch"] = JointSpec( + "thumb_cmc_pitch", + 0, + True, + thumb_pitch_view, + thumb_pitch_parent, + "thumb_cmc", zero_kind="urdf_axis_chain", - ), - "index_mcp_pitch": JointSpec( - "index_mcp_pitch", 1, True, "side", "side_base", "index_mcp", - zero_kind="urdf_axis_chain", - ), - "middle_mcp_pitch": JointSpec( - "middle_mcp_pitch", 2, True, None, None, None, - source_joint="index_mcp_pitch", zero_kind="inherited", - ), - "ring_mcp_pitch": JointSpec( - "ring_mcp_pitch", 3, True, None, None, None, - source_joint="index_mcp_pitch", zero_kind="inherited", - ), - "pinky_mcp_pitch": JointSpec( - "pinky_mcp_pitch", 4, True, None, None, None, - source_joint="index_mcp_pitch", zero_kind="inherited", - ), - "thumb_cmc_roll": JointSpec( + ) + for finger in _FINGERS: + name = f"{finger}_mcp_pitch" + source = None if finger == reference else f"{reference}_mcp_pitch" + joint_specs[name] = JointSpec( + name, + _MOTOR_BY_JOINT[name], + True, + "side" if source is None else None, + "side_base" if source is None else None, + mcp_role if source is None else None, + source_joint=source, + zero_kind="urdf_axis_chain" if source is None else "inherited", + ) + joint_specs["thumb_cmc_roll"] = JointSpec( "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc", zero_kind="urdf_axis_chain", - ), - "index_mcp_roll": JointSpec( - "index_mcp_roll", 6, True, "front", "front_base", "index_roll", - zero_kind="urdf_axis_chain", - ), - "middle_mcp_roll": JointSpec( - "middle_mcp_roll", 7, True, None, None, None, - source_joint="index_mcp_roll", zero_kind="inherited", - ), - "ring_mcp_roll": JointSpec( - "ring_mcp_roll", 8, True, None, None, None, - source_joint="index_mcp_roll", zero_kind="inherited", - ), - "pinky_mcp_roll": JointSpec( - "pinky_mcp_roll", 9, True, None, None, None, - source_joint="index_mcp_roll", zero_kind="inherited", - ), - "thumb_cmc_yaw": JointSpec( + ) + for finger in _FINGERS: + name = f"{finger}_mcp_roll" + source = None if finger == reference else f"{reference}_mcp_roll" + joint_specs[name] = JointSpec( + name, + _MOTOR_BY_JOINT[name], + True, + "front" if source is None else None, + "front_base" if source is None else None, + roll_role if source is None else None, + source_joint=source, + zero_kind="urdf_axis_chain" if source is None else "inherited", + ) + joint_specs["thumb_cmc_yaw"] = JointSpec( "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw", zero_kind="urdf_axis_chain", - ), - "thumb_mcp": JointSpec( + ) + joint_specs["thumb_mcp"] = JointSpec( "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp", zero_kind="urdf_axis_chain", - ), - "index_pip": JointSpec( - "index_pip", 16, True, "side", "index_mcp", "index_pip", - zero_kind="urdf_axis_chain", - ), - "middle_pip": JointSpec( - "middle_pip", 17, True, None, None, None, - source_joint="index_pip", zero_kind="inherited", - ), - "ring_pip": JointSpec( - "ring_pip", 18, True, None, None, None, - source_joint="index_pip", zero_kind="inherited", - ), - "pinky_pip": JointSpec( - "pinky_pip", 19, True, None, None, None, - source_joint="index_pip", zero_kind="inherited", - ), - "thumb_ip": JointSpec( + ) + for finger in _FINGERS: + name = f"{finger}_pip" + source = None if finger == reference else f"{reference}_pip" + joint_specs[name] = JointSpec( + name, + _MOTOR_BY_JOINT[name], + True, + "side" if source is None else None, + mcp_role if source is None else None, + pip_role if source is None else None, + source_joint=source, + zero_kind="urdf_axis_chain" if source is None else "inherited", + ) + joint_specs["thumb_ip"] = JointSpec( "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip", - ), - "index_dip": JointSpec( - "index_dip", 16, False, "side", "index_pip", "index_dip", - ), - "middle_dip": JointSpec( - "middle_dip", 17, False, None, None, None, - source_joint="index_dip", - ), - "ring_dip": JointSpec( - "ring_dip", 18, False, None, None, None, - source_joint="index_dip", - ), - "pinky_dip": JointSpec( - "pinky_dip", 19, False, None, None, None, - source_joint="index_dip", - ), -} + ) + for finger in _FINGERS: + name = f"{finger}_dip" + source = None if finger == reference else f"{reference}_dip" + joint_specs[name] = JointSpec( + name, + _MOTOR_BY_JOINT[f"{finger}_pip"], + False, + "side" if source is None else None, + pip_role if source is None else None, + dip_role if source is None else None, + source_joint=source, + ) + sweep_specs = ( + SweepSpec(thumb_pitch_view, 0, ("thumb_cmc_pitch",)), + SweepSpec("front", 5, ("thumb_cmc_roll",)), + SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")), + SweepSpec("front", _MOTOR_BY_JOINT[f"{reference}_mcp_roll"], ( + f"{reference}_mcp_roll", + )), + SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_mcp_pitch"], ( + f"{reference}_mcp_pitch", + )), + SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_pip"], ( + f"{reference}_pip", f"{reference}_dip", + )), + SweepSpec("top", 10, ("thumb_cmc_yaw",)), + ) + image_joints = frozenset( + { + "thumb_cmc_pitch", + "thumb_cmc_roll", + "thumb_mcp", + "thumb_ip", + f"{reference}_mcp_roll", + f"{reference}_mcp_pitch", + f"{reference}_pip", + } + ) + measured_roll = _MOTOR_BY_JOINT[f"{reference}_mcp_roll"] + # The roll channels are mirrored mechanically between hands. Command 0 + # moves the three non-reference fingers to the camera-right clearance pose + # on the left hand, while the same physical pose is command 255 on the + # right hand. Using 0 for both sides made the right index/middle/ring + # fingers lean left and could obstruct or load the pinky roll sweep. + roll_clearance_command = 0 if hand_side == "left" else 255 + clearance = { + _MOTOR_BY_JOINT[f"{finger}_mcp_roll"]: roll_clearance_command + for finger in _FINGERS + if finger != reference + } + assert measured_roll not in clearance + # Motor 0 is observed from the front with both other thumb-root channels + # fully open. Store the commands explicitly for the right hand so a + # caller-provided baseline cannot silently restore the obsolete 50/120 + # pose and hide Tag 1 from the front camera. + thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {} + return HandCalibrationProfile( + side=hand_side, + reference_finger=reference, + view_tags=view_tags, + preflight_view_roles=preflight_view_roles, + joint_specs=joint_specs, + sweep_specs=sweep_specs, + image_trajectory_joints=image_joints, + roll_clearance_commands=clearance, + thumb_pitch_clearance_commands=thumb_pitch_clearance, + ) -SWEEP_SPECS: tuple[SweepSpec, ...] = ( - SweepSpec("front", 0, ("thumb_cmc_pitch",)), - SweepSpec("front", 5, ("thumb_cmc_roll",)), - SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")), - SweepSpec("front", 6, ("index_mcp_roll",)), - SweepSpec("side", 1, ("index_mcp_pitch",)), - SweepSpec("side", 16, ("index_pip", "index_dip")), - SweepSpec("top", 10, ("thumb_cmc_yaw",)), -) +LEFT_HAND_PROFILE = _build_hand_profile("left") +RIGHT_HAND_PROFILE = _build_hand_profile("right") -MEASURED_JOINTS: tuple[str, ...] = tuple( - name for name, spec in JOINT_SPECS.items() if spec.measured -) -ACTIVE_JOINTS: tuple[str, ...] = tuple( - name for name, spec in JOINT_SPECS.items() if spec.active -) -PASSIVE_JOINTS: tuple[str, ...] = tuple( - name for name, spec in JOINT_SPECS.items() if not spec.active -) + +def get_hand_calibration_profile(side: str) -> HandCalibrationProfile: + value = str(side).lower() + if value == "left": + return LEFT_HAND_PROFILE + if value == "right": + return RIGHT_HAND_PROFILE + raise ValueError("hand side must be left or right") + + +# Backwards-compatible aliases keep the existing left-hand API stable. +VIEW_TAGS = LEFT_HAND_PROFILE.view_tags +JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs +SWEEP_SPECS = LEFT_HAND_PROFILE.sweep_specs +MEASURED_JOINTS = LEFT_HAND_PROFILE.measured_joints +ACTIVE_JOINTS = LEFT_HAND_PROFILE.active_joints +PASSIVE_JOINTS = LEFT_HAND_PROFILE.passive_joints SPLAY_JOINTS: tuple[str, ...] = ( "index_mcp_roll", "middle_mcp_roll", @@ -213,25 +330,11 @@ SPLAY_JOINTS: tuple[str, ...] = ( # 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", - "thumb_cmc_roll", - "thumb_mcp", - "thumb_ip", - "index_mcp_roll", - "index_mcp_pitch", - "index_pip", - } -) +IMAGE_TRAJECTORY_JOINTS = LEFT_HAND_PROFILE.image_trajectory_joints # Keep the three unmeasured finger-roll motors away from the front camera's # line of sight while index_mcp_roll is measured. -INDEX_ROLL_CLEARANCE_COMMANDS: dict[int, int] = { - 7: 0, - 8: 0, - 9: 0, -} +INDEX_ROLL_CLEARANCE_COMMANDS = LEFT_HAND_PROFILE.roll_clearance_commands # Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is # measured. This keeps the moving top-view tag sufficiently front-facing. @@ -240,10 +343,16 @@ THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = { } -def calibration_auxiliary_commands(spec: SweepSpec) -> dict[int, int]: +def calibration_auxiliary_commands( + spec: SweepSpec, + *, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, +) -> dict[int, int]: """Return motors that must remain fixed throughout one calibration task.""" - if spec.motor_index == 6: - return dict(INDEX_ROLL_CLEARANCE_COMMANDS) + if spec.motor_index == 0: + return dict(profile.thumb_pitch_clearance_commands) + if spec.motor_index == profile.reference_roll_motor: + return dict(profile.roll_clearance_commands) if spec.motor_index == 10: return dict(THUMB_YAW_CLEARANCE_COMMANDS) return {} @@ -253,11 +362,12 @@ def build_full_hand_command( motor_index: int, command_u8: int, baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, ) -> list[int]: if len(baseline) != 20: raise ValueError("baseline must contain exactly 20 values") motor = int(motor_index) - if motor not in {spec.motor_index for spec in JOINT_SPECS.values()}: + if motor not in {spec.motor_index for spec in profile.joint_specs.values()}: raise ValueError("motor_index is not a controlled G20 calibration channel") command = int(command_u8) if not 0 <= command <= 255: @@ -273,15 +383,17 @@ def build_calibration_motion_command( spec: SweepSpec, command_u8: int, baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, ) -> list[int]: """Build a sweep command, including any required clearance pose.""" result = build_full_hand_command( spec.motor_index, command_u8, baseline=baseline, + profile=profile, ) for motor_index, auxiliary_command in calibration_auxiliary_commands( - spec + spec, profile=profile ).items(): result[motor_index] = auxiliary_command return result @@ -293,6 +405,7 @@ def build_calibration_speed_profile( normal_speed: int, index_roll_speed: int, index_flex_speed: int, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, ) -> list[int]: """Return G20's five per-finger speeds for one calibration task.""" normal = int(normal_speed) @@ -303,10 +416,13 @@ def build_calibration_speed_profile( ): raise ValueError("calibration speeds must be in [0, 255]") speeds = [normal] * 5 - if spec.motor_index == 6: - speeds[1] = index_roll - elif spec.motor_index in {1, 16}: - speeds[1] = index_flex + if spec.motor_index == profile.reference_roll_motor: + speeds[profile.reference_speed_slot] = index_roll + elif spec.motor_index in { + profile.reference_pitch_motor, + profile.reference_pip_motor, + }: + speeds[profile.reference_speed_slot] = index_flex return speeds @@ -487,9 +603,10 @@ def fit_measured_joint_curve( image_maximum_radial_rms_px: float = 2.0, image_maximum_radial_p95_px: float = 3.5, image_minimum_radius_px: float = 20.0, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, ) -> JointCurveFit: """Select the stable trajectory representation for one measured joint.""" - if str(joint_name) in IMAGE_TRAJECTORY_JOINTS: + if str(joint_name) in profile.image_trajectory_joints: return fit_joint_image_curve( records, maximum_radial_rms_px=image_maximum_radial_rms_px, @@ -624,11 +741,13 @@ def build_compact_payload( validation_errors_rad: Sequence[float], passed: bool, baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + side: str = "left", ) -> dict[str, Any]: - if set(measured_fits) != set(MEASURED_JOINTS): + profile = get_hand_calibration_profile(side) + if set(measured_fits) != set(profile.measured_joints): raise ValueError("measured_fits must contain all directly measured joints") expected_active = { - name for name, spec in JOINT_SPECS.items() if spec.active + name for name, spec in profile.joint_specs.items() if spec.active } if set(urdf_zero_offsets_rad) != expected_active: raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set") @@ -636,7 +755,7 @@ def build_compact_payload( raise ValueError("baseline must contain exactly 20 commands") joints: dict[str, dict[str, Any]] = {} - for name, spec in JOINT_SPECS.items(): + for name, spec in profile.joint_specs.items(): source_name = spec.source_joint or name fit = measured_fits[source_name] joint: dict[str, Any] = { @@ -662,7 +781,7 @@ def build_compact_payload( payload = { "schema_version": 4, "model": "G20", - "side": "left", + "side": profile.side, "serial_number": str(serial_number), "angle_unit": "rad", "command_range": [0, 255], @@ -691,18 +810,19 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: raise ValueError("compact calibration has unexpected top-level fields") 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["model"] != "G20" or payload["side"] not in {"left", "right"}: + raise ValueError("payload must describe a left or right G20") + profile = get_hand_calibration_profile(str(payload["side"])) if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: raise ValueError("payload angle or command units are invalid") baseline = payload["baseline_command_u8"] if not isinstance(baseline, list) or len(baseline) != 20: raise ValueError("baseline_command_u8 must contain 20 values") joints = payload["joints"] - if not isinstance(joints, Mapping) or set(joints) != set(JOINT_SPECS): + if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs): raise ValueError("payload must contain exactly 21 G20 joints") - for name, spec in JOINT_SPECS.items(): + for name, spec in profile.joint_specs.items(): joint = joints[name] allowed = {"motor_index", "angle_rad"} allowed.add("zero_command_u8" if spec.active else "passive") @@ -747,8 +867,9 @@ 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") - if spec.active and joint["zero_angles"] != source["zero_angles"]: - raise ValueError(f"{name} must copy its source zero offset") + # source_joint denotes a dynamic command-angle curve source. The + # absolute URDF zero belongs to an independent motor/assembly and + # must remain per-joint unless it was independently observed. quality = payload["quality"] if set(quality) != { diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py new file mode 100644 index 0000000..e879a27 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/offline_replay.py @@ -0,0 +1,846 @@ +"""Safely replay a complete three-camera session without moving the hand.""" + +from __future__ import annotations + +import argparse +from collections import defaultdict +from dataclasses import replace +import hashlib +import json +import math +import os +from pathlib import Path +import re +import tempfile +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation +import yaml + +from .extrinsics import load_three_camera_extrinsics +from .full_hand import ( + HandCalibrationProfile, + JointCurveFit, + build_calibration_motion_command, + build_compact_payload, + get_hand_calibration_profile, + validate_compact_payload, +) +from .storage import atomic_write_json +from .urdf_zero import ( + JointAxisMeasurement, + UrdfKinematicModel, + _angles_from_state, + fit_joint_axis_measurement, + fit_rotation_joint_curve, + get_zero_calibration_profile, + rotation_curve_holdout_errors, + solve_urdf_zero_offsets, + write_zero_corrected_urdf, +) + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _output_suffix(output_tag: str | None) -> str: + """Return a filename-safe suffix for a non-destructive replay variant.""" + if output_tag is None: + return "" + tag = str(output_tag) + if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag): + raise ValueError( + "output tag must be 1-64 filename-safe characters, beginning " + "with a letter or digit" + ) + return f"_{tag}" + + +def _load_parameters(path: Path) -> dict[str, Any]: + payload = yaml.safe_load(path.read_text(encoding="utf-8")) + return dict(payload["g20_calibration"]["ros__parameters"]) + + +def _latest_attempt_records( + rows: Sequence[Mapping[str, Any]], +) -> dict[str, list[dict[str, Any]]]: + """Reproduce the online retry buffer from append-only raw samples. + + Online retry clears only the failed joint/cycle/direction from memory, + while JSONL deliberately retains every attempt for audit. Offline replay + must therefore select the greatest attempt independently for each logical + trajectory rather than mixing rejected attempts into the final fit. + """ + samples = [dict(row) for row in rows if row.get("kind") == "sample"] + latest_attempt: dict[tuple[str, int, str], int] = {} + for row in samples: + key = ( + str(row["joint"]), + int(row["cycle"]), + str(row["direction"]), + ) + latest_attempt[key] = max( + latest_attempt.get(key, 0), int(row.get("attempt", 1)) + ) + records: dict[str, list[dict[str, Any]]] = defaultdict(list) + for row in samples: + key = ( + str(row["joint"]), + int(row["cycle"]), + str(row["direction"]), + ) + if int(row.get("attempt", 1)) == latest_attempt[key]: + records[key[0]].append(row) + return dict(records) + + +def _load_raw_session( + session_dir: Path, +) -> tuple[dict[str, Any], dict[str, list[dict[str, Any]]], Path]: + raw_path = session_dir / "raw_samples.jsonl" + if not raw_path.is_file(): + raise ValueError(f"raw session does not exist: {raw_path}") + rows = [ + json.loads(line) + for line in raw_path.read_text(encoding="utf-8").splitlines() + if line.strip() + ] + starts = [row for row in rows if row.get("kind") == "session_start"] + if len(starts) != 1: + raise ValueError("raw session must contain exactly one session_start") + return starts[0], _latest_attempt_records(rows), raw_path + + +def _fit_curve( + name: str, + records: Sequence[Mapping[str, Any]], + *, + profile: HandCalibrationProfile, + baseline: Sequence[int], +) -> JointCurveFit: + motor = profile.joint_specs[name].motor_index + return fit_rotation_joint_curve( + records, + zero_command_u8=int(baseline[motor]), + ) + + +def _fit_axes( + *, + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + profile: HandCalibrationProfile, + baseline: Sequence[int], + extrinsics_file: Path, + repetitions: int, +) -> list[JointAxisMeasurement]: + zero_profile = get_zero_calibration_profile(profile.side) + extrinsics = load_three_camera_extrinsics(extrinsics_file) + cache: dict[tuple[str, int], JointAxisMeasurement] = {} + reference = profile.reference_finger + upstream_by_joint = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + f"{reference}_pip": f"{reference}_mcp_pitch", + f"{reference}_dip": f"{reference}_pip", + } + + def fit_one(name: str, cycle: int) -> JointAxisMeasurement: + key = (name, cycle) + if key in cache: + return cache[key] + upstream = upstream_by_joint.get(name) + constraint = None if upstream is None else fit_one(upstream, cycle).axis_common_xyz + spec = profile.joint_specs[name] + view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray( + [0.0, 0.0, 1.0], dtype=float + ) + result = fit_joint_axis_measurement( + name, + records_by_joint[name], + cycle=cycle, + zero_command_u8=int(baseline[spec.motor_index]), + axis_common_constraint=constraint, + constrained_circle_joints=zero_profile.constrained_circle_joints, + view_normal_common_xyz=view_normal, + ) + condition = build_calibration_motion_command( + spec, + int(baseline[spec.motor_index]), + baseline=baseline, + profile=profile, + ) + result = replace( + result, + condition_command_u8=tuple(float(value) for value in condition), + view_normal_common_xyz=tuple(float(value) for value in view_normal), + ) + cache[key] = result + return result + + return [ + fit_one(name, cycle) + for name in zero_profile.axis_joints + for cycle in range(repetitions) + ] + + +def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float: + maximum = 0.0 + for left in axes: + for right in axes: + maximum = max( + maximum, + math.acos( + abs( + float( + np.clip( + np.asarray(left, dtype=float) + @ np.asarray(right, dtype=float), + -1.0, + 1.0, + ) + ) + ) + ), + ) + return maximum + + +def _quality_failures( + *, + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + profile: HandCalibrationProfile, + baseline: Sequence[int], + fits: Mapping[str, JointCurveFit], + axes: Sequence[JointAxisMeasurement], + parameters: Mapping[str, Any], +) -> list[str]: + failures: list[str] = [] + repetitions = int(parameters["repetitions"]) + zero_profile = get_zero_calibration_profile(profile.side) + axis_by_key = {(item.joint, item.cycle): item for item in axes} + expected_directions = {"decreasing", "increasing"} + for name in profile.measured_joints: + records = list(records_by_joint.get(name, ())) + if not records: + failures.append(f"{name}: no samples") + continue + attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set) + for record in records: + attempts_by_direction[ + (int(record["cycle"]), str(record["direction"])) + ].add(int(record.get("attempt", 1))) + for cycle in range(repetitions): + for direction in expected_directions: + selected = [ + record + for record in records + if int(record["cycle"]) == cycle + and str(record["direction"]) == direction + ] + attempts = attempts_by_direction.get((cycle, direction), set()) + if len(attempts) != 1: + failures.append( + f"{name} cycle {cycle + 1} {direction}: ambiguous attempts {sorted(attempts)}" + ) + continue + commands = sorted({int(record["command_u8"]) for record in selected}) + if len(selected) < int(parameters["minimum_sweep_frames"]): + failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames") + if not commands or max(commands) - min(commands) < float( + parameters["minimum_state_span_u8"] + ): + failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span") + if len(commands) < int(parameters["minimum_sweep_bins"]): + failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins") + if 0 not in commands or 255 not in commands: + failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing") + if commands and max(np.diff(commands), default=0) > int( + parameters["maximum_bin_gap"] + ): + failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap") + + sync_p95 = float( + np.percentile( + [float(record.get("state_image_sync_error_ms", 0.0)) for record in records], + 95.0, + ) + ) + if sync_p95 > float(parameters["maximum_state_image_skew_ms"]): + failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms") + spec = profile.joint_specs[name] + fit = fits[name] + orthogonal_limit = math.radians( + float( + parameters[ + "active_maximum_rotation_orthogonal_rms_deg" + if spec.active + else "passive_maximum_rotation_orthogonal_rms_deg" + ] + ) + ) + if float(fit.quality["rotation_orthogonal_rms_rad"]) > orthogonal_limit: + failures.append(f"{name}: rotation orthogonal RMS") + if float(fit.quality["arc_rad"]) < math.radians( + float(parameters["trajectory_minimum_arc_deg"]) + ): + failures.append(f"{name}: trajectory arc") + monotonic_limit = math.radians( + float( + parameters[ + "maximum_monotonic_correction_deg" + if spec.active + else "passive_maximum_monotonic_correction_deg" + ] + ) + ) + hysteresis_limit = math.radians( + float( + parameters[ + "maximum_hysteresis_deg" + if spec.active + else "passive_maximum_hysteresis_deg" + ] + ) + ) + if fit.maximum_monotonic_correction_rad > monotonic_limit: + failures.append(f"{name}: monotonic correction") + if fit.maximum_hysteresis_rad > hysteresis_limit: + failures.append(f"{name}: hysteresis") + + cycle_travels: list[float] = [] + cycle_axes: list[Sequence[float]] = [] + cycle_axis_sources: list[str] = [] + for cycle in range(repetitions): + cycle_fit = _fit_curve( + name, + [record for record in records if int(record["cycle"]) == cycle], + profile=profile, + baseline=baseline, + ) + cycle_travels.append( + abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255])) + ) + axis = axis_by_key[(name, cycle)] + cycle_axes.append(axis.axis_common_xyz) + cycle_axis_sources.append(axis.axis_direction_source) + if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]): + failures.append(f"{name} cycle {cycle + 1}: radial RMS") + if axis.pose_axis_line_rms_m > float( + parameters["axis_maximum_pose_line_rms_m"] + ): + failures.append( + f"{name} cycle {cycle + 1}: pose axis-line RMS" + ) + if name not in zero_profile.constrained_circle_joints: + plane_limit = float( + parameters[ + "axis_maximum_plane_rms_m" + if spec.active + else "passive_axis_maximum_plane_rms_m" + ] + ) + if axis.plane_rms_m > plane_limit: + failures.append(f"{name} cycle {cycle + 1}: plane RMS") + if ( + name not in zero_profile.constrained_circle_joints + and axis.rotation_circle_axis_difference_rad > math.radians( + float(parameters["axis_maximum_rotation_circle_difference_deg"]) + ) + ): + failures.append(f"{name} cycle {cycle + 1}: axis disagreement") + travel_limit = math.radians( + float( + parameters[ + "trajectory_maximum_cycle_travel_difference_deg" + if spec.active + else "passive_maximum_cycle_travel_difference_deg" + ] + ) + ) + if max(cycle_travels) - min(cycle_travels) > travel_limit: + failures.append(f"{name}: cycle travel difference") + if ( + not all( + source == "upstream_constraint" + for source in cycle_axis_sources + ) + and _maximum_undirected_axis_difference(cycle_axes) > math.radians( + float(parameters["zero_maximum_axis_cycle_difference_deg"]) + ) + ): + failures.append(f"{name}: cycle axis difference") + return failures + + +def _joint_xml(path: Path) -> dict[str, ET.Element]: + return { + str(joint.get("name")): joint + for joint in ET.parse(path).getroot().findall("joint") + } + + +def _triplet(value: str) -> np.ndarray: + return np.asarray([float(item) for item in value.split()], dtype=float) + + +def _validate_corrected_urdf( + *, + source: Path, + corrected: Path, + offsets: Mapping[str, float], + axes: Sequence[JointAxisMeasurement], + curves: Mapping[str, JointCurveFit], + motor_by_joint: Mapping[str, int], + inherited_zero_joints: Mapping[str, str], +) -> dict[str, float]: + source_joints = _joint_xml(source) + corrected_joints = _joint_xml(corrected) + if set(source_joints) != set(corrected_joints): + raise ValueError("corrected URDF changed the joint set") + maximum_origin_rotation_error = 0.0 + maximum_origin_translation_error = 0.0 + for name, original_joint in source_joints.items(): + corrected_joint = corrected_joints[name] + original_origin = original_joint.find("origin") + corrected_origin = corrected_joint.find("origin") + if original_origin is None or corrected_origin is None: + continue + original_xyz = _triplet(original_origin.get("xyz", "0 0 0")) + corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0")) + maximum_origin_translation_error = max( + maximum_origin_translation_error, + float(np.linalg.norm(corrected_xyz - original_xyz)), + ) + original_rotation = Rotation.from_euler( + "xyz", _triplet(original_origin.get("rpy", "0 0 0")) + ) + corrected_rotation = Rotation.from_euler( + "xyz", _triplet(corrected_origin.get("rpy", "0 0 0")) + ) + expected = original_rotation + if name in offsets: + axis_node = original_joint.find("axis") + axis = _triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + axis /= np.linalg.norm(axis) + expected = original_rotation * Rotation.from_rotvec( + axis * float(offsets[name]) + ) + error = float((expected.inv() * corrected_rotation).magnitude()) + maximum_origin_rotation_error = max(maximum_origin_rotation_error, error) + original_limit = original_joint.find("limit") + corrected_limit = corrected_joint.find("limit") + if original_limit is not None and corrected_limit is not None: + if ( + original_limit.get("lower") != corrected_limit.get("lower") + or original_limit.get("upper") != corrected_limit.get("upper") + ): + raise ValueError(f"corrected URDF unexpectedly changed {name} limits") + if maximum_origin_translation_error > 1.0e-12: + raise ValueError("corrected URDF changed a joint origin translation") + if maximum_origin_rotation_error > 1.0e-10: + raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)") + original_model = UrdfKinematicModel(source) + corrected_model = UrdfKinematicModel(corrected) + maximum_axis_error = 0.0 + maximum_point_error = 0.0 + for measurement in axes: + state = ( + measurement.condition_state_u8 + if measurement.condition_command_u8 is None + else measurement.condition_command_u8 + ) + angles = _angles_from_state( + state, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=inherited_zero_joints, + ) + expected_axis, expected_point = original_model.axis_line( + measurement.joint, + zero_offsets=offsets, + joint_angles=angles, + ) + actual_axis, actual_point = corrected_model.axis_line( + measurement.joint, + zero_offsets={}, + joint_angles=angles, + ) + maximum_axis_error = max( + maximum_axis_error, + math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))), + ) + maximum_point_error = max( + maximum_point_error, + float(np.linalg.norm(expected_point - actual_point)), + ) + if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10: + raise ValueError("written URDF kinematics differ from the solved correction") + return { + "maximum_origin_rotation_error_rad": maximum_origin_rotation_error, + "maximum_origin_translation_error_m": maximum_origin_translation_error, + "maximum_axis_equivalence_error_rad": maximum_axis_error, + "maximum_axis_point_equivalence_error_m": maximum_point_error, + } + + +def replay_session( + session_dir: str | Path, + *, + serial_number: str | None = None, + config_file: str | Path | None = None, + write_outputs: bool = False, + output_tag: str | None = None, +) -> dict[str, Any]: + session = Path(session_dir).expanduser().resolve() + package_root = Path(__file__).resolve().parents[1] + config = ( + package_root / "config" / "three_camera_calibration.yaml" + if config_file is None + else Path(config_file).expanduser().resolve() + ) + parameters = _load_parameters(config) + start, records_by_joint, raw_path = _load_raw_session(session) + side = str(start["hand_type"]).lower() + profile = get_hand_calibration_profile(side) + zero_profile = get_zero_calibration_profile(side) + baseline = tuple(int(value) for value in start["baseline_command_u8"]) + if len(baseline) != 20: + raise ValueError("session baseline must contain exactly 20 commands") + if set(records_by_joint) != set(profile.measured_joints): + raise ValueError("raw session does not contain exactly the measured joint set") + source_urdf = Path(start["source_urdf_path"]).expanduser().resolve() + extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve() + if not source_urdf.is_file() or not extrinsics_file.is_file(): + raise ValueError("session source URDF or camera extrinsics is missing") + if "zero_calibrated" in source_urdf.stem.lower(): + raise ValueError("offline replay requires the original CAD URDF") + hand_serial = str(serial_number or session.parent.name) + output_suffix = _output_suffix(output_tag) + repetitions = int(parameters["repetitions"]) + + measured_fits = { + name: _fit_curve( + name, + records_by_joint[name], + profile=profile, + baseline=baseline, + ) + for name in profile.measured_joints + } + training_fits = { + name: _fit_curve( + name, + [ + record + for record in records_by_joint[name] + if int(record["cycle"]) in {0, 1} + ], + profile=profile, + baseline=baseline, + ) + for name in profile.measured_joints + } + axes = _fit_axes( + records_by_joint=records_by_joint, + profile=profile, + baseline=baseline, + extrinsics_file=extrinsics_file, + repetitions=repetitions, + ) + failures = _quality_failures( + records_by_joint=records_by_joint, + profile=profile, + baseline=baseline, + fits=measured_fits, + axes=axes, + parameters=parameters, + ) + if failures: + raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures)) + + holdout_by_joint = { + name: rotation_curve_holdout_errors( + training_fits[name], + [ + record + for record in records_by_joint[name] + if int(record["cycle"]) == 2 + ], + zero_command_u8=int( + baseline[profile.joint_specs[name].motor_index] + ), + ) + for name in profile.measured_joints + } + trajectory_errors = np.abs( + np.asarray( + [value for values in holdout_by_joint.values() for value in values], + dtype=float, + ) + ) + maximum_validation_mae = math.radians( + float(parameters["maximum_validation_mae_deg"]) + ) + maximum_validation_p95 = math.radians( + float(parameters["maximum_validation_p95_deg"]) + ) + trajectory_mae = float(np.mean(trajectory_errors)) + trajectory_p95 = float(np.percentile(trajectory_errors, 95.0)) + if ( + trajectory_mae > maximum_validation_mae + or trajectory_p95 > maximum_validation_p95 + ): + raise ValueError("third-cycle trajectory holdout failed") + + motor_by_joint = { + name: int(spec.motor_index) for name, spec in profile.joint_specs.items() + } + joint_limits: dict[str, float] = {} + solve_arguments = { + "source_urdf": source_urdf, + "measurements": axes, + "motor_by_joint": motor_by_joint, + "maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])), + "finger_maximum_offset_rad": math.radians( + float(parameters.get("zero_finger_maximum_offset_deg", 3.0)) + ), + "joint_maximum_offset_rad": joint_limits, + "maximum_cycle_difference_rad": math.radians( + float(parameters["zero_maximum_axis_cycle_difference_deg"]) + ), + "maximum_axis_cone_mismatch_rad": math.radians( + float(parameters["zero_maximum_axis_cone_mismatch_deg"]) + ), + "maximum_pose_axis_line_rms_m": float( + parameters["axis_maximum_pose_line_rms_m"] + ), + "maximum_validation_mae_rad": maximum_validation_mae, + "maximum_validation_p95_rad": maximum_validation_p95, + "hand_type": side, + } + holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments) + if not holdout_zero.passed: + failure = { + "reasons": dict(holdout_zero.failure_reasons), + "fitted_offsets_deg": { + name: math.degrees(value) + for name, value in holdout_zero.direct_offsets_rad.items() + }, + "cycle_offsets_deg": { + name: [math.degrees(value) for value in values] + for name, values in holdout_zero.cycle_offsets_rad.items() + }, + } + raise ValueError( + "third-cycle zero/URDF holdout failed: " + + json.dumps(failure, ensure_ascii=False, sort_keys=True) + ) + final_zero = solve_urdf_zero_offsets(curves=measured_fits, **solve_arguments) + if not final_zero.passed: + raise ValueError( + "all-cycle zero refit failed: " + + json.dumps( + { + "reasons": dict(final_zero.failure_reasons), + "fitted_offsets_deg": { + name: math.degrees(value) + for name, value in final_zero.direct_offsets_rad.items() + }, + }, + ensure_ascii=False, + sort_keys=True, + ) + ) + for target, source_name in zero_profile.inherited_static_zero_joints.items(): + if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]: + raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}") + for target in ( + set(zero_profile.inherited_zero_joints) + - set(zero_profile.inherited_static_zero_joints) + ): + if final_zero.all_active_offsets_rad[target] != 0.0: + raise ValueError(f"unobserved static zero must retain source CAD: {target}") + + validation_errors = [ + float(value) for values in holdout_by_joint.values() for value in values + ] + validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad) + payload = build_compact_payload( + serial_number=hand_serial, + measured_fits=measured_fits, + urdf_zero_offsets_rad=final_zero.all_active_offsets_rad, + validation_errors_rad=validation_errors, + passed=True, + baseline=baseline, + side=side, + ) + validate_compact_payload(payload) + + stamp = session.name + final_json = session / ( + f"g20_{side}_{hand_serial}_calibration{output_suffix}.json" + ) + expected_urdf_name = ( + f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}" + f"{output_suffix}.urdf" + ) + final_urdf = source_urdf.parent / expected_urdf_name + report_path = session / ( + f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json" + ) + if write_outputs: + existing = [path for path in (final_json, final_urdf, report_path) if path.exists()] + if existing: + raise ValueError( + "refusing to overwrite replay outputs: " + + ", ".join(str(path) for path in existing) + ) + + source_hash_before = _sha256(source_urdf) + with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary: + candidate = write_zero_corrected_urdf( + source_urdf=source_urdf, + output_directory=temporary, + serial_number=hand_serial, + offsets_rad=final_zero.all_active_offsets_rad, + timestamp=stamp, + ) + urdf_checks = _validate_corrected_urdf( + source=source_urdf, + corrected=candidate, + offsets=final_zero.all_active_offsets_rad, + axes=axes, + curves=measured_fits, + motor_by_joint=motor_by_joint, + inherited_zero_joints=zero_profile.inherited_zero_joints, + ) + residual_zero = solve_urdf_zero_offsets( + curves=measured_fits, + fixed_direct_zero_offsets_rad={ + name: 0.0 + for name in zero_profile.fixed_direct_zero_offsets_rad + }, + static_output_zero_offsets_rad={ + name: 0.0 + for name in zero_profile.static_output_zero_offsets_rad + }, + **{**solve_arguments, "source_urdf": candidate}, + ) + maximum_residual_offset = max( + abs(float(value)) for value in residual_zero.direct_offsets_rad.values() + ) + if not residual_zero.passed or maximum_residual_offset > math.radians(0.3): + raise ValueError("written URDF retains a significant zero correction") + candidate_hash = _sha256(candidate) + if write_outputs: + os.replace(candidate, final_urdf) + + if _sha256(source_urdf) != source_hash_before: + raise ValueError("source URDF changed during offline replay") + + report: dict[str, Any] = { + "passed": True, + "session_dir": str(session), + "side": side, + "serial_number": hand_serial, + "output_tag": output_tag, + "raw_samples_sha256": _sha256(raw_path), + "source_urdf": str(source_urdf), + "source_urdf_sha256": source_hash_before, + "joint_limits_deg": { + "finger_default": float( + parameters.get("zero_finger_maximum_offset_deg", 3.0) + ), + "thumb_default": float(parameters["zero_maximum_offset_deg"]), + }, + "static_zero_policy": "direct_measurements_only", + "fixed_zero_offsets_deg": { + name: math.degrees(value) + for name, value in zero_profile.fixed_direct_zero_offsets_rad.items() + }, + "static_output_zero_offsets_deg": { + name: math.degrees(value) + for name, value in zero_profile.static_output_zero_offsets_rad.items() + }, + "direct_offsets_deg": { + name: math.degrees(value) + for name, value in final_zero.direct_offsets_rad.items() + }, + "cycle_offsets_deg": { + name: [math.degrees(value) for value in values] + for name, values in holdout_zero.cycle_offsets_rad.items() + }, + "offset_uncertainty_deg": { + name: math.degrees(value) + for name, value in holdout_zero.offset_uncertainty_rad.items() + }, + "trajectory_holdout_mae_deg": math.degrees(trajectory_mae), + "trajectory_holdout_p95_deg": math.degrees(trajectory_p95), + "zero_holdout_error_deg": { + name: math.degrees(value) + for name, value in holdout_zero.validation_error_by_joint_rad.items() + }, + "zero_original_error_deg": { + name: math.degrees(value) + for name, value in holdout_zero.validation_original_error_by_joint_rad.items() + }, + "zero_improvement_95pct_lower_deg": { + name: math.degrees(value) + for name, value in ( + holdout_zero.validation_improvement_confidence_lower_rad.items() + ) + }, + "corrected_urdf_checks": { + **urdf_checks, + "maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset), + }, + "corrected_urdf_sha256": candidate_hash, + "final_json": str(final_json) if write_outputs else None, + "corrected_urdf": str(final_urdf) if write_outputs else None, + } + if write_outputs: + atomic_write_json(final_json, payload) + report["final_json_sha256"] = _sha256(final_json) + if _sha256(final_urdf) != candidate_hash: + raise ValueError("formal corrected URDF differs from validated candidate") + atomic_write_json(report_path, report) + report["validation_report"] = str(report_path) + return report + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Replay and independently validate a complete G20 calibration session." + ) + parser.add_argument("session_dir") + parser.add_argument("--serial-number", default=None) + parser.add_argument("--config-file", default=None) + parser.add_argument("--write", action="store_true") + parser.add_argument( + "--output-tag", + default=None, + help="safe suffix for a replay variant; existing outputs are never overwritten", + ) + arguments = parser.parse_args() + result = replay_session( + arguments.session_dir, + serial_number=arguments.serial_number, + config_file=arguments.config_file, + write_outputs=arguments.write, + output_tag=arguments.output_tag, + ) + print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True)) + + +if __name__ == "__main__": + main() diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py index 96054d8..4389939 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py @@ -41,6 +41,19 @@ def _relative_pose( return relative_rotation, relative_translation +def _normal_alignment_rad(first: SquareTagPose, second: SquareTagPose) -> float: + """Return the undirected angle between two observed Tag face normals.""" + first_normal = Rotation.from_quat(first.quaternion_xyzw).apply( + [0.0, 0.0, 1.0] + ) + second_normal = Rotation.from_quat(second.quaternion_xyzw).apply( + [0.0, 0.0, 1.0] + ) + return math.acos( + abs(float(np.clip(first_normal @ second_normal, -1.0, 1.0))) + ) + + def select_rigid_group_trajectory( frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], *, @@ -50,6 +63,8 @@ def select_rigid_group_trajectory( rotation_scale_rad: float, translation_scale_m: float, pair_geometry: str = "pose", + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float | None = None, ) -> tuple[ list[dict[str, SquareTagPose]], dict[str, float | str], @@ -66,20 +81,34 @@ def select_rigid_group_trajectory( """ role_names = tuple(str(role) for role in roles) pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) + normal_pair_names = tuple( + (str(first), str(second)) + for first, second in normal_alignment_pairs + ) if not frames: raise ValueError("at least one PnP frame is required") if len(set(role_names)) != len(role_names) or not role_names: raise ValueError("roles must be non-empty and unique") if any( parent not in role_names or child not in role_names - for parent, child in pair_names + for parent, child in (*pair_names, *normal_pair_names) ): - raise ValueError("fixed_pairs must reference roles") + raise ValueError("geometry pairs must reference roles") reprojection_scale = float(reprojection_scale_px) rotation_scale = float(rotation_scale_rad) translation_scale = float(translation_scale_m) geometry_mode = str(pair_geometry) - if min(reprojection_scale, rotation_scale, translation_scale) <= 0.0: + normal_scale = ( + rotation_scale + if normal_alignment_scale_rad is None + else float(normal_alignment_scale_rad) + ) + if min( + reprojection_scale, + rotation_scale, + translation_scale, + normal_scale, + ) <= 0.0: raise ValueError("trajectory selection scales must be positive") if geometry_mode not in {"pose", "distance"}: raise ValueError("pair_geometry must be pose or distance") @@ -110,6 +139,12 @@ def select_rigid_group_trajectory( pose.reprojection_error_px for pose in combination.values() ) / reprojection_scale + normal_alignment_cost = sum( + _normal_alignment_rad( + combination[first], combination[second] + ) + for first, second in normal_pair_names + ) / normal_scale rotation_drifts: list[float] = [] translation_drifts: list[float] = [] distance_drifts: list[float] = [] @@ -156,7 +191,7 @@ def select_rigid_group_trajectory( + sum(translation_drifts) / translation_scale ) return ( - reprojection_cost + geometry_cost, + reprojection_cost + geometry_cost + normal_alignment_cost, max(rotation_drifts, default=0.0), ( max(distance_drifts, default=0.0) @@ -384,9 +419,25 @@ def select_rigid_group_trajectory( translation_drifts_by_frame, dtype=float ) distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float) + normal_alignments = np.asarray( + [ + max( + ( + _normal_alignment_rad(frame[first], frame[second]) + for first, second in normal_pair_names + ), + default=0.0, + ) + for frame in best_path + ], + dtype=float, + ) return best_path, { "total_cost": float(best_total), "pair_geometry": geometry_mode, + "maximum_normal_alignment_rad": float( + np.max(normal_alignments, initial=0.0) + ), "maximum_pair_rotation_drift_rad": float( np.max(rotation_drifts, initial=0.0) ), @@ -414,6 +465,172 @@ def select_rigid_group_trajectory( } +def select_static_rigid_group_initialization( + frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], + *, + roles: Sequence[str], + fixed_pairs: Sequence[tuple[str, str]], + reprojection_scale_px: float, + maximum_pose_jump_rad: float, + maximum_translation_jump_m: float, + relative_rotation_scale_rad: float, + relative_translation_scale_m: float, + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float = math.radians(5.0), +) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]: + """Select a static multi-Tag IPPE branch path in bounded time. + + Group initialization is performed while the hand is held at an endpoint, + so every frame should describe the same camera and relative Tag poses. + Enumerating a full Viterbi transition matrix for every possible first-frame + branch is therefore unnecessary: with four two-branch Tags and eight + frames it performs more than one hundred thousand scipy rotations and can + block the live ROS callback for several seconds. + + Instead, treat every first-frame combination as a possible static + reference and independently select the closest combination in each later + frame. This preserves the multi-frame rigidity and normal-alignment + evidence while changing the search from O(F*C^2*C0) to O(F*C*C0). + """ + role_names = tuple(str(role) for role in roles) + pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) + normal_pair_names = tuple( + (str(first), str(second)) for first, second in normal_alignment_pairs + ) + if not frames: + raise ValueError("at least one PnP frame is required") + if not role_names or len(set(role_names)) != len(role_names): + raise ValueError("roles must be non-empty and unique") + if any( + parent not in role_names or child not in role_names + for parent, child in (*pair_names, *normal_pair_names) + ): + raise ValueError("geometry pairs must reference roles") + scales = ( + float(reprojection_scale_px), + float(maximum_pose_jump_rad), + float(maximum_translation_jump_m), + float(relative_rotation_scale_rad), + float(relative_translation_scale_m), + float(normal_alignment_scale_rad), + ) + if min(scales) <= 0.0: + raise ValueError("static initialization scales must be positive") + ( + reprojection_scale, + pose_scale, + translation_scale, + relative_rotation_scale, + relative_translation_scale, + normal_scale, + ) = scales + + combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] + for frame in frames: + candidate_lists = [tuple(frame.get(role, ())) for role in role_names] + if any(not candidates for candidates in candidate_lists): + raise ValueError("every frame must contain every requested role") + combinations_by_frame.append( + [ + dict(zip(role_names, combination)) + for combination in product(*candidate_lists) + ] + ) + + def score_against_reference( + reference: Mapping[str, SquareTagPose], + reference_pairs: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray] + ], + combination: Mapping[str, SquareTagPose], + ) -> float: + score = sum( + pose.reprojection_error_px for pose in combination.values() + ) / reprojection_scale + score += sum( + _normal_alignment_rad(combination[first], combination[second]) + for first, second in normal_pair_names + ) / normal_scale + score += sum( + rotation_distance_rad( + reference[role].quaternion_xyzw, + combination[role].quaternion_xyzw, + ) + / pose_scale + + float( + np.linalg.norm( + np.asarray(combination[role].translation_xyz_m, dtype=float) + - np.asarray(reference[role].translation_xyz_m, dtype=float) + ) + ) + / translation_scale + for role in role_names + ) + for pair, (reference_rotation, reference_translation) in ( + reference_pairs.items() + ): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + score += ( + float((reference_rotation.inv() * rotation).magnitude()) + / relative_rotation_scale + + float(np.linalg.norm(translation - reference_translation)) + / relative_translation_scale + ) + return float(score) + + best_total = float("inf") + best_path: list[dict[str, SquareTagPose]] | None = None + for reference in combinations_by_frame[0]: + reference_pairs = { + pair: _relative_pose(reference[pair[0]], reference[pair[1]]) + for pair in pair_names + } + path = [reference] + total = score_against_reference(reference, reference_pairs, reference) + for combinations in combinations_by_frame[1:]: + scored = [ + ( + score_against_reference( + reference, reference_pairs, combination + ), + combination, + ) + for combination in combinations + ] + cost, selected = min(scored, key=lambda item: item[0]) + total += cost + path.append(selected) + if total < best_total: + best_total = total + best_path = path + + if best_path is None: + raise RuntimeError("static group initialization produced no path") + + # Reuse the complete quality calculation with exactly one chosen branch + # per role and frame. This retains all existing quality fields without + # reintroducing the combinatorial branch search. + reduced_frames = [ + {role: (frame[role],) for role in role_names} for frame in best_path + ] + selected_path, quality = select_rigid_group_trajectory( + reduced_frames, + roles=role_names, + fixed_pairs=pair_names, + reprojection_scale_px=reprojection_scale, + rotation_scale_rad=relative_rotation_scale, + translation_scale_m=relative_translation_scale, + normal_alignment_pairs=normal_pair_names, + normal_alignment_scale_rad=normal_scale, + ) + quality = dict(quality) + quality["total_cost"] = float(best_total) + quality["initialization_search"] = "static_reference" + return selected_path, quality + + def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray: matrix = np.asarray(camera_matrix, dtype=np.float64) if matrix.shape != (3, 3): @@ -517,9 +734,26 @@ def rotation_distance_rad( first_xyzw: Sequence[float], second_xyzw: Sequence[float], ) -> float: - first = Rotation.from_quat(np.asarray(first_xyzw, dtype=float)) - second = Rotation.from_quat(np.asarray(second_xyzw, dtype=float)) - return float((first.inv() * second).magnitude()) + first = np.asarray(first_xyzw, dtype=float) + second = np.asarray(second_xyzw, dtype=float) + if first.shape != (4,) or second.shape != (4,): + raise ValueError("quaternions must contain four values") + first_norm = float(np.linalg.norm(first)) + second_norm = float(np.linalg.norm(second)) + if ( + not np.all(np.isfinite(first)) + or not np.all(np.isfinite(second)) + or first_norm <= 0.0 + or second_norm <= 0.0 + ): + raise ValueError("quaternions must be finite and non-zero") + # q and -q represent the same rotation. The absolute dot-product gives + # the geodesic SO(3) distance without constructing two scipy Rotation + # objects for every branch comparison in the live tracker. + cosine_half_angle = abs( + float(np.dot(first / first_norm, second / second_norm)) + ) + return 2.0 * math.acos(float(np.clip(cosine_half_angle, 0.0, 1.0))) def select_continuous_pose( @@ -775,19 +1009,30 @@ class SquareTagGroupPoseTracker: reprojection_scale_px: float, reprojection_weight: float, reset_after_seconds: float, + initialization_frames: int = 1, + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float = math.radians(5.0), + maximum_normal_alignment_rad: float | None = None, ) -> None: self.roles = tuple(str(role) for role in roles) self.adjacent_pairs = tuple( (str(parent), str(child)) for parent, child in adjacent_pairs ) + self.normal_alignment_pairs = tuple( + (str(first), str(second)) + for first, second in normal_alignment_pairs + ) if not self.roles or len(set(self.roles)) != len(self.roles): raise ValueError("roles must be non-empty and unique") if any( parent not in self.roles or child not in self.roles - for parent, child in self.adjacent_pairs + for parent, child in ( + *self.adjacent_pairs, + *self.normal_alignment_pairs, + ) ): - raise ValueError("adjacent_pairs must reference roles") + raise ValueError("group geometry pairs must reference roles") self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) self.maximum_translation_jump_m = float( maximum_translation_jump_m @@ -800,6 +1045,15 @@ class SquareTagGroupPoseTracker: ) self.reprojection_scale_px = float(reprojection_scale_px) self.reprojection_weight = float(reprojection_weight) + self.initialization_frames = int(initialization_frames) + self.normal_alignment_scale_rad = float( + normal_alignment_scale_rad + ) + self.maximum_normal_alignment_rad = ( + None + if maximum_normal_alignment_rad is None + else float(maximum_normal_alignment_rad) + ) reset_seconds = float(reset_after_seconds) if min( self.maximum_pose_jump_rad, @@ -807,19 +1061,35 @@ class SquareTagGroupPoseTracker: self.relative_rotation_scale_rad, self.relative_translation_scale_m, self.reprojection_scale_px, + self.normal_alignment_scale_rad, reset_seconds, ) <= 0.0: raise ValueError("group tracking scales must be positive") if self.reprojection_weight < 0.0: raise ValueError("reprojection_weight must be non-negative") + if self.initialization_frames < 1: + raise ValueError("initialization_frames must be positive") + if ( + self.maximum_normal_alignment_rad is not None + and self.maximum_normal_alignment_rad <= 0.0 + ): + raise ValueError("maximum normal alignment must be positive") self.reset_after_ns = int(reset_seconds * 1_000_000_000) self._previous: dict[str, SquareTagPose] = {} self._previous_stamp_ns: int | None = None + self._initial_candidates: list[ + dict[str, tuple[SquareTagPose, ...]] + ] = [] + self._initial_stamps_ns: list[int] = [] + self.last_initialization_quality: dict[str, float | str] = {} self.branch_correction_counts: dict[str, int] = {} def reset(self) -> None: self._previous.clear() self._previous_stamp_ns = None + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self.last_initialization_quality.clear() self.branch_correction_counts.clear() def select( @@ -856,13 +1126,110 @@ class SquareTagGroupPoseTracker: ) if not previous_is_fresh: - selected = min( - combinations, - key=lambda combination: sum( - pose.reprojection_error_px - for pose in combination.values() - ), - ) + if self._previous_stamp_ns is not None: + self._previous.clear() + self._previous_stamp_ns = None + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self.last_initialization_quality.clear() + if self.initialization_frames > 1: + if self._initial_stamps_ns and not ( + 0 <= stamp - self._initial_stamps_ns[-1] + <= self.reset_after_ns + ): + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self._initial_candidates.append( + { + role: tuple(candidates_by_role.get(role, ())) + for role in self.roles + } + ) + self._initial_stamps_ns.append(stamp) + if len(self._initial_candidates) < self.initialization_frames: + return ( + None, + "group_initializing:" + f"{len(self._initial_candidates)}/" + f"{self.initialization_frames}", + ) + selected_path, initialization_quality = ( + select_static_rigid_group_initialization( + self._initial_candidates, + roles=self.roles, + fixed_pairs=self.adjacent_pairs, + reprojection_scale_px=self.reprojection_scale_px, + maximum_pose_jump_rad=self.maximum_pose_jump_rad, + maximum_translation_jump_m=( + self.maximum_translation_jump_m + ), + relative_rotation_scale_rad=( + self.relative_rotation_scale_rad + ), + relative_translation_scale_m=( + self.relative_translation_scale_m + ), + normal_alignment_pairs=( + self.normal_alignment_pairs + ), + normal_alignment_scale_rad=( + self.normal_alignment_scale_rad + ), + ) + ) + selected = selected_path[-1] + stamp = self._initial_stamps_ns[-1] + self.last_initialization_quality = dict( + initialization_quality + ) + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + if ( + self.maximum_normal_alignment_rad is not None + and float( + initialization_quality[ + "maximum_normal_alignment_rad" + ] + ) + > self.maximum_normal_alignment_rad + ): + return None, "group_normal_alignment" + else: + selected = min( + combinations, + key=lambda combination: ( + sum( + pose.reprojection_error_px + for pose in combination.values() + ) + / self.reprojection_scale_px + + sum( + _normal_alignment_rad( + combination[first], combination[second] + ) + for first, second in self.normal_alignment_pairs + ) + / self.normal_alignment_scale_rad + ), + ) + maximum_alignment = max( + ( + _normal_alignment_rad( + selected[first], selected[second] + ) + for first, second in self.normal_alignment_pairs + ), + default=0.0, + ) + self.last_initialization_quality = { + "maximum_normal_alignment_rad": maximum_alignment + } + if ( + self.maximum_normal_alignment_rad is not None + and maximum_alignment + > self.maximum_normal_alignment_rad + ): + return None, "group_normal_alignment" else: previous_relative = { pair: _relative_pose( 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 77a7b40..c648bf1 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 @@ -9,7 +9,7 @@ from typing import Any, Mapping STATE_NAMES_ZH = { "PREFLIGHT": "设备和标签预检", "WAIT_START": "等待开始标定", - "RETURN_BASELINE": "正在返回基准姿态", + "RETURN_BASELINE": "正在恢复目标姿态", "PREPARE_SWEEP": "正在到达扫描起点", "SWEEP": "正在采集轨迹", "FITTING": "正在拟合轨迹和零位", @@ -35,6 +35,18 @@ JOINT_NAMES_ZH = { "index_mcp_pitch": "食指MCP屈伸", "index_pip": "食指PIP", "index_dip": "食指DIP（被动）", + "middle_mcp_roll": "中指MCP侧摆", + "middle_mcp_pitch": "中指MCP屈伸", + "middle_pip": "中指PIP", + "middle_dip": "中指DIP（被动）", + "ring_mcp_roll": "无名指MCP侧摆", + "ring_mcp_pitch": "无名指MCP屈伸", + "ring_pip": "无名指PIP", + "ring_dip": "无名指DIP（被动）", + "pinky_mcp_roll": "小指MCP侧摆", + "pinky_mcp_pitch": "小指MCP屈伸", + "pinky_pip": "小指PIP", + "pinky_dip": "小指DIP（被动）", "thumb_cmc_yaw": "拇指CMC侧摆", } @@ -59,6 +71,21 @@ def _task_text(active: Mapping[str, Any]) -> str: f"电机{active.get('motor_index')}，" f"第{active.get('attempt', 1)}次尝试" ) + if active.get("kind") == "zero_model_failure": + joints = active.get("joints", []) + joint_text = "/".join( + JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints + ) + return ( + f"{view}机位，{joint_text}零位/URDF验证失败，" + f"电机{active.get('motor_index')}，不会自动重扫" + ) + if active.get("kind") == "motion_stall": + return ( + f"电机{active.get('motor_index', '?')}运动停滞，目标" + f"{_format_u8(active.get('target_u8'))}、实际" + f"{_format_u8(active.get('actual_u8'))}" + ) if active.get("kind") == "validation": return ( f"{view}机位，随机复测，电机{active.get('motor_index')}，" @@ -72,10 +99,17 @@ def _task_text(active: Mapping[str, Any]) -> str: target = active.get("target_u8") cycle = active.get("cycle", "?") repetitions = active.get("repetitions", "?") - return ( + task = ( f"{view}机位，{joint_text}，电机{active.get('motor_index')}，" f"第{cycle}/{repetitions}轮，{start}→{target}" ) + fit_attempt = int(active.get("fit_attempt", 1)) + if fit_attempt > 1: + task += ( + f"（整关节自动重采第{fit_attempt}/" + f"{active.get('fit_attempt_limit', '?')}次）" + ) + return task def three_camera_reason_zh( @@ -93,6 +127,29 @@ def three_camera_reason_zh( ) tolerance = sample.get("endpoint_tolerance_u8", "?") + if reason.startswith("motor_state_stalled:"): + fields = reason.split(":") + context = fields[1] if len(fields) > 1 else "unknown" + error_match = re.search(r"error_u8=([0-9.]+)", reason) + error = error_match.group(1) if error_match else "未知" + motor = active.get("motor_index") + if motor is not None: + return ( + f"电机{motor}反馈连续8秒没有向目标推进；目标" + f"{_format_u8(active.get('target_u8'))}、实际" + f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，" + f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8" + f"（阶段={context}）。程序已保持当前位置。", + "若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后" + "重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。", + ) + return ( + f"电机反馈连续8秒没有向目标推进；停止位置距目标{error}个u8" + f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。", + "检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的" + "正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。", + ) + if "URDF zero offset reached the configured" in reason: bound_match = re.search( r"configured\s+([0-9.]+)\s+degree bound", reason @@ -249,6 +306,45 @@ def three_camera_reason_zh( "/g20_calibration/resume；程序只清除当前失败关节的数据" f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。", ) + if reason == "zero_model_validation_failed": + reason_names = { + "zero_offset_reached_configured_bound": "零位解触及安全边界", + "zero_offset_exceeds_configured_limit": "零位估计超过安全范围", + "zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界", + "zero_offset_cycle_difference_too_large": "三轮零位离散过大", + "zero_offset_not_statistically_significant": "零位偏移未达到统计显著性", + "zero_axis_cone_mismatch_too_large": ( + "父子轴夹角与原始URDF不一致，零位旋转无法解释" + ), + "zero_phase_axis_line_residual_too_large": ( + "整段SE(3)运动无法稳定确定平行轴线相位" + ), + "zero_offset_did_not_improve_with_95pct_confidence": ( + "第三轮留出验证未以95%置信度改善" + ), + } + details: list[str] = [] + for failure in active.get("failures", []): + joint = JOINT_NAMES_ZH.get( + str(failure.get("joint")), str(failure.get("joint")) + ) + if failure.get("metric") == "zero_guard": + reason_text = reason_names.get( + str(failure.get("reason")), str(failure.get("reason")) + ) + if "actual_deg" in failure and "limit_deg" in failure: + reason_text += ( + f"（估计{float(failure['actual_deg']):+.2f}°，" + f"允许±{float(failure['limit_deg']):.2f}°）" + ) + details.append(f"{joint}：{reason_text}") + return ( + "轨迹采集已完成，但零位/URDF几何验证失败" + + ("（" + "；".join(details) + "）" if details else "") + + "。程序没有生成正式JSON或修正URDF。", + "该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；" + "请检查Tag固定、相机外参和原始URDF后重新启动新标定。", + ) if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}: return ( "正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。", @@ -274,7 +370,7 @@ def three_camera_reason_zh( if reason.startswith("prepare_") or state == "PREPARE_SWEEP": return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。" if state == "RETURN_BASELINE": - return "正在把已使用的标定电机恢复到统一基准命令。", "无需操作。" + return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。" if state == "FITTING": return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。" return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。" @@ -290,18 +386,38 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str: progress = float(payload.get("progress", 0.0)) completed = payload.get("completed_sweeps", 0) total = payload.get("total_sweeps", 0) + scan_progress = float( + payload.get( + "scan_progress", + 0.0 if not total else float(completed) / float(total), + ) + ) lines = [ f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）", f"原因：{reason_zh}", f"建议：{action_zh}", - f"进度：{progress:.1%}（已完成{completed}/{total}个扫描方向）", + f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，" + f"扫描进度{scan_progress:.1%}）", f"当前任务：{_task_text(active)}", ] if state == "RETURN_BASELINE": - lines.append( - f"正在确认基准姿态：{payload.get('baseline_command_u8', [])}" - ) - if active and active.get("kind") != "fit_failure": + baseline_command = payload.get("baseline_command_u8", []) + return_command = payload.get("return_command_u8", baseline_command) + label = "恢复姿态" if return_command != baseline_command else "基准姿态" + lines.append(f"正在确认{label}：{return_command}") + if active and active.get("kind") not in { + "fit_failure", + "zero_model_failure", + "motion_stall", + }: + retry_count = int(active.get("automatic_retry_count", 0)) + if retry_count: + lines.append( + "自动重试：当前方向已自动重扫" + f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，" + f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，" + f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s" + ) sample = active.get("sample", {}) motion_progress = active.get("motion_progress") motion_text = ( @@ -333,6 +449,16 @@ def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str: f"{speed.get('commanded_finger_speed')}，SDK报告" f"{speed.get('reported_finger_speed')}" ) + if active.get("sweep_timeout_seconds") is not None: + lines.append( + "运动保护：扫描超时" + f"{float(active['sweep_timeout_seconds']):.1f}s，" + "连续" + f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s" + "进展不足" + f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}" + "则立即暂停" + ) lines.append("机位：") for name, view in payload.get("views", {}).items(): missing = view.get("missing_tag_ids", []) 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 d11400b..ec0ea97 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 @@ -1,9 +1,9 @@ -"""One-command three-camera calibration for the complete left G20 hand.""" +"""One-command three-camera calibration for either complete G20 hand.""" from __future__ import annotations from collections import deque -from dataclasses import dataclass, field +from dataclasses import dataclass, field, replace import json import math from pathlib import Path @@ -44,17 +44,16 @@ from .extrinsics import ( transform_matrix, ) from .full_hand import ( - JOINT_SPECS, - MEASURED_JOINTS, - SWEEP_SPECS, + LEFT_HAND_PROFILE, THREE_CAMERA_BASELINE_COMMAND, - VIEW_TAGS, + HandCalibrationProfile, JointCurveFit, SweepSpec, build_calibration_motion_command, build_calibration_speed_profile, build_compact_payload, calibration_auxiliary_commands, + get_hand_calibration_profile, ) from .hikrobot_camera import configure_fastdds_large_image_transport from .pnp import ( @@ -65,18 +64,16 @@ from .pnp import ( 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, + LEFT_ZERO_PROFILE, JointAxisMeasurement, + ZeroCalibrationProfile, ZeroSolveResult, fit_joint_axis_measurement, fit_rotation_joint_curve, measure_rotation_joint_observation, rotation_curve_holdout_errors, solve_urdf_zero_offsets, + get_zero_calibration_profile, write_zero_corrected_urdf, ) @@ -94,6 +91,29 @@ STATE_ABORTED = "ABORTED" STATE_COMPLETE = "COMPLETE" +def _overall_progress( + state: str, + *, + scan_progress: float, + validation_index: int = 0, + validation_total: int = 0, +) -> float: + """Keep 100% reserved for a validated, written calibration result.""" + scan = float(np.clip(scan_progress, 0.0, 1.0)) + if state == STATE_COMPLETE: + return 1.0 + if state in {STATE_FITTING}: + return 0.92 + if state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE}: + validation = ( + 0.0 + if validation_total <= 0 + else float(np.clip(validation_index / validation_total, 0.0, 1.0)) + ) + return 0.92 + 0.07 * validation + return min(0.90, 0.90 * scan) + + def _stamp_ns(stamp: Any) -> int: return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) @@ -121,19 +141,22 @@ def _robust_pose_payload( } -def _view_pairs(view: str) -> tuple[tuple[str, str], ...]: +def _view_pairs( + view: str, reference_finger: str = "index" +) -> tuple[tuple[str, str], ...]: if view == "front": return ( ("front_base", "thumb_cmc"), ("thumb_cmc", "thumb_mcp"), ("thumb_mcp", "thumb_ip"), - ("front_base", "index_roll"), + ("front_base", f"{reference_finger}_roll"), ) if view == "side": return ( - ("side_base", "index_mcp"), - ("index_mcp", "index_pip"), - ("index_pip", "index_dip"), + ("side_base", "thumb_cmc"), + ("side_base", f"{reference_finger}_mcp"), + (f"{reference_finger}_mcp", f"{reference_finger}_pip"), + (f"{reference_finger}_pip", f"{reference_finger}_dip"), ) if view == "top": return (("top_base", "thumb_yaw"),) @@ -183,7 +206,13 @@ class ViewRuntime: preflight_frames: int tracker: SquareTagPoseTracker group_tracker: SquareTagGroupPoseTracker + view_tags: Mapping[str, int] = field(default_factory=dict) + preflight_roles: tuple[str, ...] = () role_by_id: dict[int, str] = field(default_factory=dict) + group_trackers: dict[ + tuple[str, ...], SquareTagGroupPoseTracker + ] = field(default_factory=dict) + current_required_roles: tuple[str, ...] = () camera_matrix: np.ndarray | None = None camera_info_valid: bool = False camera_frame: str = "" @@ -203,14 +232,20 @@ class ViewRuntime: def __post_init__(self) -> None: self.role_by_id = { - tag_id: role for role, tag_id in VIEW_TAGS[self.name].items() + tag_id: role for role, tag_id in self.view_tags.items() } self.valid_flags = deque(maxlen=self.preflight_frames) self.detection_times = deque(maxlen=self.preflight_frames) + if not self.preflight_roles: + self.preflight_roles = tuple(self.view_tags) + if any(role not in self.view_tags for role in self.preflight_roles): + raise ValueError("preflight roles must be configured for the view") + self.current_required_roles = tuple(self.preflight_roles) + self.group_trackers[tuple(self.preflight_roles)] = self.group_tracker @property def roles(self) -> tuple[str, ...]: - return tuple(VIEW_TAGS[self.name]) + return tuple(self.view_tags) @property def valid_rate(self) -> float: @@ -237,7 +272,7 @@ class G20ThreeCameraCalibrationNode(Node): self.session_dir.mkdir(parents=True, exist_ok=True) self.raw_path = self.session_dir / "raw_samples.jsonl" self.final_path = self.session_dir / ( - f"g20_left_{_safe_name(self.serial_number)}_calibration.json" + f"g20_{self.hand_type}_{_safe_name(self.serial_number)}_calibration.json" ) self.views = { @@ -271,14 +306,22 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_started_at = 0.0 self.sweep_last_valid_at = 0.0 self.sweep_endpoint_since: float | None = None + self.motion_progress_reference_error_u8 = float("inf") + self.motion_last_progress_at = 0.0 + self.motion_stall_details: dict[str, Any] = {} self.retry_sweep_spec: SweepSpec | None = None self.retry_resume_index: int | None = None + self.retry_sweep_items: list[SweepItem] = [] + self.active_sweep_is_fit_retry = False self.fit_failure: dict[str, Any] = {} self.sweep_attempts: dict[int, int] = { - spec.motor_index: 1 for spec in SWEEP_SPECS + spec.motor_index: 1 for spec in self.profile.sweep_specs } + self.sweep_retry_counts: dict[tuple[int, int, str], int] = {} + self.motion_retry_counts: dict[str, int] = {} + self.validation_retry_counts: dict[tuple[int, int], int] = {} self.records_by_joint: dict[str, list[dict[str, Any]]] = { - name: [] for name in MEASURED_JOINTS + name: [] for name in self.profile.measured_joints } self.measured_fits: dict[str, JointCurveFit] = {} @@ -341,6 +384,7 @@ class G20ThreeCameraCalibrationNode(Node): ) def _declare_parameters(self) -> None: + self.declare_parameter("hand_type", "left") self.declare_parameter("serial_number", "UNSET") self.declare_parameter("session_dir", "calibration_output/session") self.declare_parameter("camera_extrinsics_file", "") @@ -384,11 +428,15 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("pnp_maximum_translation_jump_m", 0.04) self.declare_parameter("pnp_maximum_tag_tilt_deg", 75.0) self.declare_parameter("pnp_tracker_reset_seconds", 5.0) + self.declare_parameter("pnp_group_initialization_frames", 8) + self.declare_parameter("pnp_group_normal_alignment_scale_deg", 5.0) + self.declare_parameter("pnp_group_maximum_normal_alignment_deg", 15.0) self.declare_parameter("top_pnp_invalid_reset_seconds", 1.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_pose_line_rms_m", 0.001) self.declare_parameter("axis_maximum_rotation_circle_difference_deg", 1.0) self.declare_parameter( "active_maximum_rotation_orthogonal_rms_deg", 2.5 @@ -397,18 +445,35 @@ class G20ThreeCameraCalibrationNode(Node): "passive_maximum_rotation_orthogonal_rms_deg", 7.5 ) self.declare_parameter("zero_maximum_axis_cycle_difference_deg", 0.75) + self.declare_parameter("zero_maximum_axis_cone_mismatch_deg", 5.0) self.declare_parameter("zero_maximum_offset_deg", 20.0) + self.declare_parameter("zero_finger_maximum_offset_deg", 3.0) self.declare_parameter("endpoint_tolerance_u8", 2.0) - self.declare_parameter("thumb_yaw_zero_endpoint_tolerance_u8", 3.0) + self.declare_parameter("thumb_yaw_zero_endpoint_tolerance_u8", 4.0) + self.declare_parameter( + "right_thumb_yaw_255_endpoint_tolerance_u8", 5.0 + ) + self.declare_parameter("pinky_pip_zero_endpoint_tolerance_u8", 5.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) self.declare_parameter("sweep_timeout_seconds", 90.0) + self.declare_parameter("motor_stall_timeout_seconds", 8.0) + self.declare_parameter("motor_stall_minimum_progress_u8", 1.0) self.declare_parameter("invalid_timeout_seconds", 3.0) self.declare_parameter("minimum_sweep_frames", 40) self.declare_parameter("minimum_state_span_u8", 240.0) self.declare_parameter("minimum_sweep_bins", 32) self.declare_parameter("maximum_bin_gap", 16) + self.declare_parameter("automatic_sweep_retry_limit", 3) + self.declare_parameter("automatic_fit_retry_limit", 2) + self.declare_parameter("automatic_motion_retry_limit", 2) + self.declare_parameter("provisional_warning_ratio", 1.25) + self.declare_parameter("retry_minimum_speed", 3) + self.declare_parameter("retry_speed_scales", [0.8, 0.6, 0.5]) + self.declare_parameter( + "retry_endpoint_hold_seconds", [0.75, 1.0, 1.25] + ) self.declare_parameter("trajectory_maximum_plane_rms_m", 0.004) self.declare_parameter("trajectory_maximum_radial_rms_m", 0.004) self.declare_parameter("trajectory_minimum_radius_m", 0.003) @@ -438,6 +503,13 @@ class G20ThreeCameraCalibrationNode(Node): def value(name: str) -> Any: return self.get_parameter(name).value + self.hand_type = str(value("hand_type")).lower() + self.profile: HandCalibrationProfile = get_hand_calibration_profile( + self.hand_type + ) + self.zero_profile: ZeroCalibrationProfile = ( + get_zero_calibration_profile(self.hand_type) + ) self.serial_number = str(value("serial_number")) if self.serial_number == "UNSET": raise ValueError("serial_number is required") @@ -470,6 +542,13 @@ class G20ThreeCameraCalibrationNode(Node): self.command_topic = str(value("command_topic")) self.state_topic = str(value("state_topic")) self.info_topic = str(value("info_topic")) + if self.hand_type == "right": + if self.command_topic == "/g20/cb_left_hand_control_cmd": + self.command_topic = "/g20/cb_right_hand_control_cmd" + if self.state_topic == "/g20/cb_left_hand_state": + self.state_topic = "/g20/cb_right_hand_state" + if self.info_topic == "/g20/cb_left_hand_info": + self.info_topic = "/g20/cb_right_hand_info" self.setting_topic = str(value("setting_topic")) self.camera_info_topics = { view: str(value(f"{view}_camera_info_topic")) @@ -483,6 +562,7 @@ class G20ThreeCameraCalibrationNode(Node): self.baseline_command = tuple( int(item) for item in value("baseline_command_u8") ) + self.return_command_u8 = tuple(self.baseline_command) self.normal_calibration_speed = int(value("normal_calibration_speed")) self.index_roll_calibration_speed = int( value("index_roll_calibration_speed") @@ -514,6 +594,15 @@ class G20ThreeCameraCalibrationNode(Node): float(value("pnp_maximum_tag_tilt_deg")) ) self.pnp_tracker_reset_seconds = float(value("pnp_tracker_reset_seconds")) + self.pnp_group_initialization_frames = int( + value("pnp_group_initialization_frames") + ) + self.pnp_group_normal_alignment_scale_rad = math.radians( + float(value("pnp_group_normal_alignment_scale_deg")) + ) + self.pnp_group_maximum_normal_alignment_rad = math.radians( + float(value("pnp_group_maximum_normal_alignment_deg")) + ) self.top_pnp_invalid_reset_seconds = float( value("top_pnp_invalid_reset_seconds") ) @@ -529,6 +618,9 @@ class G20ThreeCameraCalibrationNode(Node): self.axis_maximum_radial_rms_m = float( value("axis_maximum_radial_rms_m") ) + self.axis_maximum_pose_line_rms_m = float( + value("axis_maximum_pose_line_rms_m") + ) self.axis_maximum_rotation_circle_difference_rad = math.radians( float(value("axis_maximum_rotation_circle_difference_deg")) ) @@ -541,22 +633,60 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cycle_difference_rad = math.radians( float(value("zero_maximum_axis_cycle_difference_deg")) ) + self.zero_maximum_axis_cone_mismatch_rad = math.radians( + float(value("zero_maximum_axis_cone_mismatch_deg")) + ) self.zero_maximum_offset_rad = math.radians( float(value("zero_maximum_offset_deg")) ) + self.zero_finger_maximum_offset_rad = math.radians( + float(value("zero_finger_maximum_offset_deg")) + ) + self.zero_joint_maximum_offsets_rad: dict[str, float] = {} 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.right_thumb_yaw_255_endpoint_tolerance_u8 = float( + value("right_thumb_yaw_255_endpoint_tolerance_u8") + ) + self.pinky_pip_zero_endpoint_tolerance_u8 = float( + value("pinky_pip_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")) self.sweep_timeout_seconds = float(value("sweep_timeout_seconds")) + self.motor_stall_timeout_seconds = float( + value("motor_stall_timeout_seconds") + ) + self.motor_stall_minimum_progress_u8 = float( + value("motor_stall_minimum_progress_u8") + ) self.invalid_timeout_seconds = float(value("invalid_timeout_seconds")) self.minimum_sweep_frames = int(value("minimum_sweep_frames")) self.minimum_state_span_u8 = float(value("minimum_state_span_u8")) self.minimum_sweep_bins = int(value("minimum_sweep_bins")) self.maximum_bin_gap = int(value("maximum_bin_gap")) + self.automatic_sweep_retry_limit = int( + value("automatic_sweep_retry_limit") + ) + self.automatic_fit_retry_limit = int( + value("automatic_fit_retry_limit") + ) + self.automatic_motion_retry_limit = int( + value("automatic_motion_retry_limit") + ) + self.provisional_warning_ratio = float( + value("provisional_warning_ratio") + ) + self.retry_minimum_speed = int(value("retry_minimum_speed")) + self.retry_speed_scales = tuple( + float(item) for item in value("retry_speed_scales") + ) + self.retry_endpoint_hold_seconds = tuple( + float(item) for item in value("retry_endpoint_hold_seconds") + ) self.trajectory_maximum_plane_rms_m = float( value("trajectory_maximum_plane_rms_m") ) @@ -623,6 +753,12 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError( f"source_urdf_path does not exist: {self.source_urdf_path}" ) + source_name = self.source_urdf_path.name.lower() + opposite = "right" if self.hand_type == "left" else "left" + if f"g20_{opposite}" in source_name: + raise ValueError( + "source_urdf_path hand side does not match hand_type" + ) if ( "zero_calibrated" in self.source_urdf_path.stem.lower() or re.search( @@ -652,16 +788,51 @@ class G20ThreeCameraCalibrationNode(Node): "thumb_yaw_zero_endpoint_tolerance_u8 must be between the " "default endpoint tolerance and 10" ) + if not ( + self.endpoint_tolerance_u8 + <= self.right_thumb_yaw_255_endpoint_tolerance_u8 + <= 10.0 + ): + raise ValueError( + "right_thumb_yaw_255_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 3 <= self.pnp_group_initialization_frames <= 30: + raise ValueError( + "pnp_group_initialization_frames must be in [3, 30]" + ) + if not ( + self.endpoint_tolerance_u8 + <= self.pinky_pip_zero_endpoint_tolerance_u8 + <= 8.0 + ): + raise ValueError( + "pinky_pip_zero_endpoint_tolerance_u8 must be between " + "endpoint_tolerance_u8 and 8" + ) + if self.motor_stall_timeout_seconds <= 0.0: + raise ValueError("motor_stall_timeout_seconds must be positive") + if not 0.0 < self.motor_stall_minimum_progress_u8 <= 10.0: + raise ValueError( + "motor_stall_minimum_progress_u8 must be in (0, 10]" + ) if not all( value > 0.0 for value in ( self.image_trajectory_maximum_radial_rms_px, self.image_trajectory_maximum_radial_p95_px, self.image_trajectory_minimum_radius_px, + self.pnp_group_normal_alignment_scale_rad, + self.pnp_group_maximum_normal_alignment_rad, self.axis_maximum_plane_rms_m, self.passive_axis_maximum_plane_rms_m, + self.axis_maximum_pose_line_rms_m, + self.zero_maximum_axis_cycle_difference_rad, + self.zero_maximum_axis_cone_mismatch_rad, + self.zero_maximum_offset_rad, + self.zero_finger_maximum_offset_rad, self.active_maximum_rotation_orthogonal_rms_rad, self.passive_maximum_rotation_orthogonal_rms_rad, self.trajectory_maximum_cycle_travel_difference_rad, @@ -671,15 +842,91 @@ class G20ThreeCameraCalibrationNode(Node): ) ): raise ValueError("trajectory quality thresholds must be positive") + if self.zero_finger_maximum_offset_rad > self.zero_maximum_offset_rad: + raise ValueError( + "zero_finger_maximum_offset_deg cannot exceed " + "zero_maximum_offset_deg" + ) if self.repetitions != 3: raise ValueError("three-camera calibration requires exactly 3 repetitions") if self.preflight_frames < 10 or self.minimum_sweep_bins < 3: raise ValueError("preflight_frames or minimum_sweep_bins is too small") + if not 0 <= self.automatic_sweep_retry_limit <= 5: + raise ValueError("automatic_sweep_retry_limit must be in [0, 5]") + if not 0 <= self.automatic_fit_retry_limit <= 2: + raise ValueError("automatic_fit_retry_limit must be in [0, 2]") + if not 0 <= self.automatic_motion_retry_limit <= 5: + raise ValueError("automatic_motion_retry_limit must be in [0, 5]") + if not 1.0 <= self.provisional_warning_ratio <= 2.0: + raise ValueError("provisional_warning_ratio must be in [1, 2]") + if not 1 <= self.retry_minimum_speed <= 255: + raise ValueError("retry_minimum_speed must be in [1, 255]") + if ( + len(self.retry_speed_scales) != self.automatic_sweep_retry_limit + or any(not 0.0 < item <= 1.0 for item in self.retry_speed_scales) + ): + raise ValueError("retry_speed_scales must match the sweep retry limit") + if ( + len(self.retry_endpoint_hold_seconds) + != self.automatic_sweep_retry_limit + or any(item < self.endpoint_hold_seconds for item in self.retry_endpoint_hold_seconds) + ): + raise ValueError( + "retry endpoint holds must match retries and not shorten the hold" + ) if not 1 <= self.validation_command_count <= 10: raise ValueError("validation_command_count must be in [1, 10]") if self.validation_frames < 3: raise ValueError("validation_frames must be at least 3") + def _make_group_pose_tracker( + self, name: str, roles: Sequence[str] + ) -> SquareTagGroupPoseTracker: + role_tuple = tuple(str(role) for role in roles) + role_set = set(role_tuple) + adjacent_pairs = tuple( + pair + for pair in _view_pairs(name, self.profile.reference_finger) + if pair[0] in role_set and pair[1] in role_set + ) + # The side-view palm/reference-finger Tags are mounted on parallel + # outward-facing surfaces at the endpoint where a task tracker is + # initialized. Temporal rigidity alone cannot distinguish two fully + # static IPPE mirror branches; their face-normal consistency can. Do + # not apply this prior to the articulated front thumb chain. + normal_alignment_pairs = ( + adjacent_pairs if name == "side" else () + ) + return SquareTagGroupPoseTracker( + roles=role_tuple, + adjacent_pairs=adjacent_pairs, + maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, + maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, + relative_rotation_scale_rad=math.radians(5.0), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=self.pnp_tracker_reset_seconds, + # Five-tag front preflight would make the exhaustive multi-frame + # search unnecessarily expensive. Four-tag side groups remain + # small enough and need the robust initializer so the fixed base + # Tag can anchor the three reference-finger IPPE branches. + initialization_frames=( + 1 + if len(role_tuple) > 4 + else self.pnp_group_initialization_frames + ), + normal_alignment_pairs=normal_alignment_pairs, + normal_alignment_scale_rad=( + self.pnp_group_normal_alignment_scale_rad + ), + maximum_normal_alignment_rad=( + self.pnp_group_maximum_normal_alignment_rad + if name == "side" + else None + ), + ) + def _make_view_runtime(self, name: str) -> ViewRuntime: tracker = SquareTagPoseTracker( maximum_reprojection_error_px=self.pnp_maximum_reprojection_error_px, @@ -689,16 +936,10 @@ class G20ThreeCameraCalibrationNode(Node): maximum_tag_tilt_rad=self.pnp_maximum_tag_tilt_rad, reset_after_seconds=self.pnp_tracker_reset_seconds, ) - group = SquareTagGroupPoseTracker( - roles=tuple(VIEW_TAGS[name]), - adjacent_pairs=_view_pairs(name), - maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, - maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, - relative_rotation_scale_rad=math.radians(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=self.pnp_tracker_reset_seconds, + preflight_roles = tuple(self.profile.preflight_view_roles[name]) + group = self._make_group_pose_tracker( + name, + preflight_roles, ) return ViewRuntime( name=name, @@ -706,8 +947,55 @@ class G20ThreeCameraCalibrationNode(Node): preflight_frames=self.preflight_frames, tracker=tracker, group_tracker=group, + view_tags=self.profile.view_tags[name], + preflight_roles=preflight_roles, ) + def _required_roles_for_view(self, view: str) -> tuple[str, ...]: + """Return only the roles required by the active task in this view.""" + runtime = self.views[view] + active_spec: SweepSpec | None = None + if self.active_sweep is not None and self.active_sweep.spec.view == view: + active_spec = self.active_sweep.spec + elif ( + self.active_validation is not None + and self.active_validation.spec.view == view + ): + active_spec = self.active_validation.spec + elif self.retry_sweep_spec is not None and self.retry_sweep_spec.view == view: + active_spec = self.retry_sweep_spec + if active_spec is None: + return tuple(runtime.preflight_roles) + required: set[str] = set() + for joint_name in active_spec.joints: + joint = self.profile.joint_specs[joint_name] + if joint.parent_role is not None: + required.add(joint.parent_role) + if joint.child_role is not None: + required.add(joint.child_role) + if view == "side": + # Keep the rigid palm Tag in every side-view task. Otherwise the + # PIP/DIP task initializes Tags 5/6/7 as an unanchored group and a + # stable all-mirror IPPE solution can survive temporal checks. + required.add("side_base") + return tuple(role for role in runtime.roles if role in required) + + def _group_tracker_for_roles( + self, runtime: ViewRuntime, roles: Sequence[str] + ) -> SquareTagGroupPoseTracker: + key = tuple(str(role) for role in roles) + group = runtime.group_trackers.get(key) + if group is None: + group = self._make_group_pose_tracker(runtime.name, key) + runtime.group_trackers[key] = group + return group + + @staticmethod + def _reset_view_trackers(runtime: ViewRuntime) -> None: + runtime.tracker.reset() + for group in runtime.group_trackers.values(): + group.reset() + def _camera_info_callback(self, view: str, message: CameraInfo) -> None: runtime = self.views[view] valid = ( @@ -730,8 +1018,7 @@ class G20ThreeCameraCalibrationNode(Node): and runtime.camera_matrix is not None and not np.allclose(matrix, runtime.camera_matrix) ): - runtime.tracker.reset() - runtime.group_tracker.reset() + self._reset_view_trackers(runtime) runtime.camera_info_valid = valid runtime.camera_matrix = matrix if valid else None runtime.camera_frame = str(message.header.frame_id) @@ -800,6 +1087,14 @@ class G20ThreeCameraCalibrationNode(Node): runtime = self.views[view] now = time.monotonic() stamp = _stamp_ns(message.header.stamp) + required_roles = self._required_roles_for_view(view) + if required_roles != runtime.current_required_roles: + runtime.current_required_roles = required_roles + runtime.valid_flags.clear() + runtime.detection_times.clear() + runtime.latest_pnp_rejections.clear() + runtime.latest_group_pnp_reason = "" + self._reset_view_trackers(runtime) qualities: dict[str, TagQuality] = {} corners_by_role: dict[str, np.ndarray] = {} centres_by_role: dict[str, np.ndarray] = {} @@ -822,16 +1117,16 @@ class G20ThreeCameraCalibrationNode(Node): corners_by_role[role] = corners centres_by_role[role] = np.mean(corners, axis=0) - detection_good = set(qualities) == set(runtime.roles) and all( - self._quality_valid(quality, include_pnp=False) - for quality in qualities.values() + detection_good = all(role in qualities for role in required_roles) and all( + self._quality_valid(qualities[role], include_pnp=False) + for role in required_roles ) selected: dict[str, SquareTagPose] | None = None pnp_rejections: dict[str, str] = {} group_pnp_reason = "" if detection_good and runtime.camera_matrix is not None: candidates: dict[str, tuple[SquareTagPose, ...]] = {} - for role in runtime.roles: + for role in required_roles: _, rejection = runtime.tracker.estimate( role, corners_by_role[role], @@ -844,7 +1139,10 @@ class G20ThreeCameraCalibrationNode(Node): candidates[role] = runtime.tracker.last_candidates_by_role.get( role, () ) - selected, group_pnp_reason = runtime.group_tracker.select( + group_tracker = self._group_tracker_for_roles( + runtime, required_roles + ) + selected, group_pnp_reason = group_tracker.select( candidates, stamp_ns=stamp ) if selected is not None: @@ -857,7 +1155,10 @@ class G20ThreeCameraCalibrationNode(Node): ) valid = bool( selected is not None - and all(self._quality_valid(q, include_pnp=True) for q in qualities.values()) + and all( + self._quality_valid(qualities[role], include_pnp=True) + for role in required_roles + ) ) runtime.latest_pnp_rejections = pnp_rejections runtime.latest_group_pnp_reason = group_pnp_reason @@ -872,8 +1173,7 @@ class G20ThreeCameraCalibrationNode(Node): reset_after_seconds=self.top_pnp_invalid_reset_seconds, ) if reset_due: - runtime.tracker.reset() - runtime.group_tracker.reset() + self._reset_view_trackers(runtime) runtime.pnp_reset_count += 1 self.get_logger().warning( "Top-view PnP was continuously invalid for " @@ -890,6 +1190,24 @@ class G20ThreeCameraCalibrationNode(Node): return runtime.last_valid_at = now + # Preflight and motion-only states need pose quality/status, but do not + # consume FrameObservation objects. Building transforms and payloads + # for every measured joint on all three 30 Hz streams was pure work + # and could starve the detection subscriptions. Only materialize an + # observation for the one view used by an active capture stage. + sweep_capture = bool( + self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} + and self.active_sweep is not None + and self.active_sweep.spec.view == view + ) + validation_capture = bool( + self.state == STATE_VALIDATION_CAPTURE + and self.active_validation is not None + and self.active_validation.spec.view == view + ) + if not sweep_capture and not validation_capture: + return + matched = interpolate_state_u8( list(self.state_history), stamp, @@ -907,10 +1225,15 @@ class G20ThreeCameraCalibrationNode(Node): 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(): + for name, spec in self.profile.joint_specs.items(): if not spec.measured or spec.view != view: continue assert spec.parent_role is not None and spec.child_role is not None + if ( + spec.parent_role not in selected + or spec.child_role not in selected + ): + continue parent = selected[spec.parent_role] child = selected[spec.child_role] parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) @@ -1043,15 +1366,51 @@ class G20ThreeCameraCalibrationNode(Node): ) def _prepare_failed_sweep_retry(self) -> SweepSpec: - """Discard only a failed spec's in-memory samples before its retry.""" + """Discard only failed cycles and queue their two sweep directions.""" if self.retry_sweep_spec is None: raise RuntimeError("no failed sweep is pending retry") spec = self.retry_sweep_spec + repetitions = int(getattr(self, "repetitions", 3)) + cycles = set(getattr(self, "retry_cycles", set(range(repetitions)))) for joint_name in spec.joints: - self.records_by_joint[joint_name].clear() + self.records_by_joint[joint_name][:] = [ + record + for record in self.records_by_joint[joint_name] + if int(record.get("cycle", -1)) not in cycles + ] + all_items = getattr(self, "sweep_items", []) + self.retry_sweep_items = [ + item + for item in all_items + if item.spec == spec and item.cycle in cycles + ] + if not self.retry_sweep_items: + self.retry_sweep_items = [ + SweepItem(spec, cycle, direction) + for cycle in sorted(cycles) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ] + # A planar Tag PnP tracker can occasionally remain on a continuous but + # biased pose branch for an otherwise clean sweep. Starting a full + # fit retry with that tracker state intact tends to reproduce the same + # orientation residual, so reset only the affected camera before the + # retry. Raw samples remain logged and all final quality gates stay + # unchanged. + runtime = getattr(self, "views", {}).get(spec.view) + reset_trackers = getattr(self, "_reset_view_trackers", None) + if runtime is not None and reset_trackers is not None: + reset_trackers(runtime) + runtime.pnp_invalid_since = None + runtime.pnp_reset_count += 1 self.sweep_attempts[spec.motor_index] = ( self.sweep_attempts.get(spec.motor_index, 1) + 1 ) + for key in list(getattr(self, "sweep_retry_counts", {})): + if key[0] == spec.motor_index: + self.sweep_retry_counts.pop(key, None) append_jsonl( self.raw_path, { @@ -1061,9 +1420,11 @@ class G20ThreeCameraCalibrationNode(Node): "joints": list(spec.joints), "attempt": self.sweep_attempts[spec.motor_index], "reason": self.paused_reason, + "cycles": [cycle + 1 for cycle in sorted(cycles)], }, ) self.fit_failure = {} + getattr(self, "motion_stall_details", {}).clear() return spec def _start_callback( @@ -1093,18 +1454,26 @@ class G20ThreeCameraCalibrationNode(Node): self.started = True self.sweep_items = [ SweepItem(spec, cycle, direction) - for spec in SWEEP_SPECS + for spec in self.profile.sweep_specs for cycle in range(self.repetitions) for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) ] self.sweep_index = 0 self.retry_sweep_spec = None self.retry_resume_index = None + self.retry_sweep_items.clear() + self.active_sweep_is_fit_retry = False self.fit_failure = {} + self.motion_stall_details.clear() self.sweep_attempts = { - spec.motor_index: 1 for spec in SWEEP_SPECS + spec.motor_index: 1 for spec in self.profile.sweep_specs + } + self.sweep_retry_counts.clear() + self.motion_retry_counts.clear() + self.validation_retry_counts.clear() + self.records_by_joint = { + name: [] for name in self.profile.measured_joints } - self.records_by_joint = {name: [] for name in MEASURED_JOINTS} self.axis_measurements.clear() self.zero_result = None self.corrected_urdf_path = None @@ -1116,6 +1485,12 @@ class G20ThreeCameraCalibrationNode(Node): self.raw_path, { "kind": "session_start", + "hand_type": self.hand_type, + "reference_finger": self.profile.reference_finger, + "view_tags": { + view: dict(tags) + for view, tags in self.profile.view_tags.items() + }, "tag_family": "36h11", "tag_size_m": float(self.tag_size_m), "baseline_command_u8": [ @@ -1151,6 +1526,15 @@ class G20ThreeCameraCalibrationNode(Node): response.success = False response.message = "calibration is not paused" return response + if self.paused_reason == "zero_model_validation_failed": + response.success = False + response.message = ( + "zero/URDF model validation failed; samples are retained but " + "resume cannot repair this result by rescanning. Inspect the " + "reported zero guards and start a new session after correcting " + "the camera/tag/URDF geometry" + ) + return response now = time.monotonic() active_view = self._active_view_for_resume() if not self._resume_preflight_ready(now): @@ -1162,8 +1546,7 @@ class G20ThreeCameraCalibrationNode(Node): return response for view in required_resume_views(active_view): runtime = self.views[view] - runtime.tracker.reset() - runtime.group_tracker.reset() + self._reset_view_trackers(runtime) runtime.pnp_invalid_since = None self.sweep_frames.clear() self.sweep_start_frames.clear() @@ -1192,7 +1575,9 @@ class G20ThreeCameraCalibrationNode(Node): ) -> Trigger.Response: del request self._publish_speed_profile(self._normal_speed_profile()) - self._publish_hold_current() + hold_current = getattr(self, "_publish_hold_current", None) + if hold_current is not None: + hold_current() self.state = STATE_ABORTED self.reason = "operator_abort" response.success = True @@ -1219,7 +1604,7 @@ class G20ThreeCameraCalibrationNode(Node): { "setting_cmd": "set_speed", "params": { - "hand_type": "left", + "hand_type": self.hand_type, "speed": list(profile), }, } @@ -1232,11 +1617,45 @@ class G20ThreeCameraCalibrationNode(Node): return [self.normal_calibration_speed] * 5 def _speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: - return build_calibration_speed_profile( + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + speeds = build_calibration_speed_profile( spec, normal_speed=self.normal_calibration_speed, index_roll_speed=self.index_roll_calibration_speed, index_flex_speed=self.index_flex_calibration_speed, + profile=profile, + ) + retry = 0 + if self.active_sweep is not None: + key = ( + self.active_sweep.spec.motor_index, + self.active_sweep.cycle, + self.active_sweep.direction, + ) + retry = self.sweep_retry_counts.get(key, 0) + if retry: + scale = self.retry_speed_scales[min(retry, len(self.retry_speed_scales)) - 1] + speeds = [ + max(self.retry_minimum_speed, int(round(speed * scale))) + for speed in speeds + ] + return speeds + + def _active_endpoint_hold_seconds(self) -> float: + if self.active_sweep is None: + return float(self.endpoint_hold_seconds) + key = ( + self.active_sweep.spec.motor_index, + self.active_sweep.cycle, + self.active_sweep.direction, + ) + retry = self.sweep_retry_counts.get(key, 0) + if retry <= 0: + return float(self.endpoint_hold_seconds) + return float( + self.retry_endpoint_hold_seconds[ + min(retry, len(self.retry_endpoint_hold_seconds)) - 1 + ] ) def _publish_hold_current(self) -> None: @@ -1250,20 +1669,123 @@ class G20ThreeCameraCalibrationNode(Node): def _begin_return_baseline(self, after: str) -> None: self.baseline_after = str(after) + self.return_command_u8 = ( + G20ThreeCameraCalibrationNode._return_command_for_transition( + self, after + ) + ) self.position_hold_since = None self.motion_stage_started_at = time.monotonic() + getattr(self, "motion_stall_details", {}).clear() + self._reset_motion_progress( + self.motion_stage_started_at, self._baseline_error_u8() + ) self.state = STATE_RETURN_BASELINE self.reason = f"return_baseline_before_{after}" self._publish_speed_profile(self._normal_speed_profile()) - self._publish_command(list(self.baseline_command)) + self._publish_command(list(self.return_command_u8)) + + def _return_command_for_transition(self, after: str) -> tuple[int, ...]: + """Keep motor 5 at clearance while a motor-10 sweep is recovered.""" + if str(after) not in {"retry_sweep", "resume_sweep"}: + return tuple(self.baseline_command) + retry_items = getattr(self, "retry_sweep_items", []) + item = retry_items[0] if retry_items else getattr(self, "active_sweep", None) + if item is None or item.spec.motor_index != 10: + return tuple(self.baseline_command) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return tuple( + build_calibration_motion_command( + item.spec, + item.start_u8, + baseline=self.baseline_command, + profile=profile, + ) + ) + + def _current_return_command(self) -> tuple[int, ...]: + command = getattr(self, "return_command_u8", self.baseline_command) + return tuple(int(value) for value in command) def _baseline_reached(self) -> bool: if len(self.latest_state_u8) != 20: return False - indices = sorted({spec.motor_index for spec in JOINT_SPECS.values()}) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return_command = G20ThreeCameraCalibrationNode._current_return_command( + self + ) + indices = sorted( + {spec.motor_index for spec in profile.joint_specs.values()} + ) return all( - abs(float(self.latest_state_u8[index]) - self.baseline_command[index]) - <= self.endpoint_tolerance_u8 + abs(float(self.latest_state_u8[index]) - return_command[index]) + <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(return_command[index]) + ) + for index in indices + ) + + def _baseline_error_details(self) -> dict[str, float | int | str]: + if len(self.latest_state_u8) != 20: + return { + "stage": "return_baseline", + "motor_index": -1, + "target_u8": float("nan"), + "actual_u8": float("nan"), + "error_u8": float("inf"), + "tolerance_u8": float(self.endpoint_tolerance_u8), + } + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return_command = G20ThreeCameraCalibrationNode._current_return_command( + self + ) + indices = sorted({spec.motor_index for spec in profile.joint_specs.values()}) + values: list[dict[str, float | int | str]] = [] + for index in indices: + target = float(return_command[index]) + actual = float(self.latest_state_u8[index]) + tolerance = ( + G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(target) + ) + ) + values.append( + { + "stage": "return_baseline", + "motor_index": int(index), + "target_u8": target, + "actual_u8": actual, + "error_u8": abs(actual - target), + "tolerance_u8": tolerance, + } + ) + outside = [ + item + for item in values + if float(item["error_u8"]) > float(item["tolerance_u8"]) + ] + candidates = outside or values + return max( + candidates, + key=lambda item: ( + float(item["error_u8"]) - float(item["tolerance_u8"]), + float(item["error_u8"]), + ), + ) + + def _baseline_error_u8(self) -> float: + if len(self.latest_state_u8) != 20: + return float("inf") + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return_command = G20ThreeCameraCalibrationNode._current_return_command( + self + ) + indices = {spec.motor_index for spec in profile.joint_specs.values()} + return max( + abs( + float(self.latest_state_u8[index]) + - float(return_command[index]) + ) for index in indices ) @@ -1277,23 +1799,189 @@ class G20ThreeCameraCalibrationNode(Node): state = self.latest_state_u8 if state_u8 is None else state_u8 if len(state) != 20: return False + profile = getattr(self, "profile", LEFT_HAND_PROFILE) expected = build_calibration_motion_command( spec, command_u8, baseline=self.baseline_command, + profile=profile, ) indices = {spec.motor_index} - indices.update(calibration_auxiliary_commands(spec)) + indices.update(calibration_auxiliary_commands(spec, profile=profile)) return all( abs(float(state[index]) - expected[index]) <= ( self._endpoint_tolerance_for_spec(spec, command_u8) if index == spec.motor_index - else self.endpoint_tolerance_u8 + else G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, expected[index] + ) ) for index in indices ) + def _motion_command_error_u8( + self, + spec: SweepSpec, + command_u8: int, + state_u8: tuple[float, ...] | None = None, + ) -> float: + """Return the worst swept/auxiliary motor error for stall detection.""" + state = self.latest_state_u8 if state_u8 is None else state_u8 + if len(state) != 20: + return float("inf") + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + expected = build_calibration_motion_command( + spec, + command_u8, + baseline=self.baseline_command, + profile=profile, + ) + indices = {spec.motor_index} + indices.update(calibration_auxiliary_commands(spec, profile=profile)) + return max( + abs(float(state[index]) - float(expected[index])) + for index in indices + ) + + def _motion_command_error_details( + self, + spec: SweepSpec, + command_u8: int, + context: str, + ) -> dict[str, float | int | str]: + """Identify the swept or clearance motor blocking a motion.""" + if len(self.latest_state_u8) != 20: + return { + "stage": str(context), + "motor_index": -1, + "target_u8": float(command_u8), + "actual_u8": float("nan"), + "error_u8": float("inf"), + "tolerance_u8": float(self.endpoint_tolerance_u8), + } + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + expected = build_calibration_motion_command( + spec, + command_u8, + baseline=self.baseline_command, + profile=profile, + ) + indices = {spec.motor_index} + indices.update(calibration_auxiliary_commands(spec, profile=profile)) + values: list[dict[str, float | int | str]] = [] + for index in indices: + target = float(expected[index]) + actual = float(self.latest_state_u8[index]) + tolerance = ( + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + self, spec, command_u8 + ) + if index == spec.motor_index + else G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(target) + ) + ) + values.append( + { + "stage": str(context), + "motor_index": int(index), + "target_u8": target, + "actual_u8": actual, + "error_u8": abs(actual - target), + "tolerance_u8": float(tolerance), + } + ) + outside = [ + item + for item in values + if float(item["error_u8"]) > float(item["tolerance_u8"]) + ] + return max( + outside or values, + key=lambda item: ( + float(item["error_u8"]) - float(item["tolerance_u8"]), + float(item["error_u8"]), + ), + ) + + def _reset_motion_progress(self, now: float, error_u8: float) -> None: + self.motion_progress_reference_error_u8 = float(error_u8) + self.motion_last_progress_at = float(now) + + def _pause_if_motion_stalled( + self, + *, + now: float, + error_u8: float, + context: str, + details: Mapping[str, Any] | None = None, + ) -> bool: + """Pause immediately when feedback stops moving toward its target.""" + error = float(error_u8) + if not math.isfinite(error): + return False + reference = float( + getattr(self, "motion_progress_reference_error_u8", float("inf")) + ) + last_progress = float(getattr(self, "motion_last_progress_at", now)) + minimum_progress = float( + getattr(self, "motor_stall_minimum_progress_u8", 1.0) + ) + if not math.isfinite(reference): + self._reset_motion_progress(now, error) + return False + if reference - error >= minimum_progress: + self._reset_motion_progress(now, error) + return False + timeout = float(getattr(self, "motor_stall_timeout_seconds", 8.0)) + if now - last_progress < timeout: + return False + detail_payload = {} if details is None else dict(details) + reason_parts = ["motor_state_stalled", str(context)] + for key in ("motor_index", "target_u8", "actual_u8", "tolerance_u8"): + if key in detail_payload: + reason_parts.append(f"{key}={detail_payload[key]}") + reason_parts.append(f"error_u8={error:.3f}") + reason = ":".join(reason_parts) + self.motion_stall_details = { + "kind": "motion_stall", + "stage": str(context), + "error_u8": error, + **detail_payload, + } + append_jsonl( + self.raw_path, + { + "kind": "mechanical_motion_stall", + "context": context, + "error_u8": error, + "timeout_seconds": timeout, + "minimum_progress_u8": minimum_progress, + "state_u8": [float(value) for value in self.latest_state_u8], + "details": detail_payload, + }, + ) + self._pause(reason) + return True + + def _active_sweep_timeout_seconds(self) -> float: + """Scale retry timeout so a deliberately slower retry can finish.""" + if self.active_sweep is None: + return float(self.sweep_timeout_seconds) + key = ( + self.active_sweep.spec.motor_index, + self.active_sweep.cycle, + self.active_sweep.direction, + ) + retry = self.sweep_retry_counts.get(key, 0) + if retry <= 0: + return float(self.sweep_timeout_seconds) + scale = self.retry_speed_scales[ + min(retry, len(self.retry_speed_scales)) - 1 + ] + return float(self.sweep_timeout_seconds) / float(scale) + def _endpoint_tolerance_for_spec( self, spec: SweepSpec, endpoint_u8: int ) -> float: @@ -1306,19 +1994,88 @@ class G20ThreeCameraCalibrationNode(Node): self.endpoint_tolerance_u8, ) ) + if spec.motor_index == 19 and int(endpoint_u8) == 0: + return float( + getattr( + self, + "pinky_pip_zero_endpoint_tolerance_u8", + self.endpoint_tolerance_u8, + ) + ) + return G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, spec.motor_index, int(endpoint_u8) + ) + + def _motor_endpoint_tolerance( + self, motor_index: int, endpoint_u8: int + ) -> float: + """Return a side/motor-specific feedback deadband when established.""" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if int(motor_index) == 10 and int(endpoint_u8) == 0: + return float( + getattr( + self, + "thumb_yaw_zero_endpoint_tolerance_u8", + 4.0, + ) + ) + if ( + profile.side == "right" + and int(motor_index) == 10 + and int(endpoint_u8) == 255 + ): + return float( + getattr( + self, + "right_thumb_yaw_255_endpoint_tolerance_u8", + 5.0, + ) + ) return float(self.endpoint_tolerance_u8) def _start_next_sweep(self) -> None: - if self.sweep_index >= len(self.sweep_items): + retry_items = getattr(self, "retry_sweep_items", []) + if retry_items: + self.active_sweep = retry_items.pop(0) + self.active_sweep_is_fit_retry = True + elif self.sweep_index >= len(self.sweep_items): self.active_sweep = None self._begin_return_baseline("fit") return - self.active_sweep = self.sweep_items[self.sweep_index] + else: + self.active_sweep = self.sweep_items[self.sweep_index] + self.active_sweep_is_fit_retry = False item = self.active_sweep - self.sweep_frames.clear() - self.sweep_start_frames.clear() + if item.direction == DIRECTION_DECREASING: + runtime = getattr(self, "views", {}).get(item.spec.view) + reset_trackers = getattr(self, "_reset_view_trackers", None) + if runtime is not None and reset_trackers is not None: + # Make every cycle an independent PnP-branch observation. + # The hand is still at the start endpoint, so the group + # tracker receives its full static initialization window + # before _accept_frame allows the sweep to start. + reset_trackers(runtime) + runtime.pnp_invalid_since = None + runtime.pnp_reset_count += 1 + raw_path = getattr(self, "raw_path", None) + if raw_path is not None: + append_jsonl( + raw_path, + { + "kind": "pnp_cycle_initialization", + "view": item.spec.view, + "motor_index": item.spec.motor_index, + "cycle": item.cycle, + }, + ) + getattr(self, "sweep_frames", []).clear() + getattr(self, "sweep_start_frames", []).clear() self.position_hold_since = None self.motion_stage_started_at = time.monotonic() + self._reset_motion_progress( + self.motion_stage_started_at, + self._motion_command_error_u8(item.spec, item.start_u8), + ) self.state = STATE_PREPARE_SWEEP self.reason = ( f"prepare_{item.spec.view}_motor_{item.spec.motor_index}_" @@ -1330,6 +2087,7 @@ class G20ThreeCameraCalibrationNode(Node): item.spec, item.start_u8, baseline=self.baseline_command, + profile=self.profile, ) ) @@ -1341,13 +2099,21 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_started_at = now self.sweep_last_valid_at = now self.sweep_endpoint_since = None + self._reset_motion_progress( + now, + self._motion_command_error_u8( + self.active_sweep.spec, self.active_sweep.target_u8 + ), + ) self.state = STATE_SWEEP self.reason = "collecting_timestamp_synchronised_tag_centres" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) self._publish_command( build_calibration_motion_command( self.active_sweep.spec, self.active_sweep.target_u8, baseline=self.baseline_command, + profile=profile, ) ) @@ -1366,8 +2132,11 @@ class G20ThreeCameraCalibrationNode(Node): relaxed: bool = False, ) -> JointCurveFit: del relaxed + profile = getattr(self, "profile", LEFT_HAND_PROFILE) zero_command = int( - self.baseline_command[JOINT_SPECS[joint_name].motor_index] + self.baseline_command[ + profile.joint_specs[joint_name].motor_index + ] ) return fit_rotation_joint_curve( records, zero_command_u8=zero_command @@ -1377,6 +2146,9 @@ class G20ThreeCameraCalibrationNode(Node): self, joint_name: str, cycle: int ) -> JointAxisMeasurement: constraint: Sequence[float] | None = None + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) + reference = profile.reference_finger upstream_joint = { # These neighbouring axes are parallel in the fixed source URDF. # A small planar Tag's monocular PnP orientation can have a stable @@ -1386,8 +2158,8 @@ class G20ThreeCameraCalibrationNode(Node): # 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", + f"{reference}_pip": f"{reference}_mcp_pitch", + f"{reference}_dip": f"{reference}_pip", }.get(joint_name) if upstream_joint is not None: # Resolve recursively so thumb_ip receives the already constrained @@ -1396,8 +2168,15 @@ class G20ThreeCameraCalibrationNode(Node): # 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( + spec = profile.joint_specs[joint_name] + extrinsics = getattr(self, "extrinsics", None) + view_normal = None + if extrinsics is not None: + view_transform = extrinsics.transform(spec.view) + view_normal = view_transform[:3, :3] @ np.asarray( + [0.0, 0.0, 1.0], dtype=float + ) + measurement = fit_joint_axis_measurement( joint_name, self.records_by_joint[joint_name], cycle=cycle, @@ -1405,12 +2184,34 @@ class G20ThreeCameraCalibrationNode(Node): self.baseline_command[spec.motor_index] ), axis_common_constraint=constraint, + constrained_circle_joints=zero_profile.constrained_circle_joints, + view_normal_common_xyz=view_normal, + ) + condition_command = build_calibration_motion_command( + spec, + int(self.baseline_command[spec.motor_index]), + baseline=self.baseline_command, + profile=profile, + ) + measurement = replace( + measurement, + condition_command_u8=tuple( + float(value) for value in condition_command + ), + ) + if extrinsics is None: + return measurement + return replace( + measurement, + view_normal_common_xyz=tuple(float(value) for value in view_normal), ) def _provisional_fit_failures( self, spec: SweepSpec ) -> list[dict[str, Any]]: """Check full-pose curve and 3-D axis after one six-way task.""" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) failures: list[dict[str, Any]] = [] if hasattr(self, "views"): valid_rate = float(self.views[spec.view].valid_rate) @@ -1473,7 +2274,7 @@ class G20ThreeCameraCalibrationNode(Node): ), math.degrees( self.active_maximum_rotation_orthogonal_rms_rad - if JOINT_SPECS[joint_name].active + if profile.joint_specs[joint_name].active else self.passive_maximum_rotation_orthogonal_rms_rad ), "maximum", @@ -1485,7 +2286,7 @@ class G20ThreeCameraCalibrationNode(Node): "minimum", ), ) - joint_spec = JOINT_SPECS[joint_name] + joint_spec = profile.joint_specs[joint_name] monotonic_limit = ( self.maximum_monotonic_correction_rad if joint_spec.active @@ -1529,6 +2330,7 @@ class G20ThreeCameraCalibrationNode(Node): cycle_travels: list[float] = [] cycle_axes: list[np.ndarray] = [] + cycle_axis_sources: list[str] = [] for cycle in range(self.repetitions): cycle_records = [ record @@ -1568,6 +2370,7 @@ class G20ThreeCameraCalibrationNode(Node): ) continue cycle_axes.append(np.asarray(axis.axis_common_xyz, dtype=float)) + cycle_axis_sources.append(axis.axis_direction_source) plane_limit = ( self.axis_maximum_plane_rms_m if joint_spec.active @@ -1579,8 +2382,13 @@ class G20ThreeCameraCalibrationNode(Node): 1000.0 * axis.radial_rms_m, 1000.0 * self.axis_maximum_radial_rms_m, ), + ( + "axis_pose_line_rms_mm", + 1000.0 * axis.pose_axis_line_rms_m, + 1000.0 * self.axis_maximum_pose_line_rms_m, + ), ] - if joint_name not in CONSTRAINED_CIRCLE_JOINTS: + if joint_name not in zero_profile.constrained_circle_joints: axis_checks.extend( [ ( @@ -1588,17 +2396,20 @@ class G20ThreeCameraCalibrationNode(Node): 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 - ), - ), ] ) + if joint_name not in zero_profile.constrained_circle_joints: + axis_checks.append( + ( + "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 @@ -1636,7 +2447,13 @@ class G20ThreeCameraCalibrationNode(Node): ], } ) - if len(cycle_axes) == self.repetitions: + if ( + len(cycle_axes) == self.repetitions + and not all( + source == "upstream_constraint" + for source in cycle_axis_sources + ) + ): maximum_difference = 0.0 for left in cycle_axes: for right in cycle_axes: @@ -1664,20 +2481,72 @@ class G20ThreeCameraCalibrationNode(Node): return failures def _pause_for_provisional_fit_failure( - self, spec: SweepSpec, failures: list[dict[str, Any]] - ) -> None: - start_index = self._sweep_spec_start_index(spec) + self, + spec: SweepSpec, + failures: list[dict[str, Any]], + *, + allow_warning: bool = False, + ) -> bool: + def is_warning(item: Mapping[str, Any]) -> bool: + if "actual" not in item or "limit" not in item: + return False + actual = float(item["actual"]) + limit = float(item["limit"]) + ratio = float(getattr(self, "provisional_warning_ratio", 1.0)) + if limit <= 0.0: + return False + if item.get("comparison") == "minimum": + return actual >= limit / ratio + return actual <= limit * ratio + + if allow_warning and failures and all(is_warning(item) for item in failures): + append_jsonl( + self.raw_path, + { + "kind": "provisional_fit_warning", + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "warning_ratio": float( + getattr(self, "provisional_warning_ratio", 1.0) + ), + "failures": failures, + }, + ) + self.reason = "provisional_fit_warning_continuing" + return False + 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 + localized_cycles = { + int(item["cycle"]) - 1 + for item in failures + if item.get("cycle") is not None + } + if failures and all( + item.get("cycle") is not None + or str(item.get("metric", "")).startswith("third_cycle_") + for item in failures + ): + if any( + str(item.get("metric", "")).startswith("third_cycle_") + for item in failures + ): + localized_cycles.add(self.repetitions - 1) + self.retry_cycles = localized_cycles + else: + self.retry_cycles = set(range(self.repetitions)) self.fit_failure = { "kind": "fit_failure", "view": spec.view, "motor_index": spec.motor_index, "joints": list(spec.joints), "attempt": attempt, - "directions_to_rescan": 2 * self.repetitions, + "cycles_to_rescan": [ + cycle + 1 for cycle in sorted(self.retry_cycles) + ], + "directions_to_rescan": 2 * len(self.retry_cycles), "failures": failures, } append_jsonl( @@ -1687,7 +2556,142 @@ class G20ThreeCameraCalibrationNode(Node): **self.fit_failure, }, ) + if attempt <= getattr(self, "automatic_fit_retry_limit", 0): + self.paused_reason = "joint_fit_check_failed" + self.reason = "automatic_retry_joint_fit_check_failed" + self._prepare_failed_sweep_retry() + self._begin_return_baseline("resume_sweep") + return True self._pause("joint_fit_check_failed") + return True + + def _pause_for_zero_model_failure( + self, zero_result: ZeroSolveResult + ) -> None: + """Stop once for a model/zero failure; repeated motion cannot fix it.""" + failures: list[dict[str, Any]] = [] + for name, reason in zero_result.failure_reasons.items(): + failure: dict[str, Any] = { + "joint": name, + "metric": "zero_guard", + "reason": reason, + } + if reason == "zero_offset_exceeds_configured_limit": + configured_limit = self.zero_joint_maximum_offsets_rad.get( + name, + ( + self.zero_finger_maximum_offset_rad + if name.startswith( + ("index_", "middle_", "ring_", "pinky_") + ) + else self.zero_maximum_offset_rad + ), + ) + failure["actual_deg"] = round( + math.degrees(zero_result.direct_offsets_rad[name]), 6 + ) + failure["limit_deg"] = round( + math.degrees(configured_limit), 6 + ) + failures.append(failure) + failures.extend( + { + "joint": observer, + "metric": "third_cycle_axis_holdout_deg", + "actual": round(math.degrees(error), 6), + "limit": math.degrees(self.maximum_validation_p95_rad), + "comparison": "maximum", + } + for observer, error in ( + zero_result.validation_error_by_joint_rad.items() + ) + if error > self.maximum_validation_p95_rad + ) + direct_joint = next( + iter(zero_result.failure_reasons), + next( + iter(zero_result.validation_error_by_joint_rad), + "thumb_cmc_pitch", + ), + ) + spec = next( + ( + item + for item in self.profile.sweep_specs + if direct_joint in item.joints + ), + self.profile.sweep_specs[0], + ) + self.retry_sweep_spec = None + self.retry_resume_index = None + self.retry_cycles = set() + self.fit_failure = { + "kind": "zero_model_failure", + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "directions_to_rescan": 0, + "recoverable_by_rescan": False, + "failures": failures, + } + append_jsonl( + self.raw_path, + {"kind": "zero_model_failure", **self.fit_failure}, + ) + self._pause("zero_model_validation_failed") + + def _retry_active_sweep_or_pause(self, reason: str) -> None: + """Retry a recoverable acquisition failure without weakening gates.""" + if self.active_sweep is None: + self._pause(reason) + return + item = self.active_sweep + key = (item.spec.motor_index, item.cycle, item.direction) + retries = self.sweep_retry_counts.get(key, 0) + retry_limit = getattr(self, "automatic_sweep_retry_limit", 0) + if retries >= retry_limit: + self._pause(reason) + return + retries += 1 + self.sweep_retry_counts[key] = retries + runtime = getattr(self, "views", {}).get(item.spec.view) + if runtime is not None: + self._reset_view_trackers(runtime) + runtime.pnp_reset_count += 1 + getattr(self, "sweep_frames", []).clear() + getattr(self, "sweep_start_frames", []).clear() + speed_scales = getattr( + self, "retry_speed_scales", (0.8, 0.6, 0.5) + ) + endpoint_holds = getattr( + self, "retry_endpoint_hold_seconds", (0.75, 1.0, 1.25) + ) + append_jsonl( + self.raw_path, + { + "kind": "automatic_sweep_retry", + "view": item.spec.view, + "motor_index": item.spec.motor_index, + "joints": list(item.spec.joints), + "cycle": item.cycle, + "direction": item.direction, + "retry": retries, + "retry_limit": retry_limit, + "reason": str(reason), + "speed_scale": float( + speed_scales[ + min(retries, len(speed_scales)) - 1 + ] + ), + "endpoint_hold_seconds": float( + endpoint_holds[ + min(retries, len(endpoint_holds)) - 1 + ] + ), + }, + ) + self.reason = f"automatic_retry_{reason}" + self._begin_return_baseline("retry_sweep") def _finish_active_sweep(self) -> None: assert self.active_sweep is not None @@ -1713,13 +2717,13 @@ class G20ThreeCameraCalibrationNode(Node): bins.setdefault(command, []).append(frame) commands = sorted(bins) if len(commands) < self.minimum_sweep_bins: - self._pause("sweep_bins_too_few") + self._retry_active_sweep_or_pause("sweep_bins_too_few") return if commands[0] != 0 or commands[-1] != 255: - self._pause("sweep_missing_endpoint_bin") + self._retry_active_sweep_or_pause("sweep_missing_endpoint_bin") return if max(np.diff(commands), default=0) > self.maximum_bin_gap: - self._pause("sweep_bin_gap_too_large") + self._retry_active_sweep_or_pause("sweep_bin_gap_too_large") return append_jsonl_many( @@ -1850,18 +2854,39 @@ class G20ThreeCameraCalibrationNode(Node): append_jsonl(self.raw_path, record) previous_spec = item.spec - self.sweep_index += 1 + was_fit_retry = bool( + getattr(self, "active_sweep_is_fit_retry", False) + ) + if not was_fit_retry: + self.sweep_index += 1 self.active_sweep = None + self.active_sweep_is_fit_retry = False + if was_fit_retry: + if self.retry_sweep_items: + self._start_next_sweep() + return + failures = self._provisional_fit_failures(previous_spec) + if failures and self._pause_for_provisional_fit_failure( + previous_spec, failures, allow_warning=True + ): + return + self.retry_sweep_spec = None + self.retry_resume_index = None + self.fit_failure = {} + if self.sweep_index >= len(self.sweep_items): + self._begin_return_baseline("fit") + else: + self._begin_return_baseline("next_sweep") + return spec_complete = bool( self.sweep_index >= len(self.sweep_items) or self.sweep_items[self.sweep_index].spec != previous_spec ) if spec_complete: failures = self._provisional_fit_failures(previous_spec) - if failures: - self._pause_for_provisional_fit_failure( - previous_spec, failures - ) + if failures and self._pause_for_provisional_fit_failure( + previous_spec, failures, allow_warning=True + ): return if self.retry_sweep_spec == previous_spec: resume_index = self.retry_resume_index @@ -1880,10 +2905,19 @@ class G20ThreeCameraCalibrationNode(Node): def _fit_all_curves(self) -> None: self.state = STATE_FITTING self.reason = "fitting_3d_axes_and_urdf_zero_offsets" + # Provisional checks have a warning band so collection can continue, + # but the final fit always re-applies the unmodified hard thresholds. + for spec in self.profile.sweep_specs: + failures = self._provisional_fit_failures(spec) + if failures: + self._pause_for_provisional_fit_failure( + spec, failures, allow_warning=False + ) + return training_fits: dict[str, JointCurveFit] = {} holdout_by_joint: dict[str, tuple[float, ...]] = {} measured: dict[str, JointCurveFit] = {} - for name in MEASURED_JOINTS: + for name in self.profile.measured_joints: training_records = [ record for record in self.records_by_joint[name] @@ -1895,7 +2929,9 @@ class G20ThreeCameraCalibrationNode(Node): if int(record["cycle"]) == 2 ] zero_command = int( - self.baseline_command[JOINT_SPECS[name].motor_index] + self.baseline_command[ + self.profile.joint_specs[name].motor_index + ] ) training_fits[name] = self._fit_joint_records( name, training_records @@ -1909,12 +2945,13 @@ class G20ThreeCameraCalibrationNode(Node): name, self.records_by_joint[name] ) axes: list[JointAxisMeasurement] = [] - for name in AXIS_JOINTS: + for name in self.zero_profile.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() + name: int(spec.motor_index) + for name, spec in self.profile.joint_specs.items() } holdout_zero_result = solve_urdf_zero_offsets( source_urdf=self.source_urdf_path, @@ -1922,8 +2959,56 @@ class G20ThreeCameraCalibrationNode(Node): curves=training_fits, motor_by_joint=motor_by_joint, maximum_offset_rad=self.zero_maximum_offset_rad, + joint_maximum_offset_rad=self.zero_joint_maximum_offsets_rad, maximum_validation_mae_rad=self.maximum_validation_mae_rad, maximum_validation_p95_rad=self.maximum_validation_p95_rad, + maximum_axis_cone_mismatch_rad=( + self.zero_maximum_axis_cone_mismatch_rad + ), + maximum_pose_axis_line_rms_m=( + self.axis_maximum_pose_line_rms_m + ), + finger_maximum_offset_rad=self.zero_finger_maximum_offset_rad, + maximum_cycle_difference_rad=( + self.zero_maximum_axis_cycle_difference_rad + ), + hand_type=self.hand_type, + ) + append_jsonl( + self.raw_path, + { + "kind": "zero_holdout_diagnostics", + "hand_type": self.hand_type, + "passed": holdout_zero_result.passed, + "direct_offsets_rad": dict( + holdout_zero_result.direct_offsets_rad + ), + "cycle_offsets_rad": { + name: list(values) + for name, values in ( + holdout_zero_result.cycle_offsets_rad.items() + ) + }, + "offset_uncertainty_rad": dict( + holdout_zero_result.offset_uncertainty_rad + ), + "validation_error_by_joint_rad": dict( + holdout_zero_result.validation_error_by_joint_rad + ), + "validation_original_error_by_joint_rad": dict( + holdout_zero_result.validation_original_error_by_joint_rad + ), + "validation_improvement_by_joint_rad": dict( + holdout_zero_result.validation_improvement_by_joint_rad + ), + "validation_improvement_confidence_lower_rad": dict( + holdout_zero_result + .validation_improvement_confidence_lower_rad + ), + "failure_reasons": dict( + holdout_zero_result.failure_reasons + ), + }, ) holdout_errors = np.abs( np.asarray( @@ -1946,52 +3031,27 @@ class G20ThreeCameraCalibrationNode(Node): fit.maximum_monotonic_correction_rad <= ( self.maximum_monotonic_correction_rad - if JOINT_SPECS[name].active + if self.profile.joint_specs[name].active else self.passive_maximum_monotonic_correction_rad ) and fit.maximum_hysteresis_rad <= ( self.maximum_hysteresis_rad - if JOINT_SPECS[name].active + if self.profile.joint_specs[name].active else self.passive_maximum_hysteresis_rad ) for name, fit in measured.items() ) - if not trajectory_holdout_passed or not holdout_zero_result.passed: + if not trajectory_holdout_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" + worst_joint = max(trajectory_score, key=trajectory_score.get) failed_spec = next( - spec for spec in SWEEP_SPECS if worst_joint in spec.joints + spec + for spec in self.profile.sweep_specs + if worst_joint in spec.joints ) failures: list[dict[str, Any]] = [ { @@ -2006,44 +3066,45 @@ class G20ThreeCameraCalibrationNode(Node): 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 + if not holdout_zero_result.passed: + self._pause_for_zero_model_failure(holdout_zero_result) + 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, + joint_maximum_offset_rad=self.zero_joint_maximum_offsets_rad, maximum_validation_mae_rad=self.maximum_validation_mae_rad, maximum_validation_p95_rad=self.maximum_validation_p95_rad, + maximum_axis_cone_mismatch_rad=( + self.zero_maximum_axis_cone_mismatch_rad + ), + maximum_pose_axis_line_rms_m=( + self.axis_maximum_pose_line_rms_m + ), + finger_maximum_offset_rad=self.zero_finger_maximum_offset_rad, + maximum_cycle_difference_rad=( + self.zero_maximum_axis_cycle_difference_rad + ), + hand_type=self.hand_type, ) if not zero_result.passed: raise RuntimeError("final_all_cycle_zero_refit_failed") + append_jsonl( + self.raw_path, + { + "kind": "zero_final_diagnostics", + "hand_type": self.hand_type, + "direct_offsets_rad": dict(zero_result.direct_offsets_rad), + "offset_uncertainty_rad": dict( + zero_result.offset_uncertainty_rad + ), + }, + ) self.measured_fits = measured self.axis_measurements = axes self.zero_result = zero_result @@ -2052,6 +3113,10 @@ class G20ThreeCameraCalibrationNode(Node): for values in holdout_by_joint.values() for value in values ] + self.validation_errors_rad.extend( + float(value) + for value in holdout_zero_result.validation_errors_rad + ) if self.validation_enabled: self._build_validation_items() self._start_next_validation() @@ -2063,7 +3128,7 @@ class G20ThreeCameraCalibrationNode(Node): def _build_validation_items(self) -> None: generator = random.Random(self.validation_seed) self.validation_items = [] - for spec in SWEEP_SPECS: + for spec in self.profile.sweep_specs: commands = generator.sample( list(range(16, 240)), self.validation_command_count ) @@ -2081,6 +3146,13 @@ class G20ThreeCameraCalibrationNode(Node): self.validation_frames_buffer.clear() self.position_hold_since = None self.validation_stage_started_at = time.monotonic() + self._reset_motion_progress( + self.validation_stage_started_at, + self._motion_command_error_u8( + self.active_validation.spec, + self.active_validation.command_u8, + ), + ) self.state = STATE_VALIDATION_MOVE self.reason = ( f"validation_{self.active_validation.spec.view}_motor_" @@ -2095,6 +3167,7 @@ class G20ThreeCameraCalibrationNode(Node): self.active_validation.spec, self.active_validation.command_u8, baseline=self.baseline_command, + profile=self.profile, ) ) @@ -2150,31 +3223,15 @@ class G20ThreeCameraCalibrationNode(Node): 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 - ) + # Zero calibration changes only joint-origin rotations. Measured + # motion curves belong in schema-v4 JSON and must never enlarge the + # original CAD/mechanical safety limits in the generated URDF. 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, @@ -2183,6 +3240,7 @@ class G20ThreeCameraCalibrationNode(Node): validation_errors_rad=self.validation_errors_rad, passed=passed, baseline=self.baseline_command, + side=self.hand_type, ) atomic_write_json(self.final_path, payload) self.completed_payload = payload @@ -2199,6 +3257,101 @@ class G20ThreeCameraCalibrationNode(Node): self.reason = str(reason) self.position_hold_since = None + def _retry_motion_or_pause(self, reason: str, now: float) -> None: + retries = self.motion_retry_counts.get(reason, 0) + if retries >= self.automatic_motion_retry_limit: + self._pause(reason) + return + retries += 1 + self.motion_retry_counts[reason] = retries + append_jsonl( + self.raw_path, + { + "kind": "automatic_motion_retry", + "state": self.state, + "reason": reason, + "retry": retries, + "retry_limit": self.automatic_motion_retry_limit, + }, + ) + self.motion_stage_started_at = now + self.position_hold_since = None + if self.state == STATE_RETURN_BASELINE: + self._reset_motion_progress(now, self._baseline_error_u8()) + self._publish_speed_profile(self._normal_speed_profile()) + self._publish_command( + list( + G20ThreeCameraCalibrationNode._current_return_command(self) + ) + ) + elif self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: + self._reset_motion_progress( + now, + self._motion_command_error_u8( + self.active_sweep.spec, + self.active_sweep.start_u8, + ), + ) + self._publish_speed_profile( + self._speed_profile_for_spec(self.active_sweep.spec) + ) + self._publish_command( + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ) + self.reason = f"automatic_retry_{reason}" + + def _retry_validation_or_pause(self, reason: str, now: float) -> None: + if self.active_validation is None: + self._pause(reason) + return + item = self.active_validation + key = (item.spec.motor_index, item.command_u8) + retries = self.validation_retry_counts.get(key, 0) + retry_limit = self.automatic_sweep_retry_limit + if retries >= retry_limit: + self._pause(reason) + return + retries += 1 + self.validation_retry_counts[key] = retries + runtime = self.views[item.spec.view] + self._reset_view_trackers(runtime) + runtime.pnp_reset_count += 1 + self.validation_frames_buffer.clear() + self.validation_stage_started_at = now + self._reset_motion_progress( + now, + self._motion_command_error_u8(item.spec, item.command_u8), + ) + self.position_hold_since = None + self.state = STATE_VALIDATION_MOVE + append_jsonl( + self.raw_path, + { + "kind": "automatic_validation_retry", + "view": item.spec.view, + "motor_index": item.spec.motor_index, + "command_u8": item.command_u8, + "reason": reason, + "retry": retries, + "retry_limit": retry_limit, + }, + ) + self._publish_speed_profile(self._speed_profile_for_spec(item.spec)) + self._publish_command( + build_calibration_motion_command( + item.spec, + item.command_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ) + self.reason = f"automatic_retry_{reason}" + def _timer_callback(self) -> None: now = time.monotonic() try: @@ -2222,16 +3375,33 @@ class G20ThreeCameraCalibrationNode(Node): self.reason = "preflight_lost" return if self.state == STATE_RETURN_BASELINE: - if now - self.motion_stage_started_at > self.position_timeout_seconds: - self._pause("return_baseline_timeout") + baseline_reached = self._baseline_reached() + baseline_details = self._baseline_error_details() + if ( + not baseline_reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(baseline_details["error_u8"]), + context="return_baseline", + details=baseline_details, + ) + ): return - if self._baseline_reached(): + if now - self.motion_stage_started_at > self.position_timeout_seconds: + self._retry_motion_or_pause("return_baseline_timeout", now) + return + if baseline_reached: if self.position_hold_since is None: self.position_hold_since = now elif now - self.position_hold_since >= self.baseline_hold_seconds: after = self.baseline_after + self.motion_retry_counts.pop("return_baseline_timeout", None) self.position_hold_since = None - if after in {"next_sweep", "resume_sweep"}: + if after in { + "next_sweep", + "resume_sweep", + "retry_sweep", + }: self._start_next_sweep() elif after == "fit": self._fit_all_curves() @@ -2245,12 +3415,30 @@ class G20ThreeCameraCalibrationNode(Node): if self.state == STATE_PREPARE_SWEEP: assert self.active_sweep is not None if now - self.motion_stage_started_at > self.position_timeout_seconds: - self._pause("sweep_start_position_timeout") + self._retry_motion_or_pause("sweep_start_position_timeout", now) return reached = self._motion_command_reached( self.active_sweep.spec, self.active_sweep.start_u8, ) + prepare_context = ( + f"prepare_motor_{self.active_sweep.spec.motor_index}" + ) + prepare_details = self._motion_command_error_details( + self.active_sweep.spec, + self.active_sweep.start_u8, + prepare_context, + ) + if ( + not reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(prepare_details["error_u8"]), + context=prepare_context, + details=prepare_details, + ) + ): + return speed_ready = bool( now - self.speed_commanded_at >= self.speed_setting_settle_seconds @@ -2261,7 +3449,10 @@ class G20ThreeCameraCalibrationNode(Node): 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: + elif now - self.position_hold_since >= self._active_endpoint_hold_seconds(): + self.motion_retry_counts.pop( + "sweep_start_position_timeout", None + ) self.position_hold_since = None self._begin_active_sweep(now) else: @@ -2270,11 +3461,13 @@ class G20ThreeCameraCalibrationNode(Node): if self.state == STATE_SWEEP: assert self.active_sweep is not None motor = self.active_sweep.spec.motor_index - if now - self.sweep_started_at > self.sweep_timeout_seconds: - self._pause("sweep_timeout") + if now - self.sweep_started_at > self._active_sweep_timeout_seconds(): + self._retry_active_sweep_or_pause("sweep_timeout") return if now - self.sweep_last_valid_at > self.invalid_timeout_seconds: - self._pause("synchronised_tag_state_timeout") + self._retry_active_sweep_or_pause( + "synchronised_tag_state_timeout" + ) return values = [ float(frame.state_u8[motor]) for frame in self.sweep_frames @@ -2284,6 +3477,22 @@ class G20ThreeCameraCalibrationNode(Node): self.active_sweep.spec, self.active_sweep.target_u8, ) + sweep_context = f"sweep_motor_{motor}" + sweep_details = self._motion_command_error_details( + self.active_sweep.spec, + self.active_sweep.target_u8, + sweep_context, + ) + if ( + not target_reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(sweep_details["error_u8"]), + context=sweep_context, + details=sweep_details, + ) + ): + return if target_reached: if self.sweep_endpoint_since is None: self.sweep_endpoint_since = now @@ -2291,7 +3500,8 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_endpoint_since = None if ( self.sweep_endpoint_since is not None - and now - self.sweep_endpoint_since >= self.endpoint_hold_seconds + and now - self.sweep_endpoint_since + >= self._active_endpoint_hold_seconds() and len(self.sweep_frames) >= self.minimum_sweep_frames and span >= self.minimum_state_span_u8 ): @@ -2303,6 +3513,25 @@ class G20ThreeCameraCalibrationNode(Node): self.active_validation.spec, self.active_validation.command_u8, ) + validation_context = ( + "validation_motor_" + f"{self.active_validation.spec.motor_index}" + ) + validation_details = self._motion_command_error_details( + self.active_validation.spec, + self.active_validation.command_u8, + validation_context, + ) + if ( + not reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(validation_details["error_u8"]), + context=validation_context, + details=validation_details, + ) + ): + return speed_ready = bool( now - self.speed_commanded_at >= self.speed_setting_settle_seconds @@ -2319,32 +3548,40 @@ class G20ThreeCameraCalibrationNode(Node): else: self.position_hold_since = None if now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._pause("validation_move_timeout") + self._retry_validation_or_pause("validation_move_timeout", now) return if self.state == STATE_VALIDATION_CAPTURE: if len(self.validation_frames_buffer) >= self.validation_frames: self._finish_validation_capture() elif now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._pause("validation_capture_timeout") + self._retry_validation_or_pause( + "validation_capture_timeout", now + ) def _publish_status(self, now: float) -> None: - views = { - name: { + views: dict[str, dict[str, Any]] = {} + for name, runtime in self.views.items(): + required_roles = set(self._required_roles_for_view(name)) + views[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), + "required_tag_ids": sorted( + runtime.view_tags[role] for role in required_roles + ), "detected_tag_ids": sorted( tag_id for tag_id, role in runtime.role_by_id.items() - if role in runtime.latest_tag_quality + if role in required_roles + and role in runtime.latest_tag_quality ), "missing_tag_ids": sorted( tag_id for tag_id, role in runtime.role_by_id.items() - if role not in runtime.latest_tag_quality + if role in required_roles + and role not in runtime.latest_tag_quality ), "pnp_rejections": dict(runtime.latest_pnp_rejections), "group_pnp_reason": runtime.latest_group_pnp_reason, @@ -2355,10 +3592,10 @@ class G20ThreeCameraCalibrationNode(Node): else round(max(0.0, now - runtime.pnp_invalid_since), 3) ), } - for name, runtime in self.views.items() - } active: dict[str, Any] = {} - if self.active_sweep is not None: + if self.motion_stall_details: + active = dict(self.motion_stall_details) + elif 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 @@ -2408,6 +3645,48 @@ class G20ThreeCameraCalibrationNode(Node): "cycle": self.active_sweep.cycle + 1, "repetitions": self.repetitions, "direction": self.active_sweep.direction, + "fit_attempt": self.sweep_attempts.get(motor, 1), + "fit_attempt_limit": self.automatic_fit_retry_limit + 1, + "automatic_retry_count": self.sweep_retry_counts.get( + ( + motor, + self.active_sweep.cycle, + self.active_sweep.direction, + ), + 0, + ), + "automatic_retry_limit": self.automatic_sweep_retry_limit, + "retry_speed_scale": ( + 1.0 + if self.sweep_retry_counts.get( + ( + motor, + self.active_sweep.cycle, + self.active_sweep.direction, + ), + 0, + ) + == 0 + else self.retry_speed_scales[ + min( + self.sweep_retry_counts[ + ( + motor, + self.active_sweep.cycle, + self.active_sweep.direction, + ) + ], + len(self.retry_speed_scales), + ) + - 1 + ] + ), + "endpoint_hold_seconds": self._active_endpoint_hold_seconds(), + "sweep_timeout_seconds": self._active_sweep_timeout_seconds(), + "motor_stall_timeout_seconds": self.motor_stall_timeout_seconds, + "motor_stall_minimum_progress_u8": ( + self.motor_stall_minimum_progress_u8 + ), "start_u8": self.active_sweep.start_u8, "target_u8": self.active_sweep.target_u8, "actual_u8": actual, @@ -2439,7 +3718,7 @@ class G20ThreeCameraCalibrationNode(Node): ), } for index, command in calibration_auxiliary_commands( - self.active_sweep.spec + self.active_sweep.spec, profile=self.profile ).items() ], "speed": { @@ -2476,7 +3755,7 @@ class G20ThreeCameraCalibrationNode(Node): ), } for index, command in calibration_auxiliary_commands( - self.active_validation.spec + self.active_validation.spec, profile=self.profile ).items() ], "speed": { @@ -2491,17 +3770,34 @@ class G20ThreeCameraCalibrationNode(Node): } elif self.fit_failure: active = dict(self.fit_failure) + scan_progress = ( + 0.0 + if not self.sweep_items + else float( + np.clip(self.sweep_index / len(self.sweep_items), 0.0, 1.0) + ) + ) payload = { + "hand_type": self.hand_type, + "reference_finger": self.profile.reference_finger, "state": self.state, "reason": self.reason, - "progress": ( - 0.0 - if not self.sweep_items - else round(self.sweep_index / len(self.sweep_items), 4) + "progress": round( + _overall_progress( + self.state, + scan_progress=scan_progress, + validation_index=self.validation_index, + validation_total=len(self.validation_items), + ), + 4, ), + "scan_progress": round(scan_progress, 4), "completed_sweeps": self.sweep_index, "total_sweeps": len(self.sweep_items), "baseline_command_u8": list(self.baseline_command), + "return_command_u8": list( + G20ThreeCameraCalibrationNode._current_return_command(self) + ), "hand_state_fresh": bool( len(self.latest_state_u8) == 20 and now - self.last_state_at <= 1.0 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 index f5f26b8..051a116 100644 --- 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 @@ -16,99 +16,183 @@ 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 .full_hand import ( + IMAGE_TRAJECTORY_JOINTS, + LEFT_HAND_PROFILE, + HandCalibrationProfile, + JointCurveFit, + get_hand_calibration_profile, +) 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"} +@dataclass(frozen=True) +class ZeroCalibrationProfile: + hand: HandCalibrationProfile + direct_zero_joints: tuple[str, ...] + axis_joints: tuple[str, ...] + # Dynamic command-angle curves may be shared by unobserved fingers. This + # mapping is intentionally *not* an authorization to copy an absolute + # encoder/URDF zero between independent motors. + inherited_zero_joints: Mapping[str, str] + # Absolute static-zero inheritance requires an independent mechanical + # guarantee. With the current 11-Tag layout only the reference finger is + # observed, so the conservative mapping is empty and unobserved active + # finger origins retain their source-CAD zero. + inherited_static_zero_joints: Mapping[str, str] + constrained_circle_joints: frozenset[str] + root_anchor_joints: frozenset[str] + axis_parent_joint: Mapping[str, str] + phase_parent_joint: Mapping[str, str] + offset_observer_joint: Mapping[str, str] + fixed_direct_zero_offsets_rad: Mapping[str, float] + static_output_zero_offsets_rad: Mapping[str, float] + + @property + def reference_finger(self) -> str: + return self.hand.reference_finger + + +def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: + reference = hand.reference_finger + reference_roll = f"{reference}_mcp_roll" + reference_pitch = f"{reference}_mcp_pitch" + reference_pip = f"{reference}_pip" + reference_dip = f"{reference}_dip" + direct = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + reference_roll, + reference_pitch, + reference_pip, + ) + inherited = { + f"{finger}_{suffix}": f"{reference}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + if finger != reference + for suffix in ("mcp_roll", "mcp_pitch", "pip") + } + axis_parent = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", + reference_pitch: reference_roll, + } + phase_parent = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + reference_pip: reference_pitch, + reference_dip: reference_pip, + } + observer = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_cmc_pitch", + "thumb_cmc_pitch": "thumb_mcp", + "thumb_mcp": "thumb_ip", + reference_roll: reference_pitch, + reference_pitch: reference_pip, + reference_pip: reference_dip, + } + fixed_direct_zero_offsets = { + "thumb_mcp": 0.0, + reference_roll: 0.0, + reference_pitch: 0.0, + reference_pip: 0.0, + } + # No per-device or per-side numeric calibration belongs in the profile. + # Any non-zero thumb CMC origin must come from this session's measured + # trajectories and pass the independent-cycle validation below. + static_output_zero_offsets: dict[str, float] = {} + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=direct, + axis_joints=( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + reference_roll, + reference_pitch, + reference_pip, + reference_dip, + ), + inherited_zero_joints=inherited, + inherited_static_zero_joints={}, + constrained_circle_joints=( + hand.image_trajectory_joints | {reference_dip, "thumb_cmc_yaw"} + ), + root_anchor_joints=frozenset({"thumb_cmc_roll", reference_roll}), + axis_parent_joint=axis_parent, + phase_parent_joint=phase_parent, + offset_observer_joint=observer, + # The active MCP trajectory is observable, but its absolute static + # phase is inferred only through the passive thumb-IP axis-line centre. + # In the fixed front monocular view that shallow arc has a 37-80 degree + # rotation/circle-axis disagreement and a repeatable phase bias. It is + # therefore not an admissible absolute-zero observation. Keep the + # original CAD zero while still measuring and validating angle_rad[256]. + # The current observations recover the reference finger's dynamic + # curves, but the physical four-finger straight pose is already the CAD + # datum. With only one of the four fingers tagged, writing its fitted + # absolute MCP/PIP phase (or copying it to independent motors) can bend + # one finger or tilt all four even when the real hand is straight. + # Keep every four-finger static origin at source CAD zero; inherit only + # the measured command-angle curve. thumb_mcp likewise depends on a + # shallow passive-IP circle whose absolute phase is not trustworthy. + # Stable systematic geometry bias must not be written as encoder zero. + fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets, + static_output_zero_offsets_rad=static_output_zero_offsets, + ) + + +def get_zero_calibration_profile(side: str) -> ZeroCalibrationProfile: + return _build_zero_profile(get_hand_calibration_profile(side)) + + +LEFT_ZERO_PROFILE = _build_zero_profile(LEFT_HAND_PROFILE) + +# Backwards-compatible left-hand aliases. +CONSTRAINED_CIRCLE_JOINTS = LEFT_ZERO_PROFILE.constrained_circle_joints +DIRECT_ZERO_JOINTS = LEFT_ZERO_PROFILE.direct_zero_joints +FIXED_DIRECT_ZERO_OFFSETS_RAD = dict( + LEFT_ZERO_PROFILE.fixed_direct_zero_offsets_rad ) - - -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( +OPTIMIZED_ZERO_JOINTS = 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", -} +AXIS_JOINTS = LEFT_ZERO_PROFILE.axis_joints +INHERITED_ZERO_JOINTS = dict(LEFT_ZERO_PROFILE.inherited_zero_joints) ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 = 16 +# Fit an axis-line point from the complete relative SE(3) trajectory instead +# of using only the moving Tag centre's free 3-D circle. For an approximately +# end-on view, planar-PnP optical depth is deliberately projected out: image +# x/y still observes the radial motion that determines an axis line, while +# the independent Tag depths are the dominant source of false zero phase. +AXIS_POINT_MINIMUM_ROTATION_RAD = math.radians(3.0) +AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD = math.radians(45.0) +AXIS_POINT_RESIDUAL_SCALE_M = 0.001 + # 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_AXIS_PARENT_JOINT = dict(LEFT_ZERO_PROFILE.axis_parent_joint) +ZERO_AXIS_OBSERVATION_JOINTS = frozenset( + set(LEFT_ZERO_PROFILE.root_anchor_joints) | set(ZERO_AXIS_PARENT_JOINT) ) -ZERO_LINE_OBSERVATION_JOINTS: frozenset[str] = frozenset( - { - "thumb_cmc_roll", - "index_mcp_roll", - "thumb_mcp", # observes thumb_cmc_pitch - } +ZERO_LINE_OBSERVATION_JOINTS = frozenset( + set(LEFT_ZERO_PROFILE.root_anchor_joints) + | set(LEFT_ZERO_PROFILE.phase_parent_joint) ) -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", -} +ZERO_ROOT_ANCHOR_JOINTS = LEFT_ZERO_PROFILE.root_anchor_joints +ZERO_MINIMUM_AXIS_CONE_RAD = math.radians(15.0) def _zero_sensitive_axis_error_rad( @@ -131,12 +215,46 @@ def _zero_sensitive_axis_error_rad( 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))) + predicted_projected = predicted - parent * float(predicted @ parent) + observed_projected = observed - parent * float(observed @ parent) + predicted_norm = float(np.linalg.norm(predicted_projected)) + observed_norm = float(np.linalg.norm(observed_projected)) + minimum_projection = math.sin(ZERO_MINIMUM_AXIS_CONE_RAD) + if min(predicted_norm, observed_norm) < minimum_projection: + raise ValueError( + "parent and downstream axes have an unobservable cone angle" + ) + predicted_projected /= predicted_norm + observed_projected /= observed_norm + return math.atan2( + float(parent @ np.cross(predicted_projected, observed_projected)), + float( + np.clip(predicted_projected @ observed_projected, -1.0, 1.0) + ), + ) + + +def _axis_cone_mismatch_rad( + predicted_axis: Sequence[float], + observed_axis: Sequence[float], + parent_axis: Sequence[float], +) -> float: + """Return zero-invariant parent/downstream cone-angle disagreement.""" + 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) + predicted_cone = math.acos( + abs(float(np.clip(parent @ predicted, -1.0, 1.0))) + ) + observed_cone = math.acos( + abs(float(np.clip(parent @ observed, -1.0, 1.0))) + ) + return abs(predicted_cone - observed_cone) + + 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)): @@ -371,6 +489,245 @@ class JointAxisMeasurement: plane_rms_m: float radial_rms_m: float rotation_circle_axis_difference_rad: float + # Optical-axis direction expressed in the shared calibration frame. It + # lets the phase solver discard the least reliable monocular-PnP depth + # component when the joint axis is viewed approximately end-on. Older + # recordings and synthetic callers may omit it and retain the 3-D path. + view_normal_common_xyz: tuple[float, float, float] | None = None + # Logical commands that produced the condition. Firmware feedback may + # saturate a few u8 short of an endpoint (for example right motor 10 reads + # 250 for command 255); kinematics must use the command-indexed curve while + # retaining condition_state_u8 for diagnostics and safety. + condition_command_u8: tuple[float, ...] | None = None + axis_direction_source: str = "unspecified" + circle_axis_observability: float = 0.0 + axis_point_source: str = "circle_center" + pose_axis_line_rms_m: float = 0.0 + + +def _fit_axis_point_from_pose_trajectory( + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + axis_parent_xyz: Sequence[float], + angle_axis_parent_xyz: Sequence[float], + phase_reference_point_parent_xyz: Sequence[float], + view_normal_common_xyz: Sequence[float] | None, +) -> tuple[np.ndarray, float, str]: + """Fit the closest point on a revolute axis from full relative poses. + + If ``T(q)`` maps the moving Tag into its parent Tag frame, then + ``T(q) @ inv(T(0))`` is a rotation about the physical joint axis and its + translation obeys ``(I - R(q)) p = t(q)``. Solving this equation over the + complete trajectory uses both pose orientation and translation and avoids + treating a monocular Tag-centre depth arc as ground-truth geometry. + """ + samples = [dict(record) for record in records] + zero_records = _near_zero_records(samples, zero_command_u8) + if not zero_records: + raise ValueError( + f"axis-point fit has no record near baseline {zero_command_u8}" + ) + + reference_rotation = Rotation.from_quat( + _baseline_reference(samples, zero_command_u8) + ).as_matrix() + reference_translation = np.median( + np.asarray( + [record["relative_translation_xyz_m"] for record in zero_records], + dtype=float, + ), + axis=0, + ) + axis = _vector(axis_parent_xyz, 3, name="axis-point direction") + axis /= np.linalg.norm(axis) + angle_axis = _vector( + angle_axis_parent_xyz, 3, name="axis-point angle direction" + ) + angle_axis /= np.linalg.norm(angle_axis) + phase_reference_point = _vector( + phase_reference_point_parent_xyz, + 3, + name="axis-point phase reference", + ) + helper = ( + np.asarray([1.0, 0.0, 0.0]) + if abs(float(axis[0])) < 0.8 + else np.asarray([0.0, 1.0, 0.0]) + ) + basis_first = np.cross(axis, helper) + basis_first /= np.linalg.norm(basis_first) + basis = np.column_stack( + (basis_first, np.cross(axis, basis_first)) + ) + + view_normal_common = None + use_image_plane_projection = False + if view_normal_common_xyz is not None: + view_normal_common = _vector( + view_normal_common_xyz, 3, name="view normal" + ) + view_normal_common /= np.linalg.norm(view_normal_common) + reference_parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix( + _pose_matrix(record["parent_pose_common"])[:3, :3] + ).as_quat() + for record in zero_records + ] + )[0] + reference_parent_rotation = Rotation.from_quat( + reference_parent_quaternion + ) + reference_view_normal_parent = reference_parent_rotation.inv().apply( + view_normal_common + ) + reference_view_normal_parent /= np.linalg.norm( + reference_view_normal_parent + ) + use_image_plane_projection = abs( + float(axis @ reference_view_normal_parent) + ) >= math.cos(AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD) + + matrices: list[np.ndarray] = [] + translations: list[np.ndarray] = [] + used_image_plane_projection = False + raw_angles: list[float] = [] + translation_angles: list[float] = [] + reference_radial = reference_translation - phase_reference_point + reference_radial -= axis * float(reference_radial @ axis) + reference_radial_norm = float(np.linalg.norm(reference_radial)) + if reference_radial_norm < 1.0e-6: + raise ValueError("axis-point phase reference radius is degenerate") + reference_radial /= reference_radial_norm + for record in samples: + observed_rotation = Rotation.from_quat( + _relative_rotation(record) + ).as_matrix() + observed_delta_rotation = observed_rotation @ reference_rotation.T + raw_angle = float( + Rotation.from_matrix(observed_delta_rotation).as_rotvec() + @ angle_axis + ) + if abs(raw_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: + continue + observed_translation = _vector( + record["relative_translation_xyz_m"], + 3, + name="relative translation", + ) + observed_radial = observed_translation - phase_reference_point + observed_radial -= axis * float(observed_radial @ axis) + observed_radial_norm = float(np.linalg.norm(observed_radial)) + if observed_radial_norm <= 1.0e-6: + continue + observed_radial /= observed_radial_norm + raw_angles.append(raw_angle) + translation_angles.append( + math.atan2( + float(axis @ np.cross(reference_radial, observed_radial)), + float( + np.clip(reference_radial @ observed_radial, -1.0, 1.0) + ), + ) + ) + orientation_sign = 1.0 + if raw_angles and float( + np.sum(np.asarray(raw_angles) * np.asarray(translation_angles)) + ) < 0.0: + orientation_sign = -1.0 + + for record in samples: + observed_rotation = Rotation.from_quat( + _relative_rotation(record) + ).as_matrix() + observed_translation = _vector( + record["relative_translation_xyz_m"], + 3, + name="relative translation", + ) + observed_delta_rotation = observed_rotation @ reference_rotation.T + signed_angle = float( + Rotation.from_matrix(observed_delta_rotation).as_rotvec() + @ angle_axis + ) * orientation_sign + if abs(signed_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: + continue + # Direction and angle have deliberately separate sources. A trusted + # upstream/circle direction may replace a biased planar-PnP axis, while + # the relative-orientation trajectory still provides a repeatable + # scalar travel. Reconstruct the admissible revolute motion before + # solving its axis line instead of feeding a contradictory 3-D + # rotation into the translation equations. + # The rotation-trajectory axis is undirected. Align its scalar sign + # with the selected physical axis using the Tag-centre trajectory; + # this remains robust even when a trusted upstream direction replaces + # a badly biased planar-PnP orientation axis. + delta_rotation = Rotation.from_rotvec( + axis * signed_angle + ).as_matrix() + delta_translation = ( + observed_translation + - delta_rotation @ reference_translation + ) + projection = np.eye(3) + axis_point_matrix = (np.eye(3) - delta_rotation) @ basis + if use_image_plane_projection: + assert view_normal_common is not None + parent_rotation = Rotation.from_matrix( + _pose_matrix(record["parent_pose_common"])[:3, :3] + ) + view_normal_parent = parent_rotation.inv().apply( + view_normal_common + ) + view_normal_parent /= np.linalg.norm(view_normal_parent) + projection -= np.outer( + view_normal_parent, view_normal_parent + ) + # A single end-on camera cannot distinguish absolute depth from a + # shift along the revolute axis. Keep the observable image-plane + # equations only; adding a free baseline-depth variable makes the + # axis point rank-deficient and pretends that this gauge is + # observable. Parallel phase chains are later compared in the + # same view, where their common depth gauge cancels. + used_image_plane_projection = True + matrices.append(projection @ axis_point_matrix) + translations.append(projection @ delta_translation) + + if len(matrices) < 6: + raise ValueError("axis-point fit has too few observable pose samples") + matrix = np.vstack(matrices) + translation = np.concatenate(translations) + singular_values = np.linalg.svd(matrix, compute_uv=False) + if ( + singular_values.size < 2 + or singular_values[-1] <= 1.0e-9 + or singular_values[-1] / singular_values[0] < 1.0e-3 + ): + raise ValueError("axis-point pose trajectory is ill-conditioned") + + unknown_count = matrix.shape[1] + solution = least_squares( + lambda value: ( + matrix @ value - translation + ) / AXIS_POINT_RESIDUAL_SCALE_M, + np.zeros(unknown_count, dtype=float), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + if not solution.success: + raise ValueError( + "axis-point pose optimization failed: " + solution.message + ) + residual = matrix @ solution.x - translation + rms = float(np.sqrt(np.mean(np.square(residual)))) + source = ( + "pose_trajectory_image_plane" + if used_image_plane_projection + else "pose_trajectory_3d" + ) + return basis @ solution.x[:2], rms, source def fit_joint_axis_measurement( @@ -380,6 +737,8 @@ def fit_joint_axis_measurement( cycle: int, zero_command_u8: int, axis_common_constraint: Sequence[float] | None = None, + constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS, + view_normal_common_xyz: Sequence[float] | None = None, ) -> JointAxisMeasurement: """Fit one physical screw axis from one complete scan cycle.""" samples = [ @@ -391,6 +750,14 @@ def fit_joint_axis_measurement( [record["relative_translation_xyz_m"] for record in samples], dtype=float ) free_circle_axis, free_plane_rms = _fit_plane_axis([points]) + singular_values = np.linalg.svd( + points - np.mean(points, axis=0), compute_uv=False + ) + circle_axis_observability = ( + 0.0 + if singular_values.size < 2 or singular_values[0] <= 1.0e-12 + else float(singular_values[1] / singular_values[0]) + ) reference = Rotation.from_quat( _baseline_reference(samples, zero_command_u8) @@ -428,6 +795,7 @@ def fit_joint_axis_measurement( ] )[0] parent_rotation = Rotation.from_quat(parent_quaternion) + axis_direction_source = "rotation" if axis_common_constraint is not None: common_axis = _vector( @@ -439,15 +807,16 @@ def fit_joint_axis_measurement( 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. + axis_direction_source = "upstream_constraint" + elif str(joint) in constrained_circle_joints: + # The side/front views are close to end-on for these axes. Monocular + # planar-PnP depth bias can therefore tilt even an apparently smooth, + # low-residual Tag-centre circle. The relative orientation trajectory + # observes the screw-axis direction directly and is invariant to an + # arbitrary fixed Tag mounting rotation; use it to constrain the 3-D + # circle and estimate only the axis line. Never switch the direction + # source based on a rotation-vs-circle threshold: that discontinuity + # makes otherwise identical repeat cycles choose different models. fitted_axis = rotation_axis circle = _fit_circle_with_axis(points, fitted_axis) plane_rms = float(circle["plane_rms_m"]) @@ -463,10 +832,19 @@ def fit_joint_axis_measurement( plane_rms = max( float(free_plane_rms), float(circle["plane_rms_m"]) ) + axis_direction_source = "rotation_circle_fusion" axis_common = parent_rotation.apply(fitted_axis) - point_common = parent_rotation.apply( - np.asarray(circle["center_xyz_m"], dtype=float) - ) + parent_translation + point_parent, pose_axis_line_rms_m, axis_point_source = ( + _fit_axis_point_from_pose_trajectory( + samples, + zero_command_u8=zero_command_u8, + axis_parent_xyz=fitted_axis, + angle_axis_parent_xyz=rotation_axis, + phase_reference_point_parent_xyz=circle["center_xyz_m"], + view_normal_common_xyz=view_normal_common_xyz, + ) + ) + point_common = parent_rotation.apply(point_parent) + parent_translation state = np.median( np.asarray([record["state_u8"] for record in zero_records], dtype=float), axis=0, @@ -480,6 +858,20 @@ def fit_joint_axis_measurement( plane_rms_m=float(plane_rms), radial_rms_m=float(circle["radial_rms_m"]), rotation_circle_axis_difference_rad=float(disagreement), + view_normal_common_xyz=( + None + if view_normal_common_xyz is None + else tuple( + float(value) + for value in _vector( + view_normal_common_xyz, 3, name="view normal" + ) + ) + ), + axis_direction_source=axis_direction_source, + circle_axis_observability=float(circle_axis_observability), + axis_point_source=axis_point_source, + pose_axis_line_rms_m=pose_axis_line_rms_m, ) @@ -616,6 +1008,12 @@ class ZeroSolveResult: validation_line_error_by_joint_m: Mapping[str, float] axis_line_rms_m: float passed: bool + cycle_offsets_rad: Mapping[str, tuple[float, ...]] + offset_uncertainty_rad: Mapping[str, float] + validation_original_error_by_joint_rad: Mapping[str, float] + validation_improvement_by_joint_rad: Mapping[str, float] + validation_improvement_confidence_lower_rad: Mapping[str, float] + failure_reasons: Mapping[str, str] def _angles_from_state( @@ -623,11 +1021,12 @@ def _angles_from_state( *, curves: Mapping[str, JointCurveFit], motor_by_joint: Mapping[str, int], + inherited_zero_joints: Mapping[str, str] = INHERITED_ZERO_JOINTS, ) -> 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) + source = inherited_zero_joints.get(joint, joint) if source not in curves: continue command = int(np.clip(np.rint(state[int(motor)]), 0, 255)) @@ -644,327 +1043,839 @@ def solve_urdf_zero_offsets( training_cycles: Sequence[int] = (0, 1), validation_cycle: int = 2, maximum_offset_rad: float = math.radians(20.0), + finger_maximum_offset_rad: float = math.radians(3.0), + joint_maximum_offset_rad: Mapping[str, float] | None = None, + maximum_cycle_difference_rad: float = math.radians(0.75), + minimum_applied_offset_rad: float = math.radians(0.3), + significance_sigma: float = 3.0, maximum_validation_mae_rad: float = math.radians(1.0), maximum_validation_p95_rad: float = math.radians(2.0), + maximum_axis_cone_mismatch_rad: float = math.radians(5.0), + maximum_pose_axis_line_rms_m: float = 0.001, + hand_type: str = "left", + fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, + static_output_zero_offsets_rad: Mapping[str, float] | None = None, ) -> ZeroSolveResult: + """Solve all directly observable zero offsets without fitting CAD errors. + + The palm pose uses the most observable root-axis direction together with + the separation of the two root lines. Encoder zeros are then fitted from + one angular residual per joint: downstream-axis direction for non-parallel + pairs and the radial phase between parallel axis lines. This prevents link + length, along-axis Tag placement, and monocular depth bias from being + absorbed as an encoder-zero correction. + """ + profile = get_zero_calibration_profile(hand_type) 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: + expected = set(profile.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") + if not 0.0 < finger_maximum_offset_rad <= maximum_offset_rad: + raise ValueError("finger maximum offset must be positive and no larger than thumb") + joint_limits = { + str(name): float(value) + for name, value in dict(joint_maximum_offset_rad or {}).items() + } + unknown_joint_limits = sorted(set(joint_limits) - set(profile.direct_zero_joints)) + if unknown_joint_limits: + raise ValueError( + "joint maximum offsets contain unknown direct joints: " + + ",".join(unknown_joint_limits) + ) + if any( + not math.isfinite(value) or not 0.0 < value <= math.radians(90.0) + for value in joint_limits.values() + ): + raise ValueError("joint maximum offsets must be finite and in (0, 90deg]") + if maximum_cycle_difference_rad <= 0.0: + raise ValueError("maximum cycle difference must be positive") + if minimum_applied_offset_rad < 0.0 or significance_sigma < 0.0: + raise ValueError("zero significance thresholds must be non-negative") + if maximum_axis_cone_mismatch_rad <= 0.0: + raise ValueError("maximum axis cone mismatch must be positive") + if maximum_pose_axis_line_rms_m <= 0.0: + raise ValueError("maximum pose axis-line RMS must be positive") def predicted_local( measurement: JointAxisMeasurement, offsets: Mapping[str, float], ) -> tuple[np.ndarray, np.ndarray]: angles = _angles_from_state( - measurement.condition_state_u8, + ( + measurement.condition_state_u8 + if measurement.condition_command_u8 is None + else measurement.condition_command_u8 + ), curves=curves, motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, ) 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. + fixed_offsets = { + str(name): float(value) + for name, value in ( + profile.fixed_direct_zero_offsets_rad + if fixed_direct_zero_offsets_rad is None + else fixed_direct_zero_offsets_rad + ).items() + } + unknown_fixed_offsets = sorted( + set(fixed_offsets) - set(profile.direct_zero_joints) + ) + if unknown_fixed_offsets: + raise ValueError( + "fixed direct zero offsets contain unknown joints: " + + ",".join(unknown_fixed_offsets) + ) + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in fixed_offsets.values() + ): + raise ValueError( + "fixed direct zero offsets must be finite and within +/-90deg" + ) + output_offsets = { + str(name): float(value) + for name, value in ( + profile.static_output_zero_offsets_rad + if static_output_zero_offsets_rad is None + else static_output_zero_offsets_rad + ).items() + } + unknown_output_offsets = sorted( + set(output_offsets) - set(profile.direct_zero_joints) + ) + if unknown_output_offsets: + raise ValueError( + "static output zero offsets contain unknown joints: " + + ",".join(unknown_output_offsets) + ) + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in output_offsets.values() + ): + raise ValueError( + "static output zero offsets must be finite and within +/-90deg" + ) + zero_offsets = {name: 0.0 for name in profile.direct_zero_joints} + zero_offsets.update(fixed_offsets) 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() + def fit_base_pose( + selected: Sequence[JointAxisMeasurement], + ) -> tuple[Rotation, np.ndarray]: + anchors_by_joint = { + name: [item for item in selected if item.joint == name] + for name in profile.root_anchor_joints } - offsets.update( - { - name: float(value) - for name, value in zip( - OPTIMIZED_ZERO_JOINTS, parameters[6:] - ) - } - ) - return offsets + if any(not items for items in anchors_by_joint.values()): + raise ValueError("zero solve is missing root axis anchors") - 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() + def undirected_axis_average( + values: Sequence[Sequence[float]], + ) -> np.ndarray: + vectors = np.asarray(values, dtype=float) + reference = vectors[0] / np.linalg.norm(vectors[0]) + signs = np.where(vectors @ reference < 0.0, -1.0, 1.0) + result = np.sum(vectors * signs[:, None], axis=0) + norm = float(np.linalg.norm(result)) + if norm < 1.0e-9: + raise ValueError("root axis average is degenerate") + return result / norm + + # The two CAD root axes are parallel, so their directions alone cannot + # resolve palm rotation about that common direction. Do not use the + # 3-D separation of two monocularly fitted axis lines for this purpose: + # a fixed optical-depth error rotates the fitted palm frame and appears + # as an equally stable thumb-roll zero bias. Four-finger static splay + # is deliberately fixed to the source-CAD straight pose, so the + # measured reference-finger MCP-pitch direction is the independent, + # non-parallel orientation anchor. Root lines remain translation-only + # anchors. + root_names = tuple(sorted(profile.root_anchor_joints)) + primary = max( + root_names, + key=lambda name: float(np.ptp(np.asarray(curves[name].angle_rad))), + ) + secondary = next(name for name in root_names if name != primary) + orientation_anchor = f"{profile.reference_finger}_mcp_pitch" + orientation_items = [ + item for item in selected if item.joint == orientation_anchor + ] + if not orientation_items: + raise ValueError( + f"zero solve is missing palm orientation anchor {orientation_anchor}" + ) + + predicted_axes: dict[str, np.ndarray] = {} + predicted_points: dict[str, np.ndarray] = {} + observed_axes: dict[str, np.ndarray] = {} + observed_points: dict[str, np.ndarray] = {} + for name in root_names: + items = anchors_by_joint[name] + predicted = [predicted_local(item, zero_offsets) for item in items] + predicted_axes[name] = undirected_axis_average( + [axis for axis, _ in predicted] + ) + predicted_points[name] = np.median( + np.asarray([point for _, point in predicted]), axis=0 + ) + observed_axes[name] = undirected_axis_average( + [item.axis_common_xyz for item in items] + ) + observed_points[name] = np.median( + np.asarray( + [item.point_common_xyz_m for item in items], dtype=float + ), + axis=0, + ) + + predicted_orientation_axis = undirected_axis_average( + [predicted_local(item, zero_offsets)[0] for item in orientation_items] + ) + observed_orientation_axis = undirected_axis_average( + [item.axis_common_xyz for item in orientation_items] + ) + + def frame_from_two_axes( + primary_axis: np.ndarray, orientation_axis: np.ndarray + ) -> np.ndarray: + first = primary_axis / np.linalg.norm(primary_axis) + second = orientation_axis - first * float(orientation_axis @ first) + second_norm = float(np.linalg.norm(second)) + if second_norm < math.sin(ZERO_MINIMUM_AXIS_CONE_RAD): + raise ValueError("palm orientation axes are nearly parallel") + second /= second_norm + return np.column_stack((first, second, np.cross(first, second))) + + predicted_frame = frame_from_two_axes( + predicted_axes[primary], + predicted_orientation_axis, + ) + candidates: list[tuple[float, Rotation, np.ndarray]] = [] + for primary_sign in (1.0, -1.0): + for orientation_sign in (1.0, -1.0): + observed_frame = frame_from_two_axes( + primary_sign * observed_axes[primary], + orientation_sign * observed_orientation_axis, ) - 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, + rotation = Rotation.from_matrix( + observed_frame @ predicted_frame.T + ) + translation = np.median( + np.asarray( + [ + observed_points[name] + - rotation.apply(predicted_points[name]) + for name in root_names + ] + ), + axis=0, + ) + + # Resolve both undirected-axis sign branches using every + # distinct measured joint. This selects a palm-frame branch; + # it does not fit link geometry or encoder offsets. + errors_by_joint: dict[str, list[float]] = {} + for item in selected: + predicted_axis, _ = predicted_local(item, zero_offsets) + predicted_axis = rotation.apply(predicted_axis) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + error = math.acos( + abs( + float( + np.clip( + predicted_axis @ observed_axis, -1.0, 1.0 + ) + ) + ) + ) + errors_by_joint.setdefault(item.joint, []).append(error) + branch_score = sum( + min(float(np.median(values)), math.radians(30.0)) ** 2 + for values in errors_by_joint.values() + ) + candidates.append((branch_score, rotation, translation)) + _, rotation, translation = min(candidates, key=lambda item: item[0]) + return rotation, translation + + def phase_error( + predicted_parent_axis: np.ndarray, + predicted_parent_point: np.ndarray, + predicted_child_point: np.ndarray, + observed_parent_axis: np.ndarray, + observed_parent_point: np.ndarray, + observed_child_point: np.ndarray, + view_normal_common: Sequence[float] | None = None, + ) -> float: + parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis) + observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis) + if float(parent_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + predicted_delta = predicted_child_point - predicted_parent_point + observed_delta = observed_child_point - observed_parent_point + predicted_radial = predicted_delta - parent_axis * float( + predicted_delta @ parent_axis + ) + observed_radial = observed_delta - observed_axis * float( + observed_delta @ observed_axis + ) + angle_axis = parent_axis + if view_normal_common is not None: + view_normal = np.asarray(view_normal_common, dtype=float) + normal_norm = float(np.linalg.norm(view_normal)) + if normal_norm > 1.0e-9: + view_normal /= normal_norm + # When looking approximately along the rotation axis, image + # x/y contains the complete radial phase while optical depth + # is both unnecessary and much noisier for a 16 mm planar + # Tag. For an edge-on axis the depth component is genuinely + # needed for observability, so retain the full 3-D residual. + if abs(float(parent_axis @ view_normal)) >= math.cos( + math.radians(45.0) + ): + # Orthographic image-plane displacement is invariant to + # independent optical-depth bias on the two planar Tags. + # With an end-on axis, along-axis placement also projects + # to zero (or a small component for a mildly oblique view). + predicted_radial = predicted_delta - view_normal * float( + predicted_delta @ view_normal + ) + observed_radial = observed_delta - view_normal * float( + observed_delta @ view_normal + ) + angle_axis = view_normal + if float(angle_axis @ parent_axis) < 0.0: + angle_axis = -angle_axis + predicted_radius = float(np.linalg.norm(predicted_radial)) + observed_radius = float(np.linalg.norm(observed_radial)) + if min(predicted_radius, observed_radius) < 0.003: + raise ValueError("parallel-axis radial phase is not observable") + predicted_radial /= predicted_radius + observed_radial /= observed_radius + return math.atan2( + float(angle_axis @ np.cross(predicted_radial, observed_radial)), + float(np.clip(predicted_radial @ observed_radial, -1.0, 1.0)), + ) + + def robust_circular_location(values: Sequence[float]) -> float: + array = np.asarray(values, dtype=float) + if array.size == 0: + raise ValueError("circular residual set is empty") + centre = math.atan2( + float(np.mean(np.sin(array))), + float(np.mean(np.cos(array))), + ) + centred = np.arctan2(np.sin(array - centre), np.cos(array - centre)) + return float( + math.atan2( + math.sin(centre + float(np.median(centred))), + math.cos(centre + float(np.median(centred))), + ) + ) + + def angular_error_samples( + offsets: Mapping[str, float], + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + ) -> dict[str, tuple[float, ...]]: + by_key = {(item.joint, item.cycle): item for item in selected} + errors: dict[str, list[float]] = { + name: [] for name in profile.direct_zero_joints + } + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + observer_items = [ + item for item in selected if item.joint == observer_joint + ] + if not observer_items: + raise ValueError(f"zero observer is missing: {observer_joint}") + for item in observer_items: + observer_axis, observer_point = predicted_local(item, offsets) + observer_axis = base_rotation.apply(observer_axis) + observer_point = ( + base_rotation.apply(observer_point) + base_translation + ) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + if observer_joint in profile.axis_parent_joint: + parent_joint = profile.axis_parent_joint[observer_joint] + angles = _angles_from_state( + ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_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 + inherited_zero_joints=profile.inherited_zero_joints, ) - / 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) + parent_axis, _ = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis = base_rotation.apply(parent_axis) + error = _zero_sensitive_axis_error_rad( + observer_axis, observed_axis, parent_axis + ) + else: + parent_joint = profile.phase_parent_joint[observer_joint] + observed_parent = by_key.get((parent_joint, item.cycle)) + if observed_parent is None: + raise ValueError( + f"phase parent is missing: {parent_joint} cycle {item.cycle}" + ) + angles = _angles_from_state( + ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_u8 + ), + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + parent_axis, parent_point = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis = base_rotation.apply(parent_axis) + parent_point = ( + base_rotation.apply(parent_point) + base_translation + ) + error = phase_error( + parent_axis, + parent_point, + observer_point, + np.asarray(observed_parent.axis_common_xyz, dtype=float), + np.asarray(observed_parent.point_common_xyz_m, dtype=float), + np.asarray(item.point_common_xyz_m, dtype=float), + item.view_normal_common_xyz, + ) + errors[offset_joint].append(float(error)) + return { + name: tuple(values) + for name, values in errors.items() + if values } - if final_bound_hits: - details = ", ".join( - f"{name}={value:+.3f}deg" - for name, value in sorted(final_bound_hits.items()) + + def angular_errors( + offsets: Mapping[str, float], + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + ) -> dict[str, float]: + return { + name: robust_circular_location(values) + for name, values in angular_error_samples( + offsets, selected, base_rotation, base_translation + ).items() + } + + offset_limits = np.asarray( + [ + joint_limits.get( + name, + ( + finger_maximum_offset_rad + if name.startswith(("index_", "middle_", "ring_", "pinky_")) + else maximum_offset_rad + ), ) - all_details = ", ".join( - f"{name}={math.degrees(value):+.3f}deg" - for name, value in sorted(offsets.items()) + for name in profile.direct_zero_joints + ], + dtype=float, + ) + # Do not make a configured safety limit the numerical optimizer's bound. + # Otherwise a genuine out-of-range estimate and a modelling failure both + # collapse to exactly +/-20 or +/-3 degrees, which hides the magnitude and + # encourages pointless rescans. Search farther for diagnostics, then keep + # the original configured limits as unchanged pass/fail gates below. + diagnostic_offset_limits = np.minimum( + math.radians(90.0), + np.maximum(3.0 * offset_limits, offset_limits + math.radians(5.0)), + ) + + def optimise_offsets( + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + initial: Mapping[str, float] | None = None, + ) -> dict[str, float]: + # The observer graph is a pair of serial chains. Solve it in that + # dependency order so a bad distal phase cannot pull an already + # observable proximal zero toward a bound. Each scalar residual keeps + # the individual cycle values; robust loss then downweights one noisy + # cycle instead of hiding it in a seven-variable compromise. + result = {name: 0.0 for name in profile.direct_zero_joints} + if initial is not None: + result.update( + { + name: float(initial[name]) + for name in profile.direct_zero_joints + } ) - raise ValueError( - "final URDF zero offset reached the configured " - f"{math.degrees(maximum_offset_rad):.3f} degree bound: " - f"{details}; all_offsets: {all_details}" + result.update(fixed_offsets) + for name, limit in zip( + profile.direct_zero_joints, diagnostic_offset_limits + ): + if name in fixed_offsets: + continue + def residual(value: np.ndarray) -> np.ndarray: + candidate = dict(result) + candidate[name] = float(value[0]) + samples = angular_error_samples( + candidate, selected, base_rotation, base_translation + )[name] + return np.asarray( + [ + math.atan2(math.sin(item), math.cos(item)) + / math.radians(1.0) + for item in samples + ], + dtype=float, + ) + + starts = [0.0, -0.5 * limit, 0.5 * limit] + if initial is not None: + starts.append(float(initial[name])) + solutions = [ + least_squares( + residual, + np.asarray( + [np.clip(start, -limit + 1.0e-9, limit - 1.0e-9)] + ), + bounds=(np.asarray([-limit]), np.asarray([limit])), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + for start in starts + ] + solution = min( + solutions, key=lambda item: float(np.sum(np.square(item.fun))) ) - 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] + if not solution.success: + raise ValueError( + f"URDF zero optimization failed for {name}: " + f"{solution.message}" + ) + result[name] = float(solution.x[0]) + return result + + def solve_selected( + selected: Sequence[JointAxisMeasurement], + initial: Mapping[str, float] | None = None, + ) -> tuple[Rotation, np.ndarray, dict[str, float]]: + rotation, translation = fit_base_pose(selected) + offsets = optimise_offsets( + selected, rotation, translation, initial=initial + ) + return rotation, translation, offsets + + base_rotation, base_translation, training_offsets = solve_selected(training) + + def zero_observation_failure_reasons( + selected: Sequence[JointAxisMeasurement], + offsets: Mapping[str, float], + rotation: Rotation, + ) -> dict[str, str]: + """Reject repeatable but geometrically inadmissible zero observers.""" + by_key = {(item.joint, item.cycle): item for item in selected} + failures: dict[str, str] = {} + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + if offset_joint in fixed_offsets: + continue + observer_items = [ + item for item in selected if item.joint == observer_joint + ] + if observer_joint in profile.axis_parent_joint: + parent_joint = profile.axis_parent_joint[observer_joint] + maximum_cone_mismatch = 0.0 + for item in observer_items: + state = ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_u8 + ) + angles = _angles_from_state( + state, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=( + profile.inherited_zero_joints + ), + ) + predicted_axis, _ = model.axis_line( + observer_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis, _ = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + maximum_cone_mismatch = max( + maximum_cone_mismatch, + _axis_cone_mismatch_rad( + rotation.apply(predicted_axis), + item.axis_common_xyz, + rotation.apply(parent_axis), + ), + ) + if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad: + failures[offset_joint] = ( + "zero_axis_cone_mismatch_too_large" + ) + else: + parent_joint = profile.phase_parent_joint[observer_joint] + phase_items: list[JointAxisMeasurement] = [] + for item in observer_items: + phase_items.append(item) + parent_item = by_key.get((parent_joint, item.cycle)) + if parent_item is not None: + phase_items.append(parent_item) + if any( + item.pose_axis_line_rms_m + > maximum_pose_axis_line_rms_m + for item in phase_items + ): + failures[offset_joint] = ( + "zero_phase_axis_line_residual_too_large" + ) + return failures + + observation_failures = zero_observation_failure_reasons( + list(measurements), training_offsets, base_rotation + ) + cycle_values: dict[str, list[float]] = { + name: [] for name in profile.direct_zero_joints + } + cycle_models: dict[int, tuple[Rotation, np.ndarray]] = {} + all_cycles = sorted({int(item.cycle) for item in measurements}) + for cycle in all_cycles: + selected = [item for item in measurements if item.cycle == cycle] + # Keep one palm pose while comparing cycles. Refitting a base pose from + # only two nearly parallel root axes per cycle makes harmless root-line + # noise appear as a large encoder-zero change. + cycle_rotation, cycle_translation = base_rotation, base_translation + cycle_offsets = optimise_offsets( + selected, + cycle_rotation, + cycle_translation, + initial=training_offsets, + ) + cycle_models[cycle] = (cycle_rotation, cycle_translation) + for name, value in cycle_offsets.items(): + cycle_values[name].append(value) + uncertainties: dict[str, float] = {} + cycle_consistent = True + inconsistent_cycles: list[str] = [] + for name, values in cycle_values.items(): + array = np.asarray(values, dtype=float) + spread = float(np.max(array) - np.min(array)) + if spread > maximum_cycle_difference_rad: + cycle_consistent = False + inconsistent_cycles.append(name) + uncertainties[name] = ( + 0.0 + if array.size < 2 + else float(np.std(array, ddof=1) / math.sqrt(array.size)) + ) + + insignificant_large: list[str] = [] + applied_training: dict[str, float] = {} + for name, value in training_offsets.items(): + if name in fixed_offsets: + applied_training[name] = fixed_offsets[name] + continue + uncertainty = uncertainties[name] + if ( + abs(value) < minimum_applied_offset_rad + or abs(value) <= significance_sigma * uncertainty + ): + applied_training[name] = 0.0 + if abs(value) >= minimum_applied_offset_rad: + insignificant_large.append(name) + else: + applied_training[name] = value + + candidate_errors = angular_errors( + applied_training, validation, base_rotation, base_translation + ) + original_errors = angular_errors( + zero_offsets, validation, base_rotation, base_translation + ) + validation_error_by_joint: dict[str, float] = {} + original_error_by_joint: dict[str, float] = {} + improvement_by_joint: dict[str, float] = {} + improvement_confidence_lower: dict[str, float] = {} + improvement_passed = True + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + if offset_joint in fixed_offsets: + improvement_by_joint[offset_joint] = 0.0 + improvement_confidence_lower[offset_joint] = 0.0 + continue + candidate = abs(candidate_errors[offset_joint]) + original = abs(original_errors[offset_joint]) + validation_error_by_joint[observer_joint] = candidate + original_error_by_joint[observer_joint] = original + improvement_by_joint[offset_joint] = original - candidate + if applied_training[offset_joint] != 0.0 and not candidate < original: + improvement_passed = False + for offset_joint in profile.direct_zero_joints: + if offset_joint in fixed_offsets: + improvement_confidence_lower[offset_joint] = 0.0 + continue + if applied_training[offset_joint] == 0.0: + improvement_confidence_lower[offset_joint] = 0.0 + continue + cycle_improvements: list[float] = [] + for cycle in all_cycles: + selected = [item for item in measurements if item.cycle == cycle] + cycle_rotation, cycle_translation = cycle_models[cycle] + candidate = abs( + angular_errors( + applied_training, + selected, + cycle_rotation, + cycle_translation, + )[offset_joint] + ) + original = abs( + angular_errors( + zero_offsets, + selected, + cycle_rotation, + cycle_translation, + )[offset_joint] + ) + cycle_improvements.append(original - candidate) + improvement_array = np.asarray(cycle_improvements, dtype=float) + bootstrap_indices = generator.integers( + 0, + improvement_array.size, + size=(2000, improvement_array.size), + ) + bootstrap_means = np.mean( + improvement_array[bootstrap_indices], axis=1 + ) + lower = float(np.percentile(bootstrap_means, 2.5)) + improvement_confidence_lower[offset_joint] = lower + if lower <= 0.0: + improvement_passed = False + validation_errors = np.asarray( + list(validation_error_by_joint.values()), dtype=float + ) + configured_limit_exceeded = [ + name + for name, limit in zip(profile.direct_zero_joints, offset_limits) + if name not in fixed_offsets + and abs(training_offsets[name]) > limit + math.radians(0.01) + ] + diagnostic_bound_hits = [ + name + for name, limit in zip( + profile.direct_zero_joints, diagnostic_offset_limits + ) + if name not in fixed_offsets + and abs(training_offsets[name]) >= limit - math.radians(0.01) + ] + failure_reasons: dict[str, str] = {} + for name in configured_limit_exceeded: + failure_reasons[name] = "zero_offset_exceeds_configured_limit" + for name in diagnostic_bound_hits: + failure_reasons[name] = "zero_offset_reached_diagnostic_bound" + for name in inconsistent_cycles: + failure_reasons[name] = "zero_offset_cycle_difference_too_large" + for name in insignificant_large: + failure_reasons[name] = "zero_offset_not_statistically_significant" + if not improvement_passed: + for name, value in applied_training.items(): + if value != 0.0 and improvement_confidence_lower.get(name, 0.0) <= 0.0: + failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence" + # Geometry failures are the root cause and must not be hidden by the + # downstream validation symptom produced by the same bad observation. + failure_reasons.update(observation_failures) + passed = bool( + validation_errors.size + == len(profile.direct_zero_joints) - len(fixed_offsets) + and float(np.mean(validation_errors)) <= maximum_validation_mae_rad + and float(np.percentile(validation_errors, 95.0)) + <= maximum_validation_p95_rad + and cycle_consistent + and not insignificant_large + and not configured_limit_exceeded + and not diagnostic_bound_hits + and not observation_failures + and improvement_passed + ) + + final_offsets = dict(applied_training) + if passed: + base_rotation, base_translation, all_offsets_fit = solve_selected( + list(measurements), initial=training_offsets + ) + final_offsets = { + name: ( + fixed_offsets[name] + if name in fixed_offsets + else 0.0 + if ( + abs(value) < minimum_applied_offset_rad + or abs(value) <= significance_sigma * uncertainties[name] + ) + else value + ) + for name, value in all_offsets_fit.items() + } + # Apply independently validated assembly datums only after trajectory + # fitting and holdout validation. A mechanical prior must define the + # written artifact without perturbing downstream yaw/pitch estimates. + final_offsets.update(output_offsets) + + # Every active joint must be present in the runtime payload/URDF writer, + # but absence of an absolute observation is not evidence for the + # reference finger's assembly offset. Preserve the source-CAD zero for + # those independent motors while continuing to share their dynamic curve. + all_offsets = { + name: 0.0 for name in profile.hand.active_joints + } + all_offsets.update(final_offsets) + for target, source in profile.inherited_static_zero_joints.items(): + all_offsets[target] = final_offsets[source] return ZeroSolveResult( - direct_offsets_rad=offsets, + direct_offsets_rad=final_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, + validation_line_error_by_joint_m={}, + axis_line_rms_m=0.0, passed=passed, + cycle_offsets_rad={ + name: tuple(float(value) for value in values) + for name, values in cycle_values.items() + }, + offset_uncertainty_rad=uncertainties, + validation_original_error_by_joint_rad=original_error_by_joint, + validation_improvement_by_joint_rad=improvement_by_joint, + validation_improvement_confidence_lower_rad=( + improvement_confidence_lower + ), + failure_reasons=failure_reasons, ) @@ -975,8 +1886,6 @@ def write_zero_corrected_urdf( 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() @@ -989,12 +1898,24 @@ def write_zero_corrected_urdf( raise ValueError( "source_urdf must be the original CAD URDF, not a calibrated URDF" ) + if not offsets_rad: + raise ValueError("offsets_rad must contain at least one joint") + offsets = {str(name): float(value) for name, value in offsets_rad.items()} + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in offsets.values() + ): + raise ValueError("URDF zero offsets must be finite and within +/-90deg") output.mkdir(parents=True, exist_ok=True) stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") + if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: + raise ValueError("URDF zero timestamp must use YYYYMMDD_HHMMSS") safe_serial = "".join( character if character.isalnum() or character in "_.-" else "_" for character in str(serial_number) ) + if not safe_serial: + raise ValueError("serial_number must not be empty") 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}") @@ -1002,71 +1923,37 @@ def write_zero_corrected_urdf( 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: + if name not in offsets: 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}", + if joint.get("type") not in {"revolute", "continuous"}: + raise ValueError(f"joint {name} is not revolute") + 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_norm = float(np.linalg.norm(axis)) + if axis_norm <= 1.0e-12: + raise ValueError(f"joint {name} has a degenerate axis") + axis /= axis_norm + 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 * offsets[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 + ) found.add(name) - missing = sorted((set(offsets_rad) | set(range_targets)) - found) + missing = sorted(set(offsets) - found) if missing: raise ValueError("source URDF is missing target joints: " + ",".join(missing)) joint_pattern = re.compile( @@ -1077,48 +1964,20 @@ def write_zero_corrected_urdf( 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: + if name not in replacement_rpy: 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) + 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])) + expected_edit_count = len(replacement_rpy) if len(edits) != expected_edit_count: raise ValueError("could not locate every target joint field in source URDF text") corrected_text = original_text diff --git a/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py b/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py new file mode 100644 index 0000000..25a4a46 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/launch/calibrated_joint_state_bridge.launch.py @@ -0,0 +1,37 @@ +"""Publish calibrated G20 URDF joint angles from raw u8 commands.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription( + [ + DeclareLaunchArgument("hand_type", default_value="right"), + DeclareLaunchArgument("calibration_file"), + DeclareLaunchArgument("input_topic", default_value=""), + DeclareLaunchArgument("output_topic", default_value=""), + Node( + package="g20_thumb_apriltag_calibration", + executable="calibrated_joint_state_bridge", + name=[ + "g20_calibrated_joint_state_bridge_", + LaunchConfiguration("hand_type"), + ], + output="screen", + emulate_tty=True, + parameters=[ + { + "hand_type": LaunchConfiguration("hand_type"), + "calibration_file": LaunchConfiguration( + "calibration_file" + ), + "input_topic": LaunchConfiguration("input_topic"), + "output_topic": LaunchConfiguration("output_topic"), + } + ], + ), + ] + ) 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 eb11bb1..515a176 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 @@ -25,7 +25,34 @@ from launch_ros.parameter_descriptions import ParameterValue VIEWS = ("front", "side", "top") +def _default_source_urdf(hand_type: str) -> Path: + relative = Path( + "assets/robots/hands/linker_hand" + ) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf" + workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative + try: + installed = Path(get_package_share_directory("linkerhand_retarget")) / relative + except Exception: + installed = workspace + return workspace if workspace.is_file() else installed + + def _launch_stack(context): + hand_type = LaunchConfiguration("hand_type").perform(context).lower() + if hand_type not in {"left", "right"}: + raise RuntimeError("hand_type must be left or right") + command_topic = f"/g20/cb_{hand_type}_hand_control_cmd" + state_topic = f"/g20/cb_{hand_type}_hand_state" + info_topic = f"/g20/cb_{hand_type}_hand_info" + requested_source = LaunchConfiguration("source_urdf_path").perform(context) + source_urdf = ( + Path(requested_source).expanduser().resolve() + if requested_source + else _default_source_urdf(hand_type).resolve() + ) + if not source_urdf.is_file(): + raise RuntimeError(f"source URDF does not exist: {source_urdf}") + hand_serial = LaunchConfiguration("serial_number").perform(context) if ( not hand_serial @@ -170,7 +197,7 @@ def _launch_stack(context): condition=IfCondition(LaunchConfiguration("start_sdk")), parameters=[ { - "hand_type": "left", + "hand_type": hand_type, "hand_joint": "G20", "can": LaunchConfiguration("can_interface"), "modbus": "None", @@ -182,6 +209,15 @@ def _launch_stack(context): "startup_torque": 80, # Match 30 Hz cameras so state/image p95 skew stays below 50 ms. "state_poll_rate": 30.0, + # Calibration does not consume measured joint velocity. A + # G20 velocity read sends another five synchronous CAN + # queries, so keep it off the trajectory-critical path. + "velocity_poll_rate": 1.0, + # Calibration sends one endpoint command per sweep. Keep + # polling the real motor state during the following motion; + # otherwise the SDK republishes a stale state for 0.2 s and + # creates 17-27 command-unit holes in the trajectory bins. + "defer_state_reads_while_commanding": False, "repeat_position_commands": False, "is_touch": False, } @@ -197,11 +233,19 @@ def _launch_stack(context): LaunchConfiguration("calibration_config"), { "serial_number": hand_serial, + "hand_type": hand_type, "session_dir": str(session_dir), + # The SDK performs roughly 25 synchronous CAN queries whenever + # cb__hand_info has a subscriber. Calibration only used + # that topic to display a speed diagnostic, while those reads + # created 17-33 command-unit holes in position trajectories. + "info_topic": "/g20_calibration/disabled_hand_info", + "command_topic": command_topic, + "state_topic": state_topic, "camera_extrinsics_file": LaunchConfiguration( "camera_extrinsics_file" ), - "source_urdf_path": LaunchConfiguration("source_urdf_path"), + "source_urdf_path": str(source_urdf), "corrected_urdf_output_dir": LaunchConfiguration( "corrected_urdf_output_dir" ), @@ -246,15 +290,20 @@ def _launch_stack(context): *raw_topics, *info_topics, *detection_topics, - "/g20/cb_left_hand_control_cmd", - "/g20/cb_left_hand_state", - "/g20/cb_left_hand_info", + command_topic, + state_topic, + info_topic, "/g20_calibration/status", ], output="screen", ) return [ - LogInfo(msg=f"G20 three-camera session: {session_dir}"), + LogInfo( + msg=( + f"G20 {hand_type} three-camera session: {session_dir}; " + f"source_urdf={source_urdf}" + ) + ), LogInfo( msg=( "Camera mapping: front=" @@ -275,31 +324,24 @@ 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( [ + # Camera processes publish ~2 MB frames across DDS. Force the + # matching RMW and provide both current and legacy profile names + # so the configured 64 MB shared-memory segment is actually used. + SetEnvironmentVariable( + name="RMW_IMPLEMENTATION", + value="rmw_fastrtps_cpp", + ), + SetEnvironmentVariable( + name="FASTDDS_DEFAULT_PROFILES_FILE", + value=str(package_share / "config" / "fastdds_large_images.xml"), + ), SetEnvironmentVariable( name="FASTRTPS_DEFAULT_PROFILES_FILE", value=str(package_share / "config" / "fastdds_large_images.xml"), ), + DeclareLaunchArgument("hand_type", default_value="left"), DeclareLaunchArgument("serial_number", default_value="UNSET"), DeclareLaunchArgument( "front_camera_serial", default_value="DB2163742" @@ -353,7 +395,7 @@ def generate_launch_description() -> LaunchDescription: ), ), DeclareLaunchArgument( - "source_urdf_path", default_value=str(default_source_urdf) + "source_urdf_path", default_value="" ), DeclareLaunchArgument( "corrected_urdf_output_dir", default_value="" 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 index 503eb57..29429d3 100644 --- a/src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/three_camera_extrinsics.launch.py @@ -112,6 +112,12 @@ def _launch(context): LaunchConfiguration("maximum_reprojection_rms_px"), value_type=float, ), + "maximum_single_camera_reprojection_rms_px": ParameterValue( + LaunchConfiguration( + "maximum_single_camera_reprojection_rms_px" + ), + value_type=float, + ), "auto_capture_default": ParameterValue( LaunchConfiguration("auto_capture_default"), value_type=bool, @@ -137,6 +143,18 @@ def generate_launch_description() -> LaunchDescription: camera_info = Path.home() / ".ros" / "camera_info" return LaunchDescription( [ + # Keep the large-image transport deterministic even when the + # calling shell selected another ROS 2 RMW implementation. + SetEnvironmentVariable( + name="RMW_IMPLEMENTATION", + value="rmw_fastrtps_cpp", + ), + SetEnvironmentVariable( + name="FASTDDS_DEFAULT_PROFILES_FILE", + value=str( + package_share / "config" / "fastdds_large_images.xml" + ), + ), SetEnvironmentVariable( name="FASTRTPS_DEFAULT_PROFILES_FILE", value=str( @@ -164,6 +182,10 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument( "maximum_reprojection_rms_px", default_value="1.2" ), + DeclareLaunchArgument( + "maximum_single_camera_reprojection_rms_px", + default_value="1.5", + ), DeclareLaunchArgument("auto_capture_default", default_value="false"), DeclareLaunchArgument( "auto_capture_stable_seconds", default_value="1.0" diff --git a/src/g20_thumb_apriltag_calibration/setup.py b/src/g20_thumb_apriltag_calibration/setup.py index f70e788..c14c7a9 100644 --- a/src/g20_thumb_apriltag_calibration/setup.py +++ b/src/g20_thumb_apriltag_calibration/setup.py @@ -51,10 +51,19 @@ setup( "three_camera_extrinsics_node = " "g20_thumb_apriltag_calibration.extrinsics_node:main" ), + ( + "offline_replay = " + "g20_thumb_apriltag_calibration.offline_replay:main" + ), ( "camera_alignment_view = " "g20_thumb_apriltag_calibration.alignment_view:main" ), + ( + "calibrated_joint_state_bridge = " + "g20_thumb_apriltag_calibration." + "calibrated_joint_state_bridge:main" + ), ], }, ) diff --git a/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py b/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py new file mode 100644 index 0000000..9b2ead7 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_calibrated_joint_state_bridge.py @@ -0,0 +1,87 @@ +import copy + +import pytest + +from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import ( + G20_COMMAND_NAMES, + G20_URDF_JOINT_NAMES, + CalibratedCommandMapper, +) +from g20_thumb_apriltag_calibration.full_hand import ( + get_hand_calibration_profile, +) + + +def _payload(side: str = "right") -> dict: + profile = get_hand_calibration_profile(side) + curve = [round((255 - command) * 0.001, 8) for command in range(256)] + joints = {} + for name, spec in profile.joint_specs.items(): + joint = { + "motor_index": spec.motor_index, + "angle_rad": list(curve), + } + if spec.active: + joint["zero_command_u8"] = 255 + joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0} + else: + joint["passive"] = True + if spec.source_joint is not None: + joint["source_joint"] = spec.source_joint + joints[name] = joint + return { + "schema_version": 4, + "model": "G20", + "side": side, + "serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT", + "angle_unit": "rad", + "command_range": [0, 255], + "baseline_command_u8": [255] * 20, + "joints": joints, + "quality": { + "passed": True, + "validation_mae_rad": 0.01, + "validation_p95_rad": 0.02, + }, + } + + +def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None: + mapper = CalibratedCommandMapper(_payload(), expected_side="right") + command = list(range(20)) + result = dict( + zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command)) + ) + + assert result["thumb_cmc_pitch"] == pytest.approx(0.255) + assert result["thumb_cmc_roll"] == pytest.approx(0.250) + assert result["thumb_cmc_yaw"] == pytest.approx(0.245) + assert result["thumb_mcp"] == pytest.approx(0.240) + assert result["thumb_ip"] == pytest.approx(0.240) + assert result["pinky_pip"] == pytest.approx(0.236) + assert result["pinky_dip"] == pytest.approx(0.236) + + +def test_mapper_uses_names_instead_of_message_order() -> None: + mapper = CalibratedCommandMapper(_payload(), expected_side="right") + command = list(range(20)) + names = list(reversed(G20_COMMAND_NAMES)) + positions = list(reversed(command)) + + assert mapper.map_positions(positions, names) == mapper.map_positions(command) + + +def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None: + with pytest.raises(ValueError, match="does not match"): + CalibratedCommandMapper(_payload("left"), expected_side="right") + + payload = copy.deepcopy(_payload()) + payload["quality"]["passed"] = False + with pytest.raises(ValueError, match="quality.passed"): + CalibratedCommandMapper(payload, expected_side="right") + + +def test_mapper_rejects_incomplete_named_command() -> None: + mapper = CalibratedCommandMapper(_payload(), expected_side="right") + with pytest.raises(ValueError, match="missing named channels"): + mapper.map_positions([255.0], ["thumb_cmc_pitch"]) diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index 0038144..f28b4d9 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/src/g20_thumb_apriltag_calibration/test/test_config.py @@ -25,6 +25,18 @@ def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None: assert participant.attrib["is_default_profile"] == "true" +def test_three_camera_launches_force_fastdds_large_image_transport() -> None: + for launch_name in ( + "three_camera_calibration.launch.py", + "three_camera_extrinsics.launch.py", + ): + launch_text = (PACKAGE_ROOT / "launch" / launch_name).read_text() + assert 'name="RMW_IMPLEMENTATION"' in launch_text + assert 'value="rmw_fastrtps_cpp"' in launch_text + assert 'name="FASTDDS_DEFAULT_PROFILES_FILE"' in launch_text + assert 'name="FASTRTPS_DEFAULT_PROFILES_FILE"' in launch_text + + def test_front_tag_parameters_match_namespaced_detector() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "front_tags.yaml").read_text() @@ -43,7 +55,7 @@ def test_front_tag_parameters_match_namespaced_detector() -> None: assert detector["detector"]["debug"] is False -def test_three_camera_tag_ids_and_topics_are_disjoint() -> None: +def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None: tags = yaml.safe_load( (PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text() ) @@ -63,6 +75,9 @@ def test_three_camera_tag_ids_and_topics_are_disjoint() -> None: assert parameters["detector"]["decimate"] == 1.5 all_ids.update(ids) assert all_ids == set(range(11)) + assert tags[ + "/g20_calibration/side/apriltag/apriltag" + ]["ros__parameters"]["tag"]["frames"][0] == "side_base" def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: @@ -99,25 +114,41 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["normal_calibration_speed"] == 15 assert parameters["index_roll_calibration_speed"] == 5 assert parameters["index_flex_calibration_speed"] == 10 + assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0 assert parameters["speed_setting_settle_seconds"] >= 0.2 assert parameters["top_pnp_invalid_reset_seconds"] == 1.0 + assert parameters["pnp_group_initialization_frames"] == 8 + assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0 + assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0 assert parameters["repetitions"] == 3 assert parameters["validation_enabled"] is False assert parameters["minimum_detection_rate"] == 0.95 assert parameters["minimum_detection_hz"] == 15.0 assert parameters["minimum_state_span_u8"] >= 240.0 assert parameters["endpoint_tolerance_u8"] == 2.0 - assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 3.0 + assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0 + assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0 assert parameters["minimum_sweep_bins"] >= 32 assert parameters["maximum_bin_gap"] <= 16 + assert parameters["automatic_sweep_retry_limit"] == 3 + assert parameters["automatic_fit_retry_limit"] == 2 + assert parameters["motor_stall_timeout_seconds"] >= 5.0 + assert parameters["motor_stall_minimum_progress_u8"] == 1.0 + assert parameters["automatic_motion_retry_limit"] == 2 + assert parameters["provisional_warning_ratio"] == 1.25 + assert parameters["retry_speed_scales"] == [0.8, 0.6, 0.5] + assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0, 1.25] assert parameters["position_timeout_seconds"] >= 20.0 assert parameters["maximum_state_image_skew_ms"] <= 50.0 assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0 + assert parameters["axis_maximum_pose_line_rms_m"] <= 0.001 assert parameters["passive_axis_maximum_plane_rms_m"] == 0.004 assert parameters["active_maximum_rotation_orthogonal_rms_deg"] == 2.5 assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5 assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75 + assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0 assert parameters["zero_maximum_offset_deg"] <= 20.0 + assert parameters["zero_finger_maximum_offset_deg"] <= 3.0 assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0 assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5 assert parameters["image_trajectory_minimum_radius_px"] >= 20.0 diff --git a/src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py b/src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py index 4352d5a..998b2e8 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py +++ b/src/g20_thumb_apriltag_calibration/test/test_extrinsics_node.py @@ -4,11 +4,14 @@ import cv2 import numpy as np from scipy.spatial.transform import Rotation +import g20_thumb_apriltag_calibration.extrinsics_node as extrinsics_node from g20_thumb_apriltag_calibration.extrinsics_node import ( BoardPose, StereoCapture, _fit_stereo_robust, + _individual_reprojection_passes, _minimum_history_skew_ns, + _pose_is_novel, _select_latest_synchronised_pair, _summarize_transform_repeatability, ) @@ -100,6 +103,135 @@ def test_repeatability_summary_exposes_current_capture_outlier(): assert translation_error > 0.0015 +def test_each_camera_must_pass_reprojection_gate_independently(): + assert _individual_reprojection_passes(0.8, 1.1, 1.2) + assert not _individual_reprojection_passes(0.5, 1.4, 1.2) + assert _individual_reprojection_passes(0.5, 1.4, 1.5) + assert not _individual_reprojection_passes(0.5, 1.6, 1.5) + assert not _individual_reprojection_passes(float("nan"), 0.5, 1.2) + + +def test_pose_novelty_is_checked_against_all_previous_captures(): + previous = [_transform(0.0, 0.0), _transform(10.0, 0.10)] + + assert not _pose_is_novel( + _transform(10.5, 0.105), + previous, + minimum_rotation_rad=np.deg2rad(2.0), + minimum_translation_m=0.015, + ) + assert _pose_is_novel( + _transform(14.0, 0.13), + previous, + minimum_rotation_rad=np.deg2rad(2.0), + minimum_translation_m=0.015, + ) + + +def test_joint_fit_trims_moderate_bad_views_without_relaxing_gate( + monkeypatch, +): + captures = [ + StereoCapture( + front_points_px=np.zeros((40, 2), dtype=np.float32), + other_points_px=np.zeros((40, 2), dtype=np.float32), + transform_candidates=(np.eye(4),), + pair_reprojection_rms_px=1.0, + skew_ns=1, + ) + for _ in range(20) + ] + + def fake_fit_once( + captures_arg, + indices, + object_points, + front_matrix, + other_matrix, + image_size, + ): + del ( + captures_arg, + object_points, + front_matrix, + other_matrix, + image_size, + ) + errors = np.asarray( + [1.1 if index < 15 else 1.5 for index in indices], + dtype=float, + ) + return float(np.sqrt(np.mean(np.square(errors)))), np.eye(4), errors + + monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once) + result = _fit_stereo_robust( + captures, + np.zeros((40, 3), dtype=np.float32), + np.eye(3), + np.eye(3), + (640, 480), + minimum_inliers=15, + maximum_rms_px=1.2, + maximum_rotation_stability_rad=np.deg2rad(0.3), + maximum_translation_stability_m=0.0015, + ) + + assert result.passed + assert result.stereo_rms_px <= 1.2 + assert len(result.inlier_indices) >= 15 + assert result.rejected_indices + assert set(result.rejected_indices) <= set(range(15, 20)) + + +def test_joint_fit_reports_finite_provisional_metrics_before_minimum( + monkeypatch, +): + captures = [ + StereoCapture( + front_points_px=np.zeros((40, 2), dtype=np.float32), + other_points_px=np.zeros((40, 2), dtype=np.float32), + transform_candidates=(np.eye(4),), + pair_reprojection_rms_px=0.8, + skew_ns=1, + ) + for _ in range(10) + ] + + def fake_fit_once( + captures_arg, + indices, + object_points, + front_matrix, + other_matrix, + image_size, + ): + del ( + captures_arg, + object_points, + front_matrix, + other_matrix, + image_size, + ) + return 0.8, np.eye(4), np.full(len(indices), 0.8) + + monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once) + result = _fit_stereo_robust( + captures, + np.zeros((40, 3), dtype=np.float32), + np.eye(3), + np.eye(3), + (640, 480), + minimum_inliers=15, + maximum_rms_px=1.2, + maximum_rotation_stability_rad=np.deg2rad(0.3), + maximum_translation_stability_m=0.0015, + ) + + assert not result.passed + assert result.stereo_rms_px == 0.8 + assert len(result.inlier_indices) == 10 + + def test_joint_stereo_fit_recovers_transform_and_rejects_bad_view(): random = np.random.default_rng(7) object_points = np.zeros((40, 3), dtype=np.float32) 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 b2d6e13..84f4808 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py +++ b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py @@ -8,8 +8,10 @@ from g20_thumb_apriltag_calibration.full_hand import ( ACTIVE_JOINTS, IMAGE_TRAJECTORY_JOINTS, JOINT_SPECS, + LEFT_HAND_PROFILE, MEASURED_JOINTS, PASSIVE_JOINTS, + RIGHT_HAND_PROFILE, SPLAY_JOINTS, SWEEP_SPECS, VIEW_TAGS, @@ -22,6 +24,7 @@ from g20_thumb_apriltag_calibration.full_hand import ( fit_joint_image_curve, fit_measured_joint_curve, fit_projected_zero, + get_hand_calibration_profile, measure_joint_observation, validate_compact_payload, ) @@ -70,6 +73,55 @@ def test_joint_layout_covers_16_active_and_5_passive_joints() -> None: assert len(JOINT_SPECS) == 21 assert len(ACTIVE_JOINTS) == 16 assert len(PASSIVE_JOINTS) == 5 + + +def test_right_profile_measures_pinky_and_inherits_to_other_fingers() -> None: + profile = get_hand_calibration_profile("right") + assert profile is RIGHT_HAND_PROFILE + assert profile.reference_finger == "pinky" + assert [spec.motor_index for spec in profile.sweep_specs] == [ + 0, 5, 15, 9, 4, 19, 10 + ] + assert profile.view_tags["front"]["pinky_roll"] == 10 + assert profile.view_tags["side"] == { + "side_base": 4, + "pinky_mcp": 5, + "pinky_pip": 6, + "pinky_dip": 7, + } + assert profile.preflight_view_roles["side"] == ( + "side_base", + "pinky_mcp", + "pinky_pip", + "pinky_dip", + ) + thumb_pitch = profile.joint_specs["thumb_cmc_pitch"] + assert thumb_pitch.view == "front" + assert thumb_pitch.parent_role == "front_base" + assert thumb_pitch.child_role == "thumb_cmc" + assert profile.joint_specs["index_mcp_roll"].source_joint == ( + "pinky_mcp_roll" + ) + assert profile.joint_specs["middle_mcp_pitch"].source_joint == ( + "pinky_mcp_pitch" + ) + assert profile.joint_specs["ring_pip"].source_joint == "pinky_pip" + assert profile.joint_specs["index_dip"].source_joint == "pinky_dip" + + roll = next(spec for spec in profile.sweep_specs if spec.motor_index == 9) + command = build_calibration_motion_command( + roll, 127, profile=profile + ) + assert command[9] == 127 + assert command[6:9] == [255, 255, 255] + speeds = build_calibration_speed_profile( + roll, + normal_speed=15, + index_roll_speed=5, + index_flex_speed=10, + profile=profile, + ) + assert speeds == [15, 15, 15, 15, 5] assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set( range(11) ) @@ -122,6 +174,20 @@ def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None: assert result[10:] == baseline[10:] +def test_right_pinky_roll_moves_other_three_fingers_camera_right() -> None: + profile = RIGHT_HAND_PROFILE + pinky_roll = next( + spec for spec in profile.sweep_specs if spec.motor_index == 9 + ) + + result = build_calibration_motion_command( + pinky_roll, 27, profile=profile + ) + + assert result[6:10] == [255, 255, 255, 27] + assert profile.roll_clearance_commands == {6: 255, 7: 255, 8: 255} + + def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None: baseline = [255] * 20 baseline[6:10] = [127, 127, 127, 127] @@ -133,6 +199,33 @@ def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None: assert result[6:10] == [127, 127, 127, 127] +def test_right_thumb_pitch_uses_front_visible_yaw_and_roll_pose() -> None: + profile = RIGHT_HAND_PROFILE + thumb_pitch = next( + spec for spec in profile.sweep_specs if spec.motor_index == 0 + ) + + result = build_calibration_motion_command( + thumb_pitch, 17, profile=profile + ) + + assert result[0] == 17 + assert result[10] == 255 + assert result[5] == 255 + assert profile.thumb_pitch_clearance_commands == {10: 255, 5: 255} + + +def test_left_thumb_pitch_keeps_legacy_baseline_pose() -> None: + thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0) + + result = build_calibration_motion_command(thumb_pitch, 17) + + assert result[0] == 17 + assert result[5] == 255 + assert result[10] == 255 + assert LEFT_HAND_PROFILE.thumb_pitch_clearance_commands == {} + + def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None: baseline = [255] * 20 baseline[6:10] = [127, 127, 127, 127] @@ -342,3 +435,66 @@ def test_compact_payload_allows_skipped_random_validation() -> None: "validation_mae_rad": None, "validation_p95_rad": None, } + + +def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None: + profile = RIGHT_HAND_PROFILE + base = fit_joint_center_curve(_records()) + splay, zero_command, _ = center_splay_curve(base) + splay = replace( + splay, + angle_rad=tuple( + value - splay.angle_rad[zero_command] + for value in splay.angle_rad + ), + ) + measured = { + name: splay if name == "pinky_mcp_roll" else base + for name in profile.measured_joints + } + offsets = { + name: (0.01 if profile.joint_specs[name].source_joint is None else 0.0) + for name in profile.active_joints + } + baseline = [255] * 20 + baseline[6:10] = [zero_command] * 4 + payload = build_compact_payload( + serial_number="G20_RIGHT_001", + measured_fits=measured, + urdf_zero_offsets_rad=offsets, + validation_errors_rad=[0.01], + passed=True, + baseline=baseline, + side="right", + ) + + validate_compact_payload(payload) + assert payload["schema_version"] == 4 + assert payload["side"] == "right" + assert set(payload) == { + "schema_version", + "model", + "side", + "serial_number", + "angle_unit", + "command_range", + "baseline_command_u8", + "joints", + "quality", + } + for finger in ("index", "middle", "ring"): + assert payload["joints"][f"{finger}_mcp_roll"]["source_joint"] == ( + "pinky_mcp_roll" + ) + assert payload["joints"][f"{finger}_mcp_pitch"]["source_joint"] == ( + "pinky_mcp_pitch" + ) + assert payload["joints"][f"{finger}_pip"]["source_joint"] == ( + "pinky_pip" + ) + for suffix in ("mcp_roll", "mcp_pitch", "pip"): + joint = payload["joints"][f"{finger}_{suffix}"] + source = payload["joints"][joint["source_joint"]] + assert joint["angle_rad"] == source["angle_rad"] + assert joint["zero_angles"] == {"urdf_zero_offset_rad": 0.0} + assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01} diff --git a/src/g20_thumb_apriltag_calibration/test/test_offline_replay.py b/src/g20_thumb_apriltag_calibration/test/test_offline_replay.py new file mode 100644 index 0000000..6173ee2 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_offline_replay.py @@ -0,0 +1,44 @@ +import pytest + +from g20_thumb_apriltag_calibration.offline_replay import ( + _latest_attempt_records, + _output_suffix, +) + + +def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict: + return { + "kind": "sample", + "joint": joint, + "cycle": cycle, + "direction": direction, + "attempt": attempt, + } + + +def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None: + rows = [ + {"kind": "session_start"}, + _sample("pinky_pip", 0, "decreasing", 1), + _sample("pinky_pip", 0, "decreasing", 3), + _sample("pinky_pip", 0, "increasing", 1), + _sample("pinky_pip", 1, "decreasing", 2), + _sample("thumb_cmc_yaw", 0, "decreasing", 1), + ] + + selected = _latest_attempt_records(rows) + + assert [ + record["attempt"] for record in selected["pinky_pip"] + ] == [3, 1, 2] + assert [ + record["attempt"] for record in selected["thumb_cmc_yaw"] + ] == [1] + + +def test_output_suffix_is_safe_and_explicit() -> None: + assert _output_suffix(None) == "" + assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2" + for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65): + with pytest.raises(ValueError, match="output tag"): + _output_suffix(invalid) diff --git a/src/g20_thumb_apriltag_calibration/test/test_pnp.py b/src/g20_thumb_apriltag_calibration/test/test_pnp.py index ceaa826..1e6c7f8 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_pnp.py +++ b/src/g20_thumb_apriltag_calibration/test/test_pnp.py @@ -324,6 +324,125 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None: assert selected == {"t4": return_t4, "t5": return_t5} +def test_group_tracker_initializes_from_multiple_static_frames() -> None: + tracker = SquareTagGroupPoseTracker( + roles=("parent", "child"), + adjacent_pairs=(("parent", "child"),), + maximum_pose_jump_rad=np.deg2rad(35.0), + maximum_translation_jump_m=0.04, + relative_rotation_scale_rad=np.deg2rad(5.0), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=5.0, + initialization_frames=8, + ) + parent = _pose(0.0, 0.00, 0.05) + true_child = _pose(20.0, 0.03, 0.10) + + for index in range(7): + false_child = _pose(5.0 * index, 0.03, 0.05) + selected, reason = tracker.select( + { + "parent": (parent,), + "child": (false_child, true_child), + }, + stamp_ns=1_000_000_000 + index * 33_000_000, + ) + assert selected is None + assert reason == f"group_initializing:{index + 1}/8" + + false_child = _pose(35.0, 0.03, 0.05) + selected, reason = tracker.select( + { + "parent": (parent,), + "child": (false_child, true_child), + }, + stamp_ns=1_231_000_000, + ) + + assert reason == "" + assert selected == {"parent": parent, "child": true_child} + assert ( + tracker.last_initialization_quality["initialization_search"] + == "static_reference" + ) + assert tracker.last_initialization_quality[ + "p95_pair_rotation_drift_rad" + ] < np.deg2rad(1.0) + + +def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None: + tracker = SquareTagGroupPoseTracker( + roles=("mcp", "pip", "dip"), + adjacent_pairs=(("mcp", "pip"), ("pip", "dip")), + normal_alignment_pairs=(("mcp", "pip"), ("pip", "dip")), + normal_alignment_scale_rad=np.deg2rad(5.0), + maximum_pose_jump_rad=np.deg2rad(35.0), + maximum_translation_jump_m=0.04, + relative_rotation_scale_rad=np.deg2rad(5.0), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=5.0, + initialization_frames=8, + maximum_normal_alignment_rad=np.deg2rad(15.0), + ) + mcp = _pose(12.0, 0.00, 0.05) + correct_pip = _pose(14.0, 0.03, 0.10) + mirror_pip = _pose(50.0, 0.03, 0.05) + dip = _pose(15.0, 0.06, 0.05) + + for index in range(8): + selected, reason = tracker.select( + { + "mcp": (mcp,), + "pip": (mirror_pip, correct_pip), + "dip": (dip,), + }, + stamp_ns=1_000_000_000 + index * 33_000_000, + ) + + assert reason == "" + assert selected == { + "mcp": mcp, + "pip": correct_pip, + "dip": dip, + } + assert tracker.last_initialization_quality[ + "maximum_normal_alignment_rad" + ] < np.deg2rad(3.0) + + +def test_static_group_rejects_when_no_aligned_branch_exists() -> None: + tracker = SquareTagGroupPoseTracker( + roles=("base", "moving"), + adjacent_pairs=(("base", "moving"),), + normal_alignment_pairs=(("base", "moving"),), + normal_alignment_scale_rad=np.deg2rad(5.0), + maximum_normal_alignment_rad=np.deg2rad(15.0), + maximum_pose_jump_rad=np.deg2rad(35.0), + maximum_translation_jump_m=0.04, + relative_rotation_scale_rad=np.deg2rad(5.0), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=5.0, + initialization_frames=8, + ) + base = _pose(0.0, 0.00, 0.05) + mirror = _pose(38.0, 0.03, 0.05) + + for index in range(8): + selected, reason = tracker.select( + {"base": (base,), "moving": (mirror,)}, + stamp_ns=1_000_000_000 + index * 33_000_000, + ) + + assert selected is None + assert reason == "group_normal_alignment" + + def test_whole_trajectory_recovers_rigid_group_from_mirror_drift() -> None: roles = ("t3", "t4", "t5") mount_rotations = { 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 9604b90..a956536 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 @@ -3,7 +3,9 @@ import json import numpy as np from g20_thumb_apriltag_calibration.three_camera_diagnostics import ( + _task_text, render_three_camera_status_text_zh, + three_camera_reason_zh, ) @@ -64,7 +66,7 @@ def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None: assert "缺少电机端点255" in text assert "实际电机范围为0.4～248.2" in text assert "第1/3轮，255→0" in text - assert "已完成6/42个扫描方向" in text + assert "计划扫描6/42个方向，扫描进度14.3%" in text assert "当前缺失Tag=2" in text assert "group_pose_jump" not in text assert "PnP拒绝" not in text @@ -135,6 +137,107 @@ def test_joint_fit_failure_names_metric_and_selective_retry() -> None: assert "运动采样：" not in text +def test_zero_model_failure_explains_that_rescan_will_not_help() -> None: + payload = { + "state": "PAUSED", + "reason": "zero_model_validation_failed", + "progress": 0.92, + "scan_progress": 1.0, + "completed_sweeps": 42, + "total_sweeps": 42, + "active": { + "kind": "zero_model_failure", + "view": "front", + "motor_index": 15, + "joints": ["thumb_mcp", "thumb_ip"], + "directions_to_rescan": 0, + "failures": [ + { + "joint": "thumb_mcp", + "metric": "zero_guard", + "reason": "zero_offset_exceeds_configured_limit", + "actual_deg": -39.81, + "limit_deg": 20.0, + } + ], + }, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "拇指MCP：零位估计超过安全范围" in text + assert "估计-39.81°，允许±20.00°" in text + assert "不会自动重扫" in text + assert "总体进度：92.0%" in text + assert "扫描进度100.0%" in text + assert "运动采样：" not in text + + +def test_sweep_status_shows_full_joint_fit_retry_attempt() -> None: + active = { + "kind": "sweep", + "view": "front", + "motor_index": 15, + "joints": ["thumb_mcp", "thumb_ip"], + "cycle": 1, + "repetitions": 3, + "start_u8": 255, + "target_u8": 0, + "fit_attempt": 2, + "fit_attempt_limit": 3, + } + + assert "整关节自动重采第2/3次" in _task_text(active) + + +def test_motor_stall_reason_is_explained_in_chinese() -> None: + reason, action = three_camera_reason_zh( + "PAUSED", + "motor_state_stalled:sweep_motor_19:error_u8=5.000", + {}, + ) + + assert "距目标5.000个u8" in reason + assert "机械端点" in action + assert "不要反复调用resume" in action + + +def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None: + payload = { + "state": "PAUSED", + "reason": ( + "motor_state_stalled:return_baseline:motor_index=10:" + "target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:" + "error_u8=5.000" + ), + "progress": 0.0, + "scan_progress": 0.0, + "completed_sweeps": 0, + "total_sweeps": 42, + "active": { + "kind": "motion_stall", + "stage": "return_baseline", + "motor_index": 10, + "target_u8": 255.0, + "actual_u8": 250.0, + "error_u8": 5.0, + "tolerance_u8": 4.0, + }, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "电机10反馈连续8秒" in text + assert "目标255.0、实际250.0、误差5.000 u8" in text + assert "允许容差±4.0 u8" in text + assert "当前任务：电机10运动停滞，目标255.0、实际250.0" in text + assert "运动采样：" not in text + + def test_return_baseline_prints_the_exact_command() -> None: baseline = [255] * 20 baseline[6:10] = [127, 127, 127, 127] @@ -152,10 +255,34 @@ def test_return_baseline_prints_the_exact_command() -> None: text = render_three_camera_status_text_zh(payload) - assert "正在返回基准姿态（RETURN_BASELINE）" in text + assert "正在恢复目标姿态（RETURN_BASELINE）" in text assert f"正在确认基准姿态：{baseline}" in text +def test_return_recovery_prints_thumb_yaw_start_and_clearance_pose() -> None: + baseline = [255] * 20 + baseline[6:10] = [127, 127, 127, 127] + recovery = list(baseline) + recovery[5] = 145 + recovery[10] = 0 + payload = { + "state": "RETURN_BASELINE", + "reason": "return_baseline_before_retry_sweep", + "progress": 0.5, + "completed_sweeps": 36, + "total_sweeps": 42, + "baseline_command_u8": baseline, + "return_command_u8": recovery, + "active": {}, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert f"正在确认恢复姿态：{recovery}" in text + + def test_status_numeric_diagnostics_are_json_serializable() -> None: bins = [0, 16, 255] payload = { @@ -214,6 +341,8 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None: "joints": ["index_mcp_roll"], "cycle": 1, "repetitions": 3, + "automatic_retry_count": 1, + "automatic_retry_limit": 2, "start_u8": 255, "target_u8": 0, "actual_u8": 44.0, @@ -241,3 +370,4 @@ def test_index_roll_status_prints_clearance_motor_feedback() -> None: assert "电机9目标0、实际0.0" in text assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text assert "SDK报告[15, 5, 15, 15, 15]" in text + assert "自动重试：当前方向已自动重扫1/2次" in text 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 b89b50c..72cf3d9 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 @@ -10,13 +10,17 @@ 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.full_hand import ( + RIGHT_HAND_PROFILE, + SWEEP_SPECS, +) from g20_thumb_apriltag_calibration.three_camera_node import ( FrameObservation, G20ThreeCameraCalibrationNode, STATE_PREPARE_SWEEP, STATE_SWEEP, SweepItem, + _overall_progress, ) @@ -45,6 +49,78 @@ def _frame(view: str, state_u8: list[float]) -> FrameObservation: ) +def test_right_thumb_pitch_requires_only_front_base_and_thumb_tag() -> None: + profile = RIGHT_HAND_PROFILE + spec = next(item for item in profile.sweep_specs if item.motor_index == 0) + runtime = SimpleNamespace( + roles=tuple(profile.view_tags["front"]), + preflight_roles=profile.preflight_view_roles["front"], + ) + node = SimpleNamespace( + profile=profile, + views={"front": runtime}, + active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), + active_validation=None, + retry_sweep_spec=None, + ) + + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "front" + ) + + assert required == ("front_base", "thumb_cmc") + + +def test_right_side_preflight_requires_only_pinky_chain_tags() -> None: + profile = RIGHT_HAND_PROFILE + runtime = SimpleNamespace( + roles=tuple(profile.view_tags["side"]), + preflight_roles=profile.preflight_view_roles["side"], + ) + node = SimpleNamespace( + profile=profile, + views={"side": runtime}, + active_sweep=None, + active_validation=None, + retry_sweep_spec=None, + ) + + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "side" + ) + + assert required == ("side_base", "pinky_mcp", "pinky_pip", "pinky_dip") + + +def test_right_pip_sweep_keeps_side_base_as_pnp_branch_anchor() -> None: + profile = RIGHT_HAND_PROFILE + spec = next( + item for item in profile.sweep_specs if item.motor_index == 19 + ) + runtime = SimpleNamespace( + roles=tuple(profile.view_tags["side"]), + preflight_roles=profile.preflight_view_roles["side"], + ) + node = SimpleNamespace( + profile=profile, + views={"side": runtime}, + active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), + active_validation=None, + retry_sweep_spec=None, + ) + + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "side" + ) + + assert required == ( + "side_base", + "pinky_mcp", + "pinky_pip", + "pinky_dip", + ) + + 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) @@ -98,6 +174,8 @@ def test_begin_sweep_carries_start_endpoint_frame_into_sweep() -> None: sweep_frames=[], sweep_start_frames=[endpoint_frame], _publish_command=lambda command: published.append(command), + _motion_command_error_u8=lambda selected, command: 255.0, + _reset_motion_progress=lambda now, error: None, ) G20ThreeCameraCalibrationNode._begin_active_sweep(node, 10.0) @@ -110,35 +188,124 @@ def test_begin_sweep_carries_start_endpoint_frame_into_sweep() -> None: assert published[0][7:10] == [0, 0, 0] -def test_only_thumb_yaw_uses_three_count_endpoint_deadband() -> None: +def test_joint_specific_zero_endpoint_deadbands() -> 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) + pinky_pip = next( + item for item in RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 19 + ) node = SimpleNamespace( endpoint_tolerance_u8=2.0, - thumb_yaw_zero_endpoint_tolerance_u8=3.0, + thumb_yaw_zero_endpoint_tolerance_u8=4.0, + right_thumb_yaw_255_endpoint_tolerance_u8=5.0, + pinky_pip_zero_endpoint_tolerance_u8=5.0, ) assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( node, yaw, 0 - ) == 3.0 + ) == 4.0 assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( node, yaw, 255 ) == 2.0 + node.profile = RIGHT_HAND_PROFILE + assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, yaw, 255 + ) == 5.0 assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( node, index_roll, 0 ) == 2.0 + assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, pinky_pip, 0 + ) == 5.0 + assert G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, pinky_pip, 255 + ) == 2.0 -def test_thumb_yaw_command_zero_accepts_feedback_three_only_for_swept_motor() -> None: +def test_pinky_pip_command_zero_accepts_firmware_feedback_five() -> None: + pinky_pip = next( + item for item in RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 19 + ) + state = [255.0] * 20 + state[19] = 5.0 + node = SimpleNamespace( + latest_state_u8=tuple(state), + baseline_command=tuple([255] * 20), + endpoint_tolerance_u8=2.0, + pinky_pip_zero_endpoint_tolerance_u8=5.0, + ) + node._endpoint_tolerance_for_spec = lambda spec, endpoint: ( + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, spec, endpoint + ) + ) + + assert G20ThreeCameraCalibrationNode._motion_command_reached( + node, pinky_pip, 0, tuple(state) + ) + state[19] = 6.0 + assert not G20ThreeCameraCalibrationNode._motion_command_reached( + node, pinky_pip, 0, tuple(state) + ) + + +def test_right_baseline_and_auxiliary_accept_thumb_yaw_feedback_250() -> None: + state = [253.0] * 20 + state[0] = 254.0 + state[5] = 254.0 + state[6:10] = [127.0] * 4 + state[10] = 250.0 + state[15] = 255.0 + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + node = SimpleNamespace( + profile=RIGHT_HAND_PROFILE, + latest_state_u8=tuple(state), + baseline_command=tuple(baseline), + endpoint_tolerance_u8=2.0, + thumb_yaw_zero_endpoint_tolerance_u8=4.0, + right_thumb_yaw_255_endpoint_tolerance_u8=5.0, + pinky_pip_zero_endpoint_tolerance_u8=5.0, + ) + node._motor_endpoint_tolerance = lambda motor, endpoint: ( + G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + node, motor, endpoint + ) + ) + node._endpoint_tolerance_for_spec = lambda spec, endpoint: ( + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + node, spec, endpoint + ) + ) + + assert G20ThreeCameraCalibrationNode._baseline_reached(node) + details = G20ThreeCameraCalibrationNode._baseline_error_details(node) + assert details["motor_index"] == 10 + assert details["actual_u8"] == 250.0 + assert details["tolerance_u8"] == 5.0 + + thumb_pitch = next( + spec for spec in RIGHT_HAND_PROFILE.sweep_specs + if spec.motor_index == 0 + ) + state[0] = 255.0 + assert G20ThreeCameraCalibrationNode._motion_command_reached( + node, thumb_pitch, 255, tuple(state) + ) + + +def test_thumb_yaw_command_zero_accepts_feedback_four_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[10] = 4.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, + thumb_yaw_zero_endpoint_tolerance_u8=4.0, ) node._endpoint_tolerance_for_spec = lambda spec, endpoint: ( G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( @@ -149,12 +316,128 @@ def test_thumb_yaw_command_zero_accepts_feedback_three_only_for_swept_motor() -> assert G20ThreeCameraCalibrationNode._motion_command_reached( node, yaw, 0, tuple(state) ) + state[10] = 5.0 + assert not G20ThreeCameraCalibrationNode._motion_command_reached( + node, yaw, 0, tuple(state) + ) + state[10] = 4.0 state[5] = 142.0 assert not G20ThreeCameraCalibrationNode._motion_command_reached( node, yaw, 0, tuple(state) ) +def test_thumb_yaw_direction_recovery_keeps_motor_five_at_clearance() -> None: + yaw = next( + item for item in RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 10 + ) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + for direction, expected_start in ( + (DIRECTION_DECREASING, 255), + (DIRECTION_INCREASING, 0), + ): + node = SimpleNamespace( + profile=RIGHT_HAND_PROFILE, + baseline_command=tuple(baseline), + active_sweep=SweepItem(yaw, 1, direction), + retry_sweep_items=[], + ) + + command = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "retry_sweep" + ) + + assert command[5] == 145 + assert command[10] == expected_start + assert all( + command[index] == baseline[index] + for index in range(20) + if index not in {5, 10} + ) + + +def test_thumb_yaw_cycle_and_manual_recovery_use_queued_direction_start() -> None: + yaw = next( + item for item in RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 10 + ) + other = next( + item for item in RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index != 10 + ) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + queued = SweepItem(yaw, 2, DIRECTION_INCREASING) + node = SimpleNamespace( + profile=RIGHT_HAND_PROFILE, + baseline_command=tuple(baseline), + active_sweep=SweepItem(other, 0, DIRECTION_DECREASING), + retry_sweep_items=[queued], + ) + + command = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "resume_sweep" + ) + + assert command[5] == 145 + assert command[10] == 0 + + +def test_thumb_yaw_clearance_is_released_only_for_normal_transition() -> None: + yaw = next( + item for item in RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 10 + ) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + node = SimpleNamespace( + profile=RIGHT_HAND_PROFILE, + baseline_command=tuple(baseline), + active_sweep=SweepItem(yaw, 2, DIRECTION_INCREASING), + retry_sweep_items=[], + ) + + command = G20ThreeCameraCalibrationNode._return_command_for_transition( + node, "fit" + ) + + assert command == tuple(baseline) + assert command[5] == 255 + + +def test_thumb_yaw_recovery_zero_accepts_feedback_four() -> None: + yaw = next( + item for item in RIGHT_HAND_PROFILE.sweep_specs + if item.motor_index == 10 + ) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + return_command = list(baseline) + return_command[5] = 145 + return_command[10] = 0 + state = [float(value) for value in return_command] + state[10] = 4.0 + node = SimpleNamespace( + profile=RIGHT_HAND_PROFILE, + baseline_command=tuple(baseline), + return_command_u8=tuple(return_command), + latest_state_u8=tuple(state), + endpoint_tolerance_u8=2.0, + thumb_yaw_zero_endpoint_tolerance_u8=4.0, + right_thumb_yaw_255_endpoint_tolerance_u8=5.0, + ) + + assert G20ThreeCameraCalibrationNode._baseline_reached(node) + details = G20ThreeCameraCalibrationNode._motion_command_error_details( + node, yaw, 0, "return_baseline" + ) + assert details["motor_index"] == 10 + assert details["actual_u8"] == 4.0 + assert details["tolerance_u8"] == 4.0 + + def _image_cycle_records( travels_rad: list[float], *, depth_slope: float = 0.0 ) -> list[dict]: @@ -225,6 +508,7 @@ def _fit_check_node(records_by_joint: dict) -> SimpleNamespace: axis_maximum_plane_rms_m=0.003, passive_axis_maximum_plane_rms_m=0.004, axis_maximum_radial_rms_m=0.003, + axis_maximum_pose_line_rms_m=0.001, 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), @@ -241,7 +525,7 @@ def _fit_check_node(records_by_joint: dict) -> SimpleNamespace: return node -def test_fit_failure_rewinds_to_failed_specs_first_direction(tmp_path) -> None: +def test_fit_failure_preserves_main_progress_and_selects_retry_scope(tmp_path) -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) node = SimpleNamespace( sweep_items=_sweep_items(), @@ -272,17 +556,97 @@ def test_fit_failure_rewinds_to_failed_specs_first_direction(tmp_path) -> None: ], ) - assert node.sweep_index == 18 + assert node.sweep_index == 24 assert node.retry_sweep_spec == spec + assert node.retry_cycles == {0, 1, 2} assert node.fit_failure["directions_to_rescan"] == 6 assert node.paused_reason == "joint_fit_check_failed" +def test_zero_model_failure_never_schedules_an_automatic_rescan(tmp_path) -> None: + pauses: list[str] = [] + node = SimpleNamespace( + profile=RIGHT_HAND_PROFILE, + raw_path=tmp_path / "raw_samples.jsonl", + maximum_validation_p95_rad=math.radians(2.0), + zero_maximum_offset_rad=math.radians(20.0), + zero_joint_maximum_offsets_rad={}, + retry_sweep_spec=RIGHT_HAND_PROFILE.sweep_specs[2], + retry_resume_index=42, + retry_cycles={0, 1, 2}, + fit_failure={}, + _pause=lambda reason: pauses.append(reason), + ) + result = SimpleNamespace( + failure_reasons={ + "thumb_mcp": "zero_offset_exceeds_configured_limit" + }, + direct_offsets_rad={"thumb_mcp": math.radians(-39.81)}, + validation_error_by_joint_rad={ + "thumb_ip": math.radians(23.5) + }, + ) + + G20ThreeCameraCalibrationNode._pause_for_zero_model_failure(node, result) + + assert pauses == ["zero_model_validation_failed"] + assert node.retry_sweep_spec is None + assert node.retry_resume_index is None + assert node.retry_cycles == set() + assert node.fit_failure["recoverable_by_rescan"] is False + assert node.fit_failure["directions_to_rescan"] == 0 + assert node.fit_failure["failures"][0]["actual_deg"] == -39.81 + assert node.fit_failure["failures"][0]["limit_deg"] == 20.0 + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["kind"] == "zero_model_failure" + + +def test_resume_rejects_zero_model_failure_without_starting_motion() -> None: + node = SimpleNamespace( + state="PAUSED", + paused_reason="zero_model_validation_failed", + ) + response = SimpleNamespace(success=None, message="") + + result = G20ThreeCameraCalibrationNode._resume_callback( + node, SimpleNamespace(), response + ) + + assert result.success is False + assert "cannot repair" in result.message + + +def test_scan_completion_is_not_reported_as_calibration_completion() -> None: + assert _overall_progress("SWEEP", scan_progress=1.0) == 0.90 + assert _overall_progress("FITTING", scan_progress=1.0) == 0.92 + assert np.isclose( + _overall_progress( + "VALIDATION_CAPTURE", + scan_progress=1.0, + validation_index=5, + validation_total=10, + ), + 0.955, + ) + assert _overall_progress("COMPLETE", scan_progress=1.0) == 1.0 + + def test_resume_discards_only_failed_specs_samples(tmp_path) -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) + reset_runtimes = [] + front_runtime = SimpleNamespace( + pnp_invalid_since=12.0, + pnp_reset_count=0, + ) records = { - "thumb_mcp": [{"old": "mcp"}], - "thumb_ip": [{"old": "ip"}], + "thumb_mcp": [ + {"cycle": 0, "keep": "mcp"}, + {"cycle": 1, "old": "mcp"}, + ], + "thumb_ip": [ + {"cycle": 0, "keep": "ip"}, + {"cycle": 1, "old": "ip"}, + ], "thumb_cmc_pitch": [{"keep": True}], } node = SimpleNamespace( @@ -292,17 +656,25 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None: raw_path=tmp_path / "raw_samples.jsonl", paused_reason="joint_fit_check_failed", fit_failure={"failures": ["old"]}, + retry_cycles={1}, + repetitions=3, + sweep_items=_sweep_items(), + views={"front": front_runtime}, + _reset_view_trackers=lambda runtime: reset_runtimes.append(runtime), ) selected = G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node) assert selected == spec - assert records["thumb_mcp"] == [] - assert records["thumb_ip"] == [] + assert records["thumb_mcp"] == [{"cycle": 0, "keep": "mcp"}] + assert records["thumb_ip"] == [{"cycle": 0, "keep": "ip"}] assert records["thumb_cmc_pitch"] == [{"keep": True}] assert node.sweep_attempts[15] == 2 assert node.retry_sweep_spec == spec assert node.fit_failure == {} + assert reset_runtimes == [front_runtime] + assert front_runtime.pnp_invalid_since is None + assert front_runtime.pnp_reset_count == 1 event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) assert event == { "kind": "retry", @@ -311,9 +683,195 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None: "joints": ["thumb_mcp", "thumb_ip"], "attempt": 2, "reason": "joint_fit_check_failed", + "cycles": [2], } +def test_recoverable_sweep_failure_retries_three_times_before_pause(tmp_path) -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) + item = SweepItem(spec, 0, DIRECTION_INCREASING) + transitions: list[str] = [] + pauses: list[str] = [] + holds: list[bool] = [] + node = SimpleNamespace( + active_sweep=item, + automatic_sweep_retry_limit=3, + sweep_retry_counts={}, + raw_path=tmp_path / "raw_samples.jsonl", + reason="", + _publish_hold_current=lambda: holds.append(True), + _begin_return_baseline=lambda after: transitions.append(after), + _pause=lambda reason: pauses.append(reason), + retry_speed_scales=(0.8, 0.6, 0.5), + retry_endpoint_hold_seconds=(0.75, 1.0, 1.25), + ) + + G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( + node, "sweep_bin_gap_too_large" + ) + G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( + node, "sweep_bin_gap_too_large" + ) + G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( + node, "sweep_bin_gap_too_large" + ) + G20ThreeCameraCalibrationNode._retry_active_sweep_or_pause( + node, "sweep_bin_gap_too_large" + ) + + assert transitions == ["retry_sweep", "retry_sweep", "retry_sweep"] + assert pauses == ["sweep_bin_gap_too_large"] + assert holds == [] + assert node.sweep_retry_counts[(6, 0, DIRECTION_INCREASING)] == 3 + events = [ + json.loads(line) + for line in (tmp_path / "raw_samples.jsonl").read_text().splitlines() + ] + assert [event["retry"] for event in events] == [1, 2, 3] + + +def test_first_provisional_fit_failure_retries_automatically(tmp_path) -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) + transitions: list[str] = [] + pauses: list[str] = [] + prepared: list[bool] = [] + node = SimpleNamespace( + sweep_items=_sweep_items(), + sweep_attempts={spec.motor_index: 1}, + retry_sweep_spec=None, + retry_resume_index=None, + fit_failure={}, + sweep_index=18, + raw_path=tmp_path / "raw_samples.jsonl", + repetitions=3, + automatic_fit_retry_limit=1, + paused_reason="", + reason="", + _sweep_spec_start_index=lambda selected: ( + G20ThreeCameraCalibrationNode._sweep_spec_start_index(node, selected) + ), + _prepare_failed_sweep_retry=lambda: prepared.append(True), + _begin_return_baseline=lambda after: transitions.append(after), + _pause=lambda reason: pauses.append(reason), + ) + + G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, + spec, + [{"joint": "thumb_ip", "metric": "rotation_orthogonal_rms_deg"}], + ) + + assert prepared == [True] + assert transitions == ["resume_sweep"] + assert pauses == [] + assert node.paused_reason == "joint_fit_check_failed" + + +def test_near_threshold_provisional_failure_warns_without_retry(tmp_path) -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) + node = SimpleNamespace( + raw_path=tmp_path / "raw_samples.jsonl", + provisional_warning_ratio=1.25, + reason="", + ) + handled = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, + spec, + [ + { + "joint": "index_mcp_roll", + "metric": "hysteresis_deg", + "actual": 5.5, + "limit": 5.0, + "comparison": "maximum", + } + ], + allow_warning=True, + ) + + assert handled is False + assert node.reason == "provisional_fit_warning_continuing" + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["kind"] == "provisional_fit_warning" + + +def test_motion_timeout_republishes_twice_before_pause(tmp_path) -> None: + commands: list[list[int]] = [] + pauses: list[str] = [] + holds: list[bool] = [] + node = SimpleNamespace( + motion_retry_counts={}, + automatic_motion_retry_limit=2, + raw_path=tmp_path / "raw_samples.jsonl", + state="RETURN_BASELINE", + baseline_command=tuple([255] * 20), + position_hold_since=1.0, + reason="", + _publish_hold_current=lambda: holds.append(True), + _publish_speed_profile=lambda profile: None, + _normal_speed_profile=lambda: [15] * 5, + _publish_command=lambda command: commands.append(command), + _pause=lambda reason: pauses.append(reason), + _baseline_error_u8=lambda: 50.0, + _reset_motion_progress=lambda now, error: None, + ) + + for now in (10.0, 20.0, 30.0): + G20ThreeCameraCalibrationNode._retry_motion_or_pause( + node, "return_baseline_timeout", now + ) + + assert len(commands) == 2 + assert holds == [] + assert pauses == ["return_baseline_timeout"] + + +def test_motion_stall_pauses_without_consuming_sweep_retries(tmp_path) -> None: + pauses: list[str] = [] + node = SimpleNamespace( + raw_path=tmp_path / "raw_samples.jsonl", + latest_state_u8=tuple([18.0] + [255.0] * 19), + motion_progress_reference_error_u8=18.0, + motion_last_progress_at=10.0, + motor_stall_timeout_seconds=8.0, + motor_stall_minimum_progress_u8=1.0, + _pause=lambda reason: pauses.append(reason), + _reset_motion_progress=lambda now, error: None, + ) + + assert not G20ThreeCameraCalibrationNode._pause_if_motion_stalled( + node, now=17.9, error_u8=18.0, context="sweep_motor_0" + ) + assert G20ThreeCameraCalibrationNode._pause_if_motion_stalled( + node, now=18.0, error_u8=18.0, context="sweep_motor_0" + ) + + assert pauses == ["motor_state_stalled:sweep_motor_0:error_u8=18.000"] + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["kind"] == "mechanical_motion_stall" + assert event["context"] == "sweep_motor_0" + + +def test_slower_sweep_retry_extends_timeout_inversely() -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) + item = SweepItem(spec, 0, DIRECTION_DECREASING) + key = (spec.motor_index, 0, DIRECTION_DECREASING) + node = SimpleNamespace( + active_sweep=item, + sweep_timeout_seconds=90.0, + sweep_retry_counts={key: 1}, + retry_speed_scales=(0.8, 0.6, 0.5), + ) + + assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds( + node + ) == 112.5 + node.sweep_retry_counts[key] = 3 + assert G20ThreeCameraCalibrationNode._active_sweep_timeout_seconds( + node + ) == 180.0 + + def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None: spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) node = _fit_check_node( @@ -379,6 +937,8 @@ def test_thumb_ip_axis_uses_same_cycle_thumb_mcp_direction() -> None: upstream = node._fit_axis_measurement("thumb_mcp", 0) passive = node._fit_axis_measurement("thumb_ip", 0) + assert upstream.condition_command_u8 is not None + assert upstream.condition_command_u8[15] == 255.0 assert abs( float( np.asarray(upstream.axis_common_xyz) diff --git a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py index 9c62507..e44bc79 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py @@ -22,6 +22,7 @@ from g20_thumb_apriltag_calibration.urdf_zero import ( fit_joint_axis_measurement, fit_rotation_joint_curve, solve_urdf_zero_offsets, + get_zero_calibration_profile, write_zero_corrected_urdf, ) from g20_thumb_apriltag_calibration.full_hand import ( @@ -30,6 +31,7 @@ from g20_thumb_apriltag_calibration.full_hand import ( MEASURED_JOINTS, PASSIVE_JOINTS, JointCurveFit, + get_hand_calibration_profile, ) @@ -38,6 +40,10 @@ SOURCE_URDF = REPOSITORY / ( "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/" "linker_hand/g20_left/linkerhand_g20_left.urdf" ) +RIGHT_SOURCE_URDF = REPOSITORY / ( + "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/" + "linker_hand/g20_right/linkerhand_g20_right.urdf" +) def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch(): @@ -58,6 +64,18 @@ def test_zero_sensitive_axis_error_ignores_fixed_cone_angle_mismatch(): ) == pytest.approx(3.0, abs=1.0e-9) +def test_zero_sensitive_axis_error_is_exact_for_an_oblique_cone() -> None: + parent = np.asarray([0.0, 0.0, 1.0]) + cone = math.radians(32.0) + phase = math.radians(7.0) + predicted = np.asarray([math.sin(cone), 0.0, math.cos(cone)]) + observed = Rotation.from_rotvec(parent * phase).apply(predicted) + + error = _zero_sensitive_axis_error_rad(predicted, observed, parent) + + assert math.degrees(error) == pytest.approx(7.0, abs=1.0e-9) + + def _payload(transform: np.ndarray) -> dict[str, list[float]]: return { "translation_xyz_m": transform[:3, 3].tolist(), @@ -139,7 +157,10 @@ def test_axis_and_curve_ignore_camera_and_tag_mount_rotation() -> None: 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: +@pytest.mark.parametrize("joint", ["thumb_cmc_pitch", "index_mcp_pitch"]) +def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth( + joint: str, +) -> None: records, expected_axis, expected_point = _arbitrary_tag_records() parent_rotation = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) axis_parent = parent_rotation.inv().apply(expected_axis) @@ -159,7 +180,7 @@ def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth() -> None biased_records.append(biased) measurement = fit_joint_axis_measurement( - "thumb_cmc_pitch", biased_records, cycle=0, zero_command_u8=255 + joint, biased_records, cycle=0, zero_command_u8=255 ) observed_axis = np.asarray(measurement.axis_common_xyz) @@ -175,6 +196,44 @@ def test_image_plane_joint_uses_rotation_axis_to_constrain_noisy_depth() -> None assert measurement.radial_rms_m < 0.003 +def test_pose_axis_point_rejects_end_on_optical_depth_bias() -> None: + records, expected_axis, expected_point = _arbitrary_tag_records() + common_from_parent = Rotation.from_euler("xyz", [-0.8, 0.55, 1.3]) + axis_parent = common_from_parent.inv().apply(expected_axis) + # Exact end-on depth is a gauge along the physical axis and therefore + # cannot alter the observable axis line. An oblique camera has a small + # irreducible coupling between monocular depth and radial position; that + # case must be bounded by the residual/holdout gates, not asserted to be + # exactly recoverable from one view. + view_normal_parent = axis_parent + view_normal_common = common_from_parent.apply(view_normal_parent) + biased = [] + for record in records: + changed = dict(record) + fraction = (255.0 - float(record["command_u8"])) / 255.0 + depth_bias = 0.03 * (fraction - 0.5) + changed["relative_translation_xyz_m"] = ( + np.asarray(record["relative_translation_xyz_m"], dtype=float) + + depth_bias * view_normal_parent + ).tolist() + biased.append(changed) + + measurement = fit_joint_axis_measurement( + "thumb_cmc_pitch", + biased, + cycle=0, + zero_command_u8=255, + view_normal_common_xyz=view_normal_common, + ) + + observed_point = np.asarray(measurement.point_common_xyz_m) + assert measurement.axis_point_source == "pose_trajectory_image_plane" + assert measurement.pose_axis_line_rms_m < 1.0e-6 + assert np.linalg.norm( + np.cross(observed_point - expected_point, expected_axis) + ) < 1.0e-6 + + def test_splay_zero_interpolates_when_scan_does_not_hit_command_127() -> None: records, expected_axis, _ = _arbitrary_tag_records() assert not any(record["command_u8"] == 127 for record in records) @@ -349,6 +408,14 @@ def test_urdf_writer_postmultiplies_joint_axis_and_never_overwrites(tmp_path: Pa offsets_rad={"thumb_cmc_yaw": offset}, timestamp="20260806_120001", ) + with pytest.raises(ValueError, match="finite and within"): + write_zero_corrected_urdf( + source_urdf=SOURCE_URDF, + output_directory=tmp_path, + serial_number="G20_LEFT_001", + offsets_rad={"thumb_cmc_yaw": math.nan}, + timestamp="20260806_120002", + ) def test_urdf_writer_changes_only_the_16_active_zero_origins( @@ -383,42 +450,12 @@ def test_urdf_writer_changes_only_the_16_active_zero_origins( 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") - ) + # A zero calibration must not silently expand mechanical/CAD safety + # limits. Dynamic measured ranges remain in the calibration JSON. + for name in (*ACTIVE_JOINTS, *PASSIVE_JOINTS): + assert _joint_limit(destination, name) == pytest.approx( + _joint_limit(SOURCE_URDF, name) + ) def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit: @@ -438,7 +475,306 @@ def _synthetic_curve(zero_command: int, travel: float) -> JointCurveFit: ) +def _solve_synthetic_offsets( + side: str, + offset_degrees: list[float], + *, + inject_oblique_optical_depth_bias: bool = False, + inject_secondary_root_axis_bias_degrees: float = 0.0, + inject_secondary_root_point_bias_m: float = 0.0, + inject_observer_cone_bias_degrees: float = 0.0, + pose_axis_line_rms_by_joint_m: dict[str, float] | None = None, + joint_maximum_offset_degrees: dict[str, float] | None = None, +): + hand = get_hand_calibration_profile(side) + zero = get_zero_calibration_profile(side) + source = SOURCE_URDF if side == "left" else RIGHT_SOURCE_URDF + baseline = [255.0] * 20 + baseline[6:10] = [127.0] * 4 + curves = { + name: _synthetic_curve( + int(baseline[hand.joint_specs[name].motor_index]), + math.radians(50.0), + ) + for name in hand.measured_joints + } + if inject_secondary_root_axis_bias_degrees: + # Make the thumb root the higher-travel, directly observed direction, + # matching the real right-hand data where the short pinky splay arc is + # the less reliable root-axis orientation estimate. + curves["thumb_cmc_roll"] = _synthetic_curve( + int(baseline[hand.joint_specs["thumb_cmc_roll"].motor_index]), + math.radians(70.0), + ) + motor_by_joint = { + name: spec.motor_index for name, spec in hand.joint_specs.items() + } + offsets = { + name: math.radians(value) + for name, value in zip(zero.direct_zero_joints, offset_degrees) + } + model = UrdfKinematicModel(source) + base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8]) + base_translation = np.asarray([0.31, -0.19, 0.72]) + measurements: list[JointAxisMeasurement] = [] + for cycle in range(3): + for joint in zero.axis_joints: + state = list(baseline) + if joint == "thumb_cmc_yaw": + state[5] = 145.0 + angles = _angles_from_state( + state, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=zero.inherited_zero_joints, + ) + axis, point = model.axis_line( + joint, zero_offsets=offsets, joint_angles=angles + ) + point_common = base_rotation.apply(point) + base_translation + if ( + inject_secondary_root_point_bias_m + and joint == f"{zero.reference_finger}_mcp_roll" + ): + # A repeatable monocular depth error on the second parallel + # root line must affect translation only, never palm rotation + # or the inferred thumb-roll zero. + point_common = point_common + base_rotation.apply( + np.asarray([0.0, 0.0, inject_secondary_root_point_bias_m]) + ) + view_normal_common = None + if ( + inject_oblique_optical_depth_bias + and joint in zero.phase_parent_joint + ): + parent_axis = model.axis_line( + zero.phase_parent_joint[joint], + zero_offsets=offsets, + joint_angles=angles, + )[0] + helper = ( + np.asarray([1.0, 0.0, 0.0]) + if abs(float(parent_axis[0])) < 0.8 + else np.asarray([0.0, 1.0, 0.0]) + ) + tilt_axis = np.cross(parent_axis, helper) + tilt_axis /= np.linalg.norm(tilt_axis) + view_normal = Rotation.from_rotvec( + math.radians(15.0) * tilt_axis + ).apply(parent_axis) + view_normal_common = tuple(base_rotation.apply(view_normal)) + # Simulate an independent planar-PnP depth error on the child + # Tag. It is large enough to drive the old 3-D phase solve to + # a configured offset bound. + point_common = point_common + 0.03 * np.asarray( + view_normal_common + ) + axis_common = base_rotation.apply(axis) + if ( + inject_observer_cone_bias_degrees + and joint == "thumb_cmc_pitch" + ): + parent_axis = model.axis_line( + zero.axis_parent_joint[joint], + zero_offsets=offsets, + joint_angles=angles, + )[0] + cone_normal = np.cross(axis, parent_axis) + cone_normal /= np.linalg.norm(cone_normal) + axis_common = base_rotation.apply( + Rotation.from_rotvec( + math.radians(inject_observer_cone_bias_degrees) + * cone_normal + ).apply(axis) + ) + if ( + inject_secondary_root_axis_bias_degrees + and joint == f"{zero.reference_finger}_mcp_roll" + ): + helper = np.asarray([0.0, 0.0, 1.0]) + if abs(float(axis_common @ helper)) > 0.8: + helper = np.asarray([0.0, 1.0, 0.0]) + bias_axis = np.cross(axis_common, helper) + bias_axis /= np.linalg.norm(bias_axis) + axis_common = Rotation.from_rotvec( + math.radians(inject_secondary_root_axis_bias_degrees) + * bias_axis + ).apply(axis_common) + measurements.append( + JointAxisMeasurement( + joint=joint, + cycle=cycle, + axis_common_xyz=tuple(axis_common), + point_common_xyz_m=tuple(point_common), + condition_state_u8=tuple(state), + plane_rms_m=0.0002, + radial_rms_m=0.0002, + rotation_circle_axis_difference_rad=math.radians(0.1), + view_normal_common_xyz=view_normal_common, + pose_axis_line_rms_m=( + pose_axis_line_rms_by_joint_m or {} + ).get(joint, 0.0), + ) + ) + result = solve_urdf_zero_offsets( + source_urdf=source, + measurements=measurements, + curves=curves, + motor_by_joint=motor_by_joint, + hand_type=side, + joint_maximum_offset_rad={ + name: math.radians(value) + for name, value in (joint_maximum_offset_degrees or {}).items() + }, + ) + return zero, result + + +def test_small_stable_offsets_are_validated_without_rewriting_urdf_zero() -> None: + zero, result = _solve_synthetic_offsets("right", [0.1] * 7) + assert result.passed is True + static_policy = { + **zero.fixed_direct_zero_offsets_rad, + **zero.static_output_zero_offsets_rad, + } + for name, value in result.direct_offsets_rad.items(): + assert value == pytest.approx(static_policy.get(name, 0.0)) + + +def test_profiles_do_not_contain_hard_coded_thumb_zero_offsets() -> None: + right = get_zero_calibration_profile("right") + left = get_zero_calibration_profile("left") + + assert "thumb_cmc_roll" not in right.fixed_direct_zero_offsets_rad + assert "thumb_cmc_roll" not in right.static_output_zero_offsets_rad + assert "thumb_cmc_roll" not in left.fixed_direct_zero_offsets_rad + assert "thumb_cmc_roll" not in left.static_output_zero_offsets_rad + + +def test_reference_finger_roll_static_zero_is_fixed_to_upright_cad() -> None: + zero, result = _solve_synthetic_offsets( + "right", [2.0, -2.0, 2.0, 1.0, 4.0, 1.0, 1.0] + ) + reference_roll = f"{zero.reference_finger}_mcp_roll" + assert result.passed is True + assert math.degrees(result.direct_offsets_rad[reference_roll]) == pytest.approx( + 0.0, abs=1.0e-12 + ) + + +def test_biased_short_root_axis_does_not_tilt_entire_zero_solution() -> None: + zero, result = _solve_synthetic_offsets( + "right", + [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], + inject_secondary_root_axis_bias_degrees=15.0, + ) + + assert result.passed is True + expected = dict( + zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]) + ) + expected.update( + { + name: math.degrees(value) + for name, value in { + **zero.fixed_direct_zero_offsets_rad, + **zero.static_output_zero_offsets_rad, + }.items() + } + ) + for name, value in expected.items(): + assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( + value, abs=0.05 + ) + + +def test_root_line_depth_bias_does_not_change_thumb_roll_zero() -> None: + zero, result = _solve_synthetic_offsets( + "right", + [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], + inject_secondary_root_point_bias_m=0.02, + ) + + assert result.passed is True + assert math.degrees( + result.direct_offsets_rad["thumb_cmc_roll"] + ) == pytest.approx(2.0, abs=0.05) + assert result.direct_offsets_rad[f"{zero.reference_finger}_mcp_roll"] == 0.0 + + +def test_thumb_mcp_static_phase_bias_cannot_override_original_cad_zero() -> None: + offsets = [2.0, -3.0, 4.0, -40.0, 1.0, -1.0, 2.0] + _, result = _solve_synthetic_offsets( + "right", + offsets, + joint_maximum_offset_degrees={"thumb_mcp": 45.0}, + ) + + assert result.passed is True + assert result.direct_offsets_rad["thumb_mcp"] == pytest.approx(0.0) + assert result.cycle_offsets_rad["thumb_mcp"] == pytest.approx((0.0, 0.0, 0.0)) + assert "thumb_ip" not in result.validation_error_by_joint_rad + for name in ("thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch"): + assert abs(math.degrees(result.direct_offsets_rad[name])) <= 20.0 + for name in ("pinky_mcp_pitch", "pinky_pip"): + assert abs(math.degrees(result.direct_offsets_rad[name])) <= 3.0 + assert result.direct_offsets_rad["pinky_mcp_roll"] == pytest.approx(0.0) + + +def test_end_on_phase_rejects_oblique_monocular_depth_bias() -> None: + zero, result = _solve_synthetic_offsets( + "right", + [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], + inject_oblique_optical_depth_bias=True, + ) + + assert result.passed is True + expected = dict( + zip(zero.direct_zero_joints, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0]) + ) + expected.update( + { + name: math.degrees(value) + for name, value in { + **zero.fixed_direct_zero_offsets_rad, + **zero.static_output_zero_offsets_rad, + }.items() + } + ) + for name, value in expected.items(): + assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( + value, abs=0.05 + ) + + +def test_zero_solver_rejects_axis_cone_geometry_that_a_zero_cannot_fix() -> None: + _, result = _solve_synthetic_offsets( + "right", + [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], + inject_observer_cone_bias_degrees=8.0, + ) + + assert result.passed is False + assert result.failure_reasons["thumb_cmc_yaw"] == ( + "zero_axis_cone_mismatch_too_large" + ) + + +def test_zero_solver_rejects_unreliable_parallel_axis_line_phase() -> None: + _, result = _solve_synthetic_offsets( + "right", + [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], + pose_axis_line_rms_by_joint_m={"thumb_mcp": 0.002}, + ) + + assert result.passed is False + assert result.failure_reasons["thumb_cmc_pitch"] == ( + "zero_phase_axis_line_residual_too_large" + ) + + def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None: + zero = get_zero_calibration_profile("left") baseline = [255.0] * 20 baseline[6:10] = [127.0] * 4 curves = { @@ -454,7 +790,7 @@ def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None: 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] + DIRECT_ZERO_JOINTS, [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0] ) } model = UrdfKinematicModel(SOURCE_URDF) @@ -514,37 +850,128 @@ def test_joint_chain_solver_recovers_offsets_and_yaw_uses_roll_145() -> None: ) ) > 1.0 assert result.passed is True + static_policy = { + **zero.fixed_direct_zero_offsets_rad, + **zero.static_output_zero_offsets_rad, + } for name, expected in true_offsets.items(): + if name in static_policy: + expected = static_policy[name] assert result.direct_offsets_rad[name] == pytest.approx( expected, abs=math.radians(0.05) ) assert set(result.all_active_offsets_rad) == set(ACTIVE_JOINTS) assert result.all_active_offsets_rad["thumb_mcp"] == pytest.approx( - 0.0, abs=1.0e-12 + 0.0, abs=math.radians(0.05) ) - for target, source in INHERITED_ZERO_JOINTS.items(): + for target in INHERITED_ZERO_JOINTS: 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( + assert result.offset_uncertainty_rad.keys() == result.direct_offsets_rad.keys() + + +def test_right_solver_uses_pinky_and_phase_ignores_length_and_depth_bias() -> None: + hand = get_hand_calibration_profile("right") + zero = get_zero_calibration_profile("right") + baseline = [255.0] * 20 + baseline[6:10] = [127.0] * 4 + curves = { + name: _synthetic_curve( + int(baseline[hand.joint_specs[name].motor_index]), + math.radians(50.0), + ) + for name in hand.measured_joints + } + motor_by_joint = { + name: spec.motor_index for name, spec in hand.joint_specs.items() + } + true_offsets = { + name: math.radians(value) + for name, value in zip( + zero.direct_zero_joints, + [2.0, -3.0, 4.0, 1.5, 0.0, -1.0, 2.0], + ) + } + model = UrdfKinematicModel(RIGHT_SOURCE_URDF) + base_rotation = Rotation.from_euler("xyz", [0.5, -0.4, 0.8]) + base_translation = np.asarray([0.31, -0.19, 0.72]) + measurements: list[JointAxisMeasurement] = [] + for cycle in range(3): + states: dict[str, list[float]] = {} + lines: dict[str, tuple[np.ndarray, np.ndarray]] = {} + for joint in zero.axis_joints: + state = list(baseline) + if joint == "thumb_cmc_yaw": + state[5] = 145.0 + states[joint] = state + angles = _angles_from_state( + state, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=zero.inherited_zero_joints, + ) + lines[joint] = model.axis_line( + joint, + zero_offsets=true_offsets, + joint_angles=angles, + ) + original_lines = { + name: (axis.copy(), point.copy()) + for name, (axis, point) in lines.items() + } + for observer, parent in zero.phase_parent_joint.items(): + parent_axis, parent_point = lines[parent] + original_parent_axis, original_parent_point = original_lines[parent] + child_axis, original_child_point = original_lines[observer] + radial = original_child_point - original_parent_point + radial -= original_parent_axis * float( + radial @ original_parent_axis + ) + # Preserve angular phase while deliberately corrupting link radius + # and along-axis depth. These components must not move a zero. + lines[observer] = ( + child_axis, + parent_point + 1.25 * radial + 0.02 * parent_axis, + ) + for joint in zero.axis_joints: + axis, point = lines[joint] + measurements.append( + JointAxisMeasurement( + joint=joint, + cycle=cycle, + axis_common_xyz=tuple(base_rotation.apply(axis)), + point_common_xyz_m=tuple( + base_rotation.apply(point) + base_translation + ), + condition_state_u8=tuple(states[joint]), + plane_rms_m=0.0002, + radial_rms_m=0.0002, + rotation_circle_axis_difference_rad=math.radians(0.1), + ) + ) + + result = solve_urdf_zero_offsets( + source_urdf=RIGHT_SOURCE_URDF, + measurements=measurements, + curves=curves, + motor_by_joint=motor_by_joint, + hand_type="right", + ) + + assert result.passed is True + static_policy = { + **zero.fixed_direct_zero_offsets_rad, + **zero.static_output_zero_offsets_rad, + } + for name, expected in true_offsets.items(): + if name in static_policy: + expected = static_policy[name] + assert result.direct_offsets_rad[name] == pytest.approx( + expected, abs=math.radians(0.05) + ) + for target in zero.inherited_zero_joints: + assert result.all_active_offsets_rad[target] == pytest.approx( 0.0, abs=1.0e-12 ) + assert set(result.all_active_offsets_rad) == set(hand.active_joints) diff --git a/src/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py index 952ac15..95d297c 100644 --- a/src/gui_control/gui_control/config/constants.py +++ b/src/gui_control/gui_control/config/constants.py @@ -75,10 +75,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = { "点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0], "握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0], "张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255], - "OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255], - "拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255], - "拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255], - "拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100], + "OK": [0, 0, 255, 255, 255, 151, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 255, 255, 255], + "拇指对中指": [0, 255, 0, 255, 255, 119, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 220, 255, 255], + "拇指对无名指": [0, 255, 255, 0, 255, 88, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 229, 254], + "拇指对小指": [0, 255, 255, 255, 0, 49, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 215], "准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0], "壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0], "贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],