diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py index 74b7c19..c069e74 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py @@ -5,11 +5,15 @@ from .config_v1 import ( product_profile_key, resolve_legacy_profile_alias, ) -from .defaults import default_product_config_path +from .defaults import ( + default_product_config_path, + default_three_camera_config_path, +) from .paths import resolve_renamed_package_path __all__ = [ "default_product_config_path", + "default_three_camera_config_path", "legacy_default_profile_key", "product_profile_key", "resolve_legacy_profile_alias", diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py b/src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py index 8349024..07084d0 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py @@ -8,3 +8,9 @@ from ament_index_python.packages import get_package_share_directory def default_product_config_path() -> Path: share = Path(get_package_share_directory("linkerhand_calibration")) return share / "config/g20_right_product.yaml" + + +def default_three_camera_config_path() -> Path: + """Resolve the installed calibration defaults through the ROS index.""" + share = Path(get_package_share_directory("linkerhand_calibration")) + return share / "config/three_camera_calibration.yaml" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index 9ed7a59..cd74a67 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -120,6 +120,12 @@ class MeasurementSpec: parent_role: str | None child_role: str | None validation_source: str | None = None + # Some measured trajectories publish only a dynamic curve while their + # static URDF zero/axis remains CAD- or mimic-owned. For those joints a + # monocular 3-D axis-line residual is useful diagnostic evidence, but it + # must not reject an otherwise clean image/SO(3) trajectory merely because + # the hand was placed at a different valid position in the camera view. + pose_axis_line_required: bool = True @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py index a030e19..f75e23d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py @@ -98,6 +98,9 @@ def adapt_profile( validation_source=(hand_profile.axis_validation_sources or {}).get( name ), + pose_axis_line_required=bool( + spec.pose_axis_line_required + ), ) for name, spec in record_specs.items() } diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py index 0bf3dda..84987ee 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py @@ -820,6 +820,52 @@ def _maximum_corner_drift_px( ) +def _resume_fixed_base_position_compatibility( + rows: Sequence[Mapping[str, Any]], + current_corners_by_view: Mapping[ + str, Sequence[Sequence[float]] | None + ], + maximum_corner_drift_px: float, +) -> tuple[dict[str, float], tuple[str, ...], tuple[str, ...]]: + """Compare the previous and current session-start palm references. + + Calibration measurements are invariant to one rigid hand placement, but + samples expressed in two independently established common frames must + never be combined. Fixed palm-Tag corners provide a camera-native check + before any durable task is imported. + """ + previous_corners_by_view: dict[str, Any] = {} + for row in rows: + if str(row.get("kind", "")) != "fixed_base_reference_locked": + continue + view = str(row.get("view", "")) + if view in current_corners_by_view: + # Keep the last lock in case a future compatible schema records a + # deliberate pre-scan relock in the same raw stream. + previous_corners_by_view[view] = row.get("corner_reference_xy") + + drift_by_view_px: dict[str, float] = {} + changed_views: list[str] = [] + unverifiable_views: list[str] = [] + for view, current in current_corners_by_view.items(): + previous = previous_corners_by_view.get(str(view)) + if previous is None or current is None: + unverifiable_views.append(str(view)) + continue + drift = _maximum_corner_drift_px(previous, current) + if not math.isfinite(drift): + unverifiable_views.append(str(view)) + continue + drift_by_view_px[str(view)] = drift + if drift > float(maximum_corner_drift_px): + changed_views.append(str(view)) + return ( + drift_by_view_px, + tuple(sorted(changed_views)), + tuple(sorted(unverifiable_views)), + ) + + def _selected_pose_qualities( selected: Mapping[str, SquareTagPose], live_qualities: Mapping[str, TagQuality], @@ -1428,6 +1474,19 @@ def _unresolved_fit_failure_tasks( # failures that mixed velocity lag or firmware tracking # deadband with mechanical hysteresis are safe to revalidate. return True + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and metric == "axis_pose_line_rms_mm" + and joint_name in profile.record_specs + and not profile.record_specs[ + joint_name + ].pose_axis_line_required + ): + # Current profile policy owns whether this monocular 3-D + # diagnostic is release-critical. Revalidate the complete + # raw task under that policy instead of making an old failure + # force another acquisition forever. + return True if ( profile.layout_id == G20_RIGHT_19_LAYOUT and joint_name @@ -2774,6 +2833,11 @@ class G20ThreeCameraCalibrationNode(Node): ) self.resumed_task_keys: tuple[str, ...] = () self.resume_source_session = "" + self.resume_checkpoint_pending = False + self.resume_position_policy = "not_requested" + self.resume_position_changed_views: tuple[str, ...] = () + self.resume_position_unverifiable_views: tuple[str, ...] = () + self.resume_position_drift_by_view_px: dict[str, float] = {} self.camera_extrinsics_file = Path( str(value("camera_extrinsics_file")) ).expanduser().resolve() @@ -5267,6 +5331,54 @@ class G20ThreeCameraCalibrationNode(Node): "resume checkpoint geometry, algorithm capabilities, Tag " "layout, baseline or source URDF differs" ) + current_corners_by_view = { + str(view): getattr(runtime, "locked_base_corners_xy", None) + for view, runtime in getattr(self, "views", {}).items() + } + if not current_corners_by_view: + raise RuntimeError( + "resume checkpoint requires current fixed-base references" + ) + ( + position_drift_by_view_px, + position_changed_views, + position_unverifiable_views, + ) = _resume_fixed_base_position_compatibility( + rows, + current_corners_by_view, + float( + getattr(self, "fixed_base_maximum_corner_drift_px", 2.0) + ), + ) + self.resume_source_session = source.parent.name + self.resume_position_drift_by_view_px = dict( + position_drift_by_view_px + ) + self.resume_position_changed_views = position_changed_views + self.resume_position_unverifiable_views = ( + position_unverifiable_views + ) + start_position_invalidated_tasks: tuple[str, ...] = () + if position_changed_views or position_unverifiable_views: + if str(getattr(self, "recalibration_scope", "full")) != "full": + affected = sorted( + {*position_changed_views, *position_unverifiable_views} + ) + raise RuntimeError( + "resume checkpoint start pose differs or cannot be " + "verified for partial recalibration: " + + ",".join(affected) + ) + start_position_invalidated_tasks = tuple( + spec.key for spec in self.profile.sweep_specs + ) + self.resume_position_policy = ( + "discard_all_tasks_for_new_start_pose" + if position_changed_views + else "discard_all_tasks_for_unverified_start_pose" + ) + else: + self.resume_position_policy = "reuse_same_start_pose" try: changed_tag_size_ids, size_invalidated_tasks = ( resume_tasks_invalidated_by_tag_size_changes( @@ -5281,6 +5393,7 @@ class G20ThreeCameraCalibrationNode(Node): ) from error invalidated_task_set = set(size_invalidated_tasks) invalidated_task_set.update(palm_axis_invalidated_tasks) + invalidated_task_set.update(start_position_invalidated_tasks) scope_invalidated_tasks = tuple( getattr(self, "recalibration_task_keys", ()) or () ) @@ -5388,7 +5501,6 @@ class G20ThreeCameraCalibrationNode(Node): ) self.resumed_task_keys = completed G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) - self.resume_source_session = source.parent.name missing_tasks = [ spec.key for spec in self.profile.sweep_specs @@ -5418,6 +5530,20 @@ class G20ThreeCameraCalibrationNode(Node): "scope_invalidated_task_keys": list( scope_invalidated_tasks ), + "start_position_policy": self.resume_position_policy, + "start_position_changed_views": list( + position_changed_views + ), + "start_position_unverifiable_views": list( + position_unverifiable_views + ), + "start_position_drift_by_view_px": { + view: round(float(value), 6) + for view, value in position_drift_by_view_px.items() + }, + "start_position_invalidated_task_keys": list( + start_position_invalidated_tasks + ), "imported_record_count": len(reusable), "imported_attempt_floor_by_task": attempt_floor_by_task, "source_raw_samples_sha256": _file_sha256(source), @@ -5571,6 +5697,19 @@ class G20ThreeCameraCalibrationNode(Node): return response self.started = True self.startup_baseline_recovered = False + self.resumed_task_keys = () + self.resume_source_session = "" + self.resume_checkpoint_pending = bool( + self.resume_raw_samples_path is not None + ) + self.resume_position_policy = ( + "pending_new_start_pose_check" + if self.resume_checkpoint_pending + else "not_requested" + ) + self.resume_position_changed_views = () + self.resume_position_unverifiable_views = () + self.resume_position_drift_by_view_px = {} self.sweep_items = [] self.pnp_task_spec = None selected_sweep_specs = list(self.profile.sweep_specs) @@ -5738,10 +5877,16 @@ class G20ThreeCameraCalibrationNode(Node): ) try: if self.resume_raw_samples_path is not None: - self.state = STATE_IMPORTING_BASE - self.reason = "reading_base_session_records" + if not self.resume_raw_samples_path.is_file(): + raise RuntimeError( + "resume raw samples do not exist: " + f"{self.resume_raw_samples_path}" + ) + self.resume_source_session = ( + self.resume_raw_samples_path.parent.name + ) self.base_import_progress = { - "phase": "reading", + "phase": "waiting_for_new_start_pose_reference", "records_read": 0, "bytes_read": 0, "total_bytes": int( @@ -5749,10 +5894,9 @@ class G20ThreeCameraCalibrationNode(Node): ), "fraction": 0.0, } - self._publish_status(time.monotonic()) - restored_tasks = self._restore_durable_task_checkpoint() except Exception as error: self.started = False + self.resume_checkpoint_pending = False response.success = False response.message = f"CFG-RESUME-009:{error}" return response @@ -5760,8 +5904,11 @@ class G20ThreeCameraCalibrationNode(Node): response.success = True response.message = ( "three-camera calibration started" - if restored_tasks == 0 - else f"calibration resumed with {restored_tasks} completed tasks" + if not self.resume_checkpoint_pending + else ( + "three-camera calibration started; checkpoint will be " + "verified after the new start-pose reference is locked" + ) ) return response @@ -8616,6 +8763,35 @@ class G20ThreeCameraCalibrationNode(Node): # error is an admissibility check for constrained circles. for metric, actual, limit in axis_checks: if actual > limit: + if ( + metric == "axis_pose_line_rms_mm" + and not joint_spec.pose_axis_line_required + ): + append_jsonl( + self.raw_path, + { + "kind": ( + "position_invariant_quality_diagnostic" + ), + "task_name": spec.key, + "joint": joint_name, + "cycle": cycle + 1, + "metric": metric, + "actual": round(float(actual), 6), + "reference_limit": round( + float(limit), 6 + ), + "decision": "diagnostic_only", + "authoritative_quality": [ + "tag_pnp", + "image_trajectory", + "relative_rotation", + "synchronisation", + "isolated_holdout", + ], + }, + ) + continue failure_joint = joint_name quality_sources: tuple[str, ...] = () if metric == "axis_pose_line_rms_mm": @@ -11932,6 +12108,21 @@ class G20ThreeCameraCalibrationNode(Node): elif self.startup_baseline_recovered and self._all_preflight_ready(now): locker = getattr(self, "_lock_fixed_base_references", None) if locker is None or locker(): + if getattr(self, "resume_checkpoint_pending", False): + self.resume_checkpoint_pending = False + self.state = STATE_IMPORTING_BASE + self.reason = "reading_base_session_records" + self.base_import_progress = { + "phase": "reading", + "records_read": 0, + "bytes_read": 0, + "total_bytes": int( + self.resume_raw_samples_path.stat().st_size + ), + "fraction": 0.0, + } + self._publish_status(time.monotonic()) + self._restore_durable_task_checkpoint() self._start_next_sweep() else: self.reason = "locking_fixed_base_references" @@ -12979,7 +13170,32 @@ class G20ThreeCameraCalibrationNode(Node): "camera_extrinsics_error": self.extrinsics_error, "resume": { "used": bool(self.resumed_task_keys), + "checkpoint_requested": bool( + self.resume_raw_samples_path is not None + ), + "checkpoint_pending": bool( + getattr(self, "resume_checkpoint_pending", False) + ), "source_session": self.resume_source_session, + "start_position_policy": getattr( + self, "resume_position_policy", "not_requested" + ), + "start_position_changed_views": list( + getattr(self, "resume_position_changed_views", ()) + ), + "start_position_unverifiable_views": list( + getattr( + self, + "resume_position_unverifiable_views", + (), + ) + ), + "start_position_drift_by_view_px": { + view: round(float(value), 6) + for view, value in getattr( + self, "resume_position_drift_by_view_px", {} + ).items() + }, "recalibration_scope": self.recalibration_scope, "recalibration_task_keys": list( self.recalibration_task_keys diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py index 7f7a9ef..8d484cb 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py @@ -19,6 +19,7 @@ import numpy as np from scipy.spatial.transform import Rotation import yaml +from ...compat import default_three_camera_config_path from ...compat.legacy import uses_coupled_full_hand_zero_solver from ...extrinsics import load_three_camera_extrinsics from .profile import ( @@ -833,6 +834,7 @@ def _quality_failures( if ( profile.record_specs[name].zero_kind != "axis_cross_view_validation" + and profile.record_specs[name].pose_axis_line_required and cross_view_side_line_source(axis) is None and axis.pose_axis_line_rms_m > float(parameters["axis_maximum_pose_line_rms_m"]) @@ -1047,9 +1049,8 @@ def replay_session( output_tag: str | None = None, ) -> dict[str, Any]: session = Path(session_dir).expanduser().resolve() - package_root = Path(__file__).resolve().parents[3] config = ( - package_root / "config" / "three_camera_calibration.yaml" + default_three_camera_config_path() if config_file is None else Path(config_file).expanduser().resolve() ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py index 90ded41..ddd88b4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py @@ -51,6 +51,12 @@ class JointSpec: child_role: str | None source_joint: str | None = None zero_kind: str | None = None + # Keep the physical axis-line gate only when that line contributes to a + # released URDF zero/axis decision. Curve-only passive measurements may + # retain the monocular line residual as a diagnostic while their image + # trajectory, relative rotation, synchronisation and holdout gates remain + # release-critical. + pose_axis_line_required: bool = True @property def measured(self) -> bool: @@ -543,7 +549,13 @@ def _build_right_19_profile() -> HandCalibrationProfile: zero_kind="urdf_axis_chain", ), "thumb_ip": JointSpec( - "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip", + "thumb_ip", + 15, + False, + "front", + "thumb_mcp", + "thumb_ip", + pose_axis_line_required=False, ), } for finger in ("index", "middle", "ring", "pinky"): diff --git a/src/linkerhand_calibration/linkerhand_calibration/operator_report.py b/src/linkerhand_calibration/linkerhand_calibration/operator_report.py index eeacbba..6b181e3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/operator_report.py +++ b/src/linkerhand_calibration/linkerhand_calibration/operator_report.py @@ -305,13 +305,24 @@ def render_progress_zh( f"{fit_attempt_limit - 1} 次异常轮补采）" ) resume = status.get("resume", {}) - if isinstance(resume, Mapping) and resume.get("used"): - lines.append( - "断点：已恢复 " - f"{int(resume.get('completed_task_count', 0))}/" - f"{int(resume.get('total_task_count', 16))} 个完整任务；" - "失败任务已丢弃并重新采集" - ) + if isinstance(resume, Mapping): + position_policy = str(resume.get("start_position_policy", "")) + if resume.get("checkpoint_pending"): + lines.append( + "断点：正在建立本次起始位置基准，完成后再决定是否复用旧任务" + ) + elif position_policy.startswith("discard_all_tasks_"): + lines.append( + "位置：检测到标定前机械手位置已变化或旧位置无法可靠验证；" + "未混用旧断点，本次整手从头采集" + ) + elif resume.get("used"): + lines.append( + "断点：已恢复 " + f"{int(resume.get('completed_task_count', 0))}/" + f"{int(resume.get('total_task_count', 16))} 个完整任务；" + "起始位置一致，失败任务已丢弃并重新采集" + ) return "\n".join(lines) diff --git a/src/linkerhand_calibration/test/test_g20_right_product.py b/src/linkerhand_calibration/test/test_g20_right_product.py index 7d0c46b..63afc3f 100644 --- a/src/linkerhand_calibration/test/test_g20_right_product.py +++ b/src/linkerhand_calibration/test/test_g20_right_product.py @@ -196,6 +196,17 @@ def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None: config.calibration_contract.typed_profile.measurement .stable_cross_view_cone_bias ) + hand_profile = config.calibration_contract.profile + typed_measurements = ( + config.calibration_contract.typed_profile.measurement.measurements + ) + assert hand_profile.record_specs["thumb_ip"].pose_axis_line_required is False + assert typed_measurements["thumb_ip"].pose_axis_line_required is False + assert all( + spec.pose_axis_line_required + for name, spec in hand_profile.record_specs.items() + if name != "thumb_ip" + ) assert config.serial_number == "G20_RIGHT_001" assert config.namespace == "/g20_calibration" assert config.required_independent_passes == 1 @@ -310,6 +321,52 @@ def _complete_resume_task_rows(profile, spec) -> list[dict]: return rows +_FIXED_BASE_CORNERS_BY_VIEW = { + "front": ( + (100.0, 100.0), + (120.0, 100.0), + (120.0, 120.0), + (100.0, 120.0), + ), + "side": ( + (200.0, 100.0), + (220.0, 100.0), + (220.0, 120.0), + (200.0, 120.0), + ), + "top": ( + (300.0, 100.0), + (320.0, 100.0), + (320.0, 120.0), + (300.0, 120.0), + ), +} + + +def _fixed_base_reference_rows( + corners_by_view: dict[str, tuple[tuple[float, float], ...]] | None = None, +) -> list[dict]: + selected = corners_by_view or _FIXED_BASE_CORNERS_BY_VIEW + return [ + { + "kind": "fixed_base_reference_locked", + "view": view, + "corner_reference_xy": [list(point) for point in corners], + } + for view, corners in selected.items() + ] + + +def _resume_reference_views( + corners_by_view: dict[str, tuple[tuple[float, float], ...]] | None = None, +) -> dict[str, SimpleNamespace]: + selected = corners_by_view or _FIXED_BASE_CORNERS_BY_VIEW + return { + view: SimpleNamespace(locked_base_corners_xy=corners) + for view, corners in selected.items() + } + + def test_resume_reuses_only_a_fully_committed_task_prefix() -> None: profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) first, second = profile.sweep_specs[:2] @@ -825,6 +882,40 @@ def test_resume_revalidates_retired_passive_dip_position_metrics() -> None: assert _unresolved_fit_failure_tasks(profile, rows) == set() +def test_resume_revalidates_thumb_ip_position_diagnostic() -> None: + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + task = next( + spec + for spec in profile.sweep_specs + if spec.key == "thumb_mcp_ip_front" + ) + rows = [ + { + "kind": "sample", + "task_name": task.key, + "attempt": 3, + }, + { + "kind": "fit_failure", + "task_name": task.key, + "view": task.view, + "motor_index": task.motor_index, + "joints": list(task.joints), + "attempt": 3, + "failures": [ + { + "joint": "thumb_ip", + "metric": "axis_pose_line_rms_mm", + "actual": 1.4, + "limit": 1.0, + } + ], + }, + ] + + assert _unresolved_fit_failure_tasks(profile, rows) == set() + + def test_resume_retires_validation_only_cross_view_curve_failure() -> None: profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) spec = next( @@ -1166,6 +1257,7 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw( "capabilities": sorted(profile.capabilities), "palm_axis_observers": _palm_axis_observer_schema(profile), }, + *_fixed_base_reference_rows(), *_complete_resume_task_rows(profile, profile.sweep_specs[0]), ] for row in rows: @@ -1208,6 +1300,9 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw( sweep_items=sweep_items, resumed_task_keys=(), resume_source_session="", + views=_resume_reference_views(), + fixed_base_maximum_corner_drift_px=2.0, + recalibration_scope="full", ) count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint( @@ -1228,6 +1323,103 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw( assert any(row["kind"] == "sample" for row in restored) +def test_full_resume_discards_all_old_tasks_after_start_position_change( + tmp_path: Path, monkeypatch +) -> None: + monkeypatch.setattr( + G20ThreeCameraCalibrationNode, + "_revalidate_imported_tasks", + lambda self, completed, **_kwargs: (tuple(completed), []), + ) + config = _config(tmp_path) + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + baseline = [255] * 20 + baseline[6:10] = [127] * 4 + source_raw = tmp_path / "old_position" / "raw_samples.jsonl" + source_raw.parent.mkdir() + rows = [ + { + "kind": "session_start", + "model": "G20", + "hand_type": "right", + "tag_layout": G20_RIGHT_19_LAYOUT, + "view_tags": { + view: dict(tags) for view, tags in profile.view_tags.items() + }, + "baseline_command_u8": baseline, + "source_urdf_sha256": config.source_urdf_sha256, + "capabilities": sorted(profile.capabilities), + "palm_axis_observers": _palm_axis_observer_schema(profile), + }, + *_fixed_base_reference_rows(), + *_complete_resume_task_rows(profile, profile.sweep_specs[0]), + ] + source_raw.write_text( + "".join(json.dumps(row) + "\n" for row in rows), + encoding="utf-8", + ) + current_raw = tmp_path / "new_position" / "raw_samples.jsonl" + current_raw.parent.mkdir() + current_raw.touch() + moved_corners = { + view: tuple((x + 18.0, y - 7.0) for x, y in corners) + for view, corners in _FIXED_BASE_CORNERS_BY_VIEW.items() + } + sweep_items = [ + SweepItem(spec, cycle, direction) + for spec in profile.sweep_specs + for cycle in range(4) + for direction in ("decreasing", "increasing") + ] + node = SimpleNamespace( + resume_raw_samples_path=source_raw, + raw_path=current_raw, + model="G20", + hand_type="right", + profile=profile, + baseline_command=tuple(baseline), + source_urdf_path=config.source_urdf, + repetitions=4, + minimum_sweep_bins=32, + 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}, + palm_axis_records_by_source={ + observer.source_name: [] + for observer in profile.palm_axis_observers + }, + sweep_index=0, + sweep_items=sweep_items, + resumed_task_keys=(), + resume_source_session="", + recalibration_scope="full", + recalibration_task_keys=(), + views=_resume_reference_views(moved_corners), + fixed_base_maximum_corner_drift_px=2.0, + ) + + count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint( + node + ) + + assert count == 0 + assert node.resumed_task_keys == () + assert node.sweep_index == 0 + assert node.resume_position_policy == ( + "discard_all_tasks_for_new_start_pose" + ) + assert node.resume_position_changed_views == ("front", "side", "top") + assert all(not values for values in node.records_by_joint.values()) + imported = json.loads(current_raw.read_text(encoding="utf-8").splitlines()[0]) + assert imported["start_position_policy"] == ( + "discard_all_tasks_for_new_start_pose" + ) + assert imported["start_position_invalidated_task_keys"] == [ + spec.key for spec in profile.sweep_specs + ] + assert imported["imported_record_count"] == 0 + + def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks( tmp_path: Path, monkeypatch ) -> None: @@ -1263,6 +1455,7 @@ def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks( "capabilities": sorted(profile.capabilities), "palm_axis_observers": _palm_axis_observer_schema(profile), }, + *_fixed_base_reference_rows(), *_complete_resume_task_rows(profile, thumb_spec), *_complete_resume_task_rows(profile, finger_spec), ] @@ -1301,6 +1494,8 @@ def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks( resume_source_session="", recalibration_scope="thumb", recalibration_task_keys=thumb_tasks, + views=_resume_reference_views(), + fixed_base_maximum_corner_drift_px=2.0, ) count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint(node) @@ -2071,6 +2266,32 @@ def test_progress_contains_stage_eta_tags_cameras_and_feedback() -> None: assert "首次拟合定位第 2 轮需复核，仅补采对应轮次双向" in text +def test_progress_reports_new_start_position_without_claiming_resume() -> None: + status = { + "state": "SWEEP", + "progress": 0.01, + "feedback_hz": 58.8, + "active": {}, + "views": {}, + "resume": { + "used": False, + "checkpoint_requested": True, + "checkpoint_pending": False, + "start_position_policy": ( + "discard_all_tasks_for_new_start_pose" + ), + }, + } + + text = render_progress_zh( + "G20_RIGHT_001", status, ProgressEstimator(started_at=0.0) + ) + + assert "标定前机械手位置已变化" in text + assert "未混用旧断点，本次整手从头采集" in text + assert "已恢复" not in text + + def test_progress_describes_nonlocalized_full_fit_retry() -> None: status = { "state": "SWEEP", @@ -2314,6 +2535,35 @@ def test_startup_state_machine_moves_before_applying_tag_gate() -> None: assert started == [True] +def test_resume_import_waits_until_new_fixed_base_reference_is_locked( + tmp_path: Path, +) -> None: + source = tmp_path / "raw_samples.jsonl" + source.write_text("{}\n", encoding="utf-8") + calls: list[str] = [] + node = SimpleNamespace( + state="PREFLIGHT", + reason="waiting_for_baseline_tags_after_recovery", + started=True, + startup_baseline_recovered=True, + resume_checkpoint_pending=True, + resume_raw_samples_path=source, + _all_preflight_ready=lambda now: True, + _lock_fixed_base_references=lambda: calls.append("lock") or True, + _publish_status=lambda now: calls.append("status"), + _restore_durable_task_checkpoint=( + lambda: calls.append("restore") or 2 + ), + _start_next_sweep=lambda: calls.append("start"), + ) + + G20ThreeCameraCalibrationNode._advance(node, 11.0) + + assert calls == ["lock", "status", "restore", "start"] + assert node.resume_checkpoint_pending is False + assert node.state == "IMPORTING_BASE" + + def test_device_preflight_progress_says_tags_are_checked_after_recovery() -> None: status = { "state": "PREFLIGHT", @@ -2481,6 +2731,7 @@ def test_node_drops_imported_task_failing_hard_gates( "capabilities": sorted(profile.capabilities), "palm_axis_observers": _palm_axis_observer_schema(profile), }, + *_fixed_base_reference_rows(), *_complete_resume_task_rows(profile, profile.sweep_specs[0]), ] source_raw.write_text( @@ -2518,6 +2769,9 @@ def test_node_drops_imported_task_failing_hard_gates( sweep_items=sweep_items, resumed_task_keys=(), resume_source_session="", + views=_resume_reference_views(), + fixed_base_maximum_corner_drift_px=2.0, + recalibration_scope="full", zero_profile=get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT), trajectory_maximum_plane_rms_m=0.004, trajectory_maximum_radial_rms_m=0.004, diff --git a/src/linkerhand_calibration/test/test_three_camera_retry.py b/src/linkerhand_calibration/test/test_three_camera_retry.py index 306b7e1..af421e9 100644 --- a/src/linkerhand_calibration/test/test_three_camera_retry.py +++ b/src/linkerhand_calibration/test/test_three_camera_retry.py @@ -48,6 +48,7 @@ from linkerhand_calibration.three_camera_node import ( _overall_progress, _preserve_pnp_task_reference_for_sweep, _previous_passed_joint_zero_offset, + _resume_fixed_base_position_compatibility, _palm_axis_observer_for_sweep, _requires_pnp_tracker_reset_for_sweep, _selected_pose_qualities, @@ -786,6 +787,37 @@ def test_fixed_base_corner_drift_uses_all_four_ordered_corners() -> None: assert _maximum_corner_drift_px(reference, moved) == pytest.approx(2.5) +def test_resume_start_position_requires_all_fixed_tags_to_match() -> None: + reference = ( + (10.0, 10.0), + (30.0, 10.0), + (30.0, 30.0), + (10.0, 30.0), + ) + rows = [ + { + "kind": "fixed_base_reference_locked", + "view": view, + "corner_reference_xy": reference, + } + for view in ("front", "side", "top") + ] + current = { + "front": np.asarray(reference) + [0.2, -0.1], + "side": np.asarray(reference) + [4.0, 0.0], + "top": None, + } + + drift, changed, unverifiable = ( + _resume_fixed_base_position_compatibility(rows, current, 2.0) + ) + + assert drift["front"] < 0.3 + assert drift["side"] == pytest.approx(4.0) + assert changed == ("side",) + assert unverifiable == ("top",) + + def test_side_only_finger_task_keeps_occluded_inactive_front_base_locked() -> None: spec = next( item @@ -4625,6 +4657,68 @@ def test_side_alias_skips_pose_line_rms_gate(tmp_path) -> None: ] +def test_right_19_thumb_ip_pose_line_is_position_diagnostic_only( + tmp_path, +) -> None: + records = _image_cycle_records([math.radians(47.0)] * 3) + spec = next( + item + for item in RIGHT_19_HAND_PROFILE.sweep_specs + if item.key == "thumb_mcp_ip_front" + ) + node = _fit_check_node( + {"thumb_mcp": records, "thumb_ip": records} + ) + node.profile = RIGHT_19_HAND_PROFILE + node.zero_profile = get_zero_calibration_profile( + "right", "g20_right_19" + ) + node.baseline_command = list(THREE_CAMERA_BASELINE_COMMAND) + node.baseline_records_by_joint = { + "thumb_mcp": records, + "thumb_ip": records, + } + node.raw_path = tmp_path / "raw_thumb_position.jsonl" + + def axis_measurement(name, cycle): + return SimpleNamespace( + axis_common_xyz=(0.0, 0.0, 1.0), + axis_direction_source="upstream_constraint", + radial_rms_m=0.0002, + pose_axis_line_rms_m=( + 0.0014 if name == "thumb_ip" else 0.0002 + ), + plane_rms_m=0.0002, + rotation_circle_axis_difference_rad=0.0, + ) + + node._fit_axis_measurement = axis_measurement + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec, include_view_validity=False + ) + + assert not [ + failure + for failure in failures + if failure["joint"] == "thumb_ip" + and failure["metric"] == "axis_pose_line_rms_mm" + ] + diagnostics = [ + json.loads(line) + for line in node.raw_path.read_text().splitlines() + ] + assert len(diagnostics) == 3 + assert all( + row["kind"] == "position_invariant_quality_diagnostic" + and row["joint"] == "thumb_ip" + and row["metric"] == "axis_pose_line_rms_mm" + and row["actual"] == 1.4 + and row["reference_limit"] == 1.0 + and row["decision"] == "diagnostic_only" + for row in diagnostics + ) + + def test_right_19_passive_dip_requires_axis_point_for_pip_zero_phase() -> None: records = _image_cycle_records([math.radians(47.0)] * 3) node = _fit_check_node(