标定代码结构修改
This commit is contained in:
@@ -5,11 +5,15 @@ from .config_v1 import (
|
|||||||
product_profile_key,
|
product_profile_key,
|
||||||
resolve_legacy_profile_alias,
|
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
|
from .paths import resolve_renamed_package_path
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
"default_product_config_path",
|
"default_product_config_path",
|
||||||
|
"default_three_camera_config_path",
|
||||||
"legacy_default_profile_key",
|
"legacy_default_profile_key",
|
||||||
"product_profile_key",
|
"product_profile_key",
|
||||||
"resolve_legacy_profile_alias",
|
"resolve_legacy_profile_alias",
|
||||||
|
|||||||
@@ -8,3 +8,9 @@ from ament_index_python.packages import get_package_share_directory
|
|||||||
def default_product_config_path() -> Path:
|
def default_product_config_path() -> Path:
|
||||||
share = Path(get_package_share_directory("linkerhand_calibration"))
|
share = Path(get_package_share_directory("linkerhand_calibration"))
|
||||||
return share / "config/g20_right_product.yaml"
|
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"
|
||||||
|
|||||||
@@ -120,6 +120,12 @@ class MeasurementSpec:
|
|||||||
parent_role: str | None
|
parent_role: str | None
|
||||||
child_role: str | None
|
child_role: str | None
|
||||||
validation_source: str | None = 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)
|
@dataclass(frozen=True)
|
||||||
|
|||||||
@@ -98,6 +98,9 @@ def adapt_profile(
|
|||||||
validation_source=(hand_profile.axis_validation_sources or {}).get(
|
validation_source=(hand_profile.axis_validation_sources or {}).get(
|
||||||
name
|
name
|
||||||
),
|
),
|
||||||
|
pose_axis_line_required=bool(
|
||||||
|
spec.pose_axis_line_required
|
||||||
|
),
|
||||||
)
|
)
|
||||||
for name, spec in record_specs.items()
|
for name, spec in record_specs.items()
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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(
|
def _selected_pose_qualities(
|
||||||
selected: Mapping[str, SquareTagPose],
|
selected: Mapping[str, SquareTagPose],
|
||||||
live_qualities: Mapping[str, TagQuality],
|
live_qualities: Mapping[str, TagQuality],
|
||||||
@@ -1428,6 +1474,19 @@ def _unresolved_fit_failure_tasks(
|
|||||||
# failures that mixed velocity lag or firmware tracking
|
# failures that mixed velocity lag or firmware tracking
|
||||||
# deadband with mechanical hysteresis are safe to revalidate.
|
# deadband with mechanical hysteresis are safe to revalidate.
|
||||||
return True
|
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 (
|
if (
|
||||||
profile.layout_id == G20_RIGHT_19_LAYOUT
|
profile.layout_id == G20_RIGHT_19_LAYOUT
|
||||||
and joint_name
|
and joint_name
|
||||||
@@ -2774,6 +2833,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
self.resumed_task_keys: tuple[str, ...] = ()
|
self.resumed_task_keys: tuple[str, ...] = ()
|
||||||
self.resume_source_session = ""
|
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(
|
self.camera_extrinsics_file = Path(
|
||||||
str(value("camera_extrinsics_file"))
|
str(value("camera_extrinsics_file"))
|
||||||
).expanduser().resolve()
|
).expanduser().resolve()
|
||||||
@@ -5267,6 +5331,54 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
"resume checkpoint geometry, algorithm capabilities, Tag "
|
"resume checkpoint geometry, algorithm capabilities, Tag "
|
||||||
"layout, baseline or source URDF differs"
|
"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:
|
try:
|
||||||
changed_tag_size_ids, size_invalidated_tasks = (
|
changed_tag_size_ids, size_invalidated_tasks = (
|
||||||
resume_tasks_invalidated_by_tag_size_changes(
|
resume_tasks_invalidated_by_tag_size_changes(
|
||||||
@@ -5281,6 +5393,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
) from error
|
) from error
|
||||||
invalidated_task_set = set(size_invalidated_tasks)
|
invalidated_task_set = set(size_invalidated_tasks)
|
||||||
invalidated_task_set.update(palm_axis_invalidated_tasks)
|
invalidated_task_set.update(palm_axis_invalidated_tasks)
|
||||||
|
invalidated_task_set.update(start_position_invalidated_tasks)
|
||||||
scope_invalidated_tasks = tuple(
|
scope_invalidated_tasks = tuple(
|
||||||
getattr(self, "recalibration_task_keys", ()) or ()
|
getattr(self, "recalibration_task_keys", ()) or ()
|
||||||
)
|
)
|
||||||
@@ -5388,7 +5501,6 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
self.resumed_task_keys = completed
|
self.resumed_task_keys = completed
|
||||||
G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self)
|
G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self)
|
||||||
self.resume_source_session = source.parent.name
|
|
||||||
missing_tasks = [
|
missing_tasks = [
|
||||||
spec.key
|
spec.key
|
||||||
for spec in self.profile.sweep_specs
|
for spec in self.profile.sweep_specs
|
||||||
@@ -5418,6 +5530,20 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
"scope_invalidated_task_keys": list(
|
"scope_invalidated_task_keys": list(
|
||||||
scope_invalidated_tasks
|
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_record_count": len(reusable),
|
||||||
"imported_attempt_floor_by_task": attempt_floor_by_task,
|
"imported_attempt_floor_by_task": attempt_floor_by_task,
|
||||||
"source_raw_samples_sha256": _file_sha256(source),
|
"source_raw_samples_sha256": _file_sha256(source),
|
||||||
@@ -5571,6 +5697,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
return response
|
return response
|
||||||
self.started = True
|
self.started = True
|
||||||
self.startup_baseline_recovered = False
|
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.sweep_items = []
|
||||||
self.pnp_task_spec = None
|
self.pnp_task_spec = None
|
||||||
selected_sweep_specs = list(self.profile.sweep_specs)
|
selected_sweep_specs = list(self.profile.sweep_specs)
|
||||||
@@ -5738,10 +5877,16 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
try:
|
try:
|
||||||
if self.resume_raw_samples_path is not None:
|
if self.resume_raw_samples_path is not None:
|
||||||
self.state = STATE_IMPORTING_BASE
|
if not self.resume_raw_samples_path.is_file():
|
||||||
self.reason = "reading_base_session_records"
|
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 = {
|
self.base_import_progress = {
|
||||||
"phase": "reading",
|
"phase": "waiting_for_new_start_pose_reference",
|
||||||
"records_read": 0,
|
"records_read": 0,
|
||||||
"bytes_read": 0,
|
"bytes_read": 0,
|
||||||
"total_bytes": int(
|
"total_bytes": int(
|
||||||
@@ -5749,10 +5894,9 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
),
|
),
|
||||||
"fraction": 0.0,
|
"fraction": 0.0,
|
||||||
}
|
}
|
||||||
self._publish_status(time.monotonic())
|
|
||||||
restored_tasks = self._restore_durable_task_checkpoint()
|
|
||||||
except Exception as error:
|
except Exception as error:
|
||||||
self.started = False
|
self.started = False
|
||||||
|
self.resume_checkpoint_pending = False
|
||||||
response.success = False
|
response.success = False
|
||||||
response.message = f"CFG-RESUME-009:{error}"
|
response.message = f"CFG-RESUME-009:{error}"
|
||||||
return response
|
return response
|
||||||
@@ -5760,8 +5904,11 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
response.success = True
|
response.success = True
|
||||||
response.message = (
|
response.message = (
|
||||||
"three-camera calibration started"
|
"three-camera calibration started"
|
||||||
if restored_tasks == 0
|
if not self.resume_checkpoint_pending
|
||||||
else f"calibration resumed with {restored_tasks} completed tasks"
|
else (
|
||||||
|
"three-camera calibration started; checkpoint will be "
|
||||||
|
"verified after the new start-pose reference is locked"
|
||||||
|
)
|
||||||
)
|
)
|
||||||
return response
|
return response
|
||||||
|
|
||||||
@@ -8616,6 +8763,35 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
# error is an admissibility check for constrained circles.
|
# error is an admissibility check for constrained circles.
|
||||||
for metric, actual, limit in axis_checks:
|
for metric, actual, limit in axis_checks:
|
||||||
if actual > limit:
|
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
|
failure_joint = joint_name
|
||||||
quality_sources: tuple[str, ...] = ()
|
quality_sources: tuple[str, ...] = ()
|
||||||
if metric == "axis_pose_line_rms_mm":
|
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):
|
elif self.startup_baseline_recovered and self._all_preflight_ready(now):
|
||||||
locker = getattr(self, "_lock_fixed_base_references", None)
|
locker = getattr(self, "_lock_fixed_base_references", None)
|
||||||
if locker is None or locker():
|
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()
|
self._start_next_sweep()
|
||||||
else:
|
else:
|
||||||
self.reason = "locking_fixed_base_references"
|
self.reason = "locking_fixed_base_references"
|
||||||
@@ -12979,7 +13170,32 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
"camera_extrinsics_error": self.extrinsics_error,
|
"camera_extrinsics_error": self.extrinsics_error,
|
||||||
"resume": {
|
"resume": {
|
||||||
"used": bool(self.resumed_task_keys),
|
"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,
|
"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_scope": self.recalibration_scope,
|
||||||
"recalibration_task_keys": list(
|
"recalibration_task_keys": list(
|
||||||
self.recalibration_task_keys
|
self.recalibration_task_keys
|
||||||
|
|||||||
@@ -19,6 +19,7 @@ import numpy as np
|
|||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
import yaml
|
import yaml
|
||||||
|
|
||||||
|
from ...compat import default_three_camera_config_path
|
||||||
from ...compat.legacy import uses_coupled_full_hand_zero_solver
|
from ...compat.legacy import uses_coupled_full_hand_zero_solver
|
||||||
from ...extrinsics import load_three_camera_extrinsics
|
from ...extrinsics import load_three_camera_extrinsics
|
||||||
from .profile import (
|
from .profile import (
|
||||||
@@ -833,6 +834,7 @@ def _quality_failures(
|
|||||||
if (
|
if (
|
||||||
profile.record_specs[name].zero_kind
|
profile.record_specs[name].zero_kind
|
||||||
!= "axis_cross_view_validation"
|
!= "axis_cross_view_validation"
|
||||||
|
and profile.record_specs[name].pose_axis_line_required
|
||||||
and cross_view_side_line_source(axis) is None
|
and cross_view_side_line_source(axis) is None
|
||||||
and axis.pose_axis_line_rms_m
|
and axis.pose_axis_line_rms_m
|
||||||
> float(parameters["axis_maximum_pose_line_rms_m"])
|
> float(parameters["axis_maximum_pose_line_rms_m"])
|
||||||
@@ -1047,9 +1049,8 @@ def replay_session(
|
|||||||
output_tag: str | None = None,
|
output_tag: str | None = None,
|
||||||
) -> dict[str, Any]:
|
) -> dict[str, Any]:
|
||||||
session = Path(session_dir).expanduser().resolve()
|
session = Path(session_dir).expanduser().resolve()
|
||||||
package_root = Path(__file__).resolve().parents[3]
|
|
||||||
config = (
|
config = (
|
||||||
package_root / "config" / "three_camera_calibration.yaml"
|
default_three_camera_config_path()
|
||||||
if config_file is None
|
if config_file is None
|
||||||
else Path(config_file).expanduser().resolve()
|
else Path(config_file).expanduser().resolve()
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -51,6 +51,12 @@ class JointSpec:
|
|||||||
child_role: str | None
|
child_role: str | None
|
||||||
source_joint: str | None = None
|
source_joint: str | None = None
|
||||||
zero_kind: 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
|
@property
|
||||||
def measured(self) -> bool:
|
def measured(self) -> bool:
|
||||||
@@ -543,7 +549,13 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
|||||||
zero_kind="urdf_axis_chain",
|
zero_kind="urdf_axis_chain",
|
||||||
),
|
),
|
||||||
"thumb_ip": JointSpec(
|
"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"):
|
for finger in ("index", "middle", "ring", "pinky"):
|
||||||
|
|||||||
@@ -305,13 +305,24 @@ def render_progress_zh(
|
|||||||
f"{fit_attempt_limit - 1} 次异常轮补采)"
|
f"{fit_attempt_limit - 1} 次异常轮补采)"
|
||||||
)
|
)
|
||||||
resume = status.get("resume", {})
|
resume = status.get("resume", {})
|
||||||
if isinstance(resume, Mapping) and resume.get("used"):
|
if isinstance(resume, Mapping):
|
||||||
lines.append(
|
position_policy = str(resume.get("start_position_policy", ""))
|
||||||
"断点:已恢复 "
|
if resume.get("checkpoint_pending"):
|
||||||
f"{int(resume.get('completed_task_count', 0))}/"
|
lines.append(
|
||||||
f"{int(resume.get('total_task_count', 16))} 个完整任务;"
|
"断点:正在建立本次起始位置基准,完成后再决定是否复用旧任务"
|
||||||
"失败任务已丢弃并重新采集"
|
)
|
||||||
)
|
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)
|
return "\n".join(lines)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -196,6 +196,17 @@ def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None:
|
|||||||
config.calibration_contract.typed_profile.measurement
|
config.calibration_contract.typed_profile.measurement
|
||||||
.stable_cross_view_cone_bias
|
.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.serial_number == "G20_RIGHT_001"
|
||||||
assert config.namespace == "/g20_calibration"
|
assert config.namespace == "/g20_calibration"
|
||||||
assert config.required_independent_passes == 1
|
assert config.required_independent_passes == 1
|
||||||
@@ -310,6 +321,52 @@ def _complete_resume_task_rows(profile, spec) -> list[dict]:
|
|||||||
return rows
|
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:
|
def test_resume_reuses_only_a_fully_committed_task_prefix() -> None:
|
||||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||||
first, second = profile.sweep_specs[:2]
|
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()
|
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:
|
def test_resume_retires_validation_only_cross_view_curve_failure() -> None:
|
||||||
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT)
|
||||||
spec = next(
|
spec = next(
|
||||||
@@ -1166,6 +1257,7 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
|||||||
"capabilities": sorted(profile.capabilities),
|
"capabilities": sorted(profile.capabilities),
|
||||||
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
||||||
},
|
},
|
||||||
|
*_fixed_base_reference_rows(),
|
||||||
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
|
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
|
||||||
]
|
]
|
||||||
for row in rows:
|
for row in rows:
|
||||||
@@ -1208,6 +1300,9 @@ def test_node_restores_complete_prefix_into_new_self_contained_raw(
|
|||||||
sweep_items=sweep_items,
|
sweep_items=sweep_items,
|
||||||
resumed_task_keys=(),
|
resumed_task_keys=(),
|
||||||
resume_source_session="",
|
resume_source_session="",
|
||||||
|
views=_resume_reference_views(),
|
||||||
|
fixed_base_maximum_corner_drift_px=2.0,
|
||||||
|
recalibration_scope="full",
|
||||||
)
|
)
|
||||||
|
|
||||||
count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint(
|
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)
|
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(
|
def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks(
|
||||||
tmp_path: Path, monkeypatch
|
tmp_path: Path, monkeypatch
|
||||||
) -> None:
|
) -> None:
|
||||||
@@ -1263,6 +1455,7 @@ def test_thumb_recalibration_imports_fingers_but_invalidates_all_thumb_tasks(
|
|||||||
"capabilities": sorted(profile.capabilities),
|
"capabilities": sorted(profile.capabilities),
|
||||||
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
||||||
},
|
},
|
||||||
|
*_fixed_base_reference_rows(),
|
||||||
*_complete_resume_task_rows(profile, thumb_spec),
|
*_complete_resume_task_rows(profile, thumb_spec),
|
||||||
*_complete_resume_task_rows(profile, finger_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="",
|
resume_source_session="",
|
||||||
recalibration_scope="thumb",
|
recalibration_scope="thumb",
|
||||||
recalibration_task_keys=thumb_tasks,
|
recalibration_task_keys=thumb_tasks,
|
||||||
|
views=_resume_reference_views(),
|
||||||
|
fixed_base_maximum_corner_drift_px=2.0,
|
||||||
)
|
)
|
||||||
|
|
||||||
count = G20ThreeCameraCalibrationNode._restore_durable_task_checkpoint(node)
|
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
|
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:
|
def test_progress_describes_nonlocalized_full_fit_retry() -> None:
|
||||||
status = {
|
status = {
|
||||||
"state": "SWEEP",
|
"state": "SWEEP",
|
||||||
@@ -2314,6 +2535,35 @@ def test_startup_state_machine_moves_before_applying_tag_gate() -> None:
|
|||||||
assert started == [True]
|
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:
|
def test_device_preflight_progress_says_tags_are_checked_after_recovery() -> None:
|
||||||
status = {
|
status = {
|
||||||
"state": "PREFLIGHT",
|
"state": "PREFLIGHT",
|
||||||
@@ -2481,6 +2731,7 @@ def test_node_drops_imported_task_failing_hard_gates(
|
|||||||
"capabilities": sorted(profile.capabilities),
|
"capabilities": sorted(profile.capabilities),
|
||||||
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
"palm_axis_observers": _palm_axis_observer_schema(profile),
|
||||||
},
|
},
|
||||||
|
*_fixed_base_reference_rows(),
|
||||||
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
|
*_complete_resume_task_rows(profile, profile.sweep_specs[0]),
|
||||||
]
|
]
|
||||||
source_raw.write_text(
|
source_raw.write_text(
|
||||||
@@ -2518,6 +2769,9 @@ def test_node_drops_imported_task_failing_hard_gates(
|
|||||||
sweep_items=sweep_items,
|
sweep_items=sweep_items,
|
||||||
resumed_task_keys=(),
|
resumed_task_keys=(),
|
||||||
resume_source_session="",
|
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),
|
zero_profile=get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||||
trajectory_maximum_plane_rms_m=0.004,
|
trajectory_maximum_plane_rms_m=0.004,
|
||||||
trajectory_maximum_radial_rms_m=0.004,
|
trajectory_maximum_radial_rms_m=0.004,
|
||||||
|
|||||||
@@ -48,6 +48,7 @@ from linkerhand_calibration.three_camera_node import (
|
|||||||
_overall_progress,
|
_overall_progress,
|
||||||
_preserve_pnp_task_reference_for_sweep,
|
_preserve_pnp_task_reference_for_sweep,
|
||||||
_previous_passed_joint_zero_offset,
|
_previous_passed_joint_zero_offset,
|
||||||
|
_resume_fixed_base_position_compatibility,
|
||||||
_palm_axis_observer_for_sweep,
|
_palm_axis_observer_for_sweep,
|
||||||
_requires_pnp_tracker_reset_for_sweep,
|
_requires_pnp_tracker_reset_for_sweep,
|
||||||
_selected_pose_qualities,
|
_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)
|
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:
|
def test_side_only_finger_task_keeps_occluded_inactive_front_base_locked() -> None:
|
||||||
spec = next(
|
spec = next(
|
||||||
item
|
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:
|
def test_right_19_passive_dip_requires_axis_point_for_pip_zero_phase() -> None:
|
||||||
records = _image_cycle_records([math.radians(47.0)] * 3)
|
records = _image_cycle_records([math.radians(47.0)] * 3)
|
||||||
node = _fit_check_node(
|
node = _fit_check_node(
|
||||||
|
|||||||
Reference in New Issue
Block a user