admin/linkerhand_retarget_ros2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

标定代码结构修改

Browse Source
This commit is contained in:
lxp
2026-09-01 11:51:28 +08:00
parent 7f84225ba8
commit 1ed36ecdd8
10 changed files with 626 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py
+5 -1
View File
@@ -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",
src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py
+6
View File
@@ -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"
src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py
+6
View File
@@ -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)
src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py
+3
View File
@@ -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()
}
src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py
+224 -8
View File
@@ -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
src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py
+3 -2
View File
@@ -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()
)
src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py
+13 -1
View File
@@ -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"):
src/linkerhand_calibration/linkerhand_calibration/operator_report.py
+18 -7
View File
@@ -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)
src/linkerhand_calibration/test/test_g20_right_product.py
+254
View File
@@ -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,
src/linkerhand_calibration/test/test_three_camera_retry.py
+94
View File
@@ -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(