diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 5021394..28262dd 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -8,7 +8,19 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right ``` -只重新采集大拇指的4项任务、四指沿用一份已经正式通过的完整会话时，使用： +完全独立地只标定大拇指4项任务时，使用： + +```bash +ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \ + --scope thumb +``` + +该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机 +方向观测以及拇指自己的机械端点；输出URDF从原始CAD生成，只修改4个拇指主动 +关节，12个四指关节保持CAD零位。独立结果发布到`latest_thumb_passed`，其JSON是 +拇指标定/诊断产物，不冒充可直接运行的完整整手曲线JSON。 + +如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定，才额外使用： ```bash ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \ @@ -16,24 +28,44 @@ ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \ --base-session calibration_output/G20_RIGHT_001/latest_passed ``` +合并模式会冻结基础会话中的12个四指主动零位；发布前再次读取基础会话JSON核对， +任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。 + +拇指专项结果重复性通过后，不必再做原来的16项整手扫描。以该拇指会话为基础， +只重新采集12项四指任务并合成完整整手URDF： + +```bash +ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \ + --scope fingers \ + --base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳> +``` + +`fingers`模式严格冻结基础会话中的4个拇指主动零位；最终完整整手URDF中的拇指 +零位与专项会话schema-v4数值完全一致。默认`full`也先调用与`thumb`完全相同的 +独立拇指内核，再冻结这4个结果求解12个四指零位，因此四指数据不能反向改写 +拇指结果。原来的默认`full`仍保留，用于需要16项全部重新采集的情况。 + 采样文件中的运动域是显式且不可混用的： `requested_command_u8` 表示下发命令，`feedback_u8` 表示电机反馈。 在线拟合和离线重放通过同一个数据契约投影到曲线索引；新会话不会把含糊的 `command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为 `IMPORTING_BASE` 和 `REVALIDATING_INHERITED`，完成复核后才允许机械手运动。 -`--base-session`必须解析到同一序列号目录下的完整PASS会话；启动前会校验源CAD +提供`--base-session`时，它必须解析到同一序列号目录下的完整PASS会话；启动前会校验源CAD URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF，不在旧校准 -URDF上叠加。该模式只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、 +URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、 `thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务，其余任务的原始记录导入后仍按 -当前硬门限重新验证。 +数据契约和产物哈希检查，但不会以历史四指拟合结果否决本次拇指专项标定。 开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经 完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的 独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与 最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据 当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收 -失败、把会话拉回靠前的关节。运行中的多视角任务按正面主测量和侧面校验测量 +失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记； +恢复时只读取该标记之后的替代采集，不能把同一尝试编号下重扫前后的稳态点合并。 +因此已经在线硬门限验收的任务保持已完成，暂停中的任务从任务开头重采，不会因 +日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量 独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面 轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次 采集确定的PnP分支参考，不会因重新初始化切换到另一组平面Tag镜像解。侧面 @@ -43,7 +75,12 @@ URDF上叠加。该模式只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、 门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一 采集事务：相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用 产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态 -过程。需要强制从第一个关节 +过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查：前三轮 +极差默认不得超过0.5°，95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的 +紧密簇、仅另一轮越界，自动补采该轮两个方向；无法明确定位时只重采完整yaw任务， +不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入 +`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装，但该历史差值 +不直接否决当前会话，也不会用旧结果约束新零位。需要强制从第一个关节 重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为 一个完整同步任务断点；只有两边数据都完整时才复用。 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 5f47411..43d09bd 100644 --- a/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml +++ b/src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml @@ -26,7 +26,7 @@ artifacts: camera_extrinsics: config/g20_three_camera_extrinsics.yaml camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80 calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml - calibration_config_sha256: 8fa16814d7a3462a00aae4eadc273e66bc167bedd5908b0fed40ce6d505b2aeb + calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3 tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41 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 0aa2473..621d7a2 100644 --- a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml @@ -55,6 +55,10 @@ g20_calibration: # 侧面当前任务所需Tag在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。 pnp_group_normal_alignment_scale_deg: 5.0 pnp_group_maximum_normal_alignment_deg: 15.0 + # 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见； + # 任一角点相对会话基准漂移超过2 px并连续5帧时，判定标定中基准被移动。 + fixed_base_maximum_corner_drift_px: 2.0 + fixed_base_movement_confirmation_frames: 5 # 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic # 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、 # 缩放或替代被动IP的自身Tag实测曲线。 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 736f537..9c947c4 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 @@ -700,7 +700,7 @@ def _build_right_19_profile() -> HandCalibrationProfile: "stable_cross_view_cone_bias", "measured_passive_dips", "urdf_zero_publication", - "palm_axis_side_channel_v2", + "palm_axis_relative_motion_v3", } ), ) 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 f2a99f5..c625e1a 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 @@ -54,11 +54,13 @@ from .urdf_zero import ( fit_partial_palm_orientation_measurements, fit_rotation_joint_curve, get_zero_calibration_profile, + get_right_19_thumb_zero_profile, derive_right_19_mechanical_endpoint_offsets, joint_curve_holdout_errors, refit_axis_line_group_with_shared_radius, RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, + merge_right_19_thumb_zero_result, select_cross_view_roll_direction_source, solve_urdf_zero_offsets, with_depth_free_axis_projection, @@ -1537,7 +1539,63 @@ def replay_session( **post_solve_endpoint_offsets, }, } - holdout_zero = solve_urdf_zero_offsets(curves=training_fits, **solve_arguments) + def solve_with_product_partition( + fits: Mapping[str, JointCurveFit], + arguments: Mapping[str, Any], + ): + if layout_id != G20_RIGHT_19_LAYOUT: + return solve_urdf_zero_offsets(curves=fits, **arguments) + thumb_profile = get_right_19_thumb_zero_profile() + thumb_names = set(thumb_profile.direct_zero_joints) + thumb = solve_urdf_zero_offsets( + curves=fits, + **{ + **arguments, + "measurements": [ + item + for item in arguments["measurements"] + if item.joint in thumb_profile.axis_joints + ], + "joint_maximum_offset_rad": { + name: value + for name, value in arguments[ + "joint_maximum_offset_rad" + ].items() + if name in thumb_names + }, + "fixed_direct_zero_offsets_rad": { + name: value + for name, value in arguments[ + "fixed_direct_zero_offsets_rad" + ].items() + if name in thumb_names + }, + "static_output_zero_offsets_rad": { + name: value + for name, value in arguments[ + "static_output_zero_offsets_rad" + ].items() + if name in thumb_names + }, + "zero_profile": thumb_profile, + }, + ) + companion = solve_urdf_zero_offsets( + curves=fits, + **{ + **arguments, + "fixed_direct_zero_offsets_rad": { + **arguments["fixed_direct_zero_offsets_rad"], + **thumb.direct_offsets_rad, + }, + }, + ) + return merge_right_19_thumb_zero_result( + thumb_result=thumb, + companion_result=companion, + ) + + holdout_zero = solve_with_product_partition(training_fits, solve_arguments) if not holdout_zero.passed: failure = { "reasons": dict(holdout_zero.failure_reasons), @@ -1625,14 +1683,14 @@ def replay_session( motor_by_joint=motor_by_joint, inherited_zero_joints=zero_profile.inherited_zero_joints, ) - residual_zero = solve_urdf_zero_offsets( + residual_zero = solve_with_product_partition( # Re-check the corrected URDF in the exact frozen trajectory # domain that produced the accepted zero offsets. Runtime # requested-command curves are a separate transfer function and # differ slightly at settled checkpoints; using them here creates # a fictitious residual zero after a structurally correct write. - curves=training_fits, - **{ + training_fits, + { **solve_arguments, "static_output_zero_offsets_rad": { name: 0.0 @@ -1727,6 +1785,10 @@ def replay_session( "rotation_orthogonal_rms_rad": ( item.rotation_orthogonal_rms_rad ), + "axis_estimator": item.axis_estimator, + "incremental_pair_count": ( + item.incremental_pair_count + ), } for item in palm_orientation_measurements ], 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 41c9717..c441fe1 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 @@ -24,6 +24,7 @@ from .hikrobot_camera import configure_fastdds_large_image_transport from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh from .product import ProductConfig, load_product_config, sha256_file from .publication import atomic_session_pointer, finalize_session_artifacts +from .storage import atomic_write_json EXIT_PASS = 0 @@ -502,7 +503,7 @@ def _resolve_partial_base_session( """Validate the complete passed session that donates non-target tasks.""" if base_session is None or not str(base_session).strip(): raise ValueError( - "--scope thumb requires --base-session pointing to a passed " + "partial scope requires --base-session pointing to a passed " "complete G20 right session" ) candidate = Path(base_session).expanduser().resolve(strict=True) @@ -564,15 +565,15 @@ def run( config = load_product_config(path, workspace=workspace, check_can=False) load_product_config(path, workspace=workspace, check_can=True) selected_scope = str(scope).strip().lower() - if selected_scope not in {"full", "thumb"}: - raise ValueError("scope must be one of: full, thumb") + if selected_scope not in {"full", "thumb", "fingers"}: + raise ValueError("scope must be one of: full, thumb, fingers") if selected_scope == "full" and base_session is not None: - raise ValueError("--base-session is valid only with --scope thumb") - partial_base = ( - None - if selected_scope == "full" - else _resolve_partial_base_session(config, base_session) - ) + raise ValueError( + "--base-session is valid only with --scope thumb/fingers" + ) + partial_base = None + if selected_scope == "fingers" or base_session is not None: + partial_base = _resolve_partial_base_session(config, base_session) except BaseException as error: print(_startup_failure_block(path, error), flush=True) return EXIT_QUALITY @@ -583,14 +584,21 @@ def run( config.session_root.mkdir(parents=True, exist_ok=True) resume_candidate = ( partial_base - if selected_scope == "thumb" + if selected_scope != "full" else (_automatic_resume_candidate(config) if allow_resume else None) ) if resume_candidate is not None: if selected_scope == "thumb": print( "拇指专项标定：四指任务继承自已通过会话 " - f"{resume_candidate.name}；4项拇指任务将全部重新采集。", + f"{resume_candidate.name}；4项拇指任务将全部重新采集，" + "四指零位保持不变。", + flush=True, + ) + elif selected_scope == "fingers": + print( + "四指专项标定：拇指任务和4个拇指零位继承自已通过会话 " + f"{resume_candidate.name}；12项四指任务将全部重新采集。", flush=True, ) else: @@ -599,6 +607,12 @@ def run( f"{resume_candidate.name}。失败中的当前任务会从头重做。", flush=True, ) + elif selected_scope == "thumb": + print( + "独立拇指标定：不导入四指会话；仅采集4项拇指任务，" + "四指URDF零位保持原始CAD值。", + flush=True, + ) maximum_sessions = config.required_independent_passes for pass_index in range(maximum_sessions): stamp = datetime.now().strftime("%Y%m%d_%H%M%S") @@ -614,13 +628,18 @@ def run( session, resume_from=( resume_candidate - if selected_scope == "thumb" or pass_index == 0 + if selected_scope != "full" or pass_index == 0 else None ), recalibration_scope=selected_scope, ) if code != EXIT_PASS: return code + # Keep the exact node-side completion contract durable before the + # independent publication layer starts. If publication itself fails, + # developers can re-run artifact checks without repeating motion or + # inventing lost combination-validation metrics. + atomic_write_json(session / "node_status.json", status) try: summary, release_ready = finalize_session_artifacts( config, session, node_status=status @@ -634,11 +653,16 @@ def run( print(block, flush=True) return EXIT_QUALITY if release_ready: + result_pointer = ( + config.session_root / "latest_thumb_passed" + if selected_scope == "thumb" and partial_base is None + else config.session_root / "latest_passed" + ) print( "\n".join( [ f"PASS：{config.model} {config.side} 标定、URDF修正和复验全部通过。", - f"正式结果：{config.session_root / 'latest_passed'}", + f"正式结果：{result_pointer}", f"JSON：{session / summary['artifacts']['json']}", f"URDF：{summary['artifacts']['urdf']}", ] @@ -657,14 +681,20 @@ def main(args: list[str] | None = None) -> None: parser.add_argument("--preflight-only", action="store_true") parser.add_argument( "--scope", - choices=("full", "thumb"), + choices=("full", "thumb", "fingers"), default="full", - help="full重新标定全手；thumb仅重采4项拇指任务", + help=( + "full重新标定全手；thumb仅重采4项拇指任务；" + "fingers复用已认证拇指并仅重采12项四指任务" + ), ) parser.add_argument( "--base-session", default=None, - help="thumb模式继承四指数据的已通过完整会话目录", + help=( + "可选：thumb模式将结果合并到该完整会话；" + "fingers模式必须提供该基础会话" + ), ) parser.add_argument( "--no-resume", 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 f62169a..eeacbba 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 @@ -289,10 +289,16 @@ def render_progress_zh( ] if fit_attempt > 1 and fit_retry_cycles: cycle_text = "/".join(str(cycle) for cycle in fit_retry_cycles) - lines.append( - f"拟合：首次拟合定位第 {cycle_text} 轮异常，" - "仅补采该轮双向；其余轮次数据保留" - ) + if len(fit_retry_cycles) >= repetitions: + lines.append( + "拟合：首次拟合未能定位单一异常轮，" + f"联合补采第 {cycle_text} 轮双向" + ) + else: + lines.append( + f"拟合：首次拟合定位第 {cycle_text} 轮需复核，" + "仅补采对应轮次双向；其余轮次数据保留" + ) elif fit_attempt_limit > 1: lines.append( f"拟合：任务级硬门限检查（最多允许 " @@ -366,6 +372,12 @@ def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, st "标定节点状态心跳停止更新", "已采集的完整任务会保留；检查calibration.log后从断点继续，不要重新采集。", ) + if value == "fixed_base_reference_moved": + return ( + "CAM-REFERENCE-204", + "顶部基准Tag在本次标定开始后发生移动", + "Tag 8可在新会话预检前重新摆放；固定Tag 8和顶部相机后启动新会话。", + ) if "palm_orientation_quality_failed" in value: return ( "VAL-QUALITY-501", 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 60bb1da..cf8a7cf 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 @@ -420,7 +420,7 @@ def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> raise ValueError("session pointer target must be a direct child of the serial root") if not target.is_dir(): raise ValueError(f"session directory does not exist: {target}") - if name not in {"latest_attempt", "latest_passed"}: + if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}: raise ValueError("unsupported session pointer name") parent.mkdir(parents=True, exist_ok=True) destination = parent / name @@ -449,14 +449,124 @@ def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]: return result +def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]: + """Validate a standalone thumb artifact and expand CAD finger zeros.""" + if ( + payload.get("schema_version") != 1 + or payload.get("artifact_type") + != "g20_right_standalone_thumb_calibration" + or payload.get("side") != "right" + or payload.get("non_thumb_zero_policy") != "source_cad_unchanged" + or not bool(payload.get("quality", {}).get("passed")) + ): + raise ValueError("standalone thumb JSON contract is invalid") + joints = payload.get("joints") + expected_joint_names = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + } + if not isinstance(joints, Mapping) or set(joints) != expected_joint_names: + raise ValueError("standalone thumb JSON has the wrong joint set") + thumb_names = expected_joint_names - {"thumb_ip"} + result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS} + for name in thumb_names: + try: + value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"]) + except (KeyError, TypeError, ValueError) as error: + raise ValueError( + f"standalone thumb JSON is missing zero for {name}" + ) from error + curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"standalone thumb JSON curve is invalid: {name}") + result[name] = value + passive_curve = np.asarray( + joints["thumb_ip"].get("angle_rad", ()), dtype=float + ) + if ( + passive_curve.shape != (256,) + or not np.all(np.isfinite(passive_curve)) + or not bool(joints["thumb_ip"].get("passive")) + or any(not math.isfinite(value) for value in result.values()) + ): + raise ValueError("standalone thumb JSON passive curve is invalid") + return result + + +def verify_partial_scope_preserves_certified_zeros( + *, + scope: str, + source_session: str | Path, + serial_root: str | Path | None = None, + serial_number: str, + current_offsets: Mapping[str, float], +) -> None: + """Reject a partial artifact that changes any certified non-target zero.""" + selected = str(scope).strip().lower() + if selected not in {"thumb", "fingers"}: + return + source = Path(source_session).expanduser() + if not source.is_absolute(): + if serial_root is None: + raise ValueError( + "relative partial base session requires its serial root" + ) + source = Path(serial_root).expanduser() / source + source = source.resolve() + base_payload = _load_json( + source / f"g20_right_{serial_number}_calibration.json" + ) + base_offsets = ( + standalone_thumb_offsets(base_payload) + if base_payload.get("artifact_type") + == "g20_right_standalone_thumb_calibration" + else active_offsets(base_payload) + ) + preserve_thumb = selected == "fingers" + changed = { + name: abs(float(current_offsets[name]) - float(base_offsets[name])) + for name in ACTIVE_ZERO_JOINTS + if name.startswith("thumb_") == preserve_thumb + and abs(float(current_offsets[name]) - float(base_offsets[name])) + > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD + } + if changed: + raise ValueError( + f"partial {selected} calibration changed certified non-target " + "zeros: " + + ",".join(sorted(changed)) + ) + + def compare_session_offsets( first: Mapping[str, Any], second: Mapping[str, Any], *, maximum_difference_rad: float, ) -> dict[str, float]: - left = active_offsets(first) - right = active_offsets(second) + first_standalone = ( + first.get("artifact_type") + == "g20_right_standalone_thumb_calibration" + ) + second_standalone = ( + second.get("artifact_type") + == "g20_right_standalone_thumb_calibration" + ) + if first_standalone != second_standalone: + raise ValueError("cannot compare standalone thumb and full-hand artifacts") + left = ( + standalone_thumb_offsets(first) + if first_standalone + else active_offsets(first) + ) + right = ( + standalone_thumb_offsets(second) + if second_standalone + else active_offsets(second) + ) differences = { name: abs(left[name] - right[name]) for name in sorted(left) @@ -570,11 +680,18 @@ def finalize_session_artifacts( if not paths[name].is_file(): raise ValueError(f"session is missing {name}: {paths[name]}") payload = _load_json(paths["json"]) - validate_compact_payload(payload) - if payload.get("schema_version") != 4 or payload.get("side") != "right": - 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") + standalone_thumb = bool( + payload.get("artifact_type") + == "g20_right_standalone_thumb_calibration" + ) + if standalone_thumb: + standalone_thumb_offsets(payload) + else: + validate_compact_payload(payload) + if payload.get("schema_version") != 4 or payload.get("side") != "right": + 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") combination = node_status.get("combination_validation") if not isinstance(combination, Mapping): raise ValueError("node status is missing combination validation") @@ -585,12 +702,12 @@ def finalize_session_artifacts( calibration_scope = str( resume.get("recalibration_scope", "full") ).strip().lower() - if calibration_scope not in {"full", "thumb"}: + if calibration_scope not in {"full", "thumb", "fingers"}: raise ValueError("node status has an unsupported recalibration scope") recalibration_tasks = tuple( str(value) for value in resume.get("recalibration_task_keys", ()) ) - if calibration_scope == "thumb" and ( + if calibration_scope == "thumb" and not standalone_thumb and ( not bool(resume.get("used")) or not str(resume.get("source_session", "")) or len(recalibration_tasks) != 4 @@ -599,7 +716,31 @@ def finalize_session_artifacts( raise ValueError( "thumb recalibration is missing its passed base-session provenance" ) - offsets = active_offsets(payload) + if calibration_scope == "fingers" and ( + not bool(resume.get("used")) + or not str(resume.get("source_session", "")) + or len(recalibration_tasks) != 12 + or any("thumb_" in name for name in recalibration_tasks) + ): + raise ValueError( + "finger recalibration is missing its certified thumb-session " + "provenance" + ) + if standalone_thumb and calibration_scope != "thumb": + raise ValueError("standalone thumb artifact has the wrong node scope") + offsets = ( + standalone_thumb_offsets(payload) + if standalone_thumb + else active_offsets(payload) + ) + if calibration_scope != "full" and not standalone_thumb: + verify_partial_scope_preserves_certified_zeros( + scope=calibration_scope, + source_session=str(resume.get("source_session", "")), + serial_root=directory.parent, + serial_number=config.serial_number, + current_offsets=offsets, + ) endpoint_offsets = { name: offsets[name] for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS @@ -610,22 +751,53 @@ def finalize_session_artifacts( expected_offsets_rad=offsets, endpoint_anchored_offsets_rad=endpoint_offsets, ) - 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"]) + if standalone_thumb: + clipped_runtime_joints = {} + else: + 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() } - commands = build_mujoco_validation_commands(payload["baseline_command_u8"]) + commands = ( + { + "artifact_type": "standalone_thumb_no_full_hand_validation", + "poses": [], + } + if standalone_thumb + else build_mujoco_validation_commands(payload["baseline_command_u8"]) + ) atomic_write_json(paths["commands"], commands) prior, differences = find_compatible_prior_session(config, directory, payload) release_ready = config.required_independent_passes == 1 or prior is not None quality = dict(payload["quality"]) + preserved_certified_zeros = sorted( + name + for name in ACTIVE_ZERO_JOINTS + if ( + calibration_scope == "thumb" + and not name.startswith("thumb_") + ) + or ( + calibration_scope == "fingers" + and name.startswith("thumb_") + ) + ) + if standalone_thumb: + preserved_certified_zeros = [] + calibrated_zero_joints = ( + sorted( + name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_") + ) + if standalone_thumb + else sorted(ACTIVE_ZERO_JOINTS) + ) summary: dict[str, Any] = { "schema_version": 1, "serial_number": config.serial_number, @@ -633,15 +805,23 @@ def finalize_session_artifacts( "calibration_scope": calibration_scope, "inherited_base_session": ( None - if calibration_scope == "full" + if calibration_scope == "full" or standalone_thumb else str(resume.get("source_session")) ), "freshly_calibrated_task_keys": list(recalibration_tasks), + "preserved_certified_zero_joints": preserved_certified_zeros, "result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION", "quality": quality, "runtime_limit_clipped_bins": clipped_runtime_joints, - "runtime_curve_domain": "requested_command_u8", - "static_zero_calibrated_joints": sorted(ACTIVE_ZERO_JOINTS), + "runtime_curve_domain": ( + "thumb_diagnostic_only" + if standalone_thumb + else "requested_command_u8" + ), + "static_zero_calibrated_joints": calibrated_zero_joints, + "non_thumb_zero_policy": ( + "source_cad_unchanged" if standalone_thumb else None + ), "retained_active_urdf_zero_joints": sorted( RETAINED_ACTIVE_ZERO_JOINTS ), @@ -693,7 +873,8 @@ def finalize_session_artifacts( # verify_corrected_urdf proves that corrected endpoint limits map back to # the original physical CAD endpoints, while the runtime curves must stay # inside those corrected-coordinate limits. - validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) + if not standalone_thumb: + validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) final_mesh_hashes = { name: sha256_file(path) for name, path in verify_urdf_mesh_resources(paths["urdf"]).items() @@ -701,5 +882,9 @@ def finalize_session_artifacts( if final_mesh_hashes != mesh_hashes: raise ValueError("URDF mesh resources changed during publication") if release_ready: - atomic_session_pointer(config.session_root, "latest_passed", directory) + atomic_session_pointer( + config.session_root, + "latest_thumb_passed" if standalone_thumb else "latest_passed", + directory, + ) return summary, release_ready 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 0e9e619..aa91837 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 @@ -498,6 +498,8 @@ def three_camera_reason_zh( "third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差", "third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS", "third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95", + "zero_cycle_offset_range_deg": "训练轮零位极差", + "zero_confidence_95_half_width_deg": "零位95%置信半宽", "state_image_sync_p95_ms": "图像与电机状态同步误差P95", "tag_valid_rate_percent": "所需Tag同时有效率", } @@ -525,6 +527,8 @@ def three_camera_reason_zh( "third_cycle_axis_holdout_deg": "°", "third_cycle_axis_line_rms_mm": "mm", "third_cycle_trajectory_p95_deg": "°", + "zero_cycle_offset_range_deg": "°", + "zero_confidence_95_half_width_deg": "°", "state_image_sync_p95_ms": "ms", "tag_valid_rate_percent": "%", "cross_view_roll_curve": "°", @@ -550,6 +554,8 @@ def three_camera_reason_zh( f"{float(value):.2f}°" for value in cycle_travel ) + "）" cycle_values = failure.get("cycle_values_deg", []) + if not cycle_values: + cycle_values = failure.get("cycle_offset_deg", []) if cycle_values: detail += "（各轮=" + "/".join( f"{float(value):.2f}°" for value in cycle_values @@ -586,14 +592,26 @@ def three_camera_reason_zh( "请直接复制诊断块给开发者，不要放宽门限。" ) else: + source_task_names = set(active.get("source_task_names", [])) + thumb_yaw_source_retry = source_task_names == { + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + } suggestion = ( "方向分支已由软件保留，不要放宽门限；请检查传动回差或高支架刚度，" "处理后重新执行一键标定命令，程序会从最近可靠断点继续。" if directional_gap_failure - else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令；" - "程序只清除当前失败关节的数据并重扫" - f"{active.get('directions_to_rescan', 6)}个方向，" - "不需要手工调用ROS服务。" + else ( + "保持顶部Tag 8/9无遮挡；程序只替换决定yaw零位的" + "CMC pitch/roll顶部轴观测并重扫" + f"{active.get('directions_to_rescan', 16)}个方向，" + "不会无效重扫yaw侧摆。" + if thumb_yaw_source_retry + else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令；" + "程序只清除当前失败关节的数据并重扫" + f"{active.get('directions_to_rescan', 6)}个方向，" + "不需要手工调用ROS服务。" + ) ) return ( detail_text + "。程序已在当前关节结束后立即停止后续步骤。", @@ -658,6 +676,13 @@ def three_camera_reason_zh( "基准形态Tag预检已通过，正在把三台相机的固定掌部Tag稳健锁定为本会话参考。", "无需操作；锁定完成后允许任务姿态遮挡固定掌部Tag。", ) + if reason == "fixed_base_reference_moved": + return ( + "顶部Tag 8在本会话基准锁定后连续多帧发生角点位移；程序已立即保持机械手当前位置，" + "本会话中已采集数据不再用于发布。", + "Tag 8允许在下一次标定预检前重新摆放，但本次不能继续；固定Tag 8和顶部相机后" + "重新启动新会话。", + ) if reason == "waiting_for_task_tags_at_sweep_start": return ( "电机已到扫描起点，正在等待当前任务的实时运动Tag；显示为“锁”的固定掌部Tag" 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 1f0e9f8..6cda1c4 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 @@ -109,13 +109,16 @@ from .urdf_zero import ( fit_partial_palm_orientation_measurements, fit_rotation_joint_curve, derive_right_19_mechanical_endpoint_offsets, + expand_right_19_thumb_zero_result_with_cad_fingers, joint_curve_holdout_errors, maximum_axis_line_cycle_spread_m, measure_joint_curve_observation, + merge_right_19_thumb_zero_result, refit_axis_line_group_with_shared_radius, select_cross_view_roll_direction_source, solve_urdf_zero_offsets, get_zero_calibration_profile, + get_right_19_thumb_zero_profile, write_zero_corrected_urdf, with_depth_free_axis_projection, ) @@ -317,7 +320,7 @@ def _palm_axis_resume_policy( session_start: Mapping[str, Any], ) -> tuple[bool, tuple[str, ...]]: """Validate optional palm-axis checkpoint data when a model uses it.""" - capability = "palm_axis_side_channel_v2" + capability = "palm_axis_relative_motion_v3" previous = {str(value) for value in session_start.get("capabilities", [])} if not profile.palm_axis_observers and capability not in profile.capabilities: return True, () @@ -502,6 +505,155 @@ def _isolated_axis_cycle_outliers( return {best_excluded} +def _isolated_scalar_cycle_outlier( + values: Sequence[float], limit_rad: float +) -> set[int]: + """Return one clearly separated scalar outlier among three rounds. + + Zero-offset fits have three training cycles, so the axis helper above + cannot localize them (it deliberately requires three inliers). Use the + unique closest pair as a two-cycle consensus only when that pair is tight + and clearly better than either alternative. Symmetric drift and + generally scattered values retain the conservative full-task retry. + """ + samples = np.asarray(values, dtype=float) + limit = float(limit_rad) + if ( + samples.ndim != 1 + or samples.size != 3 + or not np.all(np.isfinite(samples)) + or not math.isfinite(limit) + or limit <= 0.0 + ): + return set() + pair_candidates = sorted( + ( + abs(float(samples[left] - samples[right])), + left, + right, + ) + for left in range(samples.size) + for right in range(left + 1, samples.size) + ) + pair_spread, left, right = pair_candidates[0] + second_pair_spread = pair_candidates[1][0] + overall_spread = pair_candidates[-1][0] + if overall_spread <= limit or pair_spread > 0.5 * limit: + return set() + # Do not arbitrarily pick one side of a gradual or nearly symmetric + # three-cycle drift. The winning pair must be materially tighter than + # the next candidate, not merely win due to floating-point noise. + if second_pair_spread - pair_spread < 0.1 * limit: + return set() + excluded = [ + index for index in range(samples.size) if index not in {left, right} + ] + if len(excluded) != 1: + return set() + outlier = excluded[0] + inlier_center = 0.5 * float(samples[left] + samples[right]) + if abs(float(samples[outlier]) - inlier_center) < 0.75 * limit: + return set() + return {outlier} + + +def _thumb_yaw_zero_repeatability_failures( + zero_result: ZeroSolveResult, + *, + maximum_cycle_range_rad: float, + maximum_confidence_half_width_rad: float, +) -> list[dict[str, Any]]: + """Apply a stricter, retry-aware publication gate to thumb CMC yaw.""" + joint_name = "thumb_cmc_yaw" + cycle_offsets = tuple( + float(value) + for value in zero_result.cycle_offsets_rad.get(joint_name, ()) + ) + failures: list[dict[str, Any]] = [] + if len(cycle_offsets) >= 2 and all( + math.isfinite(value) for value in cycle_offsets + ): + cycle_range = max(cycle_offsets) - min(cycle_offsets) + if cycle_range > float(maximum_cycle_range_rad): + failure: dict[str, Any] = { + "joint": joint_name, + "metric": "zero_cycle_offset_range_deg", + "actual": round(math.degrees(cycle_range), 6), + "limit": round( + math.degrees(maximum_cycle_range_rad), 6 + ), + "comparison": "maximum", + "cycle_offset_deg": [ + round(math.degrees(value), 6) + for value in cycle_offsets + ], + } + outliers = _isolated_scalar_cycle_outlier( + cycle_offsets, maximum_cycle_range_rad + ) + if outliers: + outlier = next(iter(outliers)) + failure["cycle"] = outlier + 1 + failure["inlier_cycles"] = [ + index + 1 + for index in range(len(cycle_offsets)) + if index != outlier + ] + failures.append(failure) + + # A range failure is more actionable: it can often identify exactly one + # acquisition cycle. Avoid adding a second, non-localized failure that + # would unnecessarily turn that retry back into a full yaw rescan. + if failures: + return failures + confidence = zero_result.offset_confidence_half_width_rad.get(joint_name) + if ( + confidence is not None + and math.isfinite(float(confidence)) + and float(confidence) > float(maximum_confidence_half_width_rad) + ): + failures.append( + { + "joint": joint_name, + "metric": "zero_confidence_95_half_width_deg", + "actual": round(math.degrees(float(confidence)), 6), + "limit": round( + math.degrees(maximum_confidence_half_width_rad), 6 + ), + "comparison": "maximum", + } + ) + return failures + + +def _previous_passed_joint_zero_offset( + session_dir: Path, serial_number: str, joint_name: str +) -> tuple[Path, float] | None: + """Read one zero from the previous passed sibling session, if present.""" + pointer = session_dir.parent / "latest_passed" + try: + previous = pointer.resolve(strict=True) + except OSError: + return None + if previous == session_dir or previous.parent != session_dir.parent: + return None + payload_path = previous / ( + f"g20_right_{_safe_name(serial_number)}_calibration.json" + ) + try: + payload = json.loads(payload_path.read_text(encoding="utf-8")) + offset = float( + payload["joints"][joint_name]["zero_angles"][ + "urdf_zero_offset_rad" + ] + ) + except (KeyError, OSError, TypeError, ValueError, json.JSONDecodeError): + return None + if not math.isfinite(offset): + return None + return previous, offset + + def _isolated_axis_line_cycle_outliers( measurements: Sequence[JointAxisMeasurement], limit_m: float ) -> set[int]: @@ -564,6 +716,7 @@ def _requires_pnp_tracker_reset_for_sweep( item: SweepItem, *, is_fit_retry: bool, + preserve_retry_continuity: bool = False, ) -> bool: """Reset G20-right PnP once per task, not once per formal cycle. @@ -578,6 +731,14 @@ def _requires_pnp_tracker_reset_for_sweep( return False if profile.layout_id != G20_RIGHT_19_LAYOUT: return True + if is_fit_retry and preserve_retry_continuity: + # A dependent retry (currently the top-view thumb-yaw axis pair) + # replaces motion samples inside an already accepted task-relative + # PnP generation. Resetting here can move the two planar Tags to a + # different, internally smooth pose branch and shift every yaw zero + # by several degrees. Keep the exact live tracker/reference while + # replacing only the localized physical acquisition. + return False if is_fit_retry: return True if profile.supports("precheck_sweeps"): @@ -598,7 +759,7 @@ def _sweep_uses_locked_base_reference( spec: SweepSpec, view: str, ) -> bool: - """Allow the fixed front palm Tag to be hidden by finger clearance. + """Use one session-fixed base pose wherever task geometry needs it. Pinky/ring flexion is required to expose the middle/index side Tag, but that same safe pose physically covers front Tag 0. The clearance remains @@ -606,11 +767,49 @@ def _sweep_uses_locked_base_reference( where front is only an inactive status camera. Tag 0 is therefore locked from baseline for every four-finger task; any moving Tag used by the actual capture remains live and mandatory. + + Top Tag 8 has a different contract: it remains live and visible, but its + pose is frozen after preflight. Pitch and roll are collected in separate + tasks and jointly define thumb yaw. Re-solving a near-frontal planar base + Tag independently in those tasks expresses the two axes in subtly + different frames and turns image-position-dependent IPPE tilt into a yaw + zero shift. One session reference makes any fixed pose bias common to + both axes, where it cancels geometrically. """ + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return False + selected_view = str(view) + if selected_view == "front": + return spec.key.startswith( + ("pinky_", "ring_", "middle_", "index_") + ) return bool( - profile.layout_id == G20_RIGHT_19_LAYOUT - and str(view) == "front" - and spec.key.startswith(("pinky_", "ring_", "middle_", "index_")) + selected_view == "top" + and spec.key + in { + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + "thumb_cmc_yaw_top", + } + ) + + +def _maximum_corner_drift_px( + reference: Sequence[Sequence[float]], + current: Sequence[Sequence[float]], +) -> float: + """Return the largest same-corner displacement for one fixed Tag.""" + reference_array = np.asarray(reference, dtype=float) + current_array = np.asarray(current, dtype=float) + if ( + reference_array.shape != (4, 2) + or current_array.shape != (4, 2) + or not np.all(np.isfinite(reference_array)) + or not np.all(np.isfinite(current_array)) + ): + return float("inf") + return float( + np.max(np.linalg.norm(current_array - reference_array, axis=1)) ) @@ -789,7 +988,7 @@ RESUMABLE_SAMPLE_KINDS = frozenset( {"sample", "baseline_hold_sample", "steady_command_sample"} ) -RECALIBRATION_SCOPES = frozenset({"full", "thumb"}) +RECALIBRATION_SCOPES = frozenset({"full", "thumb", "fingers"}) def recalibration_task_keys( @@ -809,18 +1008,169 @@ def recalibration_task_keys( or profile.layout_id != G20_RIGHT_19_LAYOUT ): raise ValueError( - "thumb recalibration is supported only for the G20 right 19-Tag product" + "partial recalibration is supported only for the G20 right " + "19-Tag product" ) - tasks = tuple( + thumb_tasks = tuple( spec.key for spec in profile.sweep_specs if any(str(name).startswith("thumb_") for name in spec.joints) ) - if len(tasks) != 4: + if len(thumb_tasks) != 4: raise ValueError( "G20 right thumb recalibration must resolve exactly four tasks" ) - return tasks + if selected == "thumb": + return thumb_tasks + thumb_task_set = set(thumb_tasks) + return tuple( + spec.key + for spec in profile.sweep_specs + if spec.key not in thumb_task_set + ) + + +def recalibration_quality_joints( + profile: HandCalibrationProfile, scope: str +) -> tuple[str, ...]: + """Return joints whose quality is decided by freshly acquired data.""" + selected = str(scope).strip().lower() + if selected == "full": + return tuple(profile.measured_joints) + # Reuse the scope validation and product guard in the task selector. + recalibration_task_keys(profile, selected) + want_thumb = selected == "thumb" + return tuple( + name + for name in profile.measured_joints + if str(name).startswith("thumb_") == want_thumb + ) + + +def build_standalone_thumb_payload( + *, + profile: HandCalibrationProfile, + serial_number: str, + measured_fits: Mapping[str, JointCurveFit], + thumb_offsets_rad: Mapping[str, float], + validation_errors_rad: Sequence[float], + baseline_command_u8: Sequence[int], + source_urdf_sha256: str, + camera_extrinsics_sha256: str, + corrected_urdf_sha256: str, +) -> dict[str, Any]: + """Build a non-runtime artifact for a fully standalone thumb solve.""" + if profile.side != "right" or profile.layout_id != G20_RIGHT_19_LAYOUT: + raise ValueError("standalone thumb payload requires G20 right 19-Tag") + expected_fits = set(recalibration_quality_joints(profile, "thumb")) + if set(measured_fits) != expected_fits: + raise ValueError("standalone thumb fits have the wrong joint set") + expected_offsets = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + offsets = { + str(name): float(value) for name, value in thumb_offsets_rad.items() + } + if set(offsets) != expected_offsets or any( + not math.isfinite(value) for value in offsets.values() + ): + raise ValueError("standalone thumb offsets must contain four finite zeros") + if len(baseline_command_u8) != 20: + raise ValueError("standalone thumb baseline must contain 20 commands") + joints: dict[str, dict[str, Any]] = {} + for name in sorted(expected_fits): + fit = measured_fits[name] + curve = np.asarray(fit.angle_rad, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"standalone thumb curve is invalid: {name}") + spec = profile.joint_specs[name] + item: dict[str, Any] = { + "motor_index": int(spec.motor_index), + "angle_rad": [round(float(value), 8) for value in curve], + } + if name in offsets: + item["zero_command_u8"] = int( + baseline_command_u8[spec.motor_index] + ) + item["zero_angles"] = { + "urdf_zero_offset_rad": round(offsets[name], 8) + } + else: + item["passive"] = True + joints[name] = item + absolute_errors = np.abs( + np.asarray(validation_errors_rad, dtype=float) + ) + if absolute_errors.size == 0 or not np.all(np.isfinite(absolute_errors)): + raise ValueError("standalone thumb validation errors are missing") + return { + "schema_version": 1, + "artifact_type": "g20_right_standalone_thumb_calibration", + "model": "G20", + "side": "right", + "serial_number": str(serial_number), + "angle_unit": "rad", + "command_range": [0, 255], + "baseline_command_u8": [int(value) for value in baseline_command_u8], + "non_thumb_zero_policy": "source_cad_unchanged", + "joints": joints, + "quality": { + "passed": True, + "validation_mae_rad": round(float(np.mean(absolute_errors)), 8), + "validation_p95_rad": round( + float(np.percentile(absolute_errors, 95.0)), 8 + ), + }, + "hashes": { + "source_urdf_sha256": str(source_urdf_sha256), + "camera_extrinsics_sha256": str(camera_extrinsics_sha256), + "corrected_urdf_sha256": str(corrected_urdf_sha256), + }, + } + + +def _partial_scope_frozen_zero_offsets( + profile: HandCalibrationProfile, + scope: str, + payload: Mapping[str, Any], +) -> dict[str, float]: + """Return certified non-target zeros that a partial run must preserve.""" + selected = str(scope).strip().lower() + if selected == "full": + return {} + if selected not in {"thumb", "fingers"}: + raise ValueError(f"unsupported partial recalibration scope: {scope}") + joints = payload.get("joints") + if not isinstance(joints, Mapping): + raise ValueError("base calibration payload is missing joints") + freeze_thumb = selected == "fingers" + result: dict[str, float] = {} + for name in get_zero_calibration_profile( + profile.side, profile.layout_id + ).direct_zero_joints: + is_thumb = str(name).startswith("thumb_") + if is_thumb != freeze_thumb: + continue + try: + value = float( + joints[name]["zero_angles"]["urdf_zero_offset_rad"] + ) + except (KeyError, TypeError, ValueError) as error: + raise ValueError( + f"base calibration is missing certified zero for {name}" + ) from error + if not math.isfinite(value): + raise ValueError(f"base calibration zero is invalid for {name}") + result[name] = value + expected = 4 if freeze_thumb else 12 + if len(result) != expected: + raise ValueError( + f"partial {selected} scope must freeze exactly {expected} zeros" + ) + return result def _normalise_legacy_split_roll_resume_rows( @@ -857,6 +1207,91 @@ def _normalise_legacy_split_roll_resume_rows( return tuple(normalised) +def _discard_automatically_retried_sweep_rows( + rows: Sequence[Mapping[str, Any]], +) -> tuple[dict[str, Any], ...]: + """Replay automatic-sweep retry tombstones before checkpoint selection. + + An automatic acquisition retry clears that cycle/direction from the + online in-memory stores. The raw JSONL is append-only, so the superseded + steady checkpoints remain physically present before the + ``automatic_sweep_retry`` event. Treat that event as a write-ahead + tombstone: records before it must never be merged with the replacement + acquisition written afterwards. + + Older sessions did not include ``task_name`` on retry events. Their + motor/cycle/direction and joint list still identify the invalidated + acquisition unambiguously. New sessions include both task and attempt + for a stronger durable identity. + """ + materialised = tuple(dict(row) for row in rows) + retry_events = [ + (index, row) + for index, row in enumerate(materialised) + if str(row.get("kind", "")) == "automatic_sweep_retry" + ] + if not retry_events: + return materialised + + durable_kinds = set(RESUMABLE_SAMPLE_KINDS) | { + "palm_axis_sample", + "synchronised_frame", + } + + def invalidated( + row_index: int, row: Mapping[str, Any] + ) -> bool: + if str(row.get("kind", "")) not in durable_kinds: + return False + row_task = str(row.get("task_name", "")) + row_cycle = int(row.get("cycle", -999)) + row_direction = str(row.get("direction", "")) + row_motor = int(row.get("motor_index", -1)) + row_joints = { + str(name) + for name in ( + row.get("joint"), + row.get("model_joint"), + *(row.get("joints", ()) or ()), + ) + if name + } + for retry_index, retry in retry_events: + if retry_index <= row_index: + continue + if int(retry.get("cycle", -998)) != row_cycle: + continue + if str(retry.get("direction", "")) != row_direction: + continue + retry_task = str(retry.get("task_name", "")) + if retry_task and row_task != retry_task: + continue + retry_motor = int(retry.get("motor_index", -1)) + if ( + row_motor >= 0 + and retry_motor >= 0 + and row_motor != retry_motor + ): + continue + retry_joints = { + str(name) for name in (retry.get("joints", ()) or ()) if name + } + if ( + row_joints + and retry_joints + and row_joints.isdisjoint(retry_joints) + ): + continue + return True + return False + + return tuple( + row + for index, row in enumerate(materialised) + if not invalidated(index, row) + ) + + def _latest_resume_rows( rows: Sequence[Mapping[str, Any]], *, @@ -1044,7 +1479,9 @@ def resumable_completed_task_prefix( returns only a contiguous prefix; product resume can opt into sparse recovery so a failed early task does not discard independent later tasks. """ - rows = _normalise_legacy_split_roll_resume_rows(profile, rows) + rows = _discard_automatically_retried_sweep_rows( + _normalise_legacy_split_roll_resume_rows(profile, rows) + ) samples = _latest_resume_rows( rows, kind="sample", @@ -1791,9 +2228,20 @@ class ViewRuntime: fixed_base_observations: deque[ tuple[SquareTagPose, tuple[float, float], TagQuality] ] = field(default_factory=deque) + fixed_base_corner_observations: deque[np.ndarray] = field( + default_factory=deque + ) locked_base_pose: SquareTagPose | None = None locked_base_center_xy_px: tuple[float, float] | None = None + locked_base_corners_xy: tuple[ + tuple[float, float], + tuple[float, float], + tuple[float, float], + tuple[float, float], + ] | None = None locked_base_quality: TagQuality | None = None + locked_base_corner_drift_count: int = 0 + latest_locked_base_corner_drift_px: float = 0.0 def __post_init__(self) -> None: self.role_by_id = { @@ -1802,6 +2250,9 @@ class ViewRuntime: self.valid_flags = deque(maxlen=self.preflight_frames) self.detection_times = deque(maxlen=self.preflight_frames) self.fixed_base_observations = deque(maxlen=self.preflight_frames) + self.fixed_base_corner_observations = deque( + maxlen=self.preflight_frames + ) if not self.preflight_roles: self.preflight_roles = tuple(self.view_tags) if any(role not in self.view_tags for role in self.preflight_roles): @@ -1929,6 +2380,13 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_resume_index: int | None = None self.retry_sweep_items: list[SweepItem] = [] self.retry_joint_names: set[str] = set() + # Some fitted quantities are observed by data collected during other + # physical tasks. In particular, right-hand thumb yaw zero comes from + # the two top-view side channels captured during CMC pitch and roll. + self.retry_source_failure_task_key: str | None = None + self.retry_source_task_keys: tuple[str, ...] = () + self.retry_cycle_override: set[int] = set() + self.retry_preserve_pnp_continuity = False self.active_sweep_is_fit_retry = False self.fit_failure: dict[str, Any] = {} self.fit_failure_history_by_task: dict[ @@ -2100,6 +2558,8 @@ class G20ThreeCameraCalibrationNode(Node): self.declare_parameter("pnp_group_initialization_frames", 8) self.declare_parameter("pnp_group_normal_alignment_scale_deg", 5.0) self.declare_parameter("pnp_group_maximum_normal_alignment_deg", 15.0) + self.declare_parameter("fixed_base_maximum_corner_drift_px", 2.0) + self.declare_parameter("fixed_base_movement_confirmation_frames", 5) self.declare_parameter("thumb_ip_pnp_coupling_multiplier", 1.03) self.declare_parameter("thumb_ip_pnp_coupling_scale_deg", 3.0) self.declare_parameter( @@ -2119,6 +2579,15 @@ class G20ThreeCameraCalibrationNode(Node): "passive_maximum_rotation_orthogonal_rms_deg", 7.5 ) self.declare_parameter("zero_maximum_axis_cycle_difference_deg", 0.75) + self.declare_parameter( + "thumb_yaw_maximum_zero_cycle_difference_deg", 0.5 + ) + self.declare_parameter( + "thumb_yaw_maximum_confidence_half_width_deg", 0.75 + ) + self.declare_parameter( + "thumb_yaw_cross_session_diagnostic_deg", 0.75 + ) self.declare_parameter("zero_maximum_axis_cone_mismatch_deg", 5.0) self.declare_parameter( "zero_maximum_observability_condition_number", 1.0e10 @@ -2255,13 +2724,45 @@ class G20ThreeCameraCalibrationNode(Node): self.recalibration_task_keys = recalibration_task_keys( self.profile, self.recalibration_scope ) + self.standalone_thumb_calibration = bool( + self.recalibration_scope == "thumb" + and self.resume_raw_samples_path is None + ) if ( - self.recalibration_scope != "full" + self.recalibration_scope == "fingers" and self.resume_raw_samples_path is None ): raise ValueError( - "partial recalibration requires resume_raw_samples_path from " - "a passed complete session" + "finger recalibration requires resume_raw_samples_path from " + "a passed thumb or complete session" + ) + self.partial_scope_fixed_zero_offsets_rad: dict[str, float] = {} + if ( + self.recalibration_scope != "full" + and self.resume_raw_samples_path is not None + ): + assert self.resume_raw_samples_path is not None + base_payload_path = self.resume_raw_samples_path.parent / ( + f"g20_right_{_safe_name(self.serial_number)}_calibration.json" + ) + try: + base_payload = json.loads( + base_payload_path.read_text(encoding="utf-8") + ) + except (OSError, json.JSONDecodeError) as error: + raise ValueError( + "partial recalibration base calibration JSON is invalid" + ) from error + if not isinstance(base_payload, Mapping): + raise ValueError( + "partial recalibration base calibration JSON must be an object" + ) + self.partial_scope_fixed_zero_offsets_rad = ( + _partial_scope_frozen_zero_offsets( + self.profile, + self.recalibration_scope, + base_payload, + ) ) self.resumed_task_keys: tuple[str, ...] = () self.resume_source_session = "" @@ -2412,6 +2913,12 @@ class G20ThreeCameraCalibrationNode(Node): self.pnp_group_maximum_normal_alignment_rad = math.radians( float(value("pnp_group_maximum_normal_alignment_deg")) ) + self.fixed_base_maximum_corner_drift_px = float( + value("fixed_base_maximum_corner_drift_px") + ) + self.fixed_base_movement_confirmation_frames = int( + value("fixed_base_movement_confirmation_frames") + ) self.thumb_ip_pnp_coupling_multiplier = float( value("thumb_ip_pnp_coupling_multiplier") ) @@ -2451,6 +2958,15 @@ class G20ThreeCameraCalibrationNode(Node): self.zero_maximum_axis_cycle_difference_rad = math.radians( float(value("zero_maximum_axis_cycle_difference_deg")) ) + self.thumb_yaw_maximum_zero_cycle_difference_rad = math.radians( + float(value("thumb_yaw_maximum_zero_cycle_difference_deg")) + ) + self.thumb_yaw_maximum_confidence_half_width_rad = math.radians( + float(value("thumb_yaw_maximum_confidence_half_width_deg")) + ) + self.thumb_yaw_cross_session_diagnostic_rad = math.radians( + float(value("thumb_yaw_cross_session_diagnostic_deg")) + ) self.zero_maximum_axis_cone_mismatch_rad = math.radians( float(value("zero_maximum_axis_cone_mismatch_deg")) ) @@ -2602,6 +3118,10 @@ class G20ThreeCameraCalibrationNode(Node): self.combination_validation_enabled = bool( value("combination_validation_enabled") ) + if self.standalone_thumb_calibration: + # A standalone thumb artifact has no four-finger curves or + # Cartesian combination model to validate. + self.combination_validation_enabled = False self.combination_validation_frames = int( value("combination_validation_frames") ) @@ -2760,6 +3280,14 @@ class G20ThreeCameraCalibrationNode(Node): raise ValueError( "pnp_group_initialization_frames must be in [3, 30]" ) + if not 0.5 <= self.fixed_base_maximum_corner_drift_px <= 10.0: + raise ValueError( + "fixed_base_maximum_corner_drift_px must be in [0.5, 10]" + ) + if not 1 <= self.fixed_base_movement_confirmation_frames <= 30: + raise ValueError( + "fixed_base_movement_confirmation_frames must be in [1, 30]" + ) if not ( self.endpoint_tolerance_u8 <= self.pinky_pip_zero_endpoint_tolerance_u8 @@ -2792,6 +3320,9 @@ class G20ThreeCameraCalibrationNode(Node): self.passive_axis_maximum_plane_rms_m, self.axis_maximum_pose_line_rms_m, self.zero_maximum_axis_cycle_difference_rad, + self.thumb_yaw_maximum_zero_cycle_difference_rad, + self.thumb_yaw_maximum_confidence_half_width_rad, + self.thumb_yaw_cross_session_diagnostic_rad, self.zero_maximum_axis_cone_mismatch_rad, self.zero_maximum_offset_rad, self.zero_finger_maximum_offset_rad, @@ -2814,6 +3345,22 @@ class G20ThreeCameraCalibrationNode(Node): ) ): raise ValueError("trajectory quality thresholds must be positive") + if ( + self.thumb_yaw_maximum_zero_cycle_difference_rad + > self.zero_maximum_axis_cycle_difference_rad + ): + raise ValueError( + "thumb_yaw_maximum_zero_cycle_difference_deg must not " + "exceed zero_maximum_axis_cycle_difference_deg" + ) + if ( + self.thumb_yaw_maximum_confidence_half_width_rad + > self.zero_maximum_confidence_half_width_rad + ): + raise ValueError( + "thumb_yaw_maximum_confidence_half_width_deg must not " + "exceed zero_maximum_confidence_half_width_deg" + ) if not 1.0 < self.cross_view_roll_maximum_projection_scale_ratio <= 2.0: raise ValueError( "cross-view roll projection-scale ratio must be in (1, 2]" @@ -2983,6 +3530,13 @@ class G20ThreeCameraCalibrationNode(Node): reset_after_seconds=self.pnp_tracker_reset_seconds, ) preflight_roles = tuple(self.profile.preflight_view_roles[name]) + if getattr(self, "standalone_thumb_calibration", False): + base_role = _fixed_base_role(name) + preflight_roles = tuple( + role + for role in self.profile.view_tags[name] + if role == base_role or role.startswith("thumb_") + ) group = self._make_group_pose_tracker( name, preflight_roles, @@ -3087,6 +3641,11 @@ class G20ThreeCameraCalibrationNode(Node): self, view: str, required_roles: Sequence[str] ) -> tuple[str, ...]: locked = self._locked_base_role_for_active_capture(view) + # Front Tag 0 may be intentionally hidden by finger clearance. Top + # Tag 8 must remain visible so its raw corners can prove that the + # session reference has not moved, even though its PnP pose is frozen. + if str(view) == "top": + return tuple(required_roles) return tuple(role for role in required_roles if role != locked) def _lock_fixed_base_references(self) -> bool: @@ -3125,6 +3684,21 @@ class G20ThreeCameraCalibrationNode(Node): runtime.locked_base_center_xy_px = tuple( float(value) for value in np.median(centres, axis=0) ) + corner_observations = list( + getattr(runtime, "fixed_base_corner_observations", ()) + ) + if len(corner_observations) >= minimum: + locked_corners = np.median( + np.asarray(corner_observations, dtype=float), axis=0 + ) + runtime.locked_base_corners_xy = tuple( + tuple(float(value) for value in point) + for point in locked_corners + ) + else: + runtime.locked_base_corners_xy = None + runtime.locked_base_corner_drift_count = 0 + runtime.latest_locked_base_corner_drift_px = 0.0 runtime.locked_base_quality = TagQuality( hamming=max(item.hamming for item in qualities), decision_margin=min(item.decision_margin for item in qualities), @@ -3173,6 +3747,14 @@ class G20ThreeCameraCalibrationNode(Node): "translation_rms_m": round(translation_rms_m, 9), "rotation_p95_deg": round(rotation_p95_deg, 6), "reprojection_p95_px": round(reprojection, 6), + "corner_reference_xy": ( + None + if runtime.locked_base_corners_xy is None + else [ + list(point) + for point in runtime.locked_base_corners_xy + ] + ), }, ) return not pending and all( @@ -3504,6 +4086,49 @@ class G20ThreeCameraCalibrationNode(Node): corners_by_role[role] = corners centres_by_role[role] = np.mean(corners, axis=0) + if ( + view == "top" + and locked_base_role is not None + and locked_base_role in corners_by_role + and runtime.locked_base_corners_xy is not None + ): + corner_drift = _maximum_corner_drift_px( + runtime.locked_base_corners_xy, + corners_by_role[locked_base_role], + ) + runtime.latest_locked_base_corner_drift_px = corner_drift + if corner_drift > self.fixed_base_maximum_corner_drift_px: + runtime.locked_base_corner_drift_count += 1 + else: + runtime.locked_base_corner_drift_count = 0 + if ( + runtime.locked_base_corner_drift_count + >= self.fixed_base_movement_confirmation_frames + ): + append_jsonl( + self.raw_path, + { + "kind": "fixed_base_reference_moved", + "view": view, + "role": locked_base_role, + "tag_id": runtime.view_tags[locked_base_role], + "corner_drift_px": round(corner_drift, 6), + "maximum_corner_drift_px": ( + self.fixed_base_maximum_corner_drift_px + ), + "confirmation_frames": ( + runtime.locked_base_corner_drift_count + ), + "task_name": ( + "" + if self.active_sweep is None + else self.active_sweep.spec.key + ), + }, + ) + self._pause("fixed_base_reference_moved") + return + pose_roles = ( tuple( role @@ -3530,6 +4155,13 @@ class G20ThreeCameraCalibrationNode(Node): ) ) ) + if locked_base_role is not None: + # The fixed pose is injected below. A live top base remains a + # detection/movement requirement but is never re-solved into the + # pitch/roll/yaw geometry after the session reference is locked. + pose_roles = tuple( + role for role in pose_roles if role != locked_base_role + ) observable_combination_joints = ( _combination_observable_joints( self.profile, @@ -3829,6 +4461,9 @@ class G20ThreeCameraCalibrationNode(Node): live_qualities[base_role], ) ) + runtime.fixed_base_corner_observations.append( + np.asarray(corners_by_role[base_role], dtype=float).copy() + ) runtime.latest_tag_quality = live_qualities runtime.last_message_at = now runtime.detection_times.append(now) @@ -4242,9 +4877,17 @@ class G20ThreeCameraCalibrationNode(Node): observations = getattr(runtime, "fixed_base_observations", None) if observations is not None: observations.clear() + corner_observations = getattr( + runtime, "fixed_base_corner_observations", None + ) + if corner_observations is not None: + corner_observations.clear() runtime.locked_base_pose = None runtime.locked_base_center_xy_px = None + runtime.locked_base_corners_xy = None runtime.locked_base_quality = None + runtime.locked_base_corner_drift_count = 0 + runtime.latest_locked_base_corner_drift_px = 0.0 self._reset_view_trackers(runtime) self.startup_baseline_recovered = True self.state = STATE_PREFLIGHT @@ -4286,12 +4929,37 @@ class G20ThreeCameraCalibrationNode(Node): 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 + profile = _node_profile(self) + source_task_keys = ( + tuple(getattr(self, "retry_source_task_keys", ())) + if getattr(self, "retry_source_failure_task_key", None) == spec.key + else () ) - retry_joint_names.intersection_update(spec.joints) - if not retry_joint_names: - retry_joint_names = set(spec.joints) + source_key_set = set(source_task_keys) + source_specs = [ + candidate + for candidate in profile.sweep_specs + if candidate.key in source_key_set + ] + # Yaw zero is a same-view relationship between the pitch and roll + # axes. A retry must remain in the original top-camera PnP reference + # generation; otherwise the retry changes the datum it is intended to + # verify. Ordinary single-task retries still reset their camera to + # recover from a genuinely bad branch. + self.retry_preserve_pnp_continuity = bool(source_specs) + data_specs = source_specs or [spec] + retry_joint_names = set(getattr(self, "retry_joint_names", set())) + if source_specs: + retry_joint_names = { + joint_name + for candidate in source_specs + for joint_name in candidate.joints + } + else: + retry_joint_names = retry_joint_names or set(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", {} @@ -4315,15 +4983,13 @@ class G20ThreeCameraCalibrationNode(Node): for record in command_records[joint_name] if int(record.get("cycle", -1)) not in cycles ] - observer = next( - ( - item - for item in _node_profile(self).palm_axis_observers - if item.task_name == spec.key - ), - None, - ) - if observer is not None: + data_task_keys = {candidate.key for candidate in data_specs} + observers = [ + item + for item in profile.palm_axis_observers + if item.task_name in data_task_keys + ] + for observer in observers: records = self.palm_axis_records_by_source.get( observer.source_name, [] ) @@ -4336,11 +5002,12 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_sweep_items = [ item for item in all_items - if item.spec == spec and item.cycle in cycles + if item.spec in data_specs and item.cycle in cycles ] if not self.retry_sweep_items: self.retry_sweep_items = [ - SweepItem(spec, cycle, direction) + SweepItem(candidate, cycle, direction) + for candidate in data_specs for cycle in sorted(cycles) for direction in ( DIRECTION_DECREASING, @@ -4355,10 +5022,10 @@ class G20ThreeCameraCalibrationNode(Node): # unchanged. reset_trackers = getattr(self, "_reset_view_trackers", None) retry_views = { - _node_profile(self).record_specs[name].view + profile.record_specs[name].view for name in retry_joint_names } - if observer is not None: + for observer in observers: retry_views.add(observer.view) for view in retry_views: if view is None: @@ -4366,6 +5033,12 @@ class G20ThreeCameraCalibrationNode(Node): runtime = getattr(self, "views", {}).get(view) if runtime is None or reset_trackers is None: continue + if self.retry_preserve_pnp_continuity: + # Frame-quality counters are attempt-local even though the + # geometric PnP state deliberately remains continuous. + runtime.task_valid_frames = 0 + runtime.task_total_frames = 0 + continue if _node_profile(self).layout_id == G20_RIGHT_19_LAYOUT: reset_trackers(runtime, preserve_task_reference=True) else: @@ -4381,8 +5054,21 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_attempts[storage_key] = ( self.sweep_attempts.get(storage_key, 1) + 1 ) + source_attempts: dict[str, int] = {} + for source_spec in source_specs: + source_key = _sweep_storage_key(source_spec) + self.sweep_attempts[source_key] = ( + self.sweep_attempts.get(source_key, 1) + 1 + ) + source_attempts[str(source_spec.key)] = self.sweep_attempts[ + source_key + ] + invalidated_storage_keys = { + storage_key, + *(_sweep_storage_key(candidate) for candidate in source_specs), + } for key in list(getattr(self, "sweep_retry_counts", {})): - if key[0] == storage_key: + if key[0] in invalidated_storage_keys: self.sweep_retry_counts.pop(key, None) append_jsonl( self.raw_path, @@ -4393,6 +5079,21 @@ class G20ThreeCameraCalibrationNode(Node): "motor_index": spec.motor_index, "joints": list(spec.joints), "joints_to_rescan": sorted(retry_joint_names), + **( + {"source_task_names": list(source_task_keys)} + if source_task_keys + else {} + ), + **( + {"source_attempts": source_attempts} + if source_attempts + else {} + ), + **( + {"pnp_reference_policy": "preserve_existing_generation"} + if self.retry_preserve_pnp_continuity + else {} + ), "attempt": self.sweep_attempts[storage_key], "reason": self.paused_reason, "cycles": [cycle + 1 for cycle in sorted(cycles)], @@ -4439,6 +5140,10 @@ class G20ThreeCameraCalibrationNode(Node): ) return {} expected = set(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS) + if getattr(self, "standalone_thumb_calibration", False): + expected = { + name for name in expected if name.startswith("thumb_") + } actual = set(stored) if actual != expected: missing = ",".join(sorted(expected - actual)) or "-" @@ -4714,6 +5419,38 @@ class G20ThreeCameraCalibrationNode(Node): later complete tasks so only the failed task has to be re-collected; the compatibility mode still drops the remaining suffix. """ + def discard_task_records(task_key: str) -> None: + spec = next( + ( + candidate + for candidate in self.profile.sweep_specs + if candidate.key == task_key + ), + None, + ) + if spec is None: + return + for joint_name in spec.joints: + for store in ( + self.records_by_joint, + self.baseline_records_by_joint, + self.command_records_by_joint, + ): + if joint_name in store: + store[joint_name].clear() + observer = next( + ( + item + for item in self.profile.palm_axis_observers + if item.task_name == task_key + ), + None, + ) + if observer is not None: + self.palm_axis_records_by_source[ + observer.source_name + ].clear() + accepted: list[str] = [] dropped: list[dict[str, Any]] = [] for task_index, task_key in enumerate(completed, start=1): @@ -4757,6 +5494,14 @@ class G20ThreeCameraCalibrationNode(Node): ], } ) + # Make the rejection visible before checking later tasks. + # Some fits intentionally depend on an earlier reference + # task (for example every non-reference finger roll uses the + # pinky roll direction). Delaying this clear until the end + # let a dependent task pass with records that were about to + # be removed, producing a checkpoint that failed only on the + # next restart. + discard_task_records(task_key) if allow_sparse: continue break @@ -4771,34 +5516,11 @@ class G20ThreeCameraCalibrationNode(Node): } ) _publish_import_status(self) - if dropped: + if dropped and not allow_sparse: accepted_set = set(accepted) for task_key in completed: - if task_key in accepted_set: - continue - for spec in self.profile.sweep_specs: - if spec.key != task_key: - continue - for joint_name in spec.joints: - for store in ( - self.records_by_joint, - self.baseline_records_by_joint, - self.command_records_by_joint, - ): - if joint_name in store: - store[joint_name].clear() - observer = next( - ( - item - for item in self.profile.palm_axis_observers - if item.task_name == task_key - ), - None, - ) - if observer is not None: - self.palm_axis_records_by_source[ - observer.source_name - ].clear() + if task_key not in accepted_set: + discard_task_records(task_key) if isinstance(completed, tuple): accepted = tuple(accepted) return accepted, dropped @@ -4832,6 +5554,13 @@ class G20ThreeCameraCalibrationNode(Node): self.sweep_items = [] self.pnp_task_spec = None selected_sweep_specs = list(self.profile.sweep_specs) + if self.standalone_thumb_calibration: + standalone_task_keys = set(self.recalibration_task_keys) + selected_sweep_specs = [ + spec + for spec in selected_sweep_specs + if spec.key in standalone_task_keys + ] if self.cross_view_roll_diagnostic_finger: selected_sweep_specs = [ spec @@ -4855,6 +5584,10 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_resume_index = None self.retry_sweep_items.clear() self.retry_joint_names.clear() + self.retry_source_failure_task_key = None + self.retry_source_task_keys = () + self.retry_cycle_override.clear() + self.retry_preserve_pnp_continuity = False self.active_sweep_is_fit_retry = False self.fit_failure = {} self.fit_failure_history_by_task.clear() @@ -5030,6 +5763,14 @@ class G20ThreeCameraCalibrationNode(Node): "the camera/tag/URDF geometry" ) return response + if self.paused_reason == "fixed_base_reference_moved": + response.success = False + response.message = ( + "the locked top base reference moved during acquisition; " + "start a new session so preflight can establish a new " + "reference before any motion" + ) + return response if self.paused_reason == "cross_view_roll_diagnostic_complete": response.success = False response.message = ( @@ -6211,6 +6952,9 @@ class G20ThreeCameraCalibrationNode(Node): profile, item, is_fit_retry=is_fit_retry, + preserve_retry_continuity=bool( + getattr(self, "retry_preserve_pnp_continuity", False) + ), ): reset_trackers = getattr(self, "_reset_view_trackers", None) preserve_task_reference = _preserve_pnp_task_reference_for_sweep( @@ -8318,12 +9062,38 @@ class G20ThreeCameraCalibrationNode(Node): self.retry_joint_names = _fit_retry_joint_names( _node_profile(self), spec, failures ) + source_task_keys = ( + tuple(getattr(self, "retry_source_task_keys", ())) + if getattr(self, "retry_source_failure_task_key", None) == spec.key + else () + ) + source_key_set = set(source_task_keys) + source_specs = [ + candidate + for candidate in _node_profile(self).sweep_specs + if candidate.key in source_key_set + ] + if source_specs: + self.retry_joint_names = { + joint_name + for candidate in source_specs + for joint_name in candidate.joints + } localized_cycles = { int(item["cycle"]) - 1 for item in failures if item.get("cycle") is not None } - if failures and all( + cycle_override = set( + getattr(self, "retry_cycle_override", set()) + ) + if source_specs and cycle_override: + # These source tasks jointly define one PnP-relative zero. Mixing + # cycles acquired before and after a tracker/reference reset can + # make a clean branch change look like a physical zero change. + # Reacquire the whole source holdout set as one generation. + self.retry_cycles = cycle_override + elif failures and all( item.get("cycle") is not None or str(item.get("metric", "")).startswith("third_cycle_") for item in failures @@ -8364,6 +9134,11 @@ class G20ThreeCameraCalibrationNode(Node): "motor_index": spec.motor_index, "joints": list(spec.joints), "joints_to_rescan": sorted(self.retry_joint_names), + **( + {"source_task_names": list(source_task_keys)} + if source_task_keys + else {} + ), "attempt": attempt, "fit_attempt": attempt, "fit_attempt_limit": fit_retry_limit + 1, @@ -8373,7 +9148,11 @@ class G20ThreeCameraCalibrationNode(Node): cycle + 1 for cycle in sorted(self.retry_cycles) ], "directions_to_rescan": ( - 0 if systematic else 2 * len(self.retry_cycles) + 0 + if systematic + else 2 + * len(self.retry_cycles) + * max(1, len(source_specs)) ), "recoverable_by_rescan": not systematic, "repeated_branch_clusters": repeated_branch_clusters, @@ -8591,6 +9370,46 @@ class G20ThreeCameraCalibrationNode(Node): continue self._reset_view_trackers(runtime) runtime.pnp_reset_count += 1 + speed_scales = getattr( + self, "retry_speed_scales", (0.8, 0.6, 0.5) + ) + endpoint_holds = getattr( + self, "retry_endpoint_hold_seconds", (0.75, 1.0, 1.25) + ) + # Write the invalidation before mutating the authoritative in-memory + # stores. Restart replays this event as a tombstone, so a crash can + # neither resurrect the rejected sweep nor merge it with its + # replacement acquisition. + append_jsonl( + self.raw_path, + { + "kind": "automatic_sweep_retry", + "task_name": item.spec.key, + "attempt": int( + getattr(self, "sweep_attempts", {}).get( + _sweep_storage_key(item.spec), 1 + ) + ), + "view": item.spec.view, + "motor_index": item.spec.motor_index, + "joints": list(item.spec.joints), + "cycle": item.cycle, + "direction": item.direction, + "retry": retries, + "retry_limit": retry_limit, + "reason": str(reason), + "speed_scale": float( + speed_scales[ + min(retries, len(speed_scales)) - 1 + ] + ), + "endpoint_hold_seconds": float( + endpoint_holds[ + min(retries, len(endpoint_holds)) - 1 + ] + ), + }, + ) command_records = getattr(self, "command_records_by_joint", {}) for joint_name in item.spec.joints: if joint_name in command_records: @@ -8617,36 +9436,6 @@ class G20ThreeCameraCalibrationNode(Node): ] getattr(self, "sweep_frames", []).clear() getattr(self, "sweep_start_frames", []).clear() - speed_scales = getattr( - self, "retry_speed_scales", (0.8, 0.6, 0.5) - ) - endpoint_holds = getattr( - self, "retry_endpoint_hold_seconds", (0.75, 1.0, 1.25) - ) - append_jsonl( - self.raw_path, - { - "kind": "automatic_sweep_retry", - "view": item.spec.view, - "motor_index": item.spec.motor_index, - "joints": list(item.spec.joints), - "cycle": item.cycle, - "direction": item.direction, - "retry": retries, - "retry_limit": retry_limit, - "reason": str(reason), - "speed_scale": float( - speed_scales[ - min(retries, len(speed_scales)) - 1 - ] - ), - "endpoint_hold_seconds": float( - endpoint_holds[ - min(retries, len(endpoint_holds)) - 1 - ] - ), - }, - ) self.reason = f"automatic_retry_{reason}" self._begin_return_baseline("retry_sweep") @@ -9210,6 +9999,13 @@ class G20ThreeCameraCalibrationNode(Node): ) self._start_next_sweep() return + # All physical source items for a dependent retry are complete. + # Clear the routing before checking the final source task itself, + # so an ordinary source-task failure retries only that task. + self.retry_source_failure_task_key = None + self.retry_source_task_keys = () + self.retry_cycle_override.clear() + self.retry_preserve_pnp_continuity = False failures = self._provisional_fit_failures(previous_spec) diagnostic_role = ( G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( @@ -9325,7 +10121,12 @@ class G20ThreeCameraCalibrationNode(Node): ) from error # Provisional checks have a warning band so collection can continue, # but the final fit always re-applies the unmodified hard thresholds. + fresh_task_keys = set( + getattr(self, "recalibration_task_keys", ()) + ) for spec in self.profile.sweep_specs: + if fresh_task_keys and spec.key not in fresh_task_keys: + continue failures = self._provisional_fit_failures(spec) if failures: self._pause_for_provisional_fit_failure( @@ -9338,7 +10139,24 @@ class G20ThreeCameraCalibrationNode(Node): validation_cycle = self.repetitions - 1 training_cycles = tuple(range(validation_cycle)) training_cycle_set = set(training_cycles) - for name in self.profile.measured_joints: + quality_joint_names = set( + recalibration_quality_joints( + self.profile, + getattr(self, "recalibration_scope", "full"), + ) + ) + standalone_thumb_calibration = bool( + getattr(self, "standalone_thumb_calibration", False) + ) + fit_joint_names = tuple( + name + for name in self.profile.measured_joints + if ( + not standalone_thumb_calibration + or name in quality_joint_names + ) + ) + for name in fit_joint_names: training_records = [ record for record in self.records_by_joint[name] @@ -9377,7 +10195,7 @@ class G20ThreeCameraCalibrationNode(Node): if self.profile.layout_id == G20_RIGHT_19_LAYOUT: command_fits = {} command_feedback_fits = {} - for name in self.profile.measured_joints: + for name in fit_joint_names: if name in G20_REFERENCE_THUMB_CMC_JOINTS: # a609d521's dense feedback-indexed relative-rotation # curve is the validated transfer function for the three @@ -9446,6 +10264,11 @@ class G20ThreeCameraCalibrationNode(Node): for name, validation_name in ( self.profile.axis_validation_sources or {} ).items(): + if ( + name not in training_fits + or validation_name not in quality_joint_names + ): + continue validation_training_records = [ record for record in self.records_by_joint[validation_name] @@ -9507,7 +10330,7 @@ class G20ThreeCameraCalibrationNode(Node): }, } self.joint_dynamic_diagnostics: dict[str, dict[str, Any]] = {} - for name in self.profile.measured_joints: + for name in fit_joint_names: cycle_travel: list[float] = [] for cycle in range(self.repetitions): cycle_fit = self._fit_joint_records( @@ -9561,7 +10384,15 @@ class G20ThreeCameraCalibrationNode(Node): "holdout_cycle_max_rad": round(float(np.max(holdout)), 8), } axes: list[JointAxisMeasurement] = [] - for name in self.zero_profile.axis_joints: + axis_profile = ( + get_right_19_thumb_zero_profile() + if ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and self.recalibration_scope == "thumb" + ) + else self.zero_profile + ) + for name in axis_profile.axis_joints: group = [ self._fit_axis_measurement(name, cycle) for cycle in range(self.repetitions) @@ -9630,6 +10461,17 @@ class G20ThreeCameraCalibrationNode(Node): for name, spec in self.profile.joint_specs.items() } if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + if self.recalibration_scope == "full": + scoped_endpoint_joints = frozenset( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + ) + else: + want_thumb_endpoints = self.recalibration_scope == "thumb" + scoped_endpoint_joints = frozenset( + name + for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + if name.startswith("thumb_") == want_thumb_endpoints + ) command_fits = anchor_right_19_mechanical_endpoint_curves( command_fits, { @@ -9642,12 +10484,16 @@ class G20ThreeCameraCalibrationNode(Node): if name == "thumb_cmc_roll" else self.command_records_by_joint[name] ) - for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + for name in scoped_endpoint_joints }, maximum_direction_difference_rad=( self.command_maximum_direction_gap_rad ), - feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}), + feedback_endpoint_joints=( + frozenset({"thumb_cmc_roll"}) + & scoped_endpoint_joints + ), + endpoint_joints=scoped_endpoint_joints, ) measured_endpoint_zero_offsets = ( derive_right_19_mechanical_endpoint_offsets( @@ -9656,8 +10502,27 @@ class G20ThreeCameraCalibrationNode(Node): maximum_offset_rad=( self.mechanical_endpoint_maximum_offset_rad ), + endpoint_joints=scoped_endpoint_joints, ) ) + frozen_offsets = dict( + getattr( + self, + "partial_scope_fixed_zero_offsets_rad", + {}, + ) + ) + # Endpoint-derived zeros also move URDF limits or mimic offsets. + # A partial run must preserve those coupled fields together with + # the certified non-target origin, not merely freeze the solver's + # scalar offset after endpoint derivation. + measured_endpoint_zero_offsets.update( + { + name: value + for name, value in frozen_offsets.items() + if name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + } + ) endpoint_zero_offsets = { name: value for name, value in measured_endpoint_zero_offsets.items() @@ -9671,9 +10536,8 @@ class G20ThreeCameraCalibrationNode(Node): else: endpoint_zero_offsets = {} post_solve_endpoint_offsets = {} - holdout_zero_result = solve_urdf_zero_offsets( + zero_solve_arguments = dict( source_urdf=self.source_urdf_path, - measurements=axes, palm_orientation_measurements=palm_orientation_measurements, curves=training_fits, motor_by_joint=motor_by_joint, @@ -9711,14 +10575,98 @@ 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, - }, - static_output_zero_offsets_rad={ - **self.zero_profile.static_output_zero_offsets_rad, - **post_solve_endpoint_offsets, - }, + ) + frozen_offsets = dict( + getattr(self, "partial_scope_fixed_zero_offsets_rad", {}) + ) + independent_thumb_scope = bool( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and self.recalibration_scope in {"full", "thumb"} + ) + if independent_thumb_scope: + thumb_profile = get_right_19_thumb_zero_profile() + thumb_names = set(thumb_profile.direct_zero_joints) + thumb_result = solve_urdf_zero_offsets( + **zero_solve_arguments, + measurements=[ + item for item in axes if item.joint in thumb_profile.axis_joints + ], + fixed_direct_zero_offsets_rad={ + name: value + for name, value in endpoint_zero_offsets.items() + if name in thumb_names + }, + static_output_zero_offsets_rad={ + name: value + for name, value in post_solve_endpoint_offsets.items() + if name in thumb_names + }, + zero_profile=thumb_profile, + ) + if self.recalibration_scope == "thumb": + if self.standalone_thumb_calibration: + holdout_zero_result = ( + expand_right_19_thumb_zero_result_with_cad_fingers( + thumb_result + ) + ) + else: + holdout_zero_result = merge_right_19_thumb_zero_result( + thumb_result=thumb_result, + preserved_offsets_rad=frozen_offsets, + ) + else: + # The full-hand solve still determines the 12 finger zeros and + # palm nuisance pose, but all four thumb values are immutable. + # Consequently no finger observation can pull a thumb zero. + companion_result = solve_urdf_zero_offsets( + **zero_solve_arguments, + measurements=axes, + fixed_direct_zero_offsets_rad={ + **self.zero_profile.fixed_direct_zero_offsets_rad, + **endpoint_zero_offsets, + **thumb_result.direct_offsets_rad, + }, + static_output_zero_offsets_rad={ + **self.zero_profile.static_output_zero_offsets_rad, + **post_solve_endpoint_offsets, + }, + ) + holdout_zero_result = merge_right_19_thumb_zero_result( + thumb_result=thumb_result, + companion_result=companion_result, + ) + else: + holdout_zero_result = solve_urdf_zero_offsets( + **zero_solve_arguments, + measurements=axes, + fixed_direct_zero_offsets_rad={ + **self.zero_profile.fixed_direct_zero_offsets_rad, + **endpoint_zero_offsets, + **frozen_offsets, + }, + static_output_zero_offsets_rad={ + **self.zero_profile.static_output_zero_offsets_rad, + **post_solve_endpoint_offsets, + **{ + name: value + for name, value in frozen_offsets.items() + if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS + }, + }, + ) + thumb_yaw_zero_failures = ( + _thumb_yaw_zero_repeatability_failures( + holdout_zero_result, + maximum_cycle_range_rad=( + self.thumb_yaw_maximum_zero_cycle_difference_rad + ), + maximum_confidence_half_width_rad=( + self.thumb_yaw_maximum_confidence_half_width_rad + ), + ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else [] ) append_jsonl( self.raw_path, @@ -9729,6 +10677,13 @@ class G20ThreeCameraCalibrationNode(Node): "direct_offsets_rad": dict( holdout_zero_result.direct_offsets_rad ), + "partial_scope_fixed_zero_offsets_rad": dict( + getattr( + self, + "partial_scope_fixed_zero_offsets_rad", + {}, + ) + ), "model_base_translation_xyz_m": list( holdout_zero_result.base_translation_xyz_m ), @@ -9774,6 +10729,9 @@ class G20ThreeCameraCalibrationNode(Node): "failure_reasons": dict( holdout_zero_result.failure_reasons ), + "thumb_yaw_repeatability_failures": ( + thumb_yaw_zero_failures + ), "axis_cone_mismatch_by_joint_rad": dict( holdout_zero_result.axis_cone_mismatch_by_joint_rad ), @@ -9787,7 +10745,8 @@ class G20ThreeCameraCalibrationNode(Node): np.asarray( [ value - for values in holdout_by_joint.values() + for name, values in holdout_by_joint.items() + if name in quality_joint_names for value in values ], dtype=float, @@ -9811,7 +10770,8 @@ class G20ThreeCameraCalibrationNode(Node): <= self.maximum_validation_mae_rad and float(np.max(np.abs(np.asarray(values, dtype=float)))) <= self.maximum_validation_error_rad - for values in holdout_by_joint.values() + for name, values in holdout_by_joint.items() + if name in quality_joint_names ) ) ) @@ -9832,15 +10792,18 @@ class G20ThreeCameraCalibrationNode(Node): ) ) for name, fit in training_fits.items() + if name in quality_joint_names ) and all( fit.maximum_hysteresis_rad <= self.command_maximum_direction_gap_rad - for fit in command_feedback_fits.values() + for name, fit in command_feedback_fits.items() + if name in quality_joint_names ) if not trajectory_holdout_passed: trajectory_score = { name: float(np.percentile(np.abs(values), 95.0)) for name, values in holdout_by_joint.items() + if name in quality_joint_names } worst_joint = max(trajectory_score, key=trajectory_score.get) failed_spec = next( @@ -9863,6 +10826,8 @@ class G20ThreeCameraCalibrationNode(Node): ] if self.profile.layout_id == G20_RIGHT_19_LAYOUT: for name, values in holdout_by_joint.items(): + if name not in quality_joint_names: + continue absolute = np.abs(np.asarray(values, dtype=float)) mean_error = float(np.mean(absolute)) maximum_error = float(np.max(absolute)) @@ -9896,6 +10861,35 @@ class G20ThreeCameraCalibrationNode(Node): ) self._pause_for_provisional_fit_failure(failed_spec, failures) return + if thumb_yaw_zero_failures: + thumb_yaw_spec = next( + spec + for spec in self.profile.sweep_specs + if "thumb_cmc_yaw" in spec.joints + ) + yaw_pair = self.zero_profile.same_view_axis_pair_by_offset.get( + "thumb_cmc_yaw", () + ) + source_task_key_set = { + observer.task_name + for observer in self.profile.palm_axis_observers + if observer.model_joint in set(yaw_pair) + } + self.retry_source_failure_task_key = thumb_yaw_spec.key + self.retry_source_task_keys = tuple( + candidate.key + for candidate in self.profile.sweep_specs + if candidate.key in source_task_key_set + ) + # A localized scalar outlier is replaced in place while the live + # PnP tracker/reference remains continuous. Non-localized yaw + # failures still select all cycles in the generic retry policy, + # but no dependent retry is allowed to create a new yaw datum. + self.retry_cycle_override.clear() + self._pause_for_provisional_fit_failure( + thumb_yaw_spec, thumb_yaw_zero_failures + ) + return if not holdout_zero_result.passed: self._pause_for_zero_model_failure(holdout_zero_result) return @@ -9903,12 +10897,63 @@ class G20ThreeCameraCalibrationNode(Node): # isolated final cycle. Re-solving with the holdout would invalidate # the acceptance result. zero_result = holdout_zero_result + self.thumb_yaw_cross_session_diagnostic = {} + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + previous_yaw = _previous_passed_joint_zero_offset( + self.session_dir, + self.serial_number, + "thumb_cmc_yaw", + ) + current_yaw = zero_result.all_active_offsets_rad.get( + "thumb_cmc_yaw" + ) + if previous_yaw is not None and current_yaw is not None: + previous_session, previous_offset = previous_yaw + delta = float(current_yaw) - float(previous_offset) + self.thumb_yaw_cross_session_diagnostic = { + "kind": "thumb_yaw_cross_session_diagnostic", + "joint": "thumb_cmc_yaw", + "previous_session": str(previous_session), + "previous_offset_deg": round( + math.degrees(previous_offset), 6 + ), + "current_offset_deg": round( + math.degrees(float(current_yaw)), 6 + ), + "absolute_difference_deg": round( + abs(math.degrees(delta)), 6 + ), + "reference_limit_deg": round( + math.degrees( + self.thumb_yaw_cross_session_diagnostic_rad + ), + 6, + ), + "classification": ( + "review_position_or_tag_installation" + if abs(delta) + > self.thumb_yaw_cross_session_diagnostic_rad + else "within_reference_band" + ), + "decision": "diagnostic_only", + } + append_jsonl( + self.raw_path, + self.thumb_yaw_cross_session_diagnostic, + ) append_jsonl( self.raw_path, { "kind": "zero_final_diagnostics", "hand_type": self.hand_type, "direct_offsets_rad": dict(zero_result.direct_offsets_rad), + "partial_scope_fixed_zero_offsets_rad": dict( + getattr( + self, + "partial_scope_fixed_zero_offsets_rad", + {}, + ) + ), "model_base_translation_xyz_m": list( zero_result.base_translation_xyz_m ), @@ -9948,15 +10993,18 @@ class G20ThreeCameraCalibrationNode(Node): # Runtime/MuJoCo consumes requested commands, never feedback bins. # Static geometry and holdout above intentionally continue to use the # dense feedback-domain training fit. - self.measured_fits = clamp_runtime_fits_to_urdf_limits( - self.source_urdf_path, - derive_mimic_passive_fits( + if standalone_thumb_calibration: + self.measured_fits = dict(command_fits) + else: + self.measured_fits = clamp_runtime_fits_to_urdf_limits( self.source_urdf_path, - command_fits, - profile=self.profile, - ), - endpoint_anchored_offsets_rad=endpoint_zero_offsets, - ) + derive_mimic_passive_fits( + self.source_urdf_path, + command_fits, + profile=self.profile, + ), + endpoint_anchored_offsets_rad=endpoint_zero_offsets, + ) self.axis_measurements = axes self.palm_orientation_measurements = list( palm_orientation_measurements @@ -9968,7 +11016,8 @@ class G20ThreeCameraCalibrationNode(Node): self.validated_endpoint_zero_offsets_rad = dict(endpoint_zero_offsets) self.validation_errors_rad = [ float(value) - for values in holdout_by_joint.values() + for name, values in holdout_by_joint.items() + if name in quality_joint_names for value in values ] self.validation_errors_rad.extend( @@ -10525,7 +11574,34 @@ class G20ThreeCameraCalibrationNode(Node): timestamp=stamp, ) try: - payload = build_compact_payload( + if self.standalone_thumb_calibration: + payload = build_standalone_thumb_payload( + profile=self.profile, + serial_number=self.serial_number, + measured_fits=self.measured_fits, + thumb_offsets_rad={ + name: published_zero_offsets[name] + for name in ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + ) + }, + validation_errors_rad=self.validation_errors_rad, + baseline_command_u8=self.baseline_command, + source_urdf_sha256=_file_sha256( + self.source_urdf_path + ), + camera_extrinsics_sha256=_file_sha256( + self.camera_extrinsics_file + ), + corrected_urdf_sha256=_file_sha256( + self.corrected_urdf_path + ), + ) + else: + payload = build_compact_payload( serial_number=self.serial_number, measured_fits=self.measured_fits, urdf_zero_offsets_rad=published_zero_offsets, @@ -10589,6 +11665,10 @@ class G20ThreeCameraCalibrationNode(Node): "rotation_orthogonal_rms_rad": ( item.rotation_orthogonal_rms_rad ), + "axis_estimator": item.axis_estimator, + "incremental_pair_count": ( + item.incremental_pair_count + ), } for item in getattr( self, @@ -10607,7 +11687,7 @@ class G20ThreeCameraCalibrationNode(Node): if self.profile.layout_id == G20_RIGHT_19_LAYOUT else None ), - ) + ) # The JSON is the commit marker for the URDF/JSON pair. It is # atomically renamed only after the corrected URDF and all three # artifact hashes have been produced and schema-validated. @@ -11306,6 +12386,11 @@ class G20ThreeCameraCalibrationNode(Node): views: dict[str, dict[str, Any]] = {} for name, runtime in self.views.items(): required_roles = set(self._required_roles_for_view(name)) + live_required_roles = set( + self._live_required_roles_for_view( + name, tuple(required_roles) + ) + ) locked_role = self._locked_base_role_for_active_capture(name) locked_roles = set(() if locked_role is None else (locked_role,)) views[name] = { @@ -11324,6 +12409,15 @@ class G20ThreeCameraCalibrationNode(Node): "locked_reference_tag_ids": sorted( runtime.view_tags[role] for role in locked_roles ), + "locked_base_corner_drift_px": round( + runtime.latest_locked_base_corner_drift_px, 4 + ), + "locked_base_maximum_corner_drift_px": ( + self.fixed_base_maximum_corner_drift_px + ), + "locked_base_corner_drift_count": ( + runtime.locked_base_corner_drift_count + ), "configured_tag_ids": sorted(runtime.view_tags.values()), "visible_configured_tag_ids": sorted( tag_id @@ -11342,8 +12436,7 @@ class G20ThreeCameraCalibrationNode(Node): "missing_tag_ids": sorted( tag_id for tag_id, role in runtime.role_by_id.items() - if role in required_roles - and role not in locked_roles + if role in live_required_roles and role not in runtime.latest_tag_quality ), "pnp_rejections": dict(runtime.latest_pnp_rejections), @@ -11854,6 +12947,9 @@ class G20ThreeCameraCalibrationNode(Node): "quality": ( {} if self.completed_payload is None else self.completed_payload["quality"] ), + "thumb_yaw_cross_session": dict( + getattr(self, "thumb_yaw_cross_session_diagnostic", {}) + ), "combination_validation": { "enabled": bool(self.combination_validation_enabled), "completed": bool(self.combination_validation_completed), 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 f1d6b20..911e751 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 @@ -62,6 +62,10 @@ class ZeroCalibrationProfile: same_view_axis_pair_by_offset: Mapping[str, tuple[str, str]] fixed_direct_zero_offsets_rad: Mapping[str, float] static_output_zero_offsets_rad: Mapping[str, float] + # Product full-hand fitting uses the five root-axis line pattern. The + # thumb kernel instead uses only the serial thumb chain, so its nuisance + # palm pose cannot be influenced by finger observations. + base_pose_strategy: str = "full_hand" @property def reference_finger(self) -> str: @@ -260,6 +264,56 @@ def _build_right_19_zero_profile( ) +def get_right_19_thumb_zero_profile() -> ZeroCalibrationProfile: + """Return the independent four-zero thumb observer graph. + + Roll and MCP are supplied as mechanical endpoint datums by the caller; + yaw is observed by the same-view roll/pitch axis pair and pitch by the + pitch/MCP serial phase. No finger axis is part of this profile. + """ + full = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + thumb_direct = tuple( + name for name in full.direct_zero_joints if name.startswith("thumb_") + ) + thumb_axes = tuple( + name for name in full.axis_joints if name.startswith("thumb_") + ) + return replace( + full, + direct_zero_joints=thumb_direct, + axis_joints=thumb_axes, + constrained_circle_joints=frozenset( + name + for name in full.constrained_circle_joints + if name.startswith("thumb_") + ), + root_anchor_joints=frozenset({"thumb_cmc_roll"}), + axis_parent_joint={ + child: parent + for child, parent in full.axis_parent_joint.items() + if child.startswith("thumb_") and parent.startswith("thumb_") + }, + phase_parent_joint={ + child: parent + for child, parent in full.phase_parent_joint.items() + if child.startswith("thumb_") and parent.startswith("thumb_") + }, + offset_observer_joint={ + target: observer + for target, observer in full.offset_observer_joint.items() + if target.startswith("thumb_") and observer.startswith("thumb_") + }, + same_view_axis_pair_by_offset={ + target: pair + for target, pair in full.same_view_axis_pair_by_offset.items() + if target.startswith("thumb_") + }, + fixed_direct_zero_offsets_rad={}, + static_output_zero_offsets_rad={}, + base_pose_strategy="thumb_serial", + ) + + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset( { "thumb_mcp", @@ -291,6 +345,7 @@ def anchor_right_19_mechanical_endpoint_curves( maximum_direction_difference_rad: float = math.radians(1.0), maximum_curve_correction_rad: float = math.radians(3.0), feedback_endpoint_joints: frozenset[str] = frozenset(), + endpoint_joints: frozenset[str] | None = None, ) -> dict[str, JointCurveFit]: """Set mechanical-endpoint curve scale from direct SO(3) travel. @@ -319,6 +374,19 @@ def anchor_right_19_mechanical_endpoint_curves( raise ValueError("endpoint curve correction limit must be positive") result = dict(curves) + selected_endpoint_joints = ( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + if endpoint_joints is None + else frozenset(str(name) for name in endpoint_joints) + ) + unknown_endpoint_joints = sorted( + set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) + ) + if unknown_endpoint_joints: + raise ValueError( + "endpoint joints are not endpoint measurements: " + + ",".join(unknown_endpoint_joints) + ) unknown_feedback_joints = sorted( set(feedback_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) ) @@ -328,7 +396,7 @@ def anchor_right_19_mechanical_endpoint_curves( + ",".join(unknown_feedback_joints) ) - for name in sorted(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS): + for name in sorted(selected_endpoint_joints): fit = curves.get(name) records = list(records_by_joint.get(name, ())) if fit is None: @@ -456,6 +524,7 @@ def derive_right_19_mechanical_endpoint_offsets( curves: Mapping[str, JointCurveFit], *, maximum_offset_rad: float = math.radians(5.0), + endpoint_joints: frozenset[str] | None = None, ) -> dict[str, float]: """Estimate encoder origins from repeatable measured mechanical endpoints. @@ -479,7 +548,20 @@ def derive_right_19_mechanical_endpoint_offsets( 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_ENDPOINT_MEASUREMENT_JOINTS): + selected_endpoint_joints = ( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + if endpoint_joints is None + else frozenset(str(name) for name in endpoint_joints) + ) + unknown_endpoint_joints = sorted( + set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) + ) + if unknown_endpoint_joints: + raise ValueError( + "endpoint joints are not endpoint measurements: " + + ",".join(unknown_endpoint_joints) + ) + for name in sorted(selected_endpoint_joints): fit = curves.get(name) joint = joints.get(name) limit = None if joint is None else joint.find("limit") @@ -1132,6 +1214,134 @@ class PalmOrientationMeasurement: condition_state_u8: tuple[float, ...] observed_arc_rad: float rotation_orthogonal_rms_rad: float + axis_estimator: str = "baseline_relative_so3" + incremental_pair_count: int = 0 + + +PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 = 24 +PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD = math.radians(0.3) +PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT = 24 +PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE = 75.0 + + +def _incremental_common_rotation_axis( + samples_by_bin: Mapping[ + tuple[str, int], Sequence[Mapping[str, Any]] + ], + *, + command_separation_u8: int = ( + PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 + ), + minimum_rotation_rad: float = ( + PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD + ), + minimum_pair_count: int = PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT, + consensus_percentile: float = ( + PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE + ), +) -> tuple[np.ndarray, int]: + """Fit one physical axis from robust local Tag rotations. + + A single baseline-to-endpoint logarithm is sensitive to smooth planar-PnP + curvature: the fitted axis then changes when the same Tag trajectory is + translated to another part of the image. Local finite rotations are + expressed directly in the common camera frame, and their undirected + weighted consensus estimates the physical revolute axis. The worst + quartile is discarded once more after the initial consensus, which is + enough to reject endpoint branch curvature without learning a + serial-specific yaw value or an image-position correction table. + """ + separation = int(command_separation_u8) + if separation < 4 or separation > 64: + raise ValueError("palm axis command separation must be in [4, 64]") + if not 0.0 < float(minimum_rotation_rad) < math.pi: + raise ValueError("palm axis minimum increment must be in (0, pi)") + if int(minimum_pair_count) < 6: + raise ValueError("palm axis minimum pair count must be at least six") + percentile = float(consensus_percentile) + if not 50.0 <= percentile <= 90.0: + raise ValueError("palm axis consensus percentile must be in [50, 90]") + + binned_common_rotations: dict[tuple[str, int], Rotation] = {} + for key, group in samples_by_bin.items(): + child_quaternions = [] + for record in group: + pose = record.get("child_pose_common") + if not isinstance(pose, Mapping): + continue + quaternion = pose.get("quaternion_xyzw") + if quaternion is not None: + child_quaternions.append(quaternion) + if not child_quaternions: + continue + binned_common_rotations[(str(key[0]), int(key[1]))] = ( + Rotation.from_quat( + robust_rotation_summary(child_quaternions)[0] + ) + ) + + directions: list[np.ndarray] = [] + weights: list[float] = [] + for direction in ("decreasing", "increasing"): + commands = sorted( + command + for candidate_direction, command in binned_common_rotations + if candidate_direction == direction + ) + for start_command in commands: + candidates = [ + command + for command in commands + if separation <= command - start_command <= separation + 2 + ] + if not candidates: + continue + end_command = candidates[0] + start = binned_common_rotations[(direction, start_command)] + end = binned_common_rotations[(direction, end_command)] + local_rotvec = (start.inv() * end).as_rotvec() + common_rotvec = start.apply(local_rotvec) + increment = float(np.linalg.norm(common_rotvec)) + if increment < float(minimum_rotation_rad): + continue + directions.append(common_rotvec / increment) + weights.append(increment) + + if len(directions) < int(minimum_pair_count): + raise ValueError( + "palm axis has too few full-stroke local rotation pairs: " + f"{len(directions)}/{int(minimum_pair_count)}" + ) + vectors = np.asarray(directions, dtype=float) + increments = np.asarray(weights, dtype=float) + + def consensus(selected: np.ndarray) -> np.ndarray: + scatter = np.einsum( + "n,ni,nj->ij", + increments[selected], + vectors[selected], + vectors[selected], + ) + eigenvalues, eigenvectors = np.linalg.eigh(scatter) + if not np.all(np.isfinite(eigenvalues)): + raise ValueError("palm axis local-rotation consensus is invalid") + axis = eigenvectors[:, -1] + return axis / np.linalg.norm(axis) + + selected = np.ones(len(vectors), dtype=bool) + axis = consensus(selected) + # Two deterministic refinement rounds prevent the initial scatter from + # being pulled toward a dense endpoint-bias cluster. + for _ in range(2): + deviations = np.arccos( + np.clip(np.abs(vectors @ axis), -1.0, 1.0) + ) + limit = float(np.percentile(deviations, percentile)) + selected = deviations <= limit + if int(np.count_nonzero(selected)) < int(minimum_pair_count): + raise ValueError("palm axis robust consensus retained too few pairs") + axis = consensus(selected) + return axis, int(np.count_nonzero(selected)) def fit_partial_palm_orientation_measurement( @@ -1252,6 +1462,33 @@ def fit_partial_palm_orientation_measurement( axis_parent = reference.apply(axis_child) axis_common = Rotation.from_quat(parent_quaternion).apply(axis_parent) axis_common /= np.linalg.norm(axis_common) + axis_estimator = "baseline_relative_so3" + incremental_pair_count = 0 + # Product recordings persist the selected moving-Tag pose in the common + # camera frame. Use the complete visible stroke only for a local-motion + # axis consensus; retain the zero-adjacent fit above as the unchanged arc + # and residual quality gate. Legacy/synthetic records without the common + # child pose keep their previous estimator exactly. + if any( + isinstance(record.get("child_pose_common"), Mapping) + for record in cycle_samples + ): + full_stroke_bins: dict[ + tuple[str, int], list[Mapping[str, Any]] + ] = {} + for record in cycle_samples: + key = ( + str(record.get("direction", "")), + int(record["command_u8"]), + ) + full_stroke_bins.setdefault(key, []).append(record) + incremental_axis, incremental_pair_count = ( + _incremental_common_rotation_axis(full_stroke_bins) + ) + if float(incremental_axis @ axis_common) < 0.0: + incremental_axis = -incremental_axis + axis_common = incremental_axis + axis_estimator = "robust_full_stroke_local_so3_v1" state = np.median( np.asarray( [record["state_u8"] for record in zero_records], dtype=float @@ -1266,6 +1503,8 @@ def fit_partial_palm_orientation_measurement( condition_state_u8=tuple(float(value) for value in state), observed_arc_rad=observed_arc, rotation_orthogonal_rms_rad=orthogonal_rms, + axis_estimator=axis_estimator, + incremental_pair_count=incremental_pair_count, ) @@ -2242,6 +2481,148 @@ class ZeroSolveResult: failure_reasons: Mapping[str, str] +def merge_right_19_thumb_zero_result( + *, + thumb_result: ZeroSolveResult, + preserved_offsets_rad: Mapping[str, float] | None = None, + companion_result: ZeroSolveResult | None = None, +) -> ZeroSolveResult: + """Merge an independent thumb solve without allowing reverse coupling. + + ``companion_result`` is the finger/full solver result whose thumb values + were fixed to ``thumb_result``. For a thumb-only run, the 12 certified + finger values are supplied through ``preserved_offsets_rad`` instead. + """ + thumb_names = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + if set(thumb_result.direct_offsets_rad) != thumb_names: + raise ValueError("independent thumb result must contain exactly four zeros") + full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + direct_names = set(full_profile.direct_zero_joints) + preserved = { + str(name): float(value) + for name, value in (preserved_offsets_rad or {}).items() + } + if set(preserved) - (direct_names - thumb_names): + raise ValueError("preserved offsets contain a thumb or unknown joint") + if any(not math.isfinite(value) for value in preserved.values()): + raise ValueError("preserved offsets must be finite") + + if companion_result is None: + if set(preserved) != direct_names - thumb_names: + raise ValueError("thumb-only merge requires all 12 certified finger zeros") + direct = dict(preserved) + all_active = {name: 0.0 for name in full_profile.hand.active_joints} + all_active.update(preserved) + base_result = thumb_result + else: + if set(companion_result.direct_offsets_rad) != direct_names: + raise ValueError("full companion result must contain all 16 zeros") + direct = dict(companion_result.direct_offsets_rad) + all_active = dict(companion_result.all_active_offsets_rad) + base_result = companion_result + direct.update(thumb_result.direct_offsets_rad) + all_active.update(thumb_result.direct_offsets_rad) + + def merged_map(name: str) -> dict[str, Any]: + values = ( + {} + if companion_result is None + else dict(getattr(companion_result, name)) + ) + values.update(dict(getattr(thumb_result, name))) + return values + + failure_reasons = merged_map("failure_reasons") + passed = bool( + thumb_result.passed + and (companion_result is None or companion_result.passed) + ) + return replace( + base_result, + direct_offsets_rad=direct, + all_active_offsets_rad=all_active, + validation_errors_rad=( + tuple(thumb_result.validation_errors_rad) + if companion_result is None + else ( + *companion_result.validation_errors_rad, + *thumb_result.validation_errors_rad, + ) + ), + validation_error_by_joint_rad=merged_map( + "validation_error_by_joint_rad" + ), + validation_line_error_by_joint_m=merged_map( + "validation_line_error_by_joint_m" + ), + axis_line_rms_m=( + thumb_result.axis_line_rms_m + if companion_result is None + else max( + companion_result.axis_line_rms_m, + thumb_result.axis_line_rms_m, + ) + ), + passed=passed, + cycle_offsets_rad=merged_map("cycle_offsets_rad"), + offset_uncertainty_rad=merged_map("offset_uncertainty_rad"), + offset_confidence_half_width_rad=merged_map( + "offset_confidence_half_width_rad" + ), + validation_original_error_by_joint_rad=merged_map( + "validation_original_error_by_joint_rad" + ), + validation_improvement_by_joint_rad=merged_map( + "validation_improvement_by_joint_rad" + ), + validation_improvement_confidence_lower_rad=merged_map( + "validation_improvement_confidence_lower_rad" + ), + offset_covariance_rad2=merged_map("offset_covariance_rad2"), + axis_cone_mismatch_by_joint_rad=merged_map( + "axis_cone_mismatch_by_joint_rad" + ), + axis_cone_bias_classification_by_joint=merged_map( + "axis_cone_bias_classification_by_joint" + ), + failure_reasons=failure_reasons, + ) + + +def expand_right_19_thumb_zero_result_with_cad_fingers( + thumb_result: ZeroSolveResult, +) -> ZeroSolveResult: + """Build a standalone thumb result with every finger left at source CAD. + + This is intentionally different from inheriting a prior finger + calibration: the twelve non-thumb values are explicit zeros and no finger + observation or artifact is required. + """ + full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + thumb_names = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + if set(thumb_result.direct_offsets_rad) != thumb_names: + raise ValueError("independent thumb result must contain exactly four zeros") + direct = {name: 0.0 for name in full_profile.direct_zero_joints} + direct.update(thumb_result.direct_offsets_rad) + all_active = {name: 0.0 for name in full_profile.hand.active_joints} + all_active.update(thumb_result.direct_offsets_rad) + return replace( + thumb_result, + direct_offsets_rad=direct, + all_active_offsets_rad=all_active, + ) + + def _angles_from_state( state_u8: Sequence[float], *, @@ -2289,6 +2670,7 @@ def solve_urdf_zero_offsets( tag_layout: str = "legacy_11", fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, static_output_zero_offsets_rad: Mapping[str, float] | None = None, + zero_profile: ZeroCalibrationProfile | None = None, ) -> ZeroSolveResult: """Solve all directly observable zero offsets without fitting CAD errors. @@ -2299,7 +2681,15 @@ def solve_urdf_zero_offsets( per joint. This prevents link length, along-axis Tag placement and monocular depth bias from becoming encoder-zero corrections. """ - profile = get_zero_calibration_profile(hand_type, tag_layout) + profile = ( + get_zero_calibration_profile(hand_type, tag_layout) + if zero_profile is None + else zero_profile + ) + if profile.hand.side != str(hand_type).lower(): + raise ValueError("zero profile hand side does not match hand_type") + if profile.hand.layout_id != str(tag_layout): + raise ValueError("zero profile layout does not match tag_layout") model = UrdfKinematicModel(source_urdf) training = [m for m in measurements if m.cycle in set(training_cycles)] validation = [m for m in measurements if m.cycle == int(validation_cycle)] @@ -2515,7 +2905,9 @@ def solve_urdf_zero_offsets( zero_offsets.update(fixed_offsets) def fit_base_pose( selected: Sequence[JointAxisMeasurement], + pose_offsets: Mapping[str, float] | None = None, ) -> tuple[Rotation, np.ndarray]: + pose_zero_offsets = zero_offsets if pose_offsets is None else pose_offsets anchors_by_joint = { name: [item for item in selected if item.joint == name] for name in profile.root_anchor_joints @@ -2542,7 +2934,7 @@ def solve_urdf_zero_offsets( observed_points: dict[str, np.ndarray] = {} for name in root_names: items = anchors_by_joint[name] - predicted = [predicted_local(item, zero_offsets) for item in items] + predicted = [predicted_local(item, pose_zero_offsets) for item in items] predicted_axes[name] = undirected_axis_average( [axis for axis, _ in predicted] ) @@ -2559,7 +2951,10 @@ def solve_urdf_zero_offsets( axis=0, ) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: + if ( + profile.hand.layout_id == G20_RIGHT_19_LAYOUT + and profile.base_pose_strategy == "full_hand" + ): # In the product layout none of the four finger-roll zeros is a # mechanical prior. Using one finger's pitch axis to orient the # palm would therefore absorb that finger's roll zero into the @@ -2739,7 +3134,7 @@ def solve_urdf_zero_offsets( ) axis_errors: list[float] = [] for item in selected: - predicted_axis, _ = predicted_local(item, zero_offsets) + predicted_axis, _ = predicted_local(item, pose_zero_offsets) observed_axis = np.asarray(item.axis_common_xyz, dtype=float) axis_errors.append( math.acos( @@ -2794,7 +3189,7 @@ def solve_urdf_zero_offsets( residuals: list[float] = [] for item in non_root_items: predicted_axis, predicted_point = predicted_local( - item, zero_offsets + item, pose_zero_offsets ) predicted_axis = rotation.apply(predicted_axis) predicted_point = ( @@ -2842,7 +3237,11 @@ def solve_urdf_zero_offsets( np.ptp(np.asarray(curves[name].angle_rad)) ), ) - orientation_anchor = f"{profile.reference_finger}_mcp_pitch" + orientation_anchor = ( + "thumb_cmc_pitch" + if profile.base_pose_strategy == "thumb_serial" + else f"{profile.reference_finger}_mcp_pitch" + ) orientation_items = [ item for item in selected if item.joint == orientation_anchor ] @@ -2852,7 +3251,10 @@ def solve_urdf_zero_offsets( f"{orientation_anchor}" ) predicted_orientation_axis = undirected_axis_average( - [predicted_local(item, zero_offsets)[0] for item in orientation_items] + [ + predicted_local(item, pose_zero_offsets)[0] + for item in orientation_items + ] ) observed_orientation_axis = undirected_axis_average( [item.axis_common_xyz for item in orientation_items] @@ -2899,7 +3301,7 @@ def solve_urdf_zero_offsets( # it does not fit link geometry or encoder offsets. errors_by_joint: dict[str, list[float]] = {} for item in selected: - predicted_axis, _ = predicted_local(item, zero_offsets) + predicted_axis, _ = predicted_local(item, pose_zero_offsets) predicted_axis = rotation.apply(predicted_axis) observed_axis = np.asarray(item.axis_common_xyz, dtype=float) error = math.acos( @@ -3284,6 +3686,15 @@ def solve_urdf_zero_offsets( offsets = optimise_offsets( selected, rotation, translation, initial=initial ) + if profile.base_pose_strategy == "thumb_serial": + # The first pose estimate only supplies a branch for the invariant + # same-view yaw solve. Refit after yaw is known so that the + # downstream pitch phase is evaluated in the corrected serial + # thumb frame, then freeze that pose for holdout validation. + rotation, translation = fit_base_pose(selected, offsets) + offsets = optimise_offsets( + selected, rotation, translation, initial=offsets + ) return rotation, translation, offsets base_rotation, base_translation, training_offsets = solve_selected(training) @@ -3771,7 +4182,11 @@ def solve_urdf_zero_offsets( for name, limit in zip(profile.direct_zero_joints, offset_limits): if name in fixed_offsets: continue - checked_offset = training_offsets[name] + # A post-solve mechanical endpoint datum is the value that will be + # published for this joint. The visual root-axis scalar remains a + # nuisance gauge used for holdout geometry and must not be compared + # with the safety bound of a different, endpoint-anchored output. + checked_offset = output_offsets.get(name, training_offsets[name]) if name in product_finger_rolls: # The four roll motors share the same electrical centre and the # absolute palm axial datum is recovered from the root-line @@ -3795,7 +4210,7 @@ def solve_urdf_zero_offsets( ): if name in fixed_offsets: continue - checked_offset = training_offsets[name] + checked_offset = output_offsets.get(name, 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 @@ -3808,15 +4223,19 @@ def solve_urdf_zero_offsets( failure_reasons["palm_orientation"] = ( "palm_orientation_holdout_too_large" ) - if ( + requires_full_observability = bool( profile.hand.layout_id == G20_RIGHT_19_LAYOUT + and profile.base_pose_strategy == "full_hand" + ) + if ( + requires_full_observability and observability_rank < observability_parameter_count ): failure_reasons["palm_and_static_zero"] = ( "zero_observation_jacobian_rank_deficient" ) elif ( - profile.hand.layout_id == G20_RIGHT_19_LAYOUT + requires_full_observability and observability_condition_number > maximum_observability_condition_number ): @@ -3846,7 +4265,11 @@ def solve_urdf_zero_offsets( # above for every phase observation that actually uses line position. if not improvement_passed: for name, value in applied_training.items(): - if value != 0.0 and improvement_confidence_lower.get(name, 0.0) <= 0.0: + if ( + name not in fixed_offsets + and value != 0.0 + and improvement_confidence_lower.get(name, 0.0) <= 0.0 + ): failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence" # Geometry failures are the root cause and must not be hidden by the # downstream validation symptom produced by the same bad observation. @@ -3868,7 +4291,7 @@ def solve_urdf_zero_offsets( and not diagnostic_bound_hits and not observation_failures and ( - profile.hand.layout_id != G20_RIGHT_19_LAYOUT + not requires_full_observability or ( observability_rank == observability_parameter_count and observability_condition_number 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 be4421c..cab315b 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 @@ -43,8 +43,10 @@ from g20_thumb_apriltag_calibration.publication import ( build_mujoco_validation_commands, finalize_session_artifacts, session_artifact_paths, + standalone_thumb_offsets, validate_runtime_curves_against_urdf_limits, verify_corrected_urdf, + verify_partial_scope_preserves_certified_zeros, verify_urdf_mesh_resources, ) from g20_thumb_apriltag_calibration.storage import atomic_write_json @@ -56,8 +58,11 @@ from g20_thumb_apriltag_calibration.three_camera_node import ( _combination_motor_directions, _combination_validation_items, _model_link_in_observer_base, + _partial_scope_frozen_zero_offsets, _palm_axis_observer_schema, _palm_axis_resume_policy, + build_standalone_thumb_payload, + recalibration_quality_joints, recalibration_task_keys, _steady_checkpoint_commands, _unresolved_fit_failure_tasks, @@ -348,7 +353,7 @@ def test_resume_preserves_palm_axis_side_channel_without_requiring_it() -> None: def test_checkpoint_reacquires_new_thumb_yaw_side_channel_once() -> None: profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) old_capabilities = set(profile.capabilities) - { - "palm_axis_side_channel_v2" + "palm_axis_relative_motion_v3" } compatible, invalidated = _palm_axis_resume_policy( @@ -600,6 +605,102 @@ def test_resume_falls_back_from_an_interrupted_retry_to_complete_attempt() -> No assert {int(row.get("attempt", 1)) for row in checkpoints} == {1} +def test_resume_retry_tombstone_discards_superseded_rows() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + task = next( + spec + for spec in profile.sweep_specs + if spec.key == "pinky_roll_multiview" + ) + old_checkpoints = [] + for joint in task.joints: + old_checkpoints.extend( + { + "kind": "steady_command_sample", + "attempt": 1, + "task_name": task.key, + "joint": joint, + "motor_index": task.motor_index, + "cycle": 0, + "direction": "decreasing", + "requested_command_u8": command, + "feedback_u8": command, + "resume_test_generation": "superseded", + } + for command in STEADY_COMMAND_CHECKPOINTS + ) + rows = [ + *old_checkpoints, + { + # Legacy retry events had no task_name or attempt. Motor, joint, + # cycle and direction must still act as a durable tombstone. + "kind": "automatic_sweep_retry", + "view": task.view, + "motor_index": task.motor_index, + "joints": list(task.joints), + "cycle": 0, + "direction": "decreasing", + "retry": 1, + }, + *_complete_resume_task_rows(profile, task), + ] + + completed, reusable = resumable_completed_task_prefix( + profile, + 4, + [255] * 20, + rows, + allow_sparse=True, + ) + + assert completed == (task.key,) + restored_checkpoints = [ + row + for row in reusable + if row["kind"] == "steady_command_sample" + and row["direction"] == "decreasing" + ] + assert len(restored_checkpoints) == 2 * len(STEADY_COMMAND_CHECKPOINTS) + assert all( + row.get("resume_test_generation") != "superseded" + for row in restored_checkpoints + ) + + +def test_interrupted_retry_does_not_resurrect_rejected_sweep() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + task = next( + spec + for spec in profile.sweep_specs + if spec.key == "pinky_roll_multiview" + ) + rows = [ + *_complete_resume_task_rows(profile, task), + { + "kind": "automatic_sweep_retry", + "task_name": task.key, + "attempt": 1, + "view": task.view, + "motor_index": task.motor_index, + "joints": list(task.joints), + "cycle": 0, + "direction": "decreasing", + "retry": 1, + }, + ] + + completed, reusable = resumable_completed_task_prefix( + profile, + 4, + [255] * 20, + rows, + allow_sparse=True, + ) + + assert completed == () + assert reusable == () + + def test_resume_rejects_unresolved_fit_but_accepts_retired_model_metric() -> None: profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) first = profile.sweep_specs[0] @@ -804,6 +905,120 @@ def test_thumb_scope_resolves_passed_base_and_launches_partial_mode( assert f"resume_raw_samples_path:={session / 'raw_samples.jsonl'}" in command +def test_fingers_scope_reuses_thumb_and_recollects_twelve_tasks( + tmp_path: Path, +) -> None: + config = _config(tmp_path) + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + tasks = recalibration_task_keys(profile, "fingers") + command = _launch_command( + config, + config.session_root / "20260831_130000", + resume_from=config.session_root / "20260831_120000", + recalibration_scope="fingers", + ) + + assert len(tasks) == 12 + assert all("thumb_" not in name for name in tasks) + assert "recalibration_scope:=fingers" in command + + +def test_standalone_thumb_launch_has_no_resume_dependency(tmp_path: Path) -> None: + config = _config(tmp_path) + command = _launch_command( + config, + config.session_root / "20260831_140000", + recalibration_scope="thumb", + ) + + assert "recalibration_scope:=thumb" in command + assert all("resume_raw_samples_path" not in value for value in command) + + +def test_partial_quality_gates_only_fresh_scope_joints() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + thumb = recalibration_quality_joints(profile, "thumb") + fingers = recalibration_quality_joints(profile, "fingers") + + assert thumb + assert fingers + assert all(name.startswith("thumb_") for name in thumb) + assert all(not name.startswith("thumb_") for name in fingers) + assert set(thumb) | set(fingers) == set(profile.measured_joints) + assert set(thumb).isdisjoint(fingers) + + +def test_standalone_thumb_payload_expands_non_thumb_to_cad_zero() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + curve = tuple(0.001 * (127 - value) for value in range(256)) + fits = { + name: JointCurveFit( + angle_rad=curve, + decreasing_rad=curve, + increasing_rad=curve, + circle={}, + maximum_monotonic_correction_rad=0.0, + maximum_hysteresis_rad=0.0, + quality={}, + ) + for name in recalibration_quality_joints(profile, "thumb") + } + thumb_offsets = { + "thumb_cmc_roll": 0.01, + "thumb_cmc_yaw": -0.02, + "thumb_cmc_pitch": 0.03, + "thumb_mcp": -0.01, + } + payload = build_standalone_thumb_payload( + profile=profile, + serial_number="G20_RIGHT_001", + measured_fits=fits, + thumb_offsets_rad=thumb_offsets, + validation_errors_rad=(0.001, -0.002), + baseline_command_u8=profile.baseline_command, + source_urdf_sha256="a" * 64, + camera_extrinsics_sha256="b" * 64, + corrected_urdf_sha256="c" * 64, + ) + + expanded = standalone_thumb_offsets(payload) + assert set(expanded) == set(ACTIVE_ZERO_JOINTS) + assert all(expanded[name] == value for name, value in thumb_offsets.items()) + assert all( + value == 0.0 + for name, value in expanded.items() + if not name.startswith("thumb_") + ) + + +def test_partial_scopes_freeze_exactly_the_non_target_zeros() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + offsets = { + name: 0.001 * (index + 1) + for index, name in enumerate(sorted(ACTIVE_ZERO_JOINTS)) + } + payload = _payload(zero_offsets=offsets) + + thumb_frozen = _partial_scope_frozen_zero_offsets( + profile, "thumb", payload + ) + finger_frozen = _partial_scope_frozen_zero_offsets( + profile, "fingers", payload + ) + + assert len(thumb_frozen) == 12 + assert all(not name.startswith("thumb_") for name in thumb_frozen) + assert len(finger_frozen) == 4 + assert all(name.startswith("thumb_") for name in finger_frozen) + expected = { + name: float( + payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"] + ) + for name in ACTIVE_ZERO_JOINTS + } + assert {**thumb_frozen, **finger_frozen} == expected + + def test_thumb_scope_rejects_base_with_different_geometry(tmp_path: Path) -> None: config = _config(tmp_path) session = config.session_root / "20260830_120000" @@ -1126,6 +1341,7 @@ def test_publication_protects_passive_xml_and_requires_two_matching_sessions( def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> None: config = _config(tmp_path, passes=1) + base_session = _make_passed_session(config, "20260828_205005") session = _make_passed_session(config, "20260830_140000") status = _passed_node_status() tasks = list( @@ -1136,7 +1352,7 @@ def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> N ) status["resume"] = { "used": True, - "source_session": "20260828_205005", + "source_session": str(base_session), "recalibration_scope": "thumb", "recalibration_task_keys": tasks, } @@ -1147,8 +1363,202 @@ def test_publication_records_thumb_recalibration_provenance(tmp_path: Path) -> N assert ready is True assert summary["calibration_scope"] == "thumb" - assert summary["inherited_base_session"] == "20260828_205005" + assert summary["inherited_base_session"] == str(base_session) assert summary["freshly_calibrated_task_keys"] == tasks + assert len(summary["preserved_certified_zero_joints"]) == 12 + assert all( + not name.startswith("thumb_") + for name in summary["preserved_certified_zero_joints"] + ) + + +def test_publication_accepts_standalone_thumb_without_base_session( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + thumb_offsets = { + "thumb_cmc_roll": 0.01, + "thumb_cmc_yaw": -0.02, + "thumb_cmc_pitch": 0.03, + "thumb_mcp": -0.01, + } + all_offsets = {name: 0.0 for name in ACTIVE_ZERO_JOINTS} + all_offsets.update(thumb_offsets) + session = _make_passed_session( + config, "20260831_150000", zero_offsets=all_offsets + ) + full_payload = _payload( + config.serial_number, zero_offsets=all_offsets + ) + standalone_payload = { + "schema_version": 1, + "artifact_type": "g20_right_standalone_thumb_calibration", + "model": "G20", + "side": "right", + "serial_number": config.serial_number, + "angle_unit": "rad", + "command_range": [0, 255], + "baseline_command_u8": full_payload["baseline_command_u8"], + "non_thumb_zero_policy": "source_cad_unchanged", + "joints": { + name: full_payload["joints"][name] + for name in (*thumb_offsets, "thumb_ip") + }, + "quality": { + "passed": True, + "validation_mae_rad": 0.001, + "validation_p95_rad": 0.002, + }, + "hashes": {}, + } + atomic_write_json( + session / f"g20_right_{config.serial_number}_calibration.json", + standalone_payload, + ) + status = _passed_node_status() + status["combination_validation"] = {"enabled": False} + status["resume"] = { + "used": False, + "source_session": "", + "recalibration_scope": "thumb", + "recalibration_task_keys": list( + recalibration_task_keys( + get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), + "thumb", + ) + ), + } + + summary, ready = finalize_session_artifacts( + config, session, node_status=status + ) + + assert ready is True + assert summary["inherited_base_session"] is None + assert summary["non_thumb_zero_policy"] == "source_cad_unchanged" + assert summary["static_zero_calibrated_joints"] == sorted(thumb_offsets) + assert not (config.session_root / "latest_passed").exists() + assert (config.session_root / "latest_thumb_passed").resolve() == session + combined_offsets = dict(all_offsets) + combined_offsets["index_mcp_pitch"] = 0.02 + verify_partial_scope_preserves_certified_zeros( + scope="fingers", + source_session=session, + serial_number=config.serial_number, + current_offsets=combined_offsets, + ) + + +def test_publication_resolves_node_short_base_session_as_sibling( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + base_session = _make_passed_session(config, "20260828_205005") + session = _make_passed_session(config, "20260831_121828") + status = _passed_node_status() + tasks = list( + recalibration_task_keys( + get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), + "thumb", + ) + ) + status["resume"] = { + "used": True, + # The calibration node intentionally publishes the durable sibling + # session identifier, not a machine-specific absolute path. + "source_session": base_session.name, + "recalibration_scope": "thumb", + "recalibration_task_keys": tasks, + } + + summary, ready = finalize_session_artifacts( + config, session, node_status=status + ) + + assert ready is True + assert summary["inherited_base_session"] == base_session.name + + +def test_partial_publication_rejects_any_non_target_zero_change( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + base_offsets = { + name: 0.001 * (index + 1) + for index, name in enumerate(sorted(ACTIVE_ZERO_JOINTS)) + } + base_session = _make_passed_session( + config, "20260831_100000", zero_offsets=base_offsets + ) + + thumb_result = dict(base_offsets) + thumb_result["thumb_cmc_yaw"] += 0.01 + verify_partial_scope_preserves_certified_zeros( + scope="thumb", + source_session=base_session, + serial_number=config.serial_number, + current_offsets=thumb_result, + ) + changed_finger = dict(thumb_result) + changed_finger["index_mcp_pitch"] += 0.001 + with pytest.raises(ValueError, match="non-target zeros"): + verify_partial_scope_preserves_certified_zeros( + scope="thumb", + source_session=base_session, + serial_number=config.serial_number, + current_offsets=changed_finger, + ) + + finger_result = dict(base_offsets) + finger_result["index_mcp_pitch"] += 0.01 + verify_partial_scope_preserves_certified_zeros( + scope="fingers", + source_session=base_session, + serial_number=config.serial_number, + current_offsets=finger_result, + ) + changed_thumb = dict(finger_result) + changed_thumb["thumb_mcp"] += 0.001 + with pytest.raises(ValueError, match="non-target zeros"): + verify_partial_scope_preserves_certified_zeros( + scope="fingers", + source_session=base_session, + serial_number=config.serial_number, + current_offsets=changed_thumb, + ) + + +def test_publication_records_fingers_scope_with_certified_thumb( + tmp_path: Path, +) -> None: + config = _config(tmp_path, passes=1) + base_session = _make_passed_session(config, "20260831_110000") + session = _make_passed_session(config, "20260831_120000") + tasks = list( + recalibration_task_keys( + get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), + "fingers", + ) + ) + status = _passed_node_status() + status["resume"] = { + "used": True, + "source_session": str(base_session), + "recalibration_scope": "fingers", + "recalibration_task_keys": tasks, + } + + summary, ready = finalize_session_artifacts( + config, session, node_status=status + ) + + assert ready is True + assert summary["calibration_scope"] == "fingers" + assert summary["inherited_base_session"] == str(base_session) + assert summary["freshly_calibrated_task_keys"] == tasks + assert summary["preserved_certified_zero_joints"] == sorted( + name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_") + ) def test_publication_numerically_binds_json_offsets_and_urdf_limits( @@ -1537,6 +1947,16 @@ def test_stopped_status_heartbeat_has_stable_communication_code() -> None: assert "断点" in suggestion +def test_fixed_base_reference_movement_has_stable_camera_code() -> None: + code, problem, suggestion = classify_error( + "fixed_base_reference_moved", {} + ) + + assert code == "CAM-REFERENCE-204" + assert "Tag" 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", {} @@ -1617,7 +2037,33 @@ def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None: assert "反馈：98.0 Hz" in text assert "当前任务ID：正面[0✓] 侧面[0✓] 顶部[0✓]" in text assert "断点：已恢复 3/16 个完整任务" in text - assert "首次拟合定位第 2 轮异常，仅补采该轮双向" in text + assert "首次拟合定位第 2 轮需复核，仅补采对应轮次双向" in text + + +def test_progress_describes_nonlocalized_full_fit_retry() -> None: + status = { + "state": "SWEEP", + "progress": 0.9, + "feedback_hz": 58.0, + "active": { + "joints": ["thumb_cmc_pitch"], + "cycle": 1, + "repetitions": 4, + "direction": "decreasing", + "valid_frames": 20, + "automatic_retry_count": 1, + "fit_attempt": 2, + "fit_attempt_limit": 3, + "fit_retry_cycles": [1, 2, 3, 4], + }, + "views": {}, + } + + text = render_progress_zh( + "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) + ) + + assert "未能定位单一异常轮，联合补采第 1/2/3/4 轮双向" in text def test_preflight_progress_marks_non_base_tag_occlusion_as_allowed() -> None: @@ -2102,3 +2548,68 @@ def test_node_drops_imported_task_failing_hard_gates( import_record["revalidation_dropped_tasks"][0]["task"] == profile.sweep_specs[0].key ) + + +def test_sparse_resume_revalidates_against_only_retained_dependencies( + monkeypatch: pytest.MonkeyPatch, +) -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + pinky_key = "pinky_roll_multiview" + ring_key = "ring_roll_multiview" + pinky_spec = next( + spec for spec in profile.sweep_specs if spec.key == pinky_key + ) + ring_spec = next(spec for spec in profile.sweep_specs if spec.key == ring_key) + records_by_joint = {name: [] for name in profile.record_joints} + baseline_records_by_joint = {name: [] for name in profile.record_joints} + command_records_by_joint = {name: [] for name in profile.record_joints} + for joint_name in (*pinky_spec.joints, *ring_spec.joints): + records_by_joint[joint_name].append({"task_name": "fixture"}) + + node = SimpleNamespace( + state="", + profile=profile, + sweep_items=[ + SimpleNamespace(spec=pinky_spec), + SimpleNamespace(spec=ring_spec), + ], + records_by_joint=records_by_joint, + baseline_records_by_joint=baseline_records_by_joint, + command_records_by_joint=command_records_by_joint, + palm_axis_records_by_source={ + observer.source_name: [] + for observer in profile.palm_axis_observers + }, + ) + + def failures(_node, spec, *, include_view_validity): + assert include_view_validity is False + if spec.key == pinky_key: + return [{"joint": pinky_spec.joints[0], "metric": "fixture_failure"}] + pinky_records_remain = any( + records_by_joint[joint_name] for joint_name in pinky_spec.joints + ) + return [] if pinky_records_remain else [ + {"joint": ring_spec.joints[0], "metric": "missing_reference"} + ] + + monkeypatch.setattr( + G20ThreeCameraCalibrationNode, + "_provisional_fit_failures", + failures, + ) + + accepted, dropped = ( + G20ThreeCameraCalibrationNode._revalidate_imported_tasks( + node, + [pinky_key, ring_key], + allow_sparse=True, + ) + ) + + assert accepted == [] + assert [failure["task"] for failure in dropped] == [pinky_key, ring_key] + assert all( + not records_by_joint[joint_name] + for joint_name in (*pinky_spec.joints, *ring_spec.joints) + ) 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 a2c545d..e3a19a0 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 @@ -362,6 +362,38 @@ def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None: assert "未知原因" not in text +def test_thumb_yaw_repeatability_failure_names_real_source_retry() -> None: + explanation, suggestion = three_camera_reason_zh( + "PAUSED", + "joint_fit_check_failed", + { + "source_task_names": [ + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + ], + "directions_to_rescan": 16, + "failures": [ + { + "joint": "thumb_cmc_yaw", + "metric": "zero_cycle_offset_range_deg", + "actual": 0.555836, + "limit": 0.5, + "comparison": "maximum", + "cycle": 3, + "cycle_offset_deg": [-2.650924, -2.615277, -2.095088], + } + ], + }, + ) + + assert "训练轮零位极差为0.56°" in explanation + assert "各轮=-2.65°/-2.62°/-2.10°" in explanation + assert "未知原因" not in explanation + assert "CMC pitch/roll顶部轴观测" in suggestion + assert "重扫16个方向" in suggestion + assert "不会无效重扫yaw侧摆" in suggestion + + def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None: payload = { "state": "PAUSED", @@ -650,3 +682,16 @@ def test_same_finger_transition_explains_that_clearance_stays_parked() -> None: assert "继续保持当前避让姿态" in explanation assert "只调整被测关节" in explanation assert "不要手动展开" in action + + +def test_fixed_base_reference_movement_requires_a_new_session() -> None: + explanation, action = three_camera_reason_zh( + "PAUSED", + "fixed_base_reference_moved", + {}, + ) + + assert "Tag 8" in explanation + assert "基准锁定后" in explanation + assert "下一次标定预检前重新摆放" in action + assert "重新启动新会话" in action 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 4443e10..c16ce92 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 @@ -43,13 +43,18 @@ from g20_thumb_apriltag_calibration.three_camera_node import ( _frames_cover_sweep_joints, _isolated_axis_cycle_outliers, _isolated_axis_line_cycle_outliers, + _isolated_scalar_cycle_outlier, + _maximum_corner_drift_px, _overall_progress, _preserve_pnp_task_reference_for_sweep, + _previous_passed_joint_zero_offset, _palm_axis_observer_for_sweep, _requires_pnp_tracker_reset_for_sweep, _selected_pose_qualities, + _sweep_uses_locked_base_reference, _sweep_plan_transition, _sweep_views, + _thumb_yaw_zero_repeatability_failures, ) from g20_thumb_apriltag_calibration.pnp import SquareTagPose from g20_thumb_apriltag_calibration.urdf_zero import ( @@ -364,6 +369,12 @@ def test_pnp_reset_policy_preserves_legacy_and_retry_recovery() -> None: SweepItem(right_spec, 2, DIRECTION_DECREASING), is_fit_retry=True, ) + assert not _requires_pnp_tracker_reset_for_sweep( + RIGHT_19_HAND_PROFILE, + SweepItem(right_spec, 2, DIRECTION_DECREASING), + is_fit_retry=True, + preserve_retry_continuity=True, + ) assert _requires_pnp_tracker_reset_for_sweep( RIGHT_HAND_PROFILE, SweepItem(legacy_spec, 2, DIRECTION_DECREASING), @@ -703,6 +714,78 @@ def test_multiview_roll_uses_locked_front_base_but_live_moving_tag() -> None: assert live == ("middle_roll",) +@pytest.mark.parametrize( + "task_key", + ( + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + "thumb_cmc_yaw_top", + ), +) +def test_thumb_top_tasks_freeze_tag8_but_keep_it_live(task_key: str) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == task_key + ) + top = SimpleNamespace( + roles=tuple(RIGHT_19_HAND_PROFILE.view_tags["top"]), + preflight_roles=("top_base", "thumb_yaw"), + locked_base_pose=SquareTagPose( + (0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 1.0), 0.1 + ), + locked_base_center_xy_px=(100.0, 200.0), + locked_base_quality=TagQuality(0, 100.0, 30.0, 0.1), + ) + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + views={"top": top}, + active_sweep=SweepItem(spec, 0, DIRECTION_DECREASING), + active_validation=None, + retry_sweep_spec=None, + active_combination_validation=None, + ) + node._locked_base_role_for_active_capture = lambda view: ( + G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture( + node, view + ) + ) + + assert _sweep_uses_locked_base_reference( + RIGHT_19_HAND_PROFILE, spec, "top" + ) + assert ( + G20ThreeCameraCalibrationNode._locked_base_role_for_active_capture( + node, "top" + ) + == "top_base" + ) + required = G20ThreeCameraCalibrationNode._required_roles_for_view( + node, "top" + ) + live = G20ThreeCameraCalibrationNode._live_required_roles_for_view( + node, "top", required + ) + assert "top_base" in required + assert "top_base" in live + + +def test_fixed_base_corner_drift_uses_all_four_ordered_corners() -> None: + reference = np.asarray( + [[10.0, 10.0], [30.0, 10.0], [30.0, 30.0], [10.0, 30.0]] + ) + subpixel_noise = reference + np.asarray( + [[0.1, -0.1], [0.2, 0.0], [0.0, 0.2], [-0.1, 0.1]] + ) + moved = reference.copy() + moved[2] += [1.5, 2.0] + + assert _maximum_corner_drift_px( + reference, subpixel_noise + ) < 0.3 + assert _maximum_corner_drift_px(reference, moved) == pytest.approx(2.5) + + def test_side_only_finger_task_keeps_occluded_inactive_front_base_locked() -> None: spec = next( item @@ -773,8 +856,23 @@ def test_baseline_preflight_locks_robust_fixed_tag_reference(tmp_path) -> None: views = { view: SimpleNamespace( fixed_base_observations=deque(observations, maxlen=60), + fixed_base_corner_observations=deque( + ( + np.asarray( + [ + [90.0 + index % 2, 190.0], + [110.0 + index % 2, 190.0], + [110.0 + index % 2, 210.0], + [90.0 + index % 2, 210.0], + ] + ) + for index in range(30) + ), + maxlen=60, + ), locked_base_pose=None, locked_base_center_xy_px=None, + locked_base_corners_xy=None, locked_base_quality=None, view_tags={ {"front": "front_base", "side": "side_base", "top": "top_base"}[ @@ -793,6 +891,12 @@ def test_baseline_preflight_locks_robust_fixed_tag_reference(tmp_path) -> None: assert G20ThreeCameraCalibrationNode._lock_fixed_base_references(node) assert views["front"].locked_base_pose is not None assert views["front"].locked_base_center_xy_px == (100.5, 200.0) + assert views["front"].locked_base_corners_xy == ( + (90.5, 190.0), + (110.5, 190.0), + (110.5, 210.0), + (90.5, 210.0), + ) events = [ json.loads(line) for line in node.raw_path.read_text().splitlines() @@ -2631,6 +2735,121 @@ def test_resume_discards_only_failed_specs_samples(tmp_path) -> None: } +def test_thumb_yaw_retry_replaces_palm_axis_source_tasks_not_yaw_sweep( + tmp_path, +) -> None: + yaw_spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_cmc_yaw_top" + ) + source_specs = [ + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key + in {"thumb_cmc_pitch_front", "thumb_cmc_roll_front"} + ] + source_joints = { + joint for item in source_specs for joint in item.joints + } + records = { + **{ + joint: [ + {"cycle": 0, "keep": joint}, + {"cycle": 2, "old": joint}, + ] + for joint in source_joints + }, + "thumb_cmc_yaw": [{"cycle": 2, "keep": "yaw"}], + } + source_names = { + observer.source_name + for observer in RIGHT_19_HAND_PROFILE.palm_axis_observers + if observer.task_name in {item.key for item in source_specs} + } + palm_records = { + name: [ + {"cycle": 0, "keep": name}, + {"cycle": 2, "old": name}, + ] + for name in source_names + } + resets: list[str] = [] + + def runtime(name: str) -> SimpleNamespace: + return SimpleNamespace( + name=name, + pnp_invalid_since=1.0, + pnp_reset_count=0, + task_valid_frames=100, + task_total_frames=110, + ) + + views = {name: runtime(name) for name in ("front", "top")} + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + retry_sweep_spec=yaw_spec, + retry_joint_names={"thumb_cmc_yaw"}, + retry_source_failure_task_key=yaw_spec.key, + retry_source_task_keys=tuple(item.key for item in source_specs), + retry_cycles={2}, + records_by_joint=records, + baseline_records_by_joint={joint: [] for joint in source_joints}, + command_records_by_joint={joint: [] for joint in source_joints}, + palm_axis_records_by_source=palm_records, + sweep_attempts={yaw_spec.key: 1}, + sweep_retry_counts={}, + raw_path=tmp_path / "raw_samples.jsonl", + paused_reason="joint_fit_check_failed", + fit_failure={"failures": ["old"]}, + repetitions=4, + sweep_items=[ + SweepItem(item, cycle, direction) + for item in source_specs + for cycle in range(4) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ], + views=views, + _reset_view_trackers=( + lambda item, preserve_task_reference=False: resets.append( + item.name + ) + ), + ) + + G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node) + + assert records["thumb_cmc_yaw"] == [ + {"cycle": 2, "keep": "yaw"} + ] + assert all( + records[joint] == [{"cycle": 0, "keep": joint}] + for joint in source_joints + ) + assert all( + palm_records[name] == [{"cycle": 0, "keep": name}] + for name in source_names + ) + assert [(item.spec.key, item.cycle) for item in node.retry_sweep_items] == [ + (item.key, 2) + for item in source_specs + for _direction in range(2) + ] + assert resets == [] + assert node.retry_preserve_pnp_continuity is True + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event["source_task_names"] == [item.key for item in source_specs] + assert event["source_attempts"] == {item.key: 2 for item in source_specs} + assert event["joints_to_rescan"] == sorted(source_joints) + assert event["cycles"] == [3] + assert event["pnp_reference_policy"] == "preserve_existing_generation" + assert all(runtime.task_valid_frames == 0 for runtime in views.values()) + assert all(runtime.task_total_frames == 0 for runtime in views.values()) + + def test_multiview_retry_discards_only_failed_side_measurement(tmp_path) -> None: spec = next( item @@ -3280,6 +3499,222 @@ def test_axis_cycle_spread_localizes_one_outlier_for_four_rounds() -> None: ) == {0} +def test_thumb_yaw_zero_spread_localizes_181154_second_cycle() -> None: + cycle_offsets = [ + math.radians(-2.518678), + math.radians(-3.098037), + math.radians(-2.544640), + ] + result = SimpleNamespace( + cycle_offsets_rad={"thumb_cmc_yaw": cycle_offsets}, + offset_confidence_half_width_rad={ + "thumb_cmc_yaw": math.radians(0.81295) + }, + ) + + assert _isolated_scalar_cycle_outlier( + cycle_offsets, math.radians(0.5) + ) == {1} + assert _thumb_yaw_zero_repeatability_failures( + result, + maximum_cycle_range_rad=math.radians(0.5), + maximum_confidence_half_width_rad=math.radians(0.75), + ) == [ + { + "joint": "thumb_cmc_yaw", + "metric": "zero_cycle_offset_range_deg", + "actual": 0.579359, + "limit": 0.5, + "comparison": "maximum", + "cycle_offset_deg": [-2.518678, -3.098037, -2.54464], + "cycle": 2, + "inlier_cycles": [1, 3], + } + ] + + +def test_thumb_yaw_zero_spread_localizes_142322_marginal_cycle() -> None: + cycle_offsets = [ + math.radians(-2.589022), + math.radians(-2.915619), + math.radians(-2.391267), + ] + result = SimpleNamespace( + cycle_offsets_rad={"thumb_cmc_yaw": cycle_offsets}, + offset_confidence_half_width_rad={ + "thumb_cmc_yaw": math.radians(0.658) + }, + ) + + assert _isolated_scalar_cycle_outlier( + cycle_offsets, math.radians(0.5) + ) == {1} + failures = _thumb_yaw_zero_repeatability_failures( + result, + maximum_cycle_range_rad=math.radians(0.5), + maximum_confidence_half_width_rad=math.radians(0.75), + ) + assert failures[0]["actual"] == 0.524352 + assert failures[0]["cycle"] == 2 + assert failures[0]["inlier_cycles"] == [1, 3] + + +def test_thumb_yaw_zero_spread_keeps_symmetric_drift_nonlocalized() -> None: + cycle_offsets = [ + math.radians(-2.76), + math.radians(-2.50), + math.radians(-2.24), + ] + + assert _isolated_scalar_cycle_outlier( + cycle_offsets, math.radians(0.5) + ) == set() + + +def test_thumb_yaw_zero_confidence_failure_requests_full_yaw_retry() -> None: + result = SimpleNamespace( + cycle_offsets_rad={ + "thumb_cmc_yaw": [ + math.radians(-2.50), + math.radians(-2.70), + math.radians(-2.30), + ] + }, + offset_confidence_half_width_rad={ + "thumb_cmc_yaw": math.radians(0.80) + }, + ) + + assert _thumb_yaw_zero_repeatability_failures( + result, + maximum_cycle_range_rad=math.radians(0.5), + maximum_confidence_half_width_rad=math.radians(0.75), + ) == [ + { + "joint": "thumb_cmc_yaw", + "metric": "zero_confidence_95_half_width_deg", + "actual": 0.8, + "limit": 0.75, + "comparison": "maximum", + } + ] + + +def test_thumb_yaw_zero_repeatability_accepts_171406_quality() -> None: + result = SimpleNamespace( + cycle_offsets_rad={ + "thumb_cmc_yaw": [ + math.radians(-2.53708), + math.radians(-2.69820), + math.radians(-2.29168), + ] + }, + offset_confidence_half_width_rad={ + "thumb_cmc_yaw": math.radians(0.50853) + }, + ) + + assert _thumb_yaw_zero_repeatability_failures( + result, + maximum_cycle_range_rad=math.radians(0.5), + maximum_confidence_half_width_rad=math.radians(0.75), + ) == [] + + +def test_thumb_yaw_zero_outlier_retries_only_localized_source_cycle( + tmp_path, +) -> None: + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_cmc_yaw_top" + ) + failures = _thumb_yaw_zero_repeatability_failures( + SimpleNamespace( + cycle_offsets_rad={ + "thumb_cmc_yaw": [ + math.radians(-2.518678), + math.radians(-3.098037), + math.radians(-2.544640), + ] + }, + offset_confidence_half_width_rad={ + "thumb_cmc_yaw": math.radians(0.81295) + }, + ), + maximum_cycle_range_rad=math.radians(0.5), + maximum_confidence_half_width_rad=math.radians(0.75), + ) + calls: list[str] = [] + node = SimpleNamespace( + profile=RIGHT_19_HAND_PROFILE, + raw_path=tmp_path / "raw_samples.jsonl", + sweep_attempts={}, + retry_resume_index=None, + retry_sweep_spec=None, + retry_cycles=set(), + retry_joint_names=set(), + fit_failure={}, + sweep_index=96, + repetitions=3, + automatic_fit_retry_limit=2, + retry_source_failure_task_key=spec.key, + retry_source_task_keys=( + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + ), + retry_cycle_override=set(), + paused_reason="", + reason="", + _prepare_failed_sweep_retry=lambda: calls.append("prepare"), + _begin_return_baseline=lambda after: calls.append(after), + _pause=lambda reason: calls.append(f"pause:{reason}"), + ) + + handled = G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, spec, failures + ) + + assert handled is True + assert node.retry_cycles == {1} + assert node.retry_joint_names == { + "thumb_cmc_pitch", + "thumb_cmc_roll", + } + assert node.fit_failure["cycles_to_rescan"] == [2] + assert node.fit_failure["directions_to_rescan"] == 4 + assert node.fit_failure["source_task_names"] == [ + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + ] + assert calls == ["prepare", "resume_sweep"] + + +def test_previous_passed_joint_zero_offset_reads_formal_pointer( + tmp_path, +) -> None: + root = tmp_path / "G20_RIGHT_001" + previous = root / "20260830_171406" + current = root / "20260831_100000" + previous.mkdir(parents=True) + current.mkdir() + payload = { + "joints": { + "thumb_cmc_yaw": { + "zero_angles": {"urdf_zero_offset_rad": -0.04736713} + } + } + } + (previous / "g20_right_G20_RIGHT_001_calibration.json").write_text( + json.dumps(payload), encoding="utf-8" + ) + (root / "latest_passed").symlink_to(previous, target_is_directory=True) + + assert _previous_passed_joint_zero_offset( + current, "G20_RIGHT_001", "thumb_cmc_yaw" + ) == (previous, -0.04736713) + + def test_axis_line_spread_localizes_one_outlier_for_four_rounds() -> None: axes = [ [-0.043565, 0.036698, -0.998376], 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 4cc57c5..0b045ee 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py +++ b/src/g20_thumb_apriltag_calibration/test/test_urdf_zero.py @@ -30,6 +30,9 @@ from g20_thumb_apriltag_calibration.urdf_zero import ( select_cross_view_roll_direction_source, solve_urdf_zero_offsets, get_zero_calibration_profile, + get_right_19_thumb_zero_profile, + expand_right_19_thumb_zero_result_with_cad_fingers, + merge_right_19_thumb_zero_result, write_zero_corrected_urdf, ) from g20_thumb_apriltag_calibration.full_hand import ( @@ -623,8 +626,10 @@ def _solve_synthetic_offsets( joint_maximum_offset_degrees: dict[str, float] | None = None, validation_offset_bias_degrees: dict[str, float] | None = None, static_output_offsets_degrees: dict[str, float] | None = None, + fixed_direct_offsets_degrees: dict[str, float] | None = None, base_euler_xyz_rad: tuple[float, float, float] = (0.5, -0.4, 0.8), base_translation_xyz_m: tuple[float, float, float] = (0.31, -0.19, 0.72), + solve_thumb_only: bool = False, ): hand = get_hand_calibration_profile(side, layout_id) zero = get_zero_calibration_profile(side, layout_id) @@ -803,9 +808,14 @@ def _solve_synthetic_offsets( rotation_orthogonal_rms_rad=math.radians(0.1), ) ) + solve_profile = ( + get_right_19_thumb_zero_profile() if solve_thumb_only else zero + ) result = solve_urdf_zero_offsets( source_urdf=source, - measurements=measurements, + measurements=[ + item for item in measurements if item.joint in solve_profile.axis_joints + ], palm_orientation_measurements=palm_orientation_measurements, curves=curves, motor_by_joint=motor_by_joint, @@ -828,6 +838,16 @@ def _solve_synthetic_offsets( name: math.radians(value) for name, value in (static_output_offsets_degrees or {}).items() }, + fixed_direct_zero_offsets_rad=( + None + if fixed_direct_offsets_degrees is None + else { + name: math.radians(value) + for name, value in fixed_direct_offsets_degrees.items() + if name in solve_profile.direct_zero_joints + } + ), + zero_profile=solve_profile, ) return zero, result @@ -884,6 +904,128 @@ def test_right_19_solver_recovers_visual_targets_when_no_endpoint_anchor_is_supp assert set(result.offset_covariance_rad2) == set(zero.direct_zero_joints) +def test_right_19_thumb_kernel_is_independent_and_matches_full_hand() -> None: + injected = [ + 2.0, -3.0, 4.0, -1.5, + 1.0, -1.0, 0.7, + 0.8, -0.7, -0.8, + -0.5, 0.6, 0.9, + 1.1, -1.0, -0.6, + ] + _, full = _solve_synthetic_offsets( + "right", + injected, + layout_id="g20_right_19", + fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, + static_output_offsets_degrees={"thumb_cmc_roll": 2.0}, + ) + _, thumb = _solve_synthetic_offsets( + "right", + injected, + layout_id="g20_right_19", + solve_thumb_only=True, + fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, + static_output_offsets_degrees={"thumb_cmc_roll": 2.0}, + ) + + assert thumb.passed is True + assert set(thumb.direct_offsets_rad) == { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + for name in thumb.direct_offsets_rad: + assert thumb.direct_offsets_rad[name] == pytest.approx( + full.direct_offsets_rad[name], abs=math.radians(0.05) + ) + + finger_offsets = { + name: full.direct_offsets_rad[name] + for name in get_zero_calibration_profile( + "right", "g20_right_19" + ).direct_zero_joints + if not name.startswith("thumb_") + } + thumb_only = merge_right_19_thumb_zero_result( + thumb_result=thumb, + preserved_offsets_rad=finger_offsets, + ) + full_hand = merge_right_19_thumb_zero_result( + thumb_result=thumb, + companion_result=full, + ) + for name in thumb.direct_offsets_rad: + assert thumb_only.direct_offsets_rad[name] == pytest.approx( + full_hand.direct_offsets_rad[name], abs=1.0e-12 + ) + standalone = expand_right_19_thumb_zero_result_with_cad_fingers(thumb) + assert all( + standalone.direct_offsets_rad[name] == 0.0 + for name in standalone.direct_offsets_rad + if not name.startswith("thumb_") + ) + + +def test_thumb_endpoint_output_uses_published_offset_safety_limit() -> None: + # The visual roll scalar may carry a root-frame gauge beyond the global + # limit. A separately validated mechanical endpoint is the value written + # to the URDF and therefore the value the configured safety gate guards. + injected = [ + 21.0, -3.0, 4.0, -1.5, + 1.0, -1.0, 0.7, + 0.8, -0.7, -0.8, + -0.5, 0.6, 0.9, + 1.1, -1.0, -0.6, + ] + + _, result = _solve_synthetic_offsets( + "right", + injected, + layout_id="g20_right_19", + solve_thumb_only=True, + fixed_direct_offsets_degrees={"thumb_mcp": -1.5}, + static_output_offsets_degrees={"thumb_cmc_roll": 3.45}, + ) + + assert result.passed is True + assert "thumb_cmc_roll" not in result.failure_reasons + assert math.degrees( + result.direct_offsets_rad["thumb_cmc_roll"] + ) == pytest.approx(3.45, abs=0.01) + + +def test_right_19_thumb_kernel_is_invariant_to_rigid_hand_movement() -> None: + injected = [ + 2.0, -3.0, 4.0, -1.5, + 1.0, -1.0, 0.7, + 0.8, -0.7, -0.8, + -0.5, 0.6, 0.9, + 1.1, -1.0, -0.6, + ] + arguments = { + "layout_id": "g20_right_19", + "solve_thumb_only": True, + "fixed_direct_offsets_degrees": {"thumb_mcp": -1.5}, + "static_output_offsets_degrees": {"thumb_cmc_roll": 2.0}, + } + _, first = _solve_synthetic_offsets("right", injected, **arguments) + _, moved = _solve_synthetic_offsets( + "right", + injected, + base_euler_xyz_rad=(-0.2, 0.7, -0.35), + base_translation_xyz_m=(-0.15, 0.42, 0.55), + **arguments, + ) + + assert first.passed is True + assert moved.passed is True + for name in first.direct_offsets_rad: + assert moved.direct_offsets_rad[name] == pytest.approx( + first.direct_offsets_rad[name], abs=math.radians(0.01) + ) + + def test_right_19_independent_roll_output_cannot_change_yaw_solution() -> None: injected = [ 2.0, -3.0, 4.0, -1.5, @@ -919,6 +1061,7 @@ def _partial_orientation_records( common_rotation: Rotation, maximum_angle_deg: float = 30.0, tag_offset_xyz_m: tuple[float, float, float] = (0.01, 0.02, -0.015), + include_child_pose_common: bool = False, ) -> list[dict[str, object]]: axis_parent = np.asarray([0.0, 1.0, 0.0]) parent_pose = { @@ -942,8 +1085,7 @@ def _partial_orientation_records( ).apply(tag_offset_xyz_m) state = [255.0] * 20 state[1] = float(command) - records.append( - { + record: dict[str, object] = { "cycle": 0, "direction": direction, "command_u8": command, @@ -954,7 +1096,16 @@ def _partial_orientation_records( "parent_pose_common": parent_pose, "state_u8": state, } - ) + if include_child_pose_common: + child_common = common_rotation * relative + record["child_pose_common"] = { + "translation_xyz_m": list( + np.asarray(parent_pose["translation_xyz_m"]) + + common_rotation.apply(relative_translation) + ), + "quaternion_xyzw": list(child_common.as_quat()), + } + records.append(record) return records @@ -1083,6 +1234,105 @@ def test_partial_palm_direction_ignores_far_stroke_pnp_bias() -> None: ) == pytest.approx(1.0, abs=1.0e-10) +def test_partial_palm_direction_uses_robust_full_stroke_local_motion() -> None: + common_rotation = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) + records = _partial_orientation_records( + tag_mount=Rotation.from_euler("xyz", [0.2, -0.1, 0.3]), + common_rotation=common_rotation, + maximum_angle_deg=48.0, + include_child_pose_common=True, + ) + expected = fit_partial_palm_orientation_measurement( + "thumb_cmc_roll_top_axis", + "thumb_cmc_roll", + records, + cycle=0, + zero_command_u8=255, + ) + + biased: list[dict[str, object]] = [] + for source in records: + record = dict(source) + pose = dict(record["child_pose_common"]) + # Simulate a command-correlated planar-PnP branch curvature affecting + # fewer than one quarter of the full-stroke local increments. The + # zero-adjacent quality trajectory remains unchanged. + if int(record["command_u8"]) <= 187: + rotation = Rotation.from_quat(pose["quaternion_xyzw"]) + amount = math.radians(8.0) * ( + (187 - int(record["command_u8"])) / 12.0 + ) + pose["quaternion_xyzw"] = list( + ( + Rotation.from_rotvec([amount, 0.0, 0.0]) * rotation + ).as_quat() + ) + record["child_pose_common"] = pose + biased.append(record) + + fitted = fit_partial_palm_orientation_measurement( + "thumb_cmc_roll_top_axis", + "thumb_cmc_roll", + biased, + cycle=0, + zero_command_u8=255, + ) + difference = math.acos( + abs( + float( + np.clip( + np.dot(expected.axis_common_xyz, fitted.axis_common_xyz), + -1.0, + 1.0, + ) + ) + ) + ) + + assert expected.axis_estimator == "robust_full_stroke_local_so3_v1" + assert expected.incremental_pair_count >= 24 + assert fitted.axis_estimator == expected.axis_estimator + assert difference < math.radians(0.25) + + +def test_full_stroke_palm_axis_tracks_reestablished_common_frame() -> None: + tag_mount = Rotation.from_euler("xyz", [0.2, -0.1, 0.3]) + first_common = Rotation.from_euler("xyz", [0.35, -0.2, 0.6]) + repositioning = Rotation.from_euler("xyz", [-0.18, 0.27, -0.41]) + second_common = repositioning * first_common + first = fit_partial_palm_orientation_measurement( + "thumb_cmc_roll_top_axis", + "thumb_cmc_roll", + _partial_orientation_records( + tag_mount=tag_mount, + common_rotation=first_common, + maximum_angle_deg=48.0, + include_child_pose_common=True, + ), + cycle=0, + zero_command_u8=255, + ) + second = fit_partial_palm_orientation_measurement( + "thumb_cmc_roll_top_axis", + "thumb_cmc_roll", + _partial_orientation_records( + tag_mount=tag_mount, + common_rotation=second_common, + maximum_angle_deg=48.0, + include_child_pose_common=True, + ), + cycle=0, + zero_command_u8=255, + ) + expected_second = repositioning.apply(first.axis_common_xyz) + + assert first.axis_estimator == "robust_full_stroke_local_so3_v1" + assert second.axis_estimator == first.axis_estimator + assert abs(float(np.dot(expected_second, second.axis_common_xyz))) == ( + pytest.approx(1.0, abs=1.0e-8) + ) + + def test_right_19_top_axis_pair_holdout_ignores_shared_frame_bias() -> None: injected = [0.0] * 16 _, result = _solve_synthetic_offsets( @@ -1431,6 +1681,36 @@ def test_right_19_holdout_never_changes_frozen_training_offsets() -> None: assert result.validation_cycle not in result.training_cycles +def test_failed_thumb_holdout_does_not_blame_fixed_partial_scope_zeros() -> None: + zero = get_zero_calibration_profile("right", "g20_right_19") + injected = [ + 2.0, -3.0, 4.0, -1.5, + 1.0, -1.0, 0.7, + 0.8, -0.7, -0.8, + -0.5, 0.6, 0.9, + 1.1, -1.0, -0.6, + ] + frozen = { + name: injected[index] + for index, name in enumerate(zero.direct_zero_joints) + if not name.startswith("thumb_") + } + + _, result = _solve_synthetic_offsets( + "right", + injected, + layout_id="g20_right_19", + validation_offset_bias_degrees={"thumb_cmc_yaw": 8.0}, + fixed_direct_offsets_degrees=frozen, + ) + + assert result.passed is False + assert result.failure_reasons == { + "palm_orientation": "palm_orientation_holdout_too_large" + } + assert set(frozen).isdisjoint(result.failure_reasons) + + 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")