diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 3d040e7..3794598 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -2,7 +2,7 @@ ## G20右手正式一键标定 -固定三相机和15张Tag安装完成后，用户只运行： +固定三相机和19张Tag安装完成后，用户只运行： ```bash ros2 run g20_thumb_apriltag_calibration calibrate_g20_right @@ -13,21 +13,26 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right 独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与 最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据 当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收 -失败、把会话拉回靠前的关节。需要强制从第一个关节 +失败、把会话拉回靠前的关节。运行中的多视角任务按正面主测量和侧面校验测量 +独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面 +校验视角的任务级有效率只记录为诊断，完整端点、分箱覆盖和模型质量门限仍保持 +不变。需要强制从第一个关节 重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为 一个完整同步任务断点；只有两边数据都完整时才复用。 命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、 -11个可求解主动关节URDF零位修正（`thumb_mcp`固定CAD零位）、 -17条实测命令曲线以及4条 -源URDF mimic派生DIP曲线发布。 +15个会话数据求解主动关节URDF零位修正（`thumb_mcp`固定CAD参考）、 +21条视觉实测命令曲线发布；四指PIP/DIP的动态曲线均实测，四指DIP静态零位保留CAD。 终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。 +四指末端的16 mm Tag允许使用刚性延长杆避挡；软件不假设末端Tag平面与中节Tag +平面平行。延长杆和Tag在一次标定期间必须完全刚性，不能晃动、扭转或重新调整。 + 正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在 JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证 全部通过后更新。 -## G20右手15-Tag底层调试入口 +## G20右手19-Tag底层调试入口 以下内容仅保留给旧会话回放和开发调试；正式一键命令只发布上面的精简 schema v4 JSON，不再生成schema v5运行文件。 @@ -38,20 +43,21 @@ schema v4 JSON，不再生成schema v5运行文件。 ```bash ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \ hand_type:=right \ - tag_layout:=g20_right_15 \ + tag_layout:=g20_right_19 \ serial_number:=G20_RIGHT_001 \ camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \ source_urdf_expected_sha256:= ``` -15张Tag均为16 mm `tag36h11`。正面为 -`0,1,2,3,10,11,12,13`，侧面为 -`4,5,6,15,17`，上面为`8,9`；ID `7,14,16,18`不再粘贴。详细角色以 -`config/three_camera_tags_g20_right_15.yaml`为唯一软件配置源。程序执行4项拇指任务， +当前产品布局共19张`tag36h11`，所有Tag的黑色码区边长均为16 mm；四指末节ID为 +`7,14,16,18`。正面为`0,1,2,3,10,11,12,13`，侧面为 +`4,5,6,7,14,15,16,17,18`，上面为`8,9`。详细角色和逐ID尺寸以 +`config/three_camera_tags_g20_right_19.yaml`为唯一软件配置源。程序执行4项拇指任务， 以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、 -侧面PIP，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过 -各自的观测质量门限。四个DIP动态曲线由同次PIP实测曲线乘源URDF中受保护的mimic -multiplier生成，不参与视觉拟合或视觉质量门限。每项正式四轮之前自动低速往返预检 +侧面PIP/DIP联合任务，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过 +各自的观测质量门限。每个PIP任务只驱动一次对应电机，同时用“手掌→中节Tag”实测PIP、 +用“中节Tag→末节Tag”实测被动DIP；四个DIP不再由URDF mimic系数生成，并参加完整视觉 +拟合和质量门限。每项正式四轮之前自动低速往返预检 0/127/255可见性； 基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的 @@ -63,9 +69,14 @@ multiplier生成，不参与视觉拟合或视觉质量门限。每项正式四 任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把 采集质量良好的任务误判为可见性失败。 -取消末端Tag后，四个PIP缺少下游DIP轴作为任意粘贴Tag条件下的绝对零位观察基准； -因此PIP动态曲线仍为视觉实测，但PIP的URDF静态零位保留源CAD，不猜测写入。 -正式修正范围为拇指4个主动关节，以及四指各自的MCP roll/MCP pitch，共12个。 +恢复末端Tag后，程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次 +相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差；实体手在反馈0端 +能够触掌是独立的机械端点约束。因此四指MCP pitch、PIP的URDF静态零位由本次 +实测全屈曲行程与CAD触掌角之差求出，不再把同相机的平行轴PnP相位直接当作绝对 +零位，也不写死为0。生成URDF时同步修正这些关节的坐标上限和DIP mimic坐标偏置， +保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和 +第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、四指MCP roll、 +MCP pitch及PIP，共15个；只有`thumb_mcp`固定为既有CAD参考。 侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出； 同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时 运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP” @@ -92,11 +103,11 @@ IPPE镜像解。baseline标准接近和全部质量门限保持不变。 排除与MCP同步运动明显矛盾（残差超过7.5°）的ID3镜像候选。该先验不生成或缩放 `thumb_ip`曲线；通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。 -当前15-Tag产品流程发布精简schema v4：21条运行时曲线中，17条来自当前会话的 -视觉实测，四指DIP按源URDF受保护的mimic multiplier由同指PIP曲线派生。 -URDF零位字段只覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch`，共12个， -其中`thumb_mcp`字段固定为0而不会修改源URDF； -四指PIP、`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容， +当前19-Tag产品流程发布精简schema v4：21条运行时曲线全部来自当前会话的视觉实测。 +URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`，共16个， +其中四指`mcp_pitch/pip`共8个字段由视觉行程+机械端点联合求解，只有`thumb_mcp` +字段固定为0； +`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容， 当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、 轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中 相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入 @@ -115,9 +126,8 @@ URDF零位字段只覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_p 姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限； 径向残差、SE(3)轴线残差和正侧面轴/曲线一致性仍是硬门限。任一静态目标、第四轮留出、 遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。 -8个组合姿态仅保留为开发诊断，正式产品默认不执行。15-Tag布局的侧面每根手指只有一张 -PIP Tag，并不存在可独立验证的DIP Tag；同时轴线零位求解不提供适合绝对笛卡尔位置验收的 -手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用 +8个组合姿态仅保留为开发诊断，正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔 +位置验收的手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用 `a609d521`验证过的完整四轮相对旋转曲线；全部实测关节均由隔离第四轮逐关节验收。 现场需要区分某根手指的roll机构回差与单机位误差时，可设置 `cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次 @@ -421,8 +431,9 @@ calibration_output/G20_RIGHT_001/<时间戳>/ `zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从 指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或 覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能 -改变；当前15-Tag右手保留12个主动静态零位字段，其中11个由数据求解，`thumb_mcp` -固定为原始CAD零位0；四指PIP虽保留字段但数值固定为0。 +改变；当前19-Tag右手保留16个主动静态零位字段，其中15个由本会话数据求解，只有 +`thumb_mcp`固定为原始CAD零位0；四指DIP为被动关节，发布实测动态曲线并保留CAD +静态零位，其mimic坐标偏置只随上游PIP坐标系变换作等价调整。 未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到 同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、 图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。 @@ -775,7 +786,7 @@ for name, joint in data["joints"].items(): 不影响 AprilTag 的 ROI 输入。 静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。 -当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短 +当前末节16 mm Tag如果中心位置 RMS 持续不合格，应优先增加照明、缩短 相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。 启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像 diff --git a/src/g20_thumb_apriltag_calibration/config/calibration.yaml b/src/g20_thumb_apriltag_calibration/config/calibration.yaml index 329747a..8b660ab 100644 --- a/src/g20_thumb_apriltag_calibration/config/calibration.yaml +++ b/src/g20_thumb_apriltag_calibration/config/calibration.yaml @@ -44,7 +44,7 @@ g20_thumb_calibration: minimum_detection_hz: 15.0 maximum_hamming: 0 minimum_decision_margin: 30.0 - # Trial threshold for the current 10 mm tags (observed at 32-38 px). + # Trial threshold for small/far tags (historically observed at 32-38 px). # Final acceptance is still guarded by static RMS and random validation. minimum_edge_pixels: 30.0 # Current 30 px tags measure about 0.50-0.53 deg RMS while stationary. diff --git a/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml index 6711ca9..c8753fe 100644 --- a/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml +++ b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml @@ -25,9 +25,9 @@ artifacts: camera_extrinsics: config/g20_three_camera_extrinsics.yaml camera_extrinsics_sha256: aa0a1498a210ef36f20d59bd4fdc612a01fd09eb5a2bc1c0b8d84c05a44d5c5a calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml - calibration_config_sha256: 80758d0a240a7dea1426d4fad688b09eac8360560a1d5870d709b7bb103d3280 - tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_15.yaml - tag_config_sha256: 7ce50be80dfccb3529c34d5bd601090b9d378e2bf6abcef2d5b93dc6a4da99fd + calibration_config_sha256: d1a8be37c9ae21cb4f975ddd66c3cf93fed25dae98ddcf971259b395fa69b521 + tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml + tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41 release: # Each task already contains three training cycles plus an isolated fourth 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 fa1d1d6..961a216 100644 --- a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml @@ -16,7 +16,7 @@ g20_calibration: normal_calibration_speed: 15 index_roll_calibration_speed: 5 index_flex_calibration_speed: 10 - # 15-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量， + # 19-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量， # 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束， # 始终保持速度5；任一方向采样余量不足也保持原速度。 adaptive_formal_speed_enabled: true @@ -25,10 +25,16 @@ g20_calibration: adaptive_formal_speed_maximum_bin_gap: 8 speed_setting_settle_seconds: 0.25 - # tag36h11检测角点围成的黑色正方形实测为16 mm；不包含外围白边。 + # tag36h11尺寸是检测角点围成的黑色正方形边长，不包含外围白边。 + # 19张Tag的黑色码区外边长均为16 mm。自定义PnP必须与 + # apriltag_ros逐ID尺寸一致，禁止用纸张/白边尺寸代替码区尺寸。 tag_size_m: 0.016 + # ROS 2无法从YAML空数组推断整数/浮点数组类型。这四个 + # 末端Tag仍显式写16 mm，防止节点启动时得到未初始化参数。 + tag_size_override_ids: [7, 14, 16, 18] + tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016] repetitions: 3 - # 15-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。 + # 19-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。 g20_right_19_repetitions: 4 preflight_frames: 60 minimum_detection_rate: 0.95 @@ -78,8 +84,10 @@ g20_calibration: zero_maximum_axis_cone_mismatch_deg: 5.0 zero_maximum_observability_condition_number: 10000000000.0 zero_maximum_offset_deg: 20.0 - # 四指MCP roll/pitch绝对静态零偏的保护范围；PIP仍保留CAD静态零位。 + # 四指MCP roll保留严格的装配保护范围。MCP pitch/PIP静态零位由 + # 实测全屈曲行程与反馈0触掌的CAD机械端点联合求解，不允许写死为0。 zero_finger_maximum_offset_deg: 3.0 + flexion_endpoint_maximum_offset_deg: 5.0 endpoint_tolerance_u8: 2.0 # 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈 @@ -96,7 +104,7 @@ g20_calibration: # roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。 baseline_hold_seconds: 0.5 minimum_baseline_hold_frames: 10 - # 15-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。 + # 19-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。 task_precheck_hold_seconds: 2.0 position_timeout_seconds: 30.0 sweep_timeout_seconds: 90.0 @@ -136,11 +144,11 @@ g20_calibration: trajectory_maximum_cycle_travel_difference_deg: 3.0 passive_maximum_cycle_travel_difference_deg: 10.0 maximum_monotonic_correction_deg: 2.0 - # 旧布局仍用连续扫描正反程差门限；15-Tag产品的连续运动包含速度相关滞后， + # 旧布局仍用连续扫描正反程差门限；19-Tag产品的连续运动包含速度相关滞后， # 由方向曲线和最终留出验证建模，不再重复硬判。其绝对正反程门禁使用下面 # 的九点稳态command_maximum_direction_gap_deg。 maximum_hysteresis_deg: 2.0 - # 15-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差 + # 19-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差 # 不超过0.5°；四指roll例外：127以255→127为唯一物理零位，反向分支 # 保留实测偏差，并改为检查分支间隙上限及跨轮稳定性。 baseline_maximum_hysteresis_deg: 0.5 @@ -153,13 +161,12 @@ g20_calibration: # 的两条运行曲线仍原样保留固件方向死区，不能把command/feedback差算成回差。 command_maximum_direction_gap_deg: 2.0 - # 默认无额外随机动作；15-Tag产品最终一轮始终作为不可关闭的留出验证。 + # 默认无额外随机动作；19-Tag产品最终一轮始终作为不可关闭的留出验证。 validation_enabled: false # 第四轮留出求解后必须再走8个固定安全组合姿态；三机位规定Tag全部可见 # 且实测20通道到位才允许发布。只保存Tag位姿，不保存原始图像。 - # Developer diagnostic only. The 15-Tag product has one side PIP Tag per - # finger and no independent DIP Tag, while the formal fourth sweep cycle - # already provides an isolated per-joint holdout. + # Developer diagnostic only. The formal fourth sweep cycle already gives + # every isolated PIP/DIP pair an independent holdout. combination_validation_enabled: false combination_validation_frames: 10 combination_maximum_position_p95_m: 0.003 @@ -170,6 +177,6 @@ g20_calibration: validation_timeout_seconds: 20.0 maximum_validation_mae_deg: 1.0 maximum_validation_p95_deg: 2.0 - # 15-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。 + # 19-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。 maximum_validation_error_deg: 3.0 zero_maximum_confidence_half_width_deg: 1.5 diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml new file mode 100644 index 0000000..e3d6429 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml @@ -0,0 +1,63 @@ +/g20_calibration/front/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + profile: false + max_hamming: 0 + detector: + threads: 4 + decimate: 1.5 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [0, 1, 2, 3, 10, 11, 12, 13] + frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll] + sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + +/g20_calibration/side/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + profile: false + max_hamming: 0 + detector: + threads: 4 + # Distal Tags use the same measured 16 mm black-code edge as all others. + decimate: 1.0 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [4, 5, 6, 7, 14, 15, 16, 17, 18] + frames: [side_base, ring_pip, pinky_pip, pinky_dip, ring_dip, middle_pip, middle_dip, index_pip, index_dip] + sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016] + +/g20_calibration/top/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.016 + profile: false + max_hamming: 0 + detector: + threads: 4 + decimate: 1.5 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [8, 9] + frames: [top_base, thumb_yaw] + sizes: [0.016, 0.016] 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 54bdab4..1524dfd 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 @@ -160,6 +160,12 @@ RIGHT_19_END_ON_IMAGE_CURVE_JOINTS = frozenset( *(f"{finger}_pip" for finger in _FINGERS), } ) +# These joints have no independent actuator. Their distal Tags directly +# measure the passive dynamic curve, while their fitted axes serve only as +# downstream phase observers for the corresponding active PIP. +RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS = frozenset( + f"{finger}_dip" for finger in _FINGERS +) _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)}, @@ -337,11 +343,13 @@ LEFT_HAND_PROFILE = _build_hand_profile("left") RIGHT_HAND_PROFILE = _build_hand_profile("right") +# ``g20_right_15`` remains accepted as a read/CLI compatibility alias, but a +# newly recorded product session always persists the physical 19-Tag layout +# identifier. Together with the exact ``view_tags`` resume check this keeps a +# former 15-Tag checkpoint from being imported after the four distal Tags are +# installed again. G20_RIGHT_15_LAYOUT = "g20_right_15" -# Retain the established Python symbol while the product layout moves from 19 -# to 15 physical Tags. Persisted/session-facing layout IDs use the new value, -# so old 19-Tag checkpoints cannot be resumed accidentally. -G20_RIGHT_19_LAYOUT = G20_RIGHT_15_LAYOUT +G20_RIGHT_19_LAYOUT = "g20_right_19" # The three CMC axes keep the calibration domain that was proven on this hand # in a609d521: dense relative-rotation sweeps with an isolated-cycle holdout. @@ -423,7 +431,7 @@ def _right_19_auxiliary_commands( def _build_right_19_profile() -> HandCalibrationProfile: - """Build the 15-Tag/16-motion-task right-hand profile.""" + """Build the 19-Tag/16-motion-task right-hand profile.""" view_tags: dict[str, dict[str, int]] = { "front": { "front_base": 0, @@ -439,8 +447,12 @@ def _build_right_19_profile() -> HandCalibrationProfile: "side_base": 4, "ring_pip": 5, "pinky_pip": 6, + "pinky_dip": 7, + "ring_dip": 14, "middle_pip": 15, + "middle_dip": 16, "index_pip": 17, + "index_dip": 18, }, "top": {"top_base": 8, "thumb_yaw": 9}, } @@ -449,7 +461,7 @@ def _build_right_19_profile() -> HandCalibrationProfile: # palm references. Every moving Tag is still mandatory, but is checked # by its task-local visibility sweep after the other fingers have moved to # that task's clearance pose. Requiring all 15 at one instant would make - # a physically correct installation impossible to start. + # a physically correct 19-Tag installation impossible to start. preflight_view_roles = { "front": ("front_base",), "side": ("side_base",), @@ -496,9 +508,19 @@ def _build_right_19_profile() -> HandCalibrationProfile: pip, _MOTOR_BY_JOINT[pip], True, "side", "side_base", f"{finger}_pip", zero_kind="urdf_axis_chain", ) + # DIP is passive, but no longer unobserved. The PIP sweep moves the + # complete linkage once while the middle/distal Tag pair directly + # measures DIP. Its dynamic curve is therefore visual truth; its + # absolute static URDF zero deliberately remains source CAD because an + # arbitrary distal-Tag mounting angle is indistinguishable from a DIP + # zero offset without a mechanical zero fixture. joint_specs[dip] = JointSpec( - dip, _MOTOR_BY_JOINT[pip], False, None, None, None, - source_joint=pip, + dip, + _MOTOR_BY_JOINT[pip], + False, + "side", + f"{finger}_pip", + f"{finger}_dip", ) measurement_specs = dict(joint_specs) @@ -560,7 +582,7 @@ def _build_right_19_profile() -> HandCalibrationProfile: SweepSpec( "side", pip_motor, - (f"{finger}_pip",), + (f"{finger}_pip", f"{finger}_dip"), f"{finger}_pip_side", _right_19_auxiliary_commands( finger, side_view=True, target_kind="pip" @@ -601,9 +623,9 @@ def get_hand_calibration_profile( ) -> HandCalibrationProfile: value = str(side).lower() layout = str(layout_id).lower() - if layout == G20_RIGHT_19_LAYOUT: + if layout in {G20_RIGHT_19_LAYOUT, G20_RIGHT_15_LAYOUT}: if value != "right": - raise ValueError("g20_right_15 layout requires hand side right") + raise ValueError("G20 right product layout requires hand side right") return RIGHT_19_HAND_PROFILE if layout != "legacy_11": raise ValueError(f"unknown G20 tag layout: {layout_id}") @@ -620,7 +642,7 @@ def derive_mimic_passive_fits( *, profile: HandCalibrationProfile, ) -> dict[str, JointCurveFit]: - """Add four non-visual DIP curves using the source URDF mimic contract. + """Complete legacy non-visual DIP curves using the URDF mimic contract. The runtime curves are relative to the baseline command, so a URDF mimic offset cancels while the multiplier scales the measured PIP motion. The @@ -629,6 +651,18 @@ def derive_mimic_passive_fits( result = dict(fits) if profile.layout_id != G20_RIGHT_19_LAYOUT: return result + visually_measured = all( + profile.joint_specs[name].source_joint is None + for name in MIMIC_DERIVED_FINGER_DIPS + ) + if visually_measured: + missing = sorted(set(profile.joint_specs) - set(result)) + extra = sorted(set(result) - set(profile.joint_specs)) + if missing or extra: + raise ValueError( + f"runtime fit set is incomplete: missing={missing} extra={extra}" + ) + return result path = Path(source_urdf).expanduser().resolve() if not path.is_file(): raise ValueError(f"source URDF does not exist: {path}") @@ -694,12 +728,16 @@ def derive_mimic_passive_fits( def clamp_runtime_fits_to_urdf_limits( source_urdf: str | Path, fits: Mapping[str, JointCurveFit], + *, + endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, ) -> dict[str, JointCurveFit]: - """Saturate only published/runtime curves at protected CAD limits. + """Saturate published curves at protected physical limits. Raw trajectory fits and all fit/zero diagnostics remain untouched. The returned transfer functions are the commands a URDF/MuJoCo runtime can - actually represent safely. + actually represent safely. An endpoint-anchored joint uses the corrected + coordinate upper limit ``CAD_upper - origin_offset``; its physical maximum + remains exactly the original CAD maximum after the origin correction. """ path = Path(source_urdf).expanduser().resolve() joints = { @@ -707,6 +745,10 @@ def clamp_runtime_fits_to_urdf_limits( for node in ET.parse(path).getroot().findall("joint") } result: dict[str, JointCurveFit] = {} + endpoint_offsets = { + str(name): float(value) + for name, value in dict(endpoint_anchored_offsets_rad or {}).items() + } for name, fit in fits.items(): joint = joints.get(name) limit = None if joint is None else joint.find("limit") @@ -718,6 +760,8 @@ def clamp_runtime_fits_to_urdf_limits( raise ValueError(f"source URDF joint {name} has no finite position limit") lower = float(limit.get("lower")) upper = float(limit.get("upper")) + if name in endpoint_offsets: + upper -= endpoint_offsets[name] if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: raise ValueError(f"source URDF joint {name} has invalid position limits") @@ -1712,7 +1756,7 @@ def validate_compact_payload(payload: Mapping[str, Any]) -> None: if payload["model"] != "G20" or payload["side"] not in {"left", "right"}: raise ValueError("payload must describe a left or right G20") # schema-v4 has no layout field. A right-hand payload without inherited - # source_joint fields is the 15-Tag product layout; legacy + # source_joint fields is the independent right-hand product layout; legacy # payloads retain their source_joint markers and remain readable. layout_id = infer_compact_payload_layout(payload) independent_right = layout_id == G20_RIGHT_19_LAYOUT 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 index 9425395..7e01f7d 100644 --- 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 @@ -46,6 +46,7 @@ from .urdf_zero import ( fit_joint_axis_measurement, fit_rotation_joint_curve, get_zero_calibration_profile, + derive_right_19_flexion_endpoint_offsets, joint_curve_holdout_errors, solve_urdf_zero_offsets, write_zero_corrected_urdf, @@ -694,6 +695,7 @@ def _validate_corrected_urdf( source: Path, corrected: Path, offsets: Mapping[str, float], + endpoint_anchored_offsets: Mapping[str, float], axes: Sequence[JointAxisMeasurement], curves: Mapping[str, JointCurveFit], motor_by_joint: Mapping[str, int], @@ -738,11 +740,29 @@ def _validate_corrected_urdf( original_limit = original_joint.find("limit") corrected_limit = corrected_joint.find("limit") if original_limit is not None and corrected_limit is not None: + expected_lower = float(original_limit.get("lower")) + expected_upper = float(original_limit.get("upper")) + if name in endpoint_anchored_offsets: + expected_upper -= float(endpoint_anchored_offsets[name]) if ( - original_limit.get("lower") != corrected_limit.get("lower") - or original_limit.get("upper") != corrected_limit.get("upper") + abs(float(corrected_limit.get("lower")) - expected_lower) > 1.0e-12 + or abs(float(corrected_limit.get("upper")) - expected_upper) > 1.0e-12 ): raise ValueError(f"corrected URDF unexpectedly changed {name} limits") + original_mimic = original_joint.find("mimic") + corrected_mimic = corrected_joint.find("mimic") + if original_mimic is not None and corrected_mimic is not None: + source_joint = str(original_mimic.get("joint")) + multiplier = float(original_mimic.get("multiplier", "1")) + expected_mimic_offset = float(original_mimic.get("offset", "0")) + expected_mimic_offset += multiplier * float( + endpoint_anchored_offsets.get(source_joint, 0.0) + ) + if abs( + float(corrected_mimic.get("offset", "0")) + - expected_mimic_offset + ) > 1.0e-12: + raise ValueError(f"corrected URDF has invalid {name} mimic offset") 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: @@ -815,13 +835,23 @@ def replay_session( raw_path, ) = _load_raw_session(session) side = str(start["hand_type"]).lower() - layout_id = str(start.get("tag_layout", "legacy_11")).lower() - profile = get_hand_calibration_profile(side, layout_id) + requested_layout_id = str( + start.get("tag_layout", "legacy_11") + ).lower() + profile = get_hand_calibration_profile(side, requested_layout_id) + # Normalise the former g20_right_15 compatibility alias immediately so + # every downstream product guard uses the actual physical 19-Tag profile + # instead of silently falling through legacy-11 logic. + layout_id = profile.layout_id zero_profile = get_zero_calibration_profile(side, layout_id) 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 layout_id == G20_RIGHT_19_LAYOUT and write_outputs: + if ( + layout_id == G20_RIGHT_19_LAYOUT + and write_outputs + and bool(parameters.get("combination_validation_enabled", False)) + ): replay_rows = [ json.loads(line) for line in raw_path.read_text(encoding="utf-8").splitlines() @@ -923,7 +953,7 @@ def replay_session( training_cycle_set = set(training_cycles) if layout_id == G20_RIGHT_19_LAYOUT and len(training_cycles) < 3: raise ValueError( - "g20_right_15 replay requires at least 3 training cycles and " + "g20_right_19 replay requires at least 3 training cycles and " "one isolated holdout" ) training_fits = { @@ -943,7 +973,7 @@ def replay_session( if layout_id == G20_RIGHT_19_LAYOUT: if set(command_records_by_joint) != set(profile.record_joints): raise ValueError( - "g20_right_15 session is incomplete: missing steady command checkpoints" + "g20_right_19 session is incomplete: missing steady command checkpoints" ) command_fits = {} for name in profile.measured_joints: @@ -1000,13 +1030,10 @@ def replay_session( for name, value in failed_command_gaps.items() ) ) - command_fits = clamp_runtime_fits_to_urdf_limits( + command_fits = derive_mimic_passive_fits( source_urdf, - derive_mimic_passive_fits( - source_urdf, - command_fits, - profile=profile, - ), + command_fits, + profile=profile, ) cross_view_roll_metrics: dict[str, dict[str, float]] = {} for name, validation_name in ( @@ -1195,6 +1222,22 @@ def replay_session( motor_by_joint = { name: int(spec.motor_index) for name, spec in profile.joint_specs.items() } + endpoint_zero_offsets = ( + derive_right_19_flexion_endpoint_offsets( + source_urdf, + command_fits, + maximum_offset_rad=math.radians( + float(parameters.get("flexion_endpoint_maximum_offset_deg", 5.0)) + ), + ) + if layout_id == G20_RIGHT_19_LAYOUT + else {} + ) + command_fits = clamp_runtime_fits_to_urdf_limits( + source_urdf, + command_fits, + endpoint_anchored_offsets_rad=endpoint_zero_offsets, + ) joint_limits: dict[str, float] = {} solve_arguments = { "source_urdf": source_urdf, @@ -1243,6 +1286,10 @@ def replay_session( "tag_layout": layout_id, "training_cycles": training_cycles, "validation_cycle": validation_cycle, + "fixed_direct_zero_offsets_rad": { + **zero_profile.fixed_direct_zero_offsets_rad, + **endpoint_zero_offsets, + }, } holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments) if not holdout_zero.passed: @@ -1319,12 +1366,14 @@ def replay_session( output_directory=temporary, serial_number=hand_serial, offsets_rad=urdf_offsets, + endpoint_anchored_offsets_rad=endpoint_zero_offsets, timestamp=stamp, ) urdf_checks = _validate_corrected_urdf( source=source_urdf, corrected=candidate, offsets=urdf_offsets, + endpoint_anchored_offsets=endpoint_zero_offsets, axes=axes, curves=training_fits, motor_by_joint=motor_by_joint, @@ -1337,15 +1386,21 @@ def replay_session( # differ slightly at settled checkpoints; using them here creates # a fictitious residual zero after a structurally correct write. curves=training_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}, + **{ + **solve_arguments, + "source_urdf": candidate, + "fixed_direct_zero_offsets_rad": { + name: 0.0 + for name in ( + set(zero_profile.fixed_direct_zero_offsets_rad) + | set(endpoint_zero_offsets) + ) + }, + }, ) maximum_residual_offset = max( abs(float(value)) for value in residual_zero.direct_offsets_rad.values() @@ -1452,11 +1507,19 @@ def replay_session( ), "thumb_default": float(parameters["zero_maximum_offset_deg"]), }, - "static_zero_policy": "direct_measurements_only", + "static_zero_policy": ( + "visual_axes_plus_measured_contact_endpoint" + if layout_id == G20_RIGHT_19_LAYOUT + else "direct_measurements_only" + ), "fixed_zero_offsets_deg": { name: math.degrees(value) for name, value in zero_profile.fixed_direct_zero_offsets_rad.items() }, + "endpoint_anchored_zero_offsets_deg": { + name: math.degrees(value) + for name, value in endpoint_zero_offsets.items() + }, "static_output_zero_offsets_deg": { name: math.degrees(value) for name, value in zero_profile.static_output_zero_offsets_rad.items() diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py index 0a4f375..2925ffb 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/one_command.py @@ -30,6 +30,38 @@ EXIT_PASS = 0 EXIT_QUALITY = 2 EXIT_SAFETY = 3 STATUS_TIMEOUT_SECONDS = 90.0 +FITTING_STATUS_TIMEOUT_SECONDS = 600.0 + + +def _status_timeout_seconds(status: Mapping[str, Any]) -> float: + """Return the watchdog deadline for the node's current phase. + + Motion and acquisition are expected to publish twice a second and retain + the strict transport watchdog. The final 3-D fit is intentionally a + synchronous, CPU-bound operation, so its executor cannot service the + status timer until the fit returns. The node publishes an explicit + FITTING status immediately before entering that operation; allow that + known phase enough time without weakening motion safety checks. + """ + if str(status.get("state", "")) == "FITTING": + return FITTING_STATUS_TIMEOUT_SECONDS + return STATUS_TIMEOUT_SECONDS + + +def _calibration_node_exited_before_status(log_path: Path) -> bool: + """Detect a launch child crash while the parent launch is still alive.""" + try: + with log_path.open("rb") as stream: + stream.seek(0, os.SEEK_END) + size = stream.tell() + stream.seek(max(0, size - 65536), os.SEEK_SET) + tail = stream.read().decode("utf-8", errors="replace") + except OSError: + return False + return ( + "[three_camera_calibration_node-" in tail + and "]: process has died" in tail + ) class CalibrationMonitor(Node): @@ -124,7 +156,7 @@ def _launch_command( ) -> list[str]: values = { "hand_type": "right", - "tag_layout": "g20_right_15", + "tag_layout": "g20_right_19", "serial_number": config.serial_number, "can_interface": config.can_interface, "session_dir": str(session), @@ -230,6 +262,7 @@ def _run_hardware_session( ) launched_at = time.monotonic() last_render = 0.0 + last_startup_log_check = 0.0 while True: rclpy.spin_once(monitor, timeout_sec=0.1) if monitor.latest_status: @@ -262,6 +295,16 @@ def _run_hardware_session( break if process.poll() is not None: raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}") + if ( + not monitor.latest_status + and now - last_startup_log_check >= 0.5 + ): + last_startup_log_check = now + log_stream.flush() + if _calibration_node_exited_before_status(log_path): + raise RuntimeError( + "CAM-STATUS-202:calibration node exited before status" + ) if ( not monitor.latest_status and now - launched_at > STATUS_TIMEOUT_SECONDS @@ -269,7 +312,8 @@ def _run_hardware_session( raise RuntimeError("CAM-STATUS-202:no calibration status received") if ( monitor.latest_status - and now - monitor.last_status_at > STATUS_TIMEOUT_SECONDS + and now - monitor.last_status_at + > _status_timeout_seconds(monitor.latest_status) ): raise RuntimeError("MOTION-COMM-303:calibration status stopped") except KeyboardInterrupt as error: @@ -419,7 +463,7 @@ def _automatic_resume_candidate(config: ProductConfig) -> Path | None: if ( start is None or start.get("hand_type") != "right" - or start.get("tag_layout") != "g20_right_15" + or start.get("tag_layout") != "g20_right_19" or start.get("source_urdf_sha256") != config.source_urdf_sha256 ): @@ -460,7 +504,7 @@ def run( print(_startup_failure_block(path, error), flush=True) return EXIT_QUALITY if preflight_only: - print("PASS：产品文件、相机内外参、15张Tag配置和CAN接口静态预检通过。") + print("PASS：产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。") return EXIT_PASS config.session_root.mkdir(parents=True, exist_ok=True) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py index 13f6ee9..2f7960b 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/operator_report.py @@ -268,6 +268,18 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st ] if isinstance(views, Mapping) else [] if value.startswith("CFG-"): return value, "产品配置或文件预检失败", "不要移动相机；复制本诊断块给开发者。" + if value.startswith("CAM-STATUS-202"): + return ( + "CAM-STATUS-202", + "标定节点启动后未发布状态", + "机械手尚未开始运动；查看calibration.log中首个节点异常后从断点继续。", + ) + if value.startswith("MOTION-COMM-303"): + return ( + "MOTION-COMM-303", + "标定节点状态心跳停止更新", + "已采集的完整任务会保留；检查calibration.log后从断点继续，不要重新采集。", + ) if "motor_state_stalled" in value: return "MOTION-STALL-301", "电机反馈停止向目标推进，程序已保持当前位置", "先检查机械卡阻，未排除前不要重复强推。" if any( @@ -280,7 +292,11 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st ) ): return "OBS-SAMPLE-104", "当前任务的有效视觉轨迹不完整", "根据诊断中的关节、机位和缺失区间处理遮挡或反光后重新运行。" - if "synchronised" in value and not missing and group_pnp_reasons: + if ( + ("synchronised" in value or "sweep_start_tag_timeout" in value) + and not missing + and group_pnp_reasons + ): return "CAM-GEOMETRY-201", "Tag可见，但整组PnP候选持续被几何检查拒绝", "不要调整Tag；复制本诊断块给开发者检查PnP候选选择。" if missing or "tag" in value or "detection" in value or "synchronised" in value: return "OBS-TAG-103", f"所需Tag不可用或持续丢失：{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。" diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py index 4a9f1f7..46febbe 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/product.py @@ -58,17 +58,45 @@ def _camera_info_fingerprint(path: Path) -> str: ) -def _tag_ids(path: Path) -> set[int]: +def _tag_sizes_m_by_id(path: Path) -> dict[int, float]: with path.open("r", encoding="utf-8") as stream: payload = _mapping(yaml.safe_load(stream), str(path)) - result: set[int] = set() + result: dict[int, float] = {} for node in payload.values(): parameters = _mapping(_mapping(node, "tag node").get("ros__parameters"), "ros__parameters") tag = _mapping(parameters.get("tag"), "tag") ids = [int(value) for value in tag.get("ids", [])] - if result.intersection(ids): + sizes = [float(value) for value in tag.get("sizes", [])] + if len(ids) != len(sizes): + raise ValueError("Tag IDs and sizes must have the same length") + if set(result).intersection(ids): raise ValueError("Tag IDs must be unique across the three views") - result.update(ids) + if any(not 0.0 < size < 0.1 for size in sizes): + raise ValueError("Tag sizes must be in (0, 0.1) metres") + result.update(zip(ids, sizes)) + return result + + +def _custom_pnp_tag_sizes_m_by_id( + path: Path, configured_ids: set[int] +) -> dict[int, float]: + with path.open("r", encoding="utf-8") as stream: + payload = _mapping(yaml.safe_load(stream), str(path)) + parameters = _mapping( + _mapping(payload.get("g20_calibration"), "g20_calibration").get( + "ros__parameters" + ), + "ros__parameters", + ) + default = float(parameters.get("tag_size_m", 0.0)) + ids = [int(value) for value in parameters.get("tag_size_override_ids", [])] + sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])] + if len(ids) != len(sizes) or len(set(ids)) != len(ids): + raise ValueError("custom PnP Tag-size overrides are invalid") + if any(tag_id not in configured_ids for tag_id in ids): + raise ValueError("custom PnP Tag-size override ID is not configured") + result = {tag_id: default for tag_id in configured_ids} + result.update(zip(ids, sizes)) return result @@ -169,11 +197,22 @@ def load_product_config( if actual != expected: raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}") - expected_tag_ids = set(range(19)) - {7, 14, 16, 18} - if _tag_ids(tag_config) != expected_tag_ids: + expected_tag_ids = set(range(19)) + tag_sizes = _tag_sizes_m_by_id(tag_config) + if set(tag_sizes) != expected_tag_ids: raise ValueError( - "g20_right_15 Tag config must contain exactly IDs " - "0,1,2,3,4,5,6,8,9,10,11,12,13,15,17" + "g20_right_19 Tag config must contain exactly IDs 0 through 18" + ) + expected_tag_sizes = {tag_id: 0.016 for tag_id in expected_tag_ids} + if tag_sizes != expected_tag_sizes: + raise ValueError( + "g20_right_19 black-code Tag sizes must be 16 mm for every ID" + ) + if _custom_pnp_tag_sizes_m_by_id( + calibration_config, expected_tag_ids + ) != tag_sizes: + raise ValueError( + "custom PnP Tag sizes do not match the apriltag_ros Tag config" ) camera_raw = _mapping(raw.get("cameras"), "cameras") diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py index e6b7ac3..597d5eb 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/publication.py @@ -18,18 +18,23 @@ from scipy.spatial.transform import Rotation from .full_hand import ( G20_COMBINATION_REQUIRED_TARGET_KEYS, G20_RIGHT_19_LAYOUT, - MIMIC_DERIVED_FINGER_DIPS, get_hand_calibration_profile, validate_compact_payload, ) from .product import ProductConfig, sha256_file from .storage import atomic_write_json -from .urdf_zero import get_zero_calibration_profile +from .urdf_zero import ( + RIGHT_19_FLEXION_ENDPOINT_JOINTS, + get_zero_calibration_profile, +) PASSIVE_JOINTS = frozenset( {"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"} ) +VISUALLY_MEASURED_FINGER_DIPS = frozenset( + {"index_dip", "middle_dip", "ring_dip", "pinky_dip"} +) ACTIVE_ZERO_JOINTS = frozenset( get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints ) @@ -83,6 +88,40 @@ def _mask_active_origin_rpy_fields(text: str) -> str: return pattern.sub(replace, text) +def _mask_endpoint_coordinate_fields(text: str) -> str: + """Mask only limit/mimic fields induced by flexion zero coordinates.""" + pattern = re.compile( + r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", + re.DOTALL, + ) + + def replace(match: re.Match[str]) -> str: + block = match.group(0) + name = match.group("name") + if name in RIGHT_19_FLEXION_ENDPOINT_JOINTS: + return re.sub( + r"(]*\bupper\s*=\s*)([\"'])[^\"']*\2", + r"\1\2__CALIBRATED_UPPER__\2", + block, + count=1, + ) + mimic = re.search( + r"]*\bjoint\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>", + block, + re.DOTALL, + ) + if mimic is not None and mimic.group("source") in RIGHT_19_FLEXION_ENDPOINT_JOINTS: + return re.sub( + r"(]*\boffset\s*=\s*)([\"'])[^\"']*\2", + r"\1\2__CALIBRATED_MIMIC_OFFSET__\2", + block, + count=1, + ) + return block + + return pattern.sub(replace, text) + + def _triplet(value: str) -> np.ndarray: result = np.asarray([float(item) for item in value.split()], dtype=float) if result.shape != (3,) or not np.all(np.isfinite(result)): @@ -158,6 +197,7 @@ def verify_corrected_urdf( corrected: str | Path, *, expected_offsets_rad: Mapping[str, float] | None = None, + endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, ) -> tuple[str, ...]: """Prove that only active-joint origin.rpy attributes changed. @@ -173,24 +213,50 @@ def verify_corrected_urdf( # Text patchers may normalize only the final newline. It has no URDF/XML # semantics; every joint block and every non-rpy byte remains guarded # below. - if _mask_active_origin_rpy_fields(source_text).rstrip("\r\n") != ( - _mask_active_origin_rpy_fields(corrected_text).rstrip("\r\n") - ): - raise ValueError("corrected URDF changed content outside active origin.rpy") + masked_source = _mask_endpoint_coordinate_fields( + _mask_active_origin_rpy_fields(source_text) + ).rstrip("\r\n") + masked_corrected = _mask_endpoint_coordinate_fields( + _mask_active_origin_rpy_fields(corrected_text) + ).rstrip("\r\n") + if masked_source != masked_corrected: + raise ValueError( + "corrected URDF changed content outside active origin.rpy or " + "endpoint-calibrated fields" + ) changed: list[str] = [] for name in before: if before[name] == after[name]: continue - if name not in ACTIVE_ZERO_JOINTS: + if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_JOINTS: raise ValueError(f"corrected URDF changed protected joint {name}") - if _mask_origin_rpy(before[name]) != _mask_origin_rpy(after[name]): - raise ValueError(f"corrected URDF changed more than origin.rpy for {name}") - changed.append(name) - for name in PASSIVE_JOINTS: - if before.get(name) != after.get(name): - raise ValueError(f"passive joint/mimic block changed: {name}") + if name in ACTIVE_ZERO_JOINTS: + changed.append(name) if expected_offsets_rad is not None: _verify_expected_origin_offsets(source, corrected, expected_offsets_rad) + endpoint_offsets = { + str(name): float(value) + for name, value in dict(endpoint_anchored_offsets_rad or {}).items() + } + if endpoint_offsets: + source_joints = _joint_elements(source) + corrected_joints = _joint_elements(corrected) + for name, offset in endpoint_offsets.items(): + original_limit = source_joints[name].find("limit") + corrected_limit = corrected_joints[name].find("limit") + expected_upper = float(original_limit.get("upper")) - offset + if abs(float(corrected_limit.get("upper")) - expected_upper) > 1.0e-10: + raise ValueError(f"corrected URDF has invalid {name} upper limit") + for name in PASSIVE_JOINTS: + original_mimic = source_joints[name].find("mimic") + corrected_mimic = corrected_joints[name].find("mimic") + if original_mimic is None or corrected_mimic is None: + continue + source_name = str(original_mimic.get("joint")) + expected = float(original_mimic.get("offset", "0")) + expected += float(original_mimic.get("multiplier", "1")) * endpoint_offsets.get(source_name, 0.0) + if abs(float(corrected_mimic.get("offset", "0")) - expected) > 1.0e-10: + raise ValueError(f"corrected URDF has invalid {name} mimic offset") return tuple(sorted(changed)) @@ -349,7 +415,9 @@ def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]: if set(result) != ACTIVE_ZERO_JOINTS or any( not math.isfinite(value) for value in result.values() ): - raise ValueError("payload does not contain 12 finite observable zero offsets") + raise ValueError( + "payload does not contain all finite observable active zero offsets" + ) return result @@ -417,10 +485,10 @@ def find_compatible_prior_session( def _verify_combination_validation(combination: Mapping[str, Any]) -> None: # The formal product uses the independently held-out fourth sweep cycle. - # The optional eight-pose check is a developer diagnostic because the - # 15-Tag layout has no DIP Tags and the axis-line zero solve does not - # establish an absolute Cartesian hand-base transform. When explicitly - # enabled, retain its strict coverage and error checks. + # The optional eight-pose check remains a developer diagnostic because the + # axis-line zero solve does not establish an absolute Cartesian hand-base + # transform. When explicitly enabled, retain its strict coverage and + # error checks. if not bool(combination.get("enabled")): return position_p95 = float(combination.get("position_p95_m", float("inf"))) @@ -476,14 +544,6 @@ def finalize_session_artifacts( raise ValueError("runtime JSON is not the compact right-hand schema v4") if not bool(payload.get("quality", {}).get("passed")): raise ValueError("runtime JSON quality is not passed") - payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits( - payload, config.source_urdf - ) - if clipped_runtime_joints: - # A node from before runtime-limit saturation may already have written - # an otherwise valid commit candidate. Normalize it before hashing - # and publication so completed sessions can be recovered offline. - atomic_write_json(paths["json"], payload) combination = node_status.get("combination_validation") if not isinstance(combination, Mapping): raise ValueError("node status is missing combination validation") @@ -493,8 +553,17 @@ def finalize_session_artifacts( config.source_urdf, paths["urdf"], expected_offsets_rad=offsets, + endpoint_anchored_offsets_rad={ + name: offsets[name] + for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS + }, ) - validate_runtime_curves_against_urdf_limits(payload, config.source_urdf) + payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits( + payload, paths["urdf"] + ) + if clipped_runtime_joints: + atomic_write_json(paths["json"], payload) + validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) mesh_resources = verify_urdf_mesh_resources(paths["urdf"]) mesh_hashes = { name: sha256_file(path) for name, path in mesh_resources.items() @@ -518,7 +587,10 @@ def finalize_session_artifacts( RETAINED_ACTIVE_ZERO_JOINTS ), "retained_passive_urdf_joints": sorted(PASSIVE_JOINTS), - "mimic_derived_dynamic_joints": sorted(MIMIC_DERIVED_FINGER_DIPS), + "mimic_derived_dynamic_joints": [], + "visually_measured_passive_joints": sorted( + VISUALLY_MEASURED_FINGER_DIPS + ), "changed_urdf_joint_origins": list(changed_joints), "hashes": { "source_urdf_sha256": config.source_urdf_sha256, 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 2cf5875..0ad597f 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 @@ -272,6 +272,17 @@ def three_camera_reason_zh( "检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。", ) if reason == "sweep_start_tag_timeout": + group_reasons = active.get("group_pnp_reasons", {}) + if isinstance(group_reasons, Mapping) and group_reasons: + reason_text = "、".join( + f"{VIEW_NAMES_ZH.get(str(view), str(view))}={value}" + for view, value in group_reasons.items() + ) + return ( + "被测电机已经到达扫描起点，所需Tag也可见，但整组PnP初始化持续" + f"拒绝候选（{reason_text}），因此没有生成同步端点帧。", + "不要重新粘贴可见Tag；保留当前会话并复制诊断块给开发者检查PnP安装先验。", + ) return ( "被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的" "同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”，不会触发此错误。", @@ -315,8 +326,9 @@ def three_camera_reason_zh( "rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS", "axis_plane_rms_mm": "三维圆轴向RMS", "axis_radial_rms_mm": "三维圆半径RMS", + "axis_pose_line_rms_mm": "姿态轨迹轴线RMS", "rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角", - "axis_cycle_difference_deg": "三轮转轴方向极差", + "axis_cycle_difference_deg": "各轮转轴方向极差", "third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差", "third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS", "third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95", @@ -340,6 +352,7 @@ def three_camera_reason_zh( "rotation_orthogonal_rms_deg": "°", "axis_plane_rms_mm": "mm", "axis_radial_rms_mm": "mm", + "axis_pose_line_rms_mm": "mm", "rotation_circle_axis_difference_deg": "°", "axis_cycle_difference_deg": "°", "third_cycle_axis_holdout_deg": "°", 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 9cc4697..89726ce 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 @@ -17,7 +17,9 @@ from typing import Any, Mapping, Sequence import numpy as np import rclpy from apriltag_msgs.msg import AprilTagDetectionArray +from rclpy.exceptions import ParameterUninitializedException from rclpy.node import Node +from rclpy.parameter import Parameter from rclpy.qos import qos_profile_sensor_data from scipy.spatial.transform import Rotation from sensor_msgs.msg import CameraInfo, JointState @@ -51,6 +53,7 @@ from .full_hand import ( G20_RIGHT_19_LAYOUT, LEFT_HAND_PROFILE, RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, + RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, THREE_CAMERA_BASELINE_COMMAND, HandCalibrationProfile, JointCurveFit, @@ -86,6 +89,7 @@ from .urdf_zero import ( circle_direction_is_constrained, fit_joint_axis_measurement, fit_rotation_joint_curve, + derive_right_19_flexion_endpoint_offsets, joint_curve_holdout_errors, measure_joint_curve_observation, solve_urdf_zero_offsets, @@ -264,6 +268,130 @@ def _sweep_joints_for_view( ) +def resume_tasks_invalidated_by_tag_size_changes( + profile: HandCalibrationProfile, + previous_sizes_m_by_id: Mapping[int, float], + current_sizes_m_by_id: Mapping[int, float], +) -> tuple[tuple[int, ...], tuple[str, ...]]: + """Localise a corrected physical Tag size to dependent motion tasks. + + PnP translations stored in a checkpoint are already scaled by the Tag + size used during acquisition and cannot be repaired exactly afterwards. + A size-only product correction therefore invalidates every task that used + one of those Tags, while complete tasks whose required Tags are unchanged + remain valid sparse checkpoints. + """ + previous = { + int(key): float(value) + for key, value in previous_sizes_m_by_id.items() + } + current = { + int(key): float(value) + for key, value in current_sizes_m_by_id.items() + } + if set(previous) != set(current): + raise ValueError("resume Tag-size maps contain different Tag IDs") + if any( + not math.isfinite(value) or value <= 0.0 + for value in (*previous.values(), *current.values()) + ): + raise ValueError("resume Tag sizes must be finite and positive") + changed_ids = { + tag_id + for tag_id in current + if not math.isclose( + previous[tag_id], current[tag_id], rel_tol=0.0, abs_tol=1.0e-12 + ) + } + affected: list[str] = [] + base_role_by_view = { + "front": "front_base", + "side": "side_base", + "top": "top_base", + } + for spec in profile.sweep_specs: + used_ids: set[int] = set() + for view in _sweep_views(profile, spec): + used_ids.add(int(profile.view_tags[view][base_role_by_view[view]])) + for joint_name in _sweep_joints_for_view(profile, spec, view): + joint = profile.record_specs[joint_name] + for role in (joint.parent_role, joint.child_role): + if role is not None: + used_ids.add(int(profile.view_tags[view][role])) + if used_ids & changed_ids: + affected.append(spec.key) + return tuple(sorted(changed_ids)), tuple(affected) + + +def _fit_retry_joint_names( + profile: HandCalibrationProfile, + spec: SweepSpec, + failures: Sequence[Mapping[str, Any]], +) -> set[str]: + """Resolve the independently failed measurements in a physical task.""" + selected: set[str] = set() + validation_sources = profile.axis_validation_sources or {} + for failure in failures: + name = str(failure.get("joint", "")) + if str(failure.get("metric", "")) == "cross_view_roll_curve": + name = str(validation_sources.get(name, name)) + if name in spec.joints: + selected.add(name) + return selected or set(spec.joints) + + +def _isolated_axis_cycle_outliers( + axes: Sequence[np.ndarray], limit_rad: float +) -> set[int]: + """Return one unambiguous outlier outside an all-other-cycle cluster. + + At least three mutually consistent inlier cycles are required. Ambiguous + or generally scattered axes deliberately return an empty set so callers + retain the conservative full-task retry. + """ + if len(axes) < 4: + return set() + candidates: list[tuple[float, int]] = [] + for excluded in range(len(axes)): + inliers = [ + np.asarray(axis, dtype=float) + for index, axis in enumerate(axes) + if index != excluded + ] + consistent = True + maximum_inlier_difference = 0.0 + for left_index, left in enumerate(inliers): + for right in inliers[left_index + 1 :]: + difference = math.acos( + abs(float(np.clip(left @ right, -1.0, 1.0))) + ) + maximum_inlier_difference = max( + maximum_inlier_difference, difference + ) + if difference > float(limit_rad): + consistent = False + break + if not consistent: + break + if consistent: + candidates.append((maximum_inlier_difference, excluded)) + if not candidates: + return set() + candidates.sort() + best_spread, best_excluded = candidates[0] + # A gradual end-to-end drift can produce two different all-but-one + # subsets just under the formal limit. Localize only when one three-cycle + # cluster is substantially tighter than the acceptance band. + if best_spread > 0.5 * float(limit_rad): + return set() + if ( + len(candidates) > 1 + and abs(candidates[1][0] - best_spread) <= math.radians(0.01) + ): + return set() + return {best_excluded} + + def _fixed_base_role(view: str) -> str: return { "front": "front_base", @@ -575,6 +703,19 @@ def _unresolved_fit_failure_tasks( # failures that mixed velocity lag or firmware tracking # deadband with mechanical hysteresis are safe to revalidate. return True + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and joint_name + in RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS + and metric == "axis_pose_line_rms_mm" + ): + # Product v19 originally treated the correlated per-frame + # pose-line residual of an incorrectly scaled passive-DIP Tag as the + # uncertainty of its fitted axis centre. Current code + # revalidates those complete raw trajectories using radial, + # cross-cycle and holdout gates, so the old rejection itself + # must not force another eight-direction scan. + return True return metric in { "rotation_circle_axis_difference_deg", "axis_plane_rms_mm", @@ -1085,6 +1226,7 @@ class ViewRuntime: tracker: SquareTagPoseTracker group_tracker: SquareTagGroupPoseTracker view_tags: Mapping[str, int] = field(default_factory=dict) + tag_sizes_m_by_role: Mapping[str, float] = field(default_factory=dict) preflight_roles: tuple[str, ...] = () role_by_id: dict[int, str] = field(default_factory=dict) group_trackers: dict[ @@ -1132,6 +1274,13 @@ class ViewRuntime: 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") + if any(role not in self.view_tags for role in self.tag_sizes_m_by_role): + raise ValueError("Tag-size roles must be configured for the view") + if any( + not math.isfinite(float(size)) or float(size) <= 0.0 + for size in self.tag_sizes_m_by_role.values() + ): + raise ValueError("Tag sizes must be finite and positive") self.current_required_roles = tuple(self.preflight_roles) self.group_trackers[tuple(self.preflight_roles)] = self.group_tracker @@ -1237,6 +1386,7 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_sweep_spec: SweepSpec | None = None self.retry_resume_index: int | None = None self.retry_sweep_items: list[SweepItem] = [] + self.retry_joint_names: set[str] = set() self.active_sweep_is_fit_retry = False self.fit_failure: dict[str, Any] = {} self.fit_failure_history_by_task: dict[ @@ -1359,6 +1509,12 @@ class G20ThreeCameraCalibrationNode(Node): f"/g20_calibration/{view}/apriltag/detections", ) self.declare_parameter("tag_size_m", 0.016) + self.declare_parameter( + "tag_size_override_ids", Parameter.Type.INTEGER_ARRAY + ) + self.declare_parameter( + "tag_size_overrides_m", Parameter.Type.DOUBLE_ARRAY + ) self.declare_parameter( "baseline_command_u8", list(THREE_CAMERA_BASELINE_COMMAND) ) @@ -1414,6 +1570,7 @@ class G20ThreeCameraCalibrationNode(Node): ) self.declare_parameter("zero_maximum_offset_deg", 20.0) self.declare_parameter("zero_finger_maximum_offset_deg", 3.0) + self.declare_parameter("flexion_endpoint_maximum_offset_deg", 5.0) self.declare_parameter("endpoint_tolerance_u8", 2.0) self.declare_parameter( "steady_checkpoint_command_feedback_tolerance_u8", 8.0 @@ -1510,6 +1667,17 @@ class G20ThreeCameraCalibrationNode(Node): def value(name: str) -> Any: return self.get_parameter(name).value + def optional_array(name: str) -> Sequence[Any]: + # An empty YAML sequence has no element type. With a typed ROS 2 + # declaration it can therefore remain NOT_SET and ``.value`` + # raises instead of returning an empty list. These overrides are + # optional, so preserve the default-size semantics in that case. + try: + result = value(name) + except ParameterUninitializedException: + return () + return () if result is None else result + self.hand_type = str(value("hand_type")).lower() self.tag_layout = str(value("tag_layout")).lower() self.profile: HandCalibrationProfile = get_hand_calibration_profile( @@ -1578,6 +1746,46 @@ class G20ThreeCameraCalibrationNode(Node): for view in ("front", "side", "top") } self.tag_size_m = float(value("tag_size_m")) + override_ids = tuple( + int(item) for item in optional_array("tag_size_override_ids") + ) + override_sizes = tuple( + float(item) for item in optional_array("tag_size_overrides_m") + ) + if len(override_ids) != len(override_sizes): + raise ValueError( + "tag_size_override_ids and tag_size_overrides_m must have " + "the same length" + ) + if len(set(override_ids)) != len(override_ids): + raise ValueError("tag_size_override_ids must be unique") + configured_tag_ids = { + int(tag_id) + for tags in self.profile.view_tags.values() + for tag_id in tags.values() + } + if ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and any(tag_id not in configured_tag_ids for tag_id in override_ids) + ): + raise ValueError("Tag-size override contains an unconfigured Tag ID") + if ( + not math.isfinite(self.tag_size_m) + or self.tag_size_m <= 0.0 + or any( + not math.isfinite(size) or size <= 0.0 + for size in override_sizes + ) + ): + raise ValueError("Tag sizes must be finite and positive") + self.tag_sizes_m_by_id = { + tag_id: self.tag_size_m for tag_id in configured_tag_ids + } + self.tag_sizes_m_by_id.update( + (tag_id, size) + for tag_id, size in zip(override_ids, override_sizes) + if tag_id in configured_tag_ids + ) self.baseline_command = tuple( int(item) for item in value("baseline_command_u8") ) @@ -1688,7 +1896,10 @@ class G20ThreeCameraCalibrationNode(Node): 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.flexion_endpoint_maximum_offset_rad = math.radians( + float(value("flexion_endpoint_maximum_offset_deg")) + ) + self.zero_joint_maximum_offsets_rad = {} self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) self.steady_checkpoint_command_feedback_tolerance_u8 = float( value("steady_checkpoint_command_feedback_tolerance_u8") @@ -1867,7 +2078,7 @@ class G20ThreeCameraCalibrationNode(Node): r"[0-9a-f]{64}", self.source_urdf_expected_sha256 ) is None: raise ValueError( - "g20_right_15 requires source_urdf_expected_sha256 from " + "g20_right_19 requires source_urdf_expected_sha256 from " "the CAD/hardware owner; unconfirmed source CAD cannot " "produce a formal corrected URDF" ) @@ -2006,6 +2217,7 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cone_mismatch_rad, self.zero_maximum_offset_rad, self.zero_finger_maximum_offset_rad, + self.flexion_endpoint_maximum_offset_rad, self.maximum_validation_error_rad, self.zero_maximum_confidence_half_width_rad, self.task_precheck_hold_seconds, @@ -2033,7 +2245,7 @@ class G20ThreeCameraCalibrationNode(Node): and self.repetitions < 4 ): raise ValueError( - "g20_right_15 requires at least 4 repetitions: at least 3 " + "g20_right_19 requires at least 4 repetitions: at least 3 " "training cycles and one isolated holdout" ) if ( @@ -2067,7 +2279,7 @@ class G20ThreeCameraCalibrationNode(Node): and self.profile.layout_id != G20_RIGHT_19_LAYOUT ): raise ValueError( - "cross-view roll diagnostic requires g20_right_15" + "cross-view roll diagnostic requires g20_right_19" ) if not 1.0 <= self.provisional_warning_ratio <= 2.0: raise ValueError("provisional_warning_ratio must be in [1, 2]") @@ -2121,7 +2333,19 @@ class G20ThreeCameraCalibrationNode(Node): # 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" and len(role_tuple) <= 4 else () + tuple( + pair + for pair in adjacent_pairs + # Distal 16 mm Tags may be carried by a rigid extension rod + # to remain visible through PIP flexion. Its mounting plane + # is arbitrary and must not be required to be parallel to the + # middle-phalanx Tag. Static rigidity and relative-pose + # continuity still disambiguate that PIP->DIP pair; retain + # the palm/base->middle-Tag normal prior unchanged. + if not pair[1].endswith("_dip") + ) + if name == "side" and len(role_tuple) <= 4 + else () ) thumb_mcp_ip_roles = {"thumb_cmc", "thumb_mcp", "thumb_ip"} thumb_mcp_ip_coupling = () @@ -2198,6 +2422,10 @@ class G20ThreeCameraCalibrationNode(Node): tracker=tracker, group_tracker=group, view_tags=self.profile.view_tags[name], + tag_sizes_m_by_role={ + role: self.tag_sizes_m_by_id[int(tag_id)] + for role, tag_id in self.profile.view_tags[name].items() + }, preflight_roles=preflight_roles, ) @@ -2606,7 +2834,11 @@ class G20ThreeCameraCalibrationNode(Node): pose, rejection = runtime.tracker.estimate( role, corners_by_role[role], - tag_size_m=runtime.tag_size_m, + tag_size_m=float( + runtime.tag_sizes_m_by_role.get( + role, runtime.tag_size_m + ) + ), camera_matrix=runtime.camera_matrix, stamp_ns=stamp, ) @@ -2749,9 +2981,23 @@ class G20ThreeCameraCalibrationNode(Node): runtime.last_message_at = now runtime.detection_times.append(now) runtime.valid_flags.append(valid) - if runtime.current_required_roles != tuple( - runtime.preflight_roles + task_sweep_capture = bool( + self.state == STATE_SWEEP + and self.active_sweep is not None + and view in _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + ) + if ( + runtime.current_required_roles + != tuple(runtime.preflight_roles) + and task_sweep_capture ): + # Attempt-level validity is a motion-trajectory metric. PREPARE + # deliberately rejects the first N frames while the group PnP + # tracker initializes; counting those expected frames made clean + # side tasks report about 94% and triggered full four-round + # rescans despite 100% visibility prechecks and dense trajectories. runtime.task_total_frames += 1 if valid: runtime.task_valid_frames += 1 @@ -3060,16 +3306,23 @@ class G20ThreeCameraCalibrationNode(Node): ) def _prepare_failed_sweep_retry(self) -> SweepSpec: - """Discard only failed cycles and queue their two sweep directions.""" + """Discard only failed measurements/cycles and queue two 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)))) + retry_joint_names = set( + getattr(self, "retry_joint_names", set()) or spec.joints + ) + retry_joint_names.intersection_update(spec.joints) + if not retry_joint_names: + retry_joint_names = set(spec.joints) + self.retry_joint_names = retry_joint_names baseline_records_by_joint = getattr( self, "baseline_records_by_joint", {} ) - for joint_name in spec.joints: + for joint_name in retry_joint_names: self.records_by_joint[joint_name][:] = [ record for record in self.records_by_joint[joint_name] @@ -3110,13 +3363,24 @@ class G20ThreeCameraCalibrationNode(Node): # retry. Raw samples remain logged and all final quality gates stay # unchanged. reset_trackers = getattr(self, "_reset_view_trackers", None) - for view in _sweep_views(_node_profile(self), spec): + retry_views = { + _node_profile(self).record_specs[name].view + for name in retry_joint_names + } + for view in retry_views: + if view is None: + continue runtime = getattr(self, "views", {}).get(view) if runtime is None or reset_trackers is None: continue reset_trackers(runtime) runtime.pnp_invalid_since = None runtime.pnp_reset_count += 1 + # Detection quality is attempt-local. Carrying the previous + # attempt into this retry made its percentage describe two PnP + # branches instead of the data currently being admitted. + runtime.task_valid_frames = 0 + runtime.task_total_frames = 0 storage_key = _sweep_storage_key(spec) self.sweep_attempts[storage_key] = ( self.sweep_attempts.get(storage_key, 1) + 1 @@ -3132,6 +3396,7 @@ class G20ThreeCameraCalibrationNode(Node): **({"task_name": spec.key} if spec.task_name else {}), "motor_index": spec.motor_index, "joints": list(spec.joints), + "joints_to_rescan": sorted(retry_joint_names), "attempt": self.sweep_attempts[storage_key], "reason": self.paused_reason, "cycles": [cycle + 1 for cycle in sorted(cycles)], @@ -3167,6 +3432,13 @@ class G20ThreeCameraCalibrationNode(Node): if len(starts) != 1: raise RuntimeError("resume raw must contain exactly one session_start") start = starts[0] + previous_tag_sizes = { + int(tag_id): float(size) + for tag_id, size in dict( + start.get("tag_sizes_m_by_id", {}) + ).items() + } + current_tag_sizes = dict(getattr(self, "tag_sizes_m_by_id", {})) if ( str(start.get("hand_type")) != self.hand_type or str(start.get("tag_layout")) != self.profile.layout_id @@ -3180,6 +3452,25 @@ class G20ThreeCameraCalibrationNode(Node): raise RuntimeError( "resume checkpoint geometry, Tag layout, baseline or source URDF differs" ) + try: + changed_tag_size_ids, size_invalidated_tasks = ( + resume_tasks_invalidated_by_tag_size_changes( + self.profile, + previous_tag_sizes, + current_tag_sizes, + ) + ) + except ValueError as error: + raise RuntimeError( + "resume checkpoint Tag-size geometry differs incompatibly" + ) from error + invalidated_task_set = set(size_invalidated_tasks) + if invalidated_task_set: + rows = [ + row + for row in rows + if str(row.get("task_name", "")) not in invalidated_task_set + ] completed, reusable = resumable_completed_task_prefix( self.profile, self.repetitions, @@ -3241,6 +3532,8 @@ class G20ThreeCameraCalibrationNode(Node): ), "pending_task_keys": missing_tasks, "revalidation_dropped_tasks": dropped, + "tag_size_changed_ids": list(changed_tag_size_ids), + "tag_size_invalidated_task_keys": list(size_invalidated_tasks), "imported_record_count": len(reusable), "source_raw_samples_sha256": _file_sha256(source), }, @@ -3377,6 +3670,7 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_sweep_spec = None self.retry_resume_index = None self.retry_sweep_items.clear() + self.retry_joint_names.clear() self.active_sweep_is_fit_retry = False self.fit_failure = {} self.fit_failure_history_by_task.clear() @@ -3433,6 +3727,10 @@ class G20ThreeCameraCalibrationNode(Node): }, "tag_family": "36h11", "tag_size_m": float(self.tag_size_m), + "tag_sizes_m_by_id": { + str(tag_id): float(self.tag_sizes_m_by_id[tag_id]) + for tag_id in sorted(self.tag_sizes_m_by_id) + }, "baseline_command_u8": [ int(value) for value in self.baseline_command ], @@ -4659,7 +4957,16 @@ class G20ThreeCameraCalibrationNode(Node): preserve_task_reference = bool( not item.precheck and item.cycle > 0 ) - for view in _sweep_views(_node_profile(self), item.spec): + reset_views = set(_sweep_views(_node_profile(self), item.spec)) + if getattr(self, "active_sweep_is_fit_retry", False): + scoped = set(getattr(self, "retry_joint_names", set())) + if scoped: + reset_views = { + _node_profile(self).record_specs[name].view + for name in scoped + } + reset_views.discard(None) + for view in reset_views: runtime = getattr(self, "views", {}).get(view) if runtime is None or reset_trackers is None: continue @@ -4828,7 +5135,14 @@ class G20ThreeCameraCalibrationNode(Node): ) -> None: """Persist one steady requested-command/feedback/vision observation.""" motor = int(item.spec.motor_index) - for joint_name in item.spec.joints: + record_joint_names = tuple(item.spec.joints) + if getattr(self, "active_sweep_is_fit_retry", False): + scoped = set(getattr(self, "retry_joint_names", set())) + if scoped: + record_joint_names = tuple( + name for name in item.spec.joints if name in scoped + ) + for joint_name in record_joint_names: selected = _frames_for_joint(frames, joint_name) if len(selected) < 3: raise RuntimeError( @@ -5415,6 +5729,21 @@ class G20ThreeCameraCalibrationNode(Node): if include_view_validity and hasattr(self, "views"): for view in _sweep_views(profile, spec): runtime = self.views[view] + view_joints = _sweep_joints_for_view(profile, spec, view) + validation_only_view = bool(view_joints) and all( + profile.record_specs[name].zero_kind + == "axis_cross_view_validation" + for name in view_joints + ) + retry_scope = set( + getattr(self, "retry_joint_names", set()) + if getattr(self, "retry_sweep_spec", None) == spec + else () + ) + if retry_scope and not retry_scope.intersection(view_joints): + # This view's accepted records were intentionally kept; + # only the independently failed view is being measured. + continue valid_rate = float(runtime.valid_rate) # Prefer the task-scoped counters: after the last sweep the # required-role set flips back to the full preflight set and @@ -5434,10 +5763,8 @@ class G20ThreeCameraCalibrationNode(Node): if ( not imported_without_capture and valid_rate < self.minimum_detection_rate + and not validation_only_view ): - view_joints = _sweep_joints_for_view( - profile, spec, view - ) failures.append( { "joint": view_joints[0], @@ -5921,20 +6248,31 @@ class G20ThreeCameraCalibrationNode(Node): ), ) if maximum_difference > self.zero_maximum_axis_cycle_difference_rad: - failures.append( - { - "joint": joint_name, - "metric": "axis_cycle_difference_deg", - "actual": round(math.degrees(maximum_difference), 6), - "limit": round( - math.degrees( - self.zero_maximum_axis_cycle_difference_rad - ), - 6, - ), - "comparison": "maximum", - } + outliers = _isolated_axis_cycle_outliers( + cycle_axes, + self.zero_maximum_axis_cycle_difference_rad, ) + failure = { + "joint": joint_name, + "metric": "axis_cycle_difference_deg", + "actual": round(math.degrees(maximum_difference), 6), + "limit": round( + math.degrees( + self.zero_maximum_axis_cycle_difference_rad + ), + 6, + ), + "comparison": "maximum", + } + if outliers: + outlier = next(iter(outliers)) + failure["cycle"] = outlier + 1 + failure["inlier_cycles"] = [ + index + 1 + for index in range(len(cycle_axes)) + if index != outlier + ] + failures.append(failure) return failures def _cross_view_roll_diagnostic_role( @@ -6099,6 +6437,9 @@ class G20ThreeCameraCalibrationNode(Node): attempt = self.sweep_attempts.get(_sweep_storage_key(spec), 1) self.retry_resume_index = self.sweep_index self.retry_sweep_spec = spec + self.retry_joint_names = _fit_retry_joint_names( + _node_profile(self), spec, failures + ) localized_cycles = { int(item["cycle"]) - 1 for item in failures @@ -6141,6 +6482,7 @@ class G20ThreeCameraCalibrationNode(Node): "task_name": spec.key, "motor_index": spec.motor_index, "joints": list(spec.joints), + "joints_to_rescan": sorted(self.retry_joint_names), "attempt": attempt, "cycles_to_rescan": [ cycle + 1 for cycle in sorted(self.retry_cycles) @@ -6264,6 +6606,7 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_sweep_spec = None self.retry_resume_index = None self.retry_cycles = set() + self.retry_joint_names = set() self.fit_failure = { "kind": "zero_model_failure", "view": spec.view, @@ -6296,7 +6639,7 @@ class G20ThreeCameraCalibrationNode(Node): ) if self.profile.layout_id == G20_RIGHT_19_LAYOUT: atomic_write_json( - self.session_dir / "g20_right_15_failed_diagnostics.json", + self.session_dir / "g20_right_19_failed_diagnostics.json", { "schema_version": 5, "layout_id": G20_RIGHT_19_LAYOUT, @@ -6410,7 +6753,14 @@ class G20ThreeCameraCalibrationNode(Node): motor = item.spec.motor_index command = int(self.baseline_command[motor]) attempt = self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) - for joint_name in item.spec.joints: + record_joint_names = tuple(item.spec.joints) + if getattr(self, "active_sweep_is_fit_retry", False): + scoped = set(getattr(self, "retry_joint_names", set())) + if scoped: + record_joint_names = tuple( + name for name in item.spec.joints if name in scoped + ) + for joint_name in record_joint_names: selected = _frames_for_joint(frames, joint_name) if len(selected) < minimum_frames: raise RuntimeError( @@ -6549,8 +6899,15 @@ class G20ThreeCameraCalibrationNode(Node): target_tolerance = self._endpoint_tolerance_for_spec( item.spec, item.target_u8 ) + record_joint_names = tuple(item.spec.joints) + if getattr(self, "active_sweep_is_fit_retry", False): + scoped = set(getattr(self, "retry_joint_names", set())) + if scoped: + record_joint_names = tuple( + name for name in item.spec.joints if name in scoped + ) joint_bins: dict[str, dict[int, list[FrameObservation]]] = {} - for joint_name in item.spec.joints: + for joint_name in record_joint_names: bins: dict[int, list[FrameObservation]] = {} for frame in _frames_for_joint(self.sweep_frames, joint_name): state = float(frame.state_u8[motor]) @@ -6704,7 +7061,7 @@ class G20ThreeCameraCalibrationNode(Node): ) -> dict[str, Any]: names = [ name - for name in item.spec.joints + for name in record_joint_names if name in frame.joint_quaternions_xyzw ] return { @@ -6755,12 +7112,12 @@ class G20ThreeCameraCalibrationNode(Node): for frame in self.sweep_frames if any( name in frame.joint_quaternions_xyzw - for name in item.spec.joints + for name in record_joint_names ) ), ) - for joint_name in item.spec.joints: + for joint_name in record_joint_names: bins = joint_bins[joint_name] commands = sorted(bins) for command in commands: @@ -6905,6 +7262,8 @@ class G20ThreeCameraCalibrationNode(Node): return self.retry_sweep_spec = None self.retry_resume_index = None + self.retry_cycles = set() + self.retry_joint_names = set() self.fit_failure = {} if self.sweep_index >= len(self.sweep_items): self._begin_return_baseline("fit") @@ -6939,6 +7298,8 @@ class G20ThreeCameraCalibrationNode(Node): resume_index = self.retry_resume_index self.retry_sweep_spec = None self.retry_resume_index = None + self.retry_cycles = set() + self.retry_joint_names = set() if resume_index is not None: self.sweep_index = resume_index self.fit_failure = {} @@ -6965,6 +7326,14 @@ class G20ThreeCameraCalibrationNode(Node): def _fit_all_curves(self) -> None: self.state = STATE_FITTING self.reason = "fitting_3d_axes_and_urdf_zero_offsets" + # Fitting is deliberately synchronous so no capture callback can + # mutate the observation set midway through a model fit. Announce + # the state before entering the CPU-bound section: the outer product + # runner then applies its fitting-specific watchdog instead of + # mistaking a blocked ROS timer for a motion communication failure. + fitting_started_at = time.monotonic() + self.last_status_publish = fitting_started_at + self._publish_status(fitting_started_at) # 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: @@ -7233,6 +7602,15 @@ class G20ThreeCameraCalibrationNode(Node): name: int(spec.motor_index) for name, spec in self.profile.joint_specs.items() } + endpoint_zero_offsets = ( + derive_right_19_flexion_endpoint_offsets( + self.source_urdf_path, + command_fits, + maximum_offset_rad=self.flexion_endpoint_maximum_offset_rad, + ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else {} + ) holdout_zero_result = solve_urdf_zero_offsets( source_urdf=self.source_urdf_path, measurements=axes, @@ -7269,6 +7647,10 @@ class G20ThreeCameraCalibrationNode(Node): tag_layout=self.profile.layout_id, training_cycles=training_cycles, validation_cycle=validation_cycle, + fixed_direct_zero_offsets_rad={ + **self.zero_profile.fixed_direct_zero_offsets_rad, + **endpoint_zero_offsets, + }, ) append_jsonl( self.raw_path, @@ -7492,6 +7874,7 @@ class G20ThreeCameraCalibrationNode(Node): command_fits, profile=self.profile, ), + endpoint_anchored_offsets_rad=endpoint_zero_offsets, ) self.axis_measurements = axes self.zero_result = zero_result @@ -8046,6 +8429,7 @@ class G20ThreeCameraCalibrationNode(Node): output_directory=self.corrected_urdf_output_dir, serial_number=self.serial_number, offsets_rad=urdf_offsets, + endpoint_anchored_offsets_rad=endpoint_zero_offsets, timestamp=stamp, ) try: 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 bfc582d..5485704 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 @@ -23,6 +23,7 @@ from .full_hand import ( IMAGE_TRAJECTORY_JOINTS, LEFT_HAND_PROFILE, RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, + RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, HandCalibrationProfile, JointCurveFit, get_hand_calibration_profile, @@ -137,7 +138,15 @@ def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: def _build_right_19_zero_profile( hand: HandCalibrationProfile, ) -> ZeroCalibrationProfile: - """Return the 12-observable-static-zero graph for the 15-Tag layout.""" + """Return the 16-active-zero graph for the 19-Tag layout. + + Distal Tags recover all four PIP/DIP dynamic curves and axis quality, but + repeated motion with one unchanged camera/Tag installation cannot by + itself distinguish a fixed parallel-axis phase bias from an encoder zero. + The caller therefore anchors MCP-pitch and PIP to the independently known + full-flexion contact endpoint using their measured command curves. DIP + stays passive and has no independently identifiable static zero. + """ fingers = ("index", "middle", "ring", "pinky") direct = ( "thumb_cmc_roll", @@ -147,7 +156,7 @@ def _build_right_19_zero_profile( *( f"{finger}_{suffix}" for finger in fingers - for suffix in ("mcp_roll", "mcp_pitch") + for suffix in ("mcp_roll", "mcp_pitch", "pip") ), ) axis_joints = ( @@ -159,7 +168,7 @@ def _build_right_19_zero_profile( *( f"{finger}_{suffix}" for finger in fingers - for suffix in ("mcp_roll", "mcp_pitch", "pip") + for suffix in ("mcp_roll", "mcp_pitch", "pip", "dip") ), ) axis_parent = { @@ -176,7 +185,10 @@ def _build_right_19_zero_profile( **{ f"{finger}_{child}": f"{finger}_{parent}" for finger in fingers - for child, parent in (("pip", "mcp_pitch"),) + for child, parent in ( + ("pip", "mcp_pitch"), + ("dip", "pip"), + ) }, } observer = { @@ -190,6 +202,7 @@ def _build_right_19_zero_profile( for target, observed in ( ("mcp_roll", "mcp_pitch"), ("mcp_pitch", "pip"), + ("pip", "dip"), ) }, } @@ -206,6 +219,11 @@ def _build_right_19_zero_profile( # axis to constrain that circle; PIP retains its independently fitted # axis line and all final SE(3)/holdout quality gates. | set(RIGHT_19_END_ON_IMAGE_CURVE_JOINTS) + # _fit_axis_measurement_raw constrains every passive DIP direction to + # its source-URDF-parallel upstream PIP. Declare the same fact here + # so validation does not compare that trusted direction with the + # depth-biased free plane of a distal Tag trajectory. + | set(RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS) ) return ZeroCalibrationProfile( hand=hand, @@ -218,15 +236,60 @@ def _build_right_19_zero_profile( axis_parent_joint=axis_parent, phase_parent_joint=phase_parent, offset_observer_joint=observer, - # Keep the commit-proven thumb reference convention: the passive-IP - # observation is sufficiently good for the CMC/MCP trajectory, but - # not for redefining the MCP CAD zero. Optimising this value made the - # whole downstream thumb chain move during otherwise good CMC fits. + # thumb_mcp retains the commit-proven CAD reference. Four-finger + # flexion offsets are supplied per session from the measured endpoint, + # not stored as constants in this profile. fixed_direct_zero_offsets_rad={"thumb_mcp": 0.0}, static_output_zero_offsets_rad={}, ) +RIGHT_19_FLEXION_ENDPOINT_JOINTS = frozenset( + f"{finger}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + for suffix in ("mcp_pitch", "pip") +) + + +def derive_right_19_flexion_endpoint_offsets( + source_urdf: str | Path, + curves: Mapping[str, JointCurveFit], + *, + maximum_offset_rad: float = math.radians(5.0), +) -> dict[str, float]: + """Estimate flexion origins from the measured full-flexion contact. + + G20 feedback 0 is the independently verified palm-contact endpoint. The + source CAD upper angle describes that same physical endpoint. Therefore + ``origin_offset + measured_endpoint == CAD_upper``. This makes every + returned value session-derived while avoiding the fixed planar-PnP phase + bias that affected the parallel MCP/PIP/DIP axes. + """ + if not math.isfinite(maximum_offset_rad) or maximum_offset_rad <= 0.0: + raise ValueError("flexion endpoint maximum offset must be positive") + root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot() + joints = {str(node.get("name")): node for node in root.findall("joint")} + result: dict[str, float] = {} + for name in sorted(RIGHT_19_FLEXION_ENDPOINT_JOINTS): + fit = curves.get(name) + joint = joints.get(name) + limit = None if joint is None else joint.find("limit") + if fit is None: + raise ValueError(f"missing measured endpoint curve for {name}") + if limit is None or limit.get("upper") is None: + raise ValueError(f"source URDF joint {name} has no upper limit") + upper = float(limit.get("upper")) + measured_endpoint = float(np.max(np.asarray(fit.angle_rad, dtype=float))) + offset = upper - measured_endpoint + if not math.isfinite(offset) or abs(offset) > maximum_offset_rad: + raise ValueError( + f"{name} endpoint-derived zero offset is outside the safe range: " + f"{math.degrees(offset):.3f}deg" + ) + result[name] = offset + return result + + def get_zero_calibration_profile( side: str, layout_id: str = "legacy_11" ) -> ZeroCalibrationProfile: @@ -2398,10 +2461,13 @@ def solve_urdf_zero_offsets( failures[offset_joint] = ( "zero_phase_axis_line_angle_uncertainty_too_large" ) - elif any( - item.pose_axis_line_rms_m - > maximum_pose_axis_line_rms_m - for item in phase_items + elif ( + profile.hand.layout_id != G20_RIGHT_19_LAYOUT + and 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" @@ -2548,6 +2614,7 @@ def solve_urdf_zero_offsets( 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 ) @@ -2634,14 +2701,20 @@ def solve_urdf_zero_offsets( > maximum_offset_rad + math.radians(0.01) ): configured_limit_exceeded.append("finger_mcp_roll_common_mode") - 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) - ] + diagnostic_bound_hits: list[str] = [] + for name, limit in zip( + profile.direct_zero_joints, diagnostic_offset_limits + ): + if name in fixed_offsets: + continue + checked_offset = training_offsets[name] + if name in product_finger_rolls: + # Match the configured-limit and publication convention above. + # The raw common roll is a fitted-palm-frame gauge; only the + # finger-to-finger deviation is a physical zero correction. + checked_offset -= finger_roll_common_mode + if abs(checked_offset) >= limit - math.radians(0.01): + diagnostic_bound_hits.append(name) failure_reasons: dict[str, str] = {} if ( profile.hand.layout_id == G20_RIGHT_19_LAYOUT @@ -2859,6 +2932,7 @@ def write_zero_corrected_urdf( output_directory: str | Path, serial_number: str, offsets_rad: Mapping[str, float], + endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, timestamp: str | None = None, ) -> Path: source = Path(source_urdf).expanduser().resolve() @@ -2875,6 +2949,12 @@ def write_zero_corrected_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()} + endpoint_offsets = { + str(name): float(value) + for name, value in dict(endpoint_anchored_offsets_rad or {}).items() + } + if not set(endpoint_offsets) <= set(offsets): + raise ValueError("endpoint-anchored offsets must be URDF zero targets") if any( not math.isfinite(value) or abs(value) > math.radians(90.0) for value in offsets.values() @@ -2897,12 +2977,30 @@ def write_zero_corrected_urdf( root = tree.getroot() original_text = source.read_text(encoding="utf-8") replacement_rpy: dict[str, str] = {} + replacement_upper: dict[str, str] = {} + replacement_mimic_offset: dict[str, str] = {} found: set[str] = set() for joint in root.findall("joint"): name = str(joint.get("name")) if name not in offsets: + mimic = joint.find("mimic") + source_joint = None if mimic is None else str(mimic.get("joint")) + if source_joint in endpoint_offsets: + multiplier = float(mimic.get("multiplier", "1")) + old_offset = float(mimic.get("offset", "0")) + calibrated_mimic_offset = ( + old_offset + multiplier * endpoint_offsets[source_joint] + ) + replacement_mimic_offset[name] = ( + f"{calibrated_mimic_offset:.15g}" + ) continue found.add(name) + if name in endpoint_offsets: + limit = joint.find("limit") + if limit is None or limit.get("upper") is None: + raise ValueError(f"joint {name} has no upper limit") + replacement_upper[name] = f"{float(limit.get('upper')) - endpoint_offsets[name]:.15g}" # A statistically insignificant correction is represented as exact # zero. Preserve that joint's source text byte-for-byte instead of # serialising an equivalent Euler triplet. @@ -2944,19 +3042,53 @@ def write_zero_corrected_urdf( for match in joint_pattern.finditer(original_text): name = match.group("name") if name not in replacement_rpy: - continue + if name not in replacement_upper and name not in replacement_mimic_offset: + continue block = match.group(0) - 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 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") + edits.append(( + match.start() + origin_match.start("rpy"), + match.start() + origin_match.end("rpy"), + replacement_rpy[name], + )) + if name in replacement_upper: + limit_match = re.search( + r"]*\bupper\s*=\s*([\"'])(?P[^\"']*)\1", + block, + re.DOTALL, + ) + if limit_match is None: + raise ValueError(f"joint {name} limit has no upper attribute") + edits.append(( + match.start() + limit_match.start("upper"), + match.start() + limit_match.end("upper"), + replacement_upper[name], + )) + if name in replacement_mimic_offset: + mimic_match = re.search( + r"]*\boffset\s*=\s*([\"'])(?P[^\"']*)\1", + block, + re.DOTALL, + ) + if mimic_match is None: + raise ValueError(f"joint {name} mimic has no offset attribute") + edits.append(( + match.start() + mimic_match.start("offset"), + match.start() + mimic_match.end("offset"), + replacement_mimic_offset[name], + )) + expected_edit_count = ( + len(replacement_rpy) + + len(replacement_upper) + + len(replacement_mimic_offset) + ) 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/three_camera_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py index 25776a9..48b5ca1 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 @@ -43,10 +43,12 @@ def _launch_stack(context): if hand_type not in {"left", "right"}: raise RuntimeError("hand_type must be left or right") tag_layout = LaunchConfiguration("tag_layout").perform(context).lower() - if tag_layout not in {"legacy_11", "g20_right_15"}: - raise RuntimeError("tag_layout must be legacy_11 or g20_right_15") - if tag_layout == "g20_right_15" and hand_type != "right": - raise RuntimeError("g20_right_15 requires hand_type:=right") + if tag_layout not in {"legacy_11", "g20_right_15", "g20_right_19"}: + raise RuntimeError( + "tag_layout must be legacy_11, g20_right_15 or g20_right_19" + ) + if tag_layout in {"g20_right_15", "g20_right_19"} and hand_type != "right": + raise RuntimeError("G20 right product layout requires hand_type:=right") requested_tag_config = LaunchConfiguration("tag_config").perform(context) package_share = Path( get_package_share_directory("g20_thumb_apriltag_calibration") @@ -57,7 +59,9 @@ def _launch_stack(context): else package_share / "config" / ( - "three_camera_tags_g20_right_15.yaml" + "three_camera_tags_g20_right_19.yaml" + if tag_layout == "g20_right_19" + else "three_camera_tags_g20_right_15.yaml" if tag_layout == "g20_right_15" else "three_camera_tags.yaml" ) @@ -78,10 +82,10 @@ def _launch_stack(context): expected_source_hash = LaunchConfiguration( "source_urdf_expected_sha256" ).perform(context).strip().lower() - if tag_layout == "g20_right_15": + if tag_layout in {"g20_right_15", "g20_right_19"}: if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None: raise RuntimeError( - "g20_right_15 requires source_urdf_expected_sha256 confirmed " + "G20 right product layout requires source_urdf_expected_sha256 confirmed " "by the CAD/hardware owner" ) actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest() 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 index bff09a6..5afd475 100644 --- 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 @@ -161,7 +161,7 @@ def test_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> No mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))) assert mapper.input_domain == "command_u8" - assert mapper.layout_id == "g20_right_15" + assert mapper.layout_id == "g20_right_19" # midpoint(1.1, 0.9) is the original 0.001-rad/u8 curve. assert mapped["thumb_cmc_pitch"] == pytest.approx(0.155) assert payload["joints"]["pinky_dip"]["passive"] is True diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index 94bb0d1..1d28d68 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/src/g20_thumb_apriltag_calibration/test/test_config.py @@ -80,17 +80,17 @@ def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None: ]["ros__parameters"]["tag"]["frames"][0] == "side_base" -def test_right_15_tag_config_matches_the_physical_layout() -> None: +def test_right_19_tag_config_matches_the_physical_layout() -> None: tags = yaml.safe_load( ( PACKAGE_ROOT / "config" - / "three_camera_tags_g20_right_15.yaml" + / "three_camera_tags_g20_right_19.yaml" ).read_text() ) expected = { "front": [0, 1, 2, 3, 10, 11, 12, 13], - "side": [4, 5, 6, 15, 17], + "side": [4, 5, 6, 7, 14, 15, 16, 17, 18], "top": [8, 9], } all_ids: set[int] = set() @@ -99,14 +99,16 @@ def test_right_15_tag_config_matches_the_physical_layout() -> None: f"/g20_calibration/{view}/apriltag/apriltag" ]["ros__parameters"] assert parameters["tag"]["ids"] == ids - assert parameters["tag"]["sizes"] == [0.016] * len(ids) + expected_sizes = [0.016 for _tag_id in ids] + assert parameters["tag"]["sizes"] == expected_sizes all_ids.update(ids) - assert all_ids == set(range(19)) - {7, 14, 16, 18} + assert all_ids == set(range(19)) side_frames = tags[ "/g20_calibration/side/apriltag/apriltag" ]["ros__parameters"]["tag"]["frames"] assert side_frames == [ - "side_base", "ring_pip", "pinky_pip", "middle_pip", "index_pip", + "side_base", "ring_pip", "pinky_pip", "pinky_dip", "ring_dip", + "middle_pip", "middle_dip", "index_pip", "index_dip", ] @@ -117,6 +119,8 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: parameters = config["g20_calibration"]["ros__parameters"] assert parameters["tag_size_m"] == 0.016 + assert parameters["tag_size_override_ids"] == [7, 14, 16, 18] + assert parameters["tag_size_overrides_m"] == [0.016] * 4 assert parameters["baseline_command_u8"] == [ 255, @@ -202,6 +206,7 @@ def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: assert parameters["zero_maximum_observability_condition_number"] >= 1.0 assert parameters["zero_maximum_offset_deg"] <= 20.0 assert parameters["zero_finger_maximum_offset_deg"] <= 3.0 + assert parameters["flexion_endpoint_maximum_offset_deg"] == 5.0 assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0 assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5 assert parameters["image_trajectory_minimum_radius_px"] >= 20.0 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 c7f6fed..6762364 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py +++ b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py @@ -63,17 +63,17 @@ def test_runtime_fit_saturates_at_source_urdf_limit() -> None: assert bounded.circle["runtime_urdf_limit_clipped_bins"]["angle_rad"] > 0 -def test_right_15_profile_has_exact_layout_tasks_and_derived_dips() -> None: - profile = get_hand_calibration_profile("right", "g20_right_15") +def test_right_19_profile_has_exact_layout_tasks_and_measured_dips() -> None: + profile = get_hand_calibration_profile("right", "g20_right_19") assert profile is RIGHT_19_HAND_PROFILE assert len(profile.sweep_specs) == 16 - assert len(profile.measured_joints) == 17 - assert len(profile.record_joints) == 21 + assert len(profile.measured_joints) == 21 + assert len(profile.record_joints) == 25 assert list(profile.view_tags["front"].values()) == [ 0, 1, 2, 3, 10, 11, 12, 13 ] assert list(profile.view_tags["side"].values()) == [ - 4, 5, 6, 15, 17 + 4, 5, 6, 7, 14, 15, 16, 17, 18 ] assert list(profile.view_tags["top"].values()) == [8, 9] assert profile.preflight_view_roles == { @@ -98,23 +98,30 @@ def test_right_15_profile_has_exact_layout_tasks_and_derived_dips() -> None: "pinky_mcp_roll", "pinky_mcp_roll_side", ) + assert profile.sweep_specs[6].joints == ("pinky_pip", "pinky_dip") assert { name: spec.source_joint for name, spec in profile.joint_specs.items() if spec.source_joint is not None - } == { - "index_dip": "index_pip", - "middle_dip": "middle_pip", - "ring_dip": "ring_pip", - "pinky_dip": "pinky_pip", - } + } == {} -def test_right_15_derives_four_dip_curves_from_source_urdf_mimic() -> None: +def test_right_19_keeps_four_visual_dip_curves_independent() -> None: profile = RIGHT_19_HAND_PROFILE values = tuple(0.002 * (255 - command) for command in range(256)) fit = JointCurveFit(values, values, values, {}, 0.0, 0.01, {}) measured = {name: fit for name in profile.measured_joints} + dip_values = tuple(0.0013 * (255 - command) for command in range(256)) + for finger in ("index", "middle", "ring", "pinky"): + measured[f"{finger}_dip"] = JointCurveFit( + dip_values, + dip_values, + dip_values, + {"visual_measurement": True}, + 0.0, + 0.007, + {"visual_measurement": 1.0}, + ) source_urdf = ( Path(__file__).resolve().parents[3] / "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/" @@ -127,14 +134,10 @@ def test_right_15_derives_four_dip_curves_from_source_urdf_mimic() -> None: assert set(runtime) == set(profile.joint_specs) for finger in ("index", "middle", "ring", "pinky"): - pip = runtime[f"{finger}_pip"] dip = runtime[f"{finger}_dip"] - assert dip.angle_rad == pytest.approx( - tuple(0.89 * value for value in pip.angle_rad) - ) - assert dip.maximum_hysteresis_rad == pytest.approx(0.0089) - assert dip.circle["curve_source"] == f"{finger}_pip" - assert dip.circle["visual_measurement"] is False + assert dip.angle_rad == pytest.approx(dip_values) + assert dip.maximum_hysteresis_rad == pytest.approx(0.007) + assert dip.circle["visual_measurement"] is True def _changed_motors( @@ -878,13 +881,13 @@ def test_right_compact_payload_keeps_v4_shape_and_uses_pinky_sources() -> None: assert source["zero_angles"] == {"urdf_zero_offset_rad": 0.01} -def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() -> None: +def test_right_19_schema_v4_records_visual_dips_and_16_static_zeros() -> None: from g20_thumb_apriltag_calibration.urdf_zero import ( get_zero_calibration_profile, ) profile = RIGHT_19_HAND_PROFILE - zero = get_zero_calibration_profile("right", "g20_right_15") + zero = get_zero_calibration_profile("right", "g20_right_19") baseline = [255] * 20 baseline[6:10] = [127] * 4 fits = {} @@ -920,7 +923,7 @@ def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() -> passed=True, baseline=baseline, side="right", - layout_id="g20_right_15", + layout_id="g20_right_19", zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints}, zero_cycle_offsets_rad={ name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints @@ -950,7 +953,12 @@ def test_right_15_schema_v4_records_derived_dip_curves_and_12_static_zeros() -> "schema_version", "model", "side", "serial_number", "angle_unit", "command_range", "baseline_command_u8", "joints", "quality", } - assert len(zero.direct_zero_joints) == 12 + assert len(zero.direct_zero_joints) == 16 + assert all( + f"{finger}_pip" in zero.direct_zero_joints + and f"{finger}_dip" not in zero.direct_zero_joints + for finger in ("index", "middle", "ring", "pinky") + ) assert payload["joints"]["thumb_mcp"]["zero_angles"] == { "urdf_zero_offset_rad": 0.0 } diff --git a/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py b/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py index db10681..88fc414 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py +++ b/src/g20_thumb_apriltag_calibration/test/test_g20_right_product.py @@ -29,6 +29,8 @@ from g20_thumb_apriltag_calibration.operator_report import ( ) from g20_thumb_apriltag_calibration.one_command import ( _automatic_resume_candidate, + _calibration_node_exited_before_status, + _status_timeout_seconds, ) from g20_thumb_apriltag_calibration.product import load_product_config from g20_thumb_apriltag_calibration.publication import ( @@ -53,9 +55,11 @@ from g20_thumb_apriltag_calibration.three_camera_node import ( _steady_checkpoint_commands, _unresolved_fit_failure_tasks, combination_target_coverage, + resume_tasks_invalidated_by_tag_size_changes, resumable_completed_task_prefix, ) from g20_thumb_apriltag_calibration.urdf_zero import ( + RIGHT_19_FLEXION_ENDPOINT_JOINTS, UrdfKinematicModel, get_zero_calibration_profile, write_zero_corrected_urdf, @@ -117,6 +121,9 @@ def _make_passed_session(config, stamp: str) -> Path: output_directory=session, serial_number=config.serial_number, offsets_rad={name: 0.01 for name in ACTIVE_ZERO_JOINTS}, + endpoint_anchored_offsets_rad={ + name: 0.01 for name in RIGHT_19_FLEXION_ENDPOINT_JOINTS + }, timestamp=stamp, ) (session / "raw_samples.jsonl").write_text('{"kind":"session_start"}\n') @@ -299,6 +306,28 @@ def test_sparse_resume_keeps_complete_tasks_after_failed_task() -> None: } +def test_corrected_distal_tag_sizes_invalidate_only_four_pip_tasks() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + previous = { + tag_id: (0.010 if tag_id in {7, 14, 16, 18} else 0.016) + for tag_id in range(19) + } + current = {tag_id: 0.016 for tag_id in range(19)} + + changed, invalidated = resume_tasks_invalidated_by_tag_size_changes( + profile, previous, current + ) + + assert changed == (7, 14, 16, 18) + assert invalidated == ( + "pinky_pip_side", + "ring_pip_side", + "middle_pip_side", + "index_pip_side", + ) + assert len(profile.sweep_specs) - len(invalidated) == 12 + + def test_sparse_resume_revalidates_retired_product_dynamic_hysteresis() -> None: profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) task = next( @@ -523,6 +552,44 @@ def test_resume_rejects_unresolved_fit_but_accepts_retired_model_metric() -> Non assert _unresolved_fit_failure_tasks(profile, retired_rows) == set() +def test_resume_revalidates_retired_passive_dip_position_metrics() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + pip = next( + spec for spec in profile.sweep_specs if spec.key == "pinky_pip_side" + ) + rows = [ + { + "kind": "sample", + "task_name": pip.key, + "attempt": 3, + }, + { + "kind": "fit_failure", + "task_name": pip.key, + "view": pip.view, + "motor_index": pip.motor_index, + "joints": list(pip.joints), + "attempt": 3, + "failures": [ + { + "joint": "pinky_dip", + "metric": "axis_pose_line_rms_mm", + "actual": 1.9, + "limit": 1.0, + }, + { + "joint": "pinky_dip", + "metric": "rotation_circle_axis_difference_deg", + "actual": 15.5, + "limit": 1.0, + }, + ], + }, + ] + + assert _unresolved_fit_failure_tasks(profile, rows) == set() + + def test_automatic_resume_requires_failed_matching_geometry(tmp_path: Path) -> None: config = _config(tmp_path) session = config.session_root / "20260819_150000" @@ -926,6 +993,34 @@ def test_synchronised_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: P assert "不要调整" in payload["automatic_action_zh"] +def test_sweep_start_timeout_with_visible_tags_reports_pnp_geometry(tmp_path: Path) -> None: + config = _config(tmp_path, passes=1) + session = config.session_root / "20260821_154105" + session.mkdir(parents=True) + status = { + "state": "PAUSED", + "reason": "sweep_start_tag_timeout", + "feedback_hz": 58.8, + "active": {"valid_frames": 0}, + "views": { + "front": {"ready": True, "missing_tag_ids": []}, + "side": { + "ready": True, + "missing_tag_ids": [], + "group_pnp_reason": "group_normal_alignment", + }, + "top": {"ready": True, "missing_tag_ids": []}, + }, + } + + payload, _block = build_failure_report( + config, session, status, reason=status["reason"] + ) + + assert payload["error_code"] == "CAM-GEOMETRY-201" + assert "PnP" in payload["problem_zh"] + + def test_multiview_sample_failure_has_stable_observation_code() -> None: code, problem, _suggestion = classify_error( "sweep_bins_too_few:pinky_mcp_roll_side", {} @@ -935,6 +1030,69 @@ def test_multiview_sample_failure_has_stable_observation_code() -> None: assert "轨迹不完整" in problem +def test_fitting_uses_long_watchdog_without_weakening_motion_timeout() -> None: + assert _status_timeout_seconds({"state": "FITTING"}) == 600.0 + assert _status_timeout_seconds({"state": "SWEEP"}) == 90.0 + assert _status_timeout_seconds({"state": "RETURN_BASELINE"}) == 90.0 + + +def test_fitting_announces_state_before_synchronous_model_work() -> None: + spec = SimpleNamespace() + published = [] + node = SimpleNamespace( + profile=SimpleNamespace(sweep_specs=(spec,)), + last_status_publish=0.0, + _publish_status=lambda now: published.append( + (node.state, node.reason, now) + ), + _provisional_fit_failures=lambda _spec: [{"metric": "test_failure"}], + _pause_for_provisional_fit_failure=lambda *_args, **_kwargs: None, + ) + + G20ThreeCameraCalibrationNode._fit_all_curves(node) + + assert len(published) == 1 + assert published[0][0] == "FITTING" + assert published[0][1] == "fitting_3d_axes_and_urdf_zero_offsets" + assert node.last_status_publish == published[0][2] + + +def test_stopped_status_heartbeat_has_stable_communication_code() -> None: + code, problem, suggestion = classify_error( + "MOTION-COMM-303:calibration status stopped", {} + ) + + assert code == "MOTION-COMM-303" + assert "状态心跳" in problem + assert "断点" in suggestion + + +def test_missing_startup_status_has_specific_camera_status_code() -> None: + code, problem, suggestion = classify_error( + "CAM-STATUS-202:no calibration status received", {} + ) + + assert code == "CAM-STATUS-202" + assert "未发布状态" in problem + assert "尚未开始运动" in suggestion + + +def test_startup_watchdog_detects_only_calibration_child_exit( + tmp_path: Path, +) -> None: + log = tmp_path / "calibration.log" + log.write_text( + "[three_camera_calibration_node-6]: process has died " + "[pid 123, exit code 1]\n", + encoding="utf-8", + ) + assert _calibration_node_exited_before_status(log) is True + + log.write_text( + "[component_container_mt-4]: process has died [exit code -11]\n", + encoding="utf-8", + ) + assert _calibration_node_exited_before_status(log) is False def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None: 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 f0c8ad0..9124739 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 @@ -85,6 +85,22 @@ def test_combination_prediction_failure_is_not_reported_as_unclassified() -> Non assert "不要重新采集16个单关节任务" in action +def test_sweep_start_visible_tags_reports_group_pnp_rejection() -> None: + explanation, action = three_camera_reason_zh( + "PAUSED", + "sweep_start_tag_timeout", + { + "group_pnp_reasons": { + "side": "group_normal_alignment", + } + }, + ) + + assert "所需Tag也可见" in explanation + assert "group_normal_alignment" in explanation + assert "不要重新粘贴" in action + + def test_multiview_failure_names_the_camera_specific_joint() -> None: explanation, action = three_camera_reason_zh( "PAUSED", 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 5abe5dc..50da502 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 @@ -16,6 +16,7 @@ from g20_thumb_apriltag_calibration.acquisition import TagQuality from g20_thumb_apriltag_calibration.full_hand import ( RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, RIGHT_19_HAND_PROFILE, + RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, RIGHT_HAND_PROFILE, SWEEP_SPECS, THREE_CAMERA_BASELINE_COMMAND, @@ -33,6 +34,7 @@ from g20_thumb_apriltag_calibration.three_camera_node import ( _combination_observable_joints, _fit_failure_is_systematic, _frames_cover_sweep_joints, + _isolated_axis_cycle_outliers, _overall_progress, _selected_pose_qualities, _sweep_views, @@ -486,7 +488,7 @@ def test_right_19_combination_allows_occluded_neighbouring_fingers() -> None: ) assert required == ("side_base",) - assert observable == ("index_pip",) + assert observable == ("index_pip", "index_dip") def test_right_19_task_requires_only_target_chain_after_clearance_pose() -> None: @@ -513,7 +515,7 @@ def test_right_19_task_requires_only_target_chain_after_clearance_pose() -> None node, "side" ) - assert required == ("side_base", "index_pip") + assert required == ("side_base", "index_pip", "index_dip") def test_right_pip_sweep_keeps_side_base_as_pnp_branch_anchor() -> None: @@ -1694,6 +1696,35 @@ def test_thumb_mcp_ip_group_uses_mimic_only_as_pnp_branch_prior() -> None: ) +def test_side_pip_group_allows_arbitrary_distal_extension_tag_plane() -> None: + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + pnp_maximum_pose_jump_rad=math.radians(35.0), + pnp_maximum_translation_jump_m=0.04, + pnp_group_initialization_frames=8, + pnp_group_normal_alignment_scale_rad=math.radians(5.0), + pnp_group_maximum_normal_alignment_rad=math.radians(15.0), + pnp_tracker_reset_seconds=5.0, + thumb_ip_pnp_coupling_multiplier=1.03, + thumb_ip_pnp_coupling_scale_rad=math.radians(3.0), + thumb_ip_pnp_maximum_coupling_residual_rad=math.radians(7.5), + ) + + tracker = G20ThreeCameraCalibrationNode._make_group_pose_tracker( + node, + "side", + ("side_base", "middle_pip", "middle_dip"), + ) + + assert tracker.adjacent_pairs == ( + ("side_base", "middle_pip"), + ("middle_pip", "middle_dip"), + ) + assert tracker.normal_alignment_pairs == ( + ("side_base", "middle_pip"), + ) + + 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( @@ -1850,12 +1881,78 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None: "view": "front", "motor_index": 15, "joints": ["thumb_mcp", "thumb_ip"], + "joints_to_rescan": ["thumb_ip", "thumb_mcp"], "attempt": 2, "reason": "joint_fit_check_failed", "cycles": [2], } +def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "middle_roll_multiview" + ) + primary, validation = spec.joints + records = { + primary: [ + {"cycle": 0, "accepted": "front"}, + {"cycle": 1, "accepted": "front"}, + ], + validation: [ + {"cycle": 0, "old": "side"}, + {"cycle": 1, "accepted": "side"}, + ], + } + side_runtime = SimpleNamespace( + pnp_invalid_since=12.0, + pnp_reset_count=0, + task_valid_frames=4927, + task_total_frames=5212, + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + retry_sweep_spec=spec, + retry_joint_names={validation}, + records_by_joint=records, + baseline_records_by_joint={primary: [], validation: []}, + command_records_by_joint={primary: [], validation: []}, + sweep_attempts={spec.key: 1}, + raw_path=tmp_path / "raw_samples.jsonl", + paused_reason="joint_fit_check_failed", + fit_failure={"failures": ["old"]}, + retry_cycles={0}, + repetitions=4, + sweep_items=[ + SweepItem(spec, cycle, direction) + for cycle in range(4) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ], + views={"side": side_runtime}, + _reset_view_trackers=lambda runtime: None, + ) + + G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node) + + assert records[primary] == [ + {"cycle": 0, "accepted": "front"}, + {"cycle": 1, "accepted": "front"}, + ] + assert records[validation] == [ + {"cycle": 1, "accepted": "side"} + ] + assert node.retry_joint_names == {validation} + assert side_runtime.task_valid_frames == 0 + assert side_runtime.task_total_frames == 0 + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["joints_to_rescan"] == [validation] + assert event["cycles"] == [1] + + 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) @@ -2239,6 +2336,52 @@ def test_provisional_fit_accepts_constrained_circle_for_image_joint() -> None: ] +def test_axis_cycle_spread_localizes_one_outlier_for_four_rounds() -> None: + axes = [ + np.asarray([-0.05713560, -0.06623617, -0.99616680]), + np.asarray([-0.05620951, -0.05515776, -0.99689423]), + np.asarray([-0.05700579, -0.05346630, -0.99694117]), + np.asarray([-0.05589426, -0.05231845, -0.99706500]), + ] + + assert _isolated_axis_cycle_outliers( + axes, math.radians(0.75) + ) == {0} + + +def test_provisional_axis_spread_marks_only_the_isolated_round() -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) + records = _image_cycle_records([math.radians(47.0)] * 4) + node = _fit_check_node({"thumb_cmc_pitch": records}) + node.repetitions = 4 + axes = [ + [-0.05713560, -0.06623617, -0.99616680], + [-0.05620951, -0.05515776, -0.99689423], + [-0.05700579, -0.05346630, -0.99694117], + [-0.05589426, -0.05231845, -0.99706500], + ] + node._fit_axis_measurement = lambda name, cycle: SimpleNamespace( + axis_common_xyz=axes[cycle], + axis_direction_source="pose_rotation_3d", + radial_rms_m=0.0001, + pose_axis_line_rms_m=0.0001, + plane_rms_m=0.0001, + rotation_circle_axis_difference_rad=0.0, + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + + spread = next( + failure + for failure in failures + if failure["metric"] == "axis_cycle_difference_deg" + ) + assert spread["cycle"] == 1 + assert spread["inlier_cycles"] == [2, 3, 4] + + def test_side_roll_alias_does_not_reject_free_circle_axis_disagreement() -> None: spec = next( item @@ -2258,6 +2401,14 @@ def test_side_roll_alias_does_not_reject_free_circle_axis_disagreement() -> None ) node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) node.baseline_records_by_joint = {name: records} + node.minimum_detection_rate = 0.95 + node.views = { + "side": SimpleNamespace( + valid_rate=0.9453, + task_valid_frames=2653, + task_total_frames=2807, + ) + } failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( node, spec @@ -2270,6 +2421,7 @@ def test_side_roll_alias_does_not_reject_free_circle_axis_disagreement() -> None in { "rotation_circle_axis_difference_deg", "axis_plane_rms_mm", + "tag_valid_rate_percent", } ] @@ -2674,6 +2826,60 @@ def test_side_alias_skips_pose_line_rms_gate(tmp_path) -> None: ] +def test_right_19_passive_dip_requires_axis_point_for_pip_zero_phase() -> None: + records = _image_cycle_records([math.radians(47.0)] * 3) + node = _fit_check_node( + {"pinky_pip": records, "pinky_dip": records} + ) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_19" + ) + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = { + "pinky_pip": records, + "pinky_dip": records, + } + + def axis_measurement(name, cycle): + is_dip = name in RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS + return SimpleNamespace( + axis_common_xyz=(0.0, 0.0, 1.0), + axis_direction_source="upstream_constraint", + radial_rms_m=0.0002, + pose_axis_line_rms_m=0.002 if is_dip else 0.0002, + plane_rms_m=0.006 if is_dip else 0.0002, + rotation_circle_axis_difference_rad=( + math.radians(16.0) if is_dip else math.radians(0.1) + ), + ) + + node._fit_axis_measurement = axis_measurement + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "pinky_pip_side" + ) + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + + pose_failures = [ + failure + for failure in failures + if failure["joint"] == "pinky_dip" + and failure["metric"] == "axis_pose_line_rms_mm" + ] + assert len(pose_failures) == 3 + assert not [ + failure + for failure in failures + if failure["joint"] == "pinky_dip" + and failure["metric"] + in {"axis_plane_rms_mm", "rotation_circle_axis_difference_deg"} + ] + + def _ring_group_end_state() -> list[float]: """Ring-group avoidance pose: pinky flexed, index/middle rolls parked.""" state = [float(value) for value in THREE_CAMERA_BASELINE_COMMAND] 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 e616820..02cf76a 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py @@ -20,6 +20,7 @@ from g20_thumb_apriltag_calibration.urdf_zero import ( _angles_from_state, _zero_sensitive_axis_error_rad, baseline_hysteresis_by_cycle_rad, + derive_right_19_flexion_endpoint_offsets, fit_joint_axis_measurement, fit_rotation_joint_curve, solve_urdf_zero_offsets, @@ -31,6 +32,7 @@ from g20_thumb_apriltag_calibration.full_hand import ( JOINT_SPECS, MEASURED_JOINTS, PASSIVE_JOINTS, + RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, JointCurveFit, get_hand_calibration_profile, ) @@ -621,7 +623,7 @@ def _solve_synthetic_offsets( 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] = [] - validation_cycle = 3 if layout_id == "g20_right_15" else 2 + validation_cycle = 3 if hand.layout_id == "g20_right_19" else 2 training_cycles = tuple(range(validation_cycle)) for cycle in range(validation_cycle + 1): for joint in zero.axis_joints: @@ -749,20 +751,20 @@ def _solve_synthetic_offsets( return zero, result -def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> None: +def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supplied() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, - 0.8, -0.7, - -0.5, 0.6, - 1.1, -1.0, + 1.0, -1.0, 0.7, + 0.8, -0.7, -0.8, + -0.5, 0.6, 0.9, + 1.1, -1.0, -0.6, ] zero, result = _solve_synthetic_offsets( - "right", injected, layout_id="g20_right_15" + "right", injected, layout_id="g20_right_19" ) - assert len(zero.direct_zero_joints) == 12 - assert len(zero.axis_joints) == 17 + assert len(zero.direct_zero_joints) == 16 + assert len(zero.axis_joints) == 21 assert result.passed is True finger_rolls = tuple( name @@ -778,8 +780,8 @@ def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> N ) ) for name, expected in zip(zero.direct_zero_joints, injected): - if name == "thumb_mcp": - expected = 0.0 + if name in zero.fixed_direct_zero_offsets_rad: + expected = zero.fixed_direct_zero_offsets_rad[name] elif name in finger_rolls: expected -= roll_common assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( @@ -788,35 +790,63 @@ def test_right_15_solver_recovers_11_targets_and_keeps_thumb_mcp_cad_zero() -> N assert math.degrees(result.all_active_offsets_rad["thumb_mcp"]) == pytest.approx( 0.0, abs=0.05 ) + assert zero.fixed_direct_zero_offsets_rad == {"thumb_mcp": 0.0} assert result.training_cycles == (0, 1, 2) assert result.validation_cycle == 3 assert all( value == pytest.approx(0.0, abs=1.0e-12) for value in result.offset_confidence_half_width_rad.values() ) - assert all( - result.all_active_offsets_rad[f"{finger}_pip"] == 0.0 - for finger in ("index", "middle", "ring", "pinky") - ) - assert result.observability_parameter_count == 18 - assert result.observability_rank == 18 + assert result.observability_parameter_count == 22 + assert result.observability_rank == 22 assert math.isfinite(result.observability_condition_number) assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints) +def test_right_19_flexion_zero_is_derived_from_measured_contact_endpoint() -> None: + hand = get_hand_calibration_profile("right", "g20_right_19") + curves = { + name: _synthetic_curve(255, math.radians(50.0)) + for name in hand.measured_joints + } + for finger in ("index", "middle", "ring", "pinky"): + curves[f"{finger}_mcp_pitch"] = _synthetic_curve( + 255, math.radians(71.0) + ) + curves[f"{finger}_pip"] = _synthetic_curve( + 255, math.radians(103.0) + ) + + offsets = derive_right_19_flexion_endpoint_offsets( + RIGHT_SOURCE_URDF, curves + ) + + assert set(offsets) == { + f"{finger}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + for suffix in ("mcp_pitch", "pip") + } + assert math.degrees(offsets["index_mcp_pitch"]) == pytest.approx( + math.degrees(1.22) - 71.0 + ) + assert math.degrees(offsets["index_pip"]) == pytest.approx( + math.degrees(1.75) - 103.0 + ) + + def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None: # A shared electrical centre belongs to the four-motor common datum; the # strict 3 deg assembly guard applies to finger-to-finger deviations. injected = [ 2.0, -3.0, 4.0, -1.5, - 6.0, -1.0, - 6.4, -0.7, - 5.7, 0.6, - 6.2, -1.0, + 6.0, -1.0, 0.7, + 6.4, -0.7, -0.8, + 5.7, 0.6, 0.9, + 6.2, -1.0, -0.6, ] zero, result = _solve_synthetic_offsets( - "right", injected, layout_id="g20_right_15" + "right", injected, layout_id="g20_right_19" ) assert result.passed is True @@ -834,8 +864,8 @@ def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None: ) ) for name, expected in zip(zero.direct_zero_joints, injected): - if name == "thumb_mcp": - expected = 0.0 + if name in zero.fixed_direct_zero_offsets_rad: + expected = zero.fixed_direct_zero_offsets_rad[name] elif name in finger_rolls: expected -= roll_common assert math.degrees(result.direct_offsets_rad[name]) == pytest.approx( @@ -846,18 +876,46 @@ def test_right_15_finger_roll_limit_applies_to_independent_deviation() -> None: ) == pytest.approx(0.0, abs=1.0e-10) +def test_right_19_roll_gauge_at_search_bound_uses_relative_deviation() -> None: + # One raw roll reaches the diagnostic search edge, as in session 160326, + # while all four physical deviations from their shared datum remain safe. + injected = [ + 2.0, -3.0, 4.0, -1.5, + 9.3, -1.0, 0.0, + 8.4, -0.7, 0.0, + 8.7, 0.6, 0.0, + 8.8, -1.0, 0.0, + ] + + zero, result = _solve_synthetic_offsets( + "right", injected, layout_id="g20_right_19" + ) + + assert result.passed is True + finger_rolls = tuple( + name + for name in zero.direct_zero_joints + if name.endswith("_mcp_roll") and not name.startswith("thumb_") + ) + assert max( + abs(math.degrees(result.direct_offsets_rad[name])) + for name in finger_rolls + ) < 3.0 + assert not result.failure_reasons + + def test_right_19_holdout_never_changes_frozen_training_offsets() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, - 1.0, -1.0, - 0.8, -0.7, - -0.5, 0.6, - 1.1, -1.0, + 1.0, -1.0, 0.7, + 0.8, -0.7, -0.8, + -0.5, 0.6, 0.9, + 1.1, -1.0, -0.6, ] zero, result = _solve_synthetic_offsets( "right", injected, - layout_id="g20_right_15", + layout_id="g20_right_19", validation_offset_bias_degrees={"thumb_cmc_roll": 2.0}, ) @@ -867,9 +925,9 @@ def test_right_19_holdout_never_changes_frozen_training_offsets() -> None: assert result.validation_cycle not in result.training_cycles -def test_right_15_palm_pose_and_12_zero_observation_jacobian_is_full_rank() -> None: - """Guard the reviewed 6-palm-DOF plus 12-static-zero observability.""" - zero = get_zero_calibration_profile("right", "g20_right_15") +def test_right_19_palm_pose_and_16_zero_observation_jacobian_is_full_rank() -> None: + """Guard the reviewed 6-palm-DOF plus 16-static-zero observability.""" + zero = get_zero_calibration_profile("right", "g20_right_19") model = UrdfKinematicModel(RIGHT_SOURCE_URDF) parameter_count = 6 + len(zero.direct_zero_joints) @@ -914,14 +972,14 @@ def test_right_15_palm_pose_and_12_zero_observation_jacobian_is_full_rank() -> N ] ) - assert parameter_count == 18 + assert parameter_count == 22 assert np.linalg.matrix_rank(jacobian, tol=1.0e-7) == parameter_count -def test_right_15_urdf_writer_changes_only_the_12_static_targets( +def test_right_19_urdf_writer_changes_only_the_16_static_targets( tmp_path: Path, ) -> None: - zero = get_zero_calibration_profile("right", "g20_right_15") + zero = get_zero_calibration_profile("right", "g20_right_19") offsets = {name: math.radians(1.0) for name in zero.direct_zero_joints} destination = write_zero_corrected_urdf( source_urdf=RIGHT_SOURCE_URDF, @@ -957,7 +1015,7 @@ def test_right_15_urdf_writer_changes_only_the_12_static_targets( assert changed == set(zero.direct_zero_joints) assert "thumb_mcp" in changed assert not set(get_hand_calibration_profile( - "right", "g20_right_15" + "right", "g20_right_19" ).passive_joints) & changed