From 7f84225ba85c921b4db8298c678d6ab2743b4cd2 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Mon, 31 Aug 2026 19:22:51 +0800 Subject: [PATCH] refactor: dispatch formal calibration through model profiles --- src/linkerhand_calibration/README.md | 10 + .../launch/three_camera_calibration.launch.py | 6 +- .../linkerhand_calibration/compat/__init__.py | 8 +- .../compat/config_v1.py | 17 + .../compat/legacy/__init__.py | 3 + .../compat/legacy/session_v1.py | 24 + .../core/domain/profile.py | 7 + .../core/geometry/__init__.py | 16 + .../core/geometry/extrinsics.py | 212 + .../linkerhand_calibration/extrinsics.py | 203 +- .../linkerhand_calibration/full_hand.py | 2242 +-- .../models/g20/_adapter.py | 18 +- .../models/g20/artifacts.py | 3 + .../models/g20/golden_regression.py | 144 + .../models/g20/legacy_11.py | 2 +- .../models/g20/motion.py | 2 +- .../linkerhand_calibration/models/g20/node.py | 13060 +++++++++++++++ .../models/g20/offline_replay.py | 2032 +++ .../models/g20/profile.py | 2241 +++ .../models/g20/publication.py | 914 ++ .../models/g20/right_19.py | 2 +- .../models/g20/runner.py | 2 +- .../models/g20/zero_policy.py | 4 +- .../models/g20/zero_solver.py | 4635 ++++++ .../linkerhand_calibration/models/registry.py | 1 + .../linkerhand_calibration/offline_replay.py | 1942 +-- .../linkerhand_calibration/product.py | 47 +- .../linkerhand_calibration/publication.py | 915 +- .../runtime/nodes/__init__.py | 2 +- .../runtime/nodes/calibration.py | 23 + .../three_camera_node.py | 13061 +--------------- .../linkerhand_calibration/urdf_zero.py | 4636 +----- src/linkerhand_calibration/setup.py | 8 +- .../test/test_architecture.py | 88 + .../test/test_g20_right_product.py | 31 +- .../test/test_golden_regression.py | 72 + .../test/test_offline_replay.py | 17 + .../test/test_package_rename.py | 14 +- 38 files changed, 23710 insertions(+), 22954 deletions(-) create mode 100644 src/linkerhand_calibration/linkerhand_calibration/compat/legacy/session_v1.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/geometry/extrinsics.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/publication.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/calibration.py create mode 100644 src/linkerhand_calibration/test/test_golden_regression.py diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index 75ce488..f51fc83 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -34,6 +34,16 @@ Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改；v `profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明，通用层 不要求 `front/side/top`，也不假设固定 20 个命令。 +仓库中存在已审定硬件会话时，可执行只读金标准检查（不会覆盖任何产物）： + +```bash +ros2 run linkerhand_calibration validate_g20_goldens \ + calibration_output/G20_RIGHT_001 +``` + +它严格核对完整整手、合并拇指、独立拇指、拟合失败和零位失败五个会话；完整 +整手还会重新离线求解并要求 JSON、URDF 的 SHA-256 与正式产物一致。 + 完全独立地只标定大拇指4项任务时，使用： ```bash diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index 3d53e8a..06c0831 100644 --- a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py @@ -88,7 +88,7 @@ def _launch_stack(context): expected_source_hash = LaunchConfiguration( "source_urdf_expected_sha256" ).perform(context).strip().lower() - if contract.profile.supports("urdf_zero_publication"): + if contract.typed_profile.artifacts.publish_corrected_urdf: if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None: raise RuntimeError( "G20 right product layout requires source_urdf_expected_sha256 confirmed " @@ -276,6 +276,10 @@ def _launch_stack(context): name="g20_calibration", output="screen", emulate_tty=True, + arguments=[ + "--profile-id", + contract.typed_profile.key.profile_id, + ], parameters=[ LaunchConfiguration("calibration_config"), { diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py index 1065fa8..74b7c19 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py @@ -1,11 +1,17 @@ """Compatibility adapters for one-release calibration migrations.""" -from .config_v1 import product_profile_key +from .config_v1 import ( + legacy_default_profile_key, + product_profile_key, + resolve_legacy_profile_alias, +) from .defaults import default_product_config_path from .paths import resolve_renamed_package_path __all__ = [ "default_product_config_path", + "legacy_default_profile_key", "product_profile_key", + "resolve_legacy_profile_alias", "resolve_renamed_package_path", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py b/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py index 4de28f9..a54cffc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py @@ -28,3 +28,20 @@ def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey: if not layout and (model, side) == ("G20", "right"): layout = "g20_right_19" return ProfileKey(model, side, layout, 1) + + +def legacy_default_profile_key() -> ProfileKey: + """Preserve the former no-argument executable for one release.""" + return ProfileKey("G20", "right", "g20_right_19", 1) + + +def resolve_legacy_profile_alias(key: ProfileKey) -> ProfileKey: + """Map retired layout identifiers to their reviewed physical profile.""" + if ( + key.model == "G20" + and key.side == "right" + and key.layout == "g20_right_15" + and key.revision == 1 + ): + return ProfileKey("G20", "right", "g20_right_19", 1) + return key diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py index 75ffb11..eab30d7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py @@ -1 +1,4 @@ """Legacy single-camera algorithms retained for one compatibility release.""" +from .session_v1 import uses_coupled_full_hand_zero_solver + +__all__ = ["uses_coupled_full_hand_zero_solver"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/session_v1.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/session_v1.py new file mode 100644 index 0000000..daa5103 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/session_v1.py @@ -0,0 +1,24 @@ +"""Version selection for replaying durable pre-v3 hardware sessions.""" + +from __future__ import annotations + +from typing import Any, Mapping + + +def uses_coupled_full_hand_zero_solver( + session_start: Mapping[str, Any], +) -> bool: + """Return the solver contract recorded by the legacy session header. + + Capabilities are not consulted by the live runtime. This adapter reads + the durable v1 header only so offline replay can reproduce an artifact + created before the independent thumb solver was introduced. + """ + capabilities = { + str(value) for value in session_start.get("capabilities", ()) + } + return ( + int(session_start.get("sample_schema_version", 1)) == 1 + and "palm_axis_side_channel_v2" in capabilities + and "palm_axis_relative_motion_v3" not in capabilities + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py index de37899..9ed7a59 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -23,6 +23,8 @@ class ProfileKey: object.__setattr__(self, "revision", int(self.revision)) if not self.model or not self.side or not self.layout: raise ValueError("profile identity fields must be non-empty") + if self.side not in {"left", "right"}: + raise ValueError("profile side must be left or right") if self.revision < 1: raise ValueError("profile revision must be positive") @@ -74,6 +76,10 @@ class VisionRigSpec: views: tuple[ViewSpec, ...] common_frame: str extrinsic_reference_view: str + extrinsics_quality_limits: Mapping[str, float] = field( + default_factory=dict + ) + minimum_capture_counts: Mapping[str, int] = field(default_factory=dict) @property def view_names(self) -> tuple[str, ...]: @@ -168,6 +174,7 @@ class ArtifactPolicy: protected_input_fields: frozenset[str] publication_pointer: str = "latest_passed" session_compatibility_tokens: frozenset[str] = frozenset() + publish_corrected_urdf: bool = False @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py index 6691cb1..2bc55d4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py @@ -1,5 +1,14 @@ """Pure geometry used by online and offline calibration.""" +from .extrinsics import ( + CameraCalibrationIdentity, + CameraExtrinsics, + camera_info_fingerprint, + load_camera_extrinsics, + matrix_payload, + transform_matrix, + validate_camera_extrinsics_payload, +) from .rotation import ( delta_rotation_vector, fit_rotation_axis, @@ -13,13 +22,20 @@ from .rotation import ( ) __all__ = [ + "CameraCalibrationIdentity", + "CameraExtrinsics", + "camera_info_fingerprint", "delta_rotation_vector", "fit_rotation_axis", "image_plane_tag_quaternion_xyzw", + "load_camera_extrinsics", + "matrix_payload", "normalize_quaternion_xyzw", "relative_quaternion_xyzw", "robust_rotation_summary", "rotation_inlier_fraction", "rotation_rms_rad", "rotation_spread_rad", + "transform_matrix", + "validate_camera_extrinsics_payload", ] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/extrinsics.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/extrinsics.py new file mode 100644 index 0000000..6825976 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/extrinsics.py @@ -0,0 +1,212 @@ +"""Camera-extrinsic data model shared by calibration and runtime nodes.""" + +from __future__ import annotations + +import hashlib +import json +from dataclasses import dataclass +from pathlib import Path +from typing import Any, Mapping, Sequence + +import numpy as np +import yaml +from scipy.spatial.transform import Rotation + + +def camera_info_fingerprint( + *, + width: int, + height: int, + camera_matrix: Sequence[Sequence[float]] | Sequence[float], + distortion: Sequence[float] = (), + rectification: Sequence[float] = (), + projection: Sequence[float] = (), +) -> str: + """Return a stable fingerprint for rectified image geometry.""" + matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3) + payload = { + "width": int(width), + "height": int(height), + "camera_matrix": [round(float(value), 12) for value in matrix.flat], + "distortion": [round(float(value), 12) for value in distortion], + "rectification": [round(float(value), 12) for value in rectification], + "projection": [round(float(value), 12) for value in projection], + } + encoded = json.dumps( + payload, sort_keys=True, separators=(",", ":") + ).encode("utf-8") + return hashlib.sha256(encoded).hexdigest() + + +def transform_matrix( + translation_xyz_m: Sequence[float], + quaternion_xyzw: Sequence[float], +) -> np.ndarray: + translation = np.asarray(translation_xyz_m, dtype=float) + quaternion = np.asarray(quaternion_xyzw, dtype=float) + if translation.shape != (3,) or not np.all(np.isfinite(translation)): + raise ValueError("translation_xyz_m must contain three finite values") + if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)): + raise ValueError("quaternion_xyzw must contain four finite values") + norm = float(np.linalg.norm(quaternion)) + if norm < 1.0e-12: + raise ValueError("quaternion_xyzw has zero norm") + result = np.eye(4, dtype=float) + result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix() + result[:3, 3] = translation + return result + + +def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]: + value = np.asarray(matrix, dtype=float) + if value.shape != (4, 4) or not np.all(np.isfinite(value)): + raise ValueError("transform must be a finite 4x4 matrix") + return { + "translation_xyz_m": [float(item) for item in value[:3, 3]], + "quaternion_xyzw": [ + float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat() + ], + } + + +@dataclass(frozen=True) +class CameraCalibrationIdentity: + serial_number: str + width: int + height: int + intrinsics_sha256: str + + +@dataclass(frozen=True) +class CameraExtrinsics: + """Transforms every declared camera into one Profile-selected reference.""" + + cameras: Mapping[str, CameraCalibrationIdentity] + reference_view: str + reference_from_view: Mapping[str, np.ndarray] + quality: Mapping[str, float] + + def transform(self, view: str) -> np.ndarray: + if view not in self.reference_from_view: + raise KeyError(f"extrinsics do not contain view {view}") + return np.asarray(self.reference_from_view[view], dtype=float).copy() + + +def validate_camera_extrinsics_payload( + payload: Mapping[str, Any], + *, + required_views: Sequence[str], + reference_view: str, + quality_limits: Mapping[str, float] | None = None, + minimum_capture_counts: Mapping[str, int] | None = None, +) -> None: + if int(payload.get("schema_version", -1)) != 1: + raise ValueError("camera extrinsics schema_version must be 1") + reference = str(reference_view) + if payload.get("reference_view") != reference: + raise ValueError( + "camera extrinsics reference_view differs from the Profile" + ) + cameras = payload.get("cameras") + transforms = payload.get(f"{reference}_from_view") + quality = payload.get("quality") + views = tuple(str(view) for view in required_views) + if not views or len(set(views)) != len(views): + raise ValueError("required extrinsic views must be non-empty and unique") + if reference not in views: + raise ValueError("extrinsic reference view is not required") + if not isinstance(cameras, Mapping) or set(cameras) != set(views): + raise ValueError("camera extrinsics differ from the Profile views") + if not isinstance(transforms, Mapping) or set(transforms) != set(views): + raise ValueError("camera transforms differ from the Profile views") + if not isinstance(quality, Mapping) or not bool(quality.get("passed")): + raise ValueError("camera extrinsics quality is not passed") + for key, limit in dict(quality_limits or {}).items(): + value = float(quality.get(key, float("inf"))) + if not np.isfinite(value) or value > limit: + raise ValueError( + f"camera extrinsics {key}={value} exceeds {limit}" + ) + for key, minimum in dict(minimum_capture_counts or {}).items(): + if int(quality.get(key, 0)) < int(minimum): + raise ValueError( + f"camera extrinsics {key} must be at least {minimum}" + ) + for view in views: + identity = cameras[view] + if not isinstance(identity, Mapping): + raise ValueError(f"{view} camera identity must be an object") + if not str(identity.get("serial_number", "")): + raise ValueError(f"{view} camera serial_number is missing") + if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0: + raise ValueError(f"{view} camera image dimensions are invalid") + fingerprint = str(identity.get("intrinsics_sha256", "")) + if len(fingerprint) != 64: + raise ValueError(f"{view} camera intrinsics fingerprint is invalid") + transform = transforms[view] + if not isinstance(transform, Mapping): + raise ValueError(f"{view} transform must be an object") + matrix = transform_matrix( + transform.get("translation_xyz_m", ()), + transform.get("quaternion_xyzw", ()), + ) + if view == reference and not np.allclose( + matrix, np.eye(4), atol=1.0e-9 + ): + raise ValueError("reference-view transform must be identity") + serials = [str(cameras[view]["serial_number"]) for view in views] + if len(set(serials)) != len(views): + raise ValueError("camera extrinsics serial numbers must be unique") + + +def load_camera_extrinsics( + path: str | Path, + *, + required_views: Sequence[str], + reference_view: str, + quality_limits: Mapping[str, float] | None = None, + minimum_capture_counts: Mapping[str, int] | None = None, +) -> CameraExtrinsics: + source = Path(path).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"camera extrinsics file does not exist: {source}") + with source.open("r", encoding="utf-8") as stream: + payload = yaml.safe_load(stream) + if not isinstance(payload, Mapping): + raise ValueError("camera extrinsics file must contain an object") + validate_camera_extrinsics_payload( + payload, + required_views=required_views, + reference_view=reference_view, + quality_limits=quality_limits, + minimum_capture_counts=minimum_capture_counts, + ) + views = tuple(str(view) for view in required_views) + transform_key = f"{reference_view}_from_view" + cameras = { + view: CameraCalibrationIdentity( + serial_number=str(payload["cameras"][view]["serial_number"]), + width=int(payload["cameras"][view]["width"]), + height=int(payload["cameras"][view]["height"]), + intrinsics_sha256=str( + payload["cameras"][view]["intrinsics_sha256"] + ), + ) + for view in views + } + transforms = { + view: transform_matrix( + payload[transform_key][view]["translation_xyz_m"], + payload[transform_key][view]["quaternion_xyzw"], + ) + for view in views + } + return CameraExtrinsics( + cameras=cameras, + reference_view=str(reference_view), + reference_from_view=transforms, + quality={ + str(key): float(value) if isinstance(value, (int, float)) else value + for key, value in payload["quality"].items() + }, + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/extrinsics.py b/src/linkerhand_calibration/linkerhand_calibration/extrinsics.py index 3cea37b..7003789 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/extrinsics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/extrinsics.py @@ -1,83 +1,35 @@ -"""Camera-extrinsic data model shared by calibration and runtime nodes.""" +"""Three-view compatibility policy over generic camera extrinsics.""" from __future__ import annotations -import hashlib -import json from dataclasses import dataclass from pathlib import Path from typing import Any, Mapping, Sequence import numpy as np import yaml -from scipy.spatial.transform import Rotation + +from .core.geometry.extrinsics import ( + CameraCalibrationIdentity, + CameraExtrinsics, + camera_info_fingerprint, + load_camera_extrinsics, + matrix_payload, + transform_matrix, + validate_camera_extrinsics_payload, +) VIEWS: tuple[str, ...] = ("front", "side", "top") - - -def camera_info_fingerprint( - *, - width: int, - height: int, - camera_matrix: Sequence[Sequence[float]] | Sequence[float], - distortion: Sequence[float] = (), - rectification: Sequence[float] = (), - projection: Sequence[float] = (), -) -> str: - """Return a stable fingerprint for rectified image geometry.""" - matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3) - payload = { - "width": int(width), - "height": int(height), - "camera_matrix": [round(float(value), 12) for value in matrix.flat], - "distortion": [round(float(value), 12) for value in distortion], - "rectification": [round(float(value), 12) for value in rectification], - "projection": [round(float(value), 12) for value in projection], - } - encoded = json.dumps( - payload, sort_keys=True, separators=(",", ":") - ).encode("utf-8") - return hashlib.sha256(encoded).hexdigest() - - -def transform_matrix( - translation_xyz_m: Sequence[float], - quaternion_xyzw: Sequence[float], -) -> np.ndarray: - translation = np.asarray(translation_xyz_m, dtype=float) - quaternion = np.asarray(quaternion_xyzw, dtype=float) - if translation.shape != (3,) or not np.all(np.isfinite(translation)): - raise ValueError("translation_xyz_m must contain three finite values") - if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)): - raise ValueError("quaternion_xyzw must contain four finite values") - norm = float(np.linalg.norm(quaternion)) - if norm < 1.0e-12: - raise ValueError("quaternion_xyzw has zero norm") - result = np.eye(4, dtype=float) - result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix() - result[:3, 3] = translation - return result - - -def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]: - value = np.asarray(matrix, dtype=float) - if value.shape != (4, 4) or not np.all(np.isfinite(value)): - raise ValueError("transform must be a finite 4x4 matrix") - return { - "translation_xyz_m": [float(item) for item in value[:3, 3]], - "quaternion_xyzw": [ - float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat() - ], - } - - -@dataclass(frozen=True) -class CameraCalibrationIdentity: - serial_number: str - width: int - height: int - intrinsics_sha256: str +_QUALITY_LIMITS = { + "reprojection_rms_px": 1.2, + "maximum_rotation_repeatability_deg": 0.3, + "maximum_translation_repeatability_m": 0.0015, +} +_MINIMUM_CAPTURE_COUNTS = { + "front_side_captures": 15, + "front_top_captures": 15, +} @dataclass(frozen=True) @@ -113,92 +65,27 @@ class ThreeCameraExtrinsics: def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None: - if int(payload.get("schema_version", -1)) != 1: - raise ValueError("camera extrinsics schema_version must be 1") - if payload.get("reference_view") != "front": - raise ValueError("camera extrinsics reference_view must be front") - cameras = payload.get("cameras") - transforms = payload.get("front_from_view") - quality = payload.get("quality") - if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS): - raise ValueError("camera extrinsics must contain front/side/top cameras") - if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS): - raise ValueError("camera extrinsics must contain all three transforms") - if not isinstance(quality, Mapping) or not bool(quality.get("passed")): - raise ValueError("camera extrinsics quality is not passed") - quality_limits = { - "reprojection_rms_px": 1.2, - "maximum_rotation_repeatability_deg": 0.3, - "maximum_translation_repeatability_m": 0.0015, - } - for key, limit in quality_limits.items(): - value = float(quality.get(key, float("inf"))) - if not np.isfinite(value) or value > limit: - raise ValueError( - f"camera extrinsics {key}={value} exceeds {limit}" - ) - for key in ("front_side_captures", "front_top_captures"): - if int(quality.get(key, 0)) < 15: - raise ValueError(f"camera extrinsics {key} must be at least 15") - for view in VIEWS: - identity = cameras[view] - if not isinstance(identity, Mapping): - raise ValueError(f"{view} camera identity must be an object") - if not str(identity.get("serial_number", "")): - raise ValueError(f"{view} camera serial_number is missing") - if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0: - raise ValueError(f"{view} camera image dimensions are invalid") - fingerprint = str(identity.get("intrinsics_sha256", "")) - if len(fingerprint) != 64: - raise ValueError(f"{view} camera intrinsics fingerprint is invalid") - transform = transforms[view] - if not isinstance(transform, Mapping): - raise ValueError(f"{view} transform must be an object") - matrix = transform_matrix( - transform.get("translation_xyz_m", ()), - transform.get("quaternion_xyzw", ()), - ) - if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9): - raise ValueError("front_from_view.front must be identity") - serials = [str(cameras[view]["serial_number"]) for view in VIEWS] - if len(set(serials)) != len(VIEWS): - raise ValueError("camera extrinsics serial numbers must be unique") + validate_camera_extrinsics_payload( + payload, + required_views=VIEWS, + reference_view="front", + quality_limits=_QUALITY_LIMITS, + minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS, + ) def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics: - source = Path(path).expanduser().resolve() - if not source.is_file(): - raise ValueError(f"camera extrinsics file does not exist: {source}") - with source.open("r", encoding="utf-8") as stream: - payload = yaml.safe_load(stream) - if not isinstance(payload, Mapping): - raise ValueError("camera extrinsics file must contain an object") - validate_extrinsics_payload(payload) - cameras = { - view: CameraCalibrationIdentity( - serial_number=str(payload["cameras"][view]["serial_number"]), - width=int(payload["cameras"][view]["width"]), - height=int(payload["cameras"][view]["height"]), - intrinsics_sha256=str( - payload["cameras"][view]["intrinsics_sha256"] - ), - ) - for view in VIEWS - } - transforms = { - view: transform_matrix( - payload["front_from_view"][view]["translation_xyz_m"], - payload["front_from_view"][view]["quaternion_xyzw"], - ) - for view in VIEWS - } + generic = load_camera_extrinsics( + path, + required_views=VIEWS, + reference_view="front", + quality_limits=_QUALITY_LIMITS, + minimum_capture_counts=_MINIMUM_CAPTURE_COUNTS, + ) return ThreeCameraExtrinsics( - cameras=cameras, - front_from_view=transforms, - quality={ - str(key): float(value) if isinstance(value, (int, float)) else value - for key, value in payload["quality"].items() - }, + cameras=generic.cameras, + front_from_view=generic.reference_from_view, + quality=generic.quality, ) @@ -225,3 +112,19 @@ def dump_three_camera_extrinsics( with temporary.open("w", encoding="utf-8") as stream: yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False) temporary.replace(destination) + + +__all__ = [ + "CameraCalibrationIdentity", + "CameraExtrinsics", + "ThreeCameraExtrinsics", + "VIEWS", + "camera_info_fingerprint", + "dump_three_camera_extrinsics", + "load_camera_extrinsics", + "load_three_camera_extrinsics", + "matrix_payload", + "transform_matrix", + "validate_camera_extrinsics_payload", + "validate_extrinsics_payload", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/full_hand.py b/src/linkerhand_calibration/linkerhand_calibration/full_hand.py index b808e7a..71a0664 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/full_hand.py +++ b/src/linkerhand_calibration/linkerhand_calibration/full_hand.py @@ -1,2241 +1,7 @@ -"""Pure three-camera G20 calibration model and compact runtime schema. +"""One-release module alias for the relocated model profile implementation.""" -The hardware node records one parent/child AprilTag trajectory for each -directly observable joint. This module deliberately contains no ROS imports: -curve fitting, baseline centring, inheritance, and schema validation -remain deterministic and unit-testable without connected cameras or a hand. -""" +import sys -from __future__ import annotations +from .models.g20 import profile as _implementation -from dataclasses import dataclass, replace -import math -from pathlib import Path -import re -from typing import Any, Iterable, Mapping, Sequence -import xml.etree.ElementTree as ET - -import numpy as np - -from .models.g20.command_layout import G20_COMMAND_NAMES as COMMAND_NAMES -from .trajectory import ( - _angle_for_circle, - _fit_circle_with_axis, - _fit_joint_curve, - _fit_plane_axis, - _orient_circle_positive, -) -from .zero_calibration import ( - _fit_circle, - _trajectory_arc_rad, - circular_median_rad, - maximum_pairwise_angle_difference_rad, - wrap_angle_rad, -) - - -THREE_CAMERA_BASELINE_COMMAND: tuple[int, ...] = ( - 255, 255, 255, 255, 255, 255, - 127, 127, 127, 127, - 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, -) - - -@dataclass(frozen=True) -class JointSpec: - name: str - motor_index: int - active: bool - view: str | None - parent_role: str | None - child_role: str | None - source_joint: str | None = None - zero_kind: str | None = None - - @property - def measured(self) -> bool: - return self.source_joint is None - - -@dataclass(frozen=True) -class SweepSpec: - view: str - motor_index: int - joints: tuple[str, ...] - task_name: str = "" - auxiliary_commands: tuple[tuple[int, int], ...] = () - validation_only: bool = False - - @property - def key(self) -> str: - if self.task_name: - return self.task_name - return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}" - - -@dataclass(frozen=True) -class PalmAxisObserver: - """Non-blocking direction observation attached to an existing task. - - This is deliberately not a ``JointSpec``: it has no curve, endpoint or - retry semantics. The camera callback records it only while both Tags are - visible during the named sweep. - """ - - source_name: str - task_name: str - view: str - parent_role: str - child_role: str - model_joint: str - motor_index: int - - -@dataclass(frozen=True) -class JointCurveFit: - angle_rad: tuple[float, ...] - decreasing_rad: tuple[float, ...] - increasing_rad: tuple[float, ...] - circle: Mapping[str, Any] - maximum_monotonic_correction_rad: float - maximum_hysteresis_rad: float - quality: Mapping[str, float] - zero_offset_rad: float = 0.0 - - -@dataclass(frozen=True) -class HandCalibrationProfile: - side: str - reference_finger: str - view_tags: Mapping[str, Mapping[str, int]] - preflight_view_roles: Mapping[str, tuple[str, ...]] - joint_specs: Mapping[str, JointSpec] - sweep_specs: tuple[SweepSpec, ...] - image_trajectory_joints: frozenset[str] - roll_clearance_commands: Mapping[int, int] - thumb_pitch_clearance_commands: Mapping[int, int] - layout_id: str = "legacy_11" - measurement_specs: Mapping[str, JointSpec] | None = None - axis_validation_sources: Mapping[str, str] | None = None - palm_axis_observers: tuple[PalmAxisObserver, ...] = () - minimum_palm_orientation_sources: int = 0 - palm_orientation_minimum_arc_rad: float = math.radians(15.0) - palm_orientation_maximum_command_distance_u8: int = 255 - # Hardware and algorithm differences belong to the product profile, not - # to the acquisition state machine. Keeping these fields here lets a new - # model (or the mirrored hand) reuse the same recorder/fitter by supplying - # a different declarative profile. - model: str = "G20" - command_names: tuple[str, ...] = COMMAND_NAMES - baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND - capabilities: frozenset[str] = frozenset() - precheck_sweeps: bool = False - steady_command_checkpoints: bool = False - directional_zero: bool = False - isolated_holdout: bool = False - cross_view_roll_curve: bool = False - stable_cross_view_cone_bias: bool = False - - @property - def command_count(self) -> int: - return len(self.command_names) - - def supports(self, capability: str) -> bool: - return str(capability) in self.capabilities - - @property - def measured_joints(self) -> tuple[str, ...]: - return tuple( - name for name, spec in self.joint_specs.items() if spec.measured - ) - - @property - def record_specs(self) -> Mapping[str, JointSpec]: - return self.joint_specs if self.measurement_specs is None else self.measurement_specs - - @property - def record_joints(self) -> tuple[str, ...]: - return tuple( - name for name, spec in self.record_specs.items() if spec.measured - ) - - @property - def palm_orientation_sources(self) -> Mapping[str, str]: - return { - observer.source_name: observer.model_joint - for observer in self.palm_axis_observers - } - - @property - def palm_axis_motor_by_source(self) -> Mapping[str, int]: - return { - observer.source_name: int(observer.motor_index) - for observer in self.palm_axis_observers - } - - @property - def active_joints(self) -> tuple[str, ...]: - return tuple( - name for name, spec in self.joint_specs.items() if spec.active - ) - - @property - def passive_joints(self) -> tuple[str, ...]: - return tuple( - name for name, spec in self.joint_specs.items() if not spec.active - ) - - @property - def reference_roll_motor(self) -> int: - return int(self.joint_specs[f"{self.reference_finger}_mcp_roll"].motor_index) - - @property - def reference_pitch_motor(self) -> int: - return int(self.joint_specs[f"{self.reference_finger}_mcp_pitch"].motor_index) - - @property - def reference_pip_motor(self) -> int: - return int(self.joint_specs[f"{self.reference_finger}_pip"].motor_index) - - @property - def reference_speed_slot(self) -> int: - return {"index": 1, "middle": 2, "ring": 3, "pinky": 4}[ - self.reference_finger - ] - - -_FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky") - -# The side camera looks almost along these physical flexion axes. The child -# Tag centre therefore traces a high-resolution image-plane circle against the -# fixed palm Tag, while monocular planar-PnP tilt is not accurate enough to be -# the dynamic angle source. Passive DIP is intentionally excluded because -# its parent Tag moves with PIP; a raw image-centre difference would mix both -# joint angles. -RIGHT_19_END_ON_IMAGE_CURVE_JOINTS = frozenset( - { - *(f"{finger}_mcp_pitch" for finger in _FINGERS), - *(f"{finger}_pip" for finger in _FINGERS), - } -) -# These joints have no independent actuator. Their distal Tags directly -# measure the passive dynamic curve, while their fitted axes serve only as -# downstream phase observers for the corresponding active PIP. -RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS = frozenset( - f"{finger}_dip" for finger in _FINGERS -) -_MOTOR_BY_JOINT: dict[str, int] = { - **{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)}, - **{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)}, - **{f"{finger}_pip": index + 16 for index, finger in enumerate(_FINGERS)}, -} - - -def _build_hand_profile(side: str) -> HandCalibrationProfile: - hand_side = str(side).lower() - if hand_side not in {"left", "right"}: - raise ValueError("hand side must be left or right") - reference = "index" if hand_side == "left" else "pinky" - roll_role = f"{reference}_roll" - mcp_role = f"{reference}_mcp" - pip_role = f"{reference}_pip" - dip_role = f"{reference}_dip" - view_tags: dict[str, dict[str, int]] = { - "front": { - "front_base": 0, - "thumb_cmc": 1, - "thumb_mcp": 2, - "thumb_ip": 3, - roll_role: 10, - }, - "side": { - "side_base": 4, - mcp_role: 5, - pip_role: 6, - dip_role: 7, - }, - "top": {"top_base": 8, "thumb_yaw": 9}, - } - preflight_view_roles = { - view: tuple(tags) for view, tags in view_tags.items() - } - joint_specs: dict[str, JointSpec] = {} - # Both hands use the front base/Tag 1 pair for motor 0. Keeping the - # complete thumb root chain in one calibrated view avoids introducing a - # cross-camera axis-line phase into the URDF zero solve. - thumb_pitch_view = "front" - thumb_pitch_parent = "front_base" - joint_specs["thumb_cmc_pitch"] = JointSpec( - "thumb_cmc_pitch", - 0, - True, - thumb_pitch_view, - thumb_pitch_parent, - "thumb_cmc", - zero_kind="urdf_axis_chain", - ) - for finger in _FINGERS: - name = f"{finger}_mcp_pitch" - source = None if finger == reference else f"{reference}_mcp_pitch" - joint_specs[name] = JointSpec( - name, - _MOTOR_BY_JOINT[name], - True, - "side" if source is None else None, - "side_base" if source is None else None, - mcp_role if source is None else None, - source_joint=source, - zero_kind="urdf_axis_chain" if source is None else "inherited", - ) - joint_specs["thumb_cmc_roll"] = JointSpec( - "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc", - zero_kind="urdf_axis_chain", - ) - for finger in _FINGERS: - name = f"{finger}_mcp_roll" - source = None if finger == reference else f"{reference}_mcp_roll" - joint_specs[name] = JointSpec( - name, - _MOTOR_BY_JOINT[name], - True, - "front" if source is None else None, - "front_base" if source is None else None, - roll_role if source is None else None, - source_joint=source, - zero_kind="urdf_axis_chain" if source is None else "inherited", - ) - joint_specs["thumb_cmc_yaw"] = JointSpec( - "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw", - zero_kind="urdf_axis_chain", - ) - joint_specs["thumb_mcp"] = JointSpec( - "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp", - zero_kind="urdf_axis_chain", - ) - for finger in _FINGERS: - name = f"{finger}_pip" - source = None if finger == reference else f"{reference}_pip" - joint_specs[name] = JointSpec( - name, - _MOTOR_BY_JOINT[name], - True, - "side" if source is None else None, - mcp_role if source is None else None, - pip_role if source is None else None, - source_joint=source, - zero_kind="urdf_axis_chain" if source is None else "inherited", - ) - joint_specs["thumb_ip"] = JointSpec( - "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip", - ) - for finger in _FINGERS: - name = f"{finger}_dip" - source = None if finger == reference else f"{reference}_dip" - joint_specs[name] = JointSpec( - name, - _MOTOR_BY_JOINT[f"{finger}_pip"], - False, - "side" if source is None else None, - pip_role if source is None else None, - dip_role if source is None else None, - source_joint=source, - ) - sweep_specs = ( - SweepSpec(thumb_pitch_view, 0, ("thumb_cmc_pitch",)), - SweepSpec("front", 5, ("thumb_cmc_roll",)), - SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")), - SweepSpec("front", _MOTOR_BY_JOINT[f"{reference}_mcp_roll"], ( - f"{reference}_mcp_roll", - )), - SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_mcp_pitch"], ( - f"{reference}_mcp_pitch", - )), - SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_pip"], ( - f"{reference}_pip", f"{reference}_dip", - )), - SweepSpec("top", 10, ("thumb_cmc_yaw",)), - ) - image_joints = frozenset( - { - "thumb_cmc_pitch", - "thumb_cmc_roll", - "thumb_mcp", - "thumb_ip", - f"{reference}_mcp_roll", - f"{reference}_mcp_pitch", - f"{reference}_pip", - } - ) - measured_roll = _MOTOR_BY_JOINT[f"{reference}_mcp_roll"] - # The roll channels are mirrored mechanically between hands. Command 0 - # moves the three non-reference fingers to the camera-right clearance pose - # on the left hand, while the same physical pose is command 255 on the - # right hand. Using 0 for both sides made the right index/middle/ring - # fingers lean left and could obstruct or load the pinky roll sweep. - roll_clearance_command = 0 if hand_side == "left" else 255 - clearance = { - _MOTOR_BY_JOINT[f"{finger}_mcp_roll"]: roll_clearance_command - for finger in _FINGERS - if finger != reference - } - assert measured_roll not in clearance - # Motor 0 is observed from the front with both other thumb-root channels - # fully open. Store the commands explicitly for the right hand so a - # caller-provided baseline cannot silently restore the obsolete 50/120 - # pose and hide Tag 1 from the front camera. - thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {} - return HandCalibrationProfile( - side=hand_side, - reference_finger=reference, - view_tags=view_tags, - preflight_view_roles=preflight_view_roles, - joint_specs=joint_specs, - sweep_specs=sweep_specs, - image_trajectory_joints=image_joints, - roll_clearance_commands=clearance, - thumb_pitch_clearance_commands=thumb_pitch_clearance, - ) - - -LEFT_HAND_PROFILE = _build_hand_profile("left") -RIGHT_HAND_PROFILE = _build_hand_profile("right") - - -# ``g20_right_15`` remains accepted as a read/CLI compatibility alias, but a -# newly recorded product session always persists the physical 19-Tag layout -# identifier. Together with the exact ``view_tags`` resume check this keeps a -# former 15-Tag checkpoint from being imported after the four distal Tags are -# installed again. -G20_RIGHT_15_LAYOUT = "g20_right_15" -G20_RIGHT_19_LAYOUT = "g20_right_19" - -# The three CMC axes keep the calibration domain that was proven on this hand -# in a609d521: dense relative-rotation sweeps with an isolated-cycle holdout. -# Monocular Tag translation is useful as a diagnostic, but is not an -# independent quality measurement for these short, strongly coupled links. -G20_REFERENCE_THUMB_CMC_JOINTS: frozenset[str] = frozenset( - {"thumb_cmc_pitch", "thumb_cmc_roll", "thumb_cmc_yaw"} -) - -G20_COMBINATION_REQUIRED_TARGET_KEYS: tuple[str, ...] = tuple( - sorted( - { - "front:index_mcp_roll", - "front:middle_mcp_roll", - "front:ring_mcp_roll", - "front:pinky_mcp_roll", - "side:index_pip", - "side:middle_pip", - "side:ring_pip", - "side:pinky_pip", - } - ) -) - -MIMIC_DERIVED_FINGER_DIPS: Mapping[str, str] = { - f"{finger}_dip": f"{finger}_pip" for finger in _FINGERS -} - - -def canonical_zero_direction( - profile: HandCalibrationProfile, joint_name: str -) -> str | None: - """Return the repeatable approach direction used as physical zero. - - A mid-range encoder value does not uniquely determine a linkage pose when - the transmission has backlash. The right-hand product four-finger roll profile - therefore defines command 127 after a 255->127 approach as the sole - physical zero. The opposite branch remains offset in the published - direction-aware curve instead of being silently re-zeroed. - """ - name = str(joint_name) - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return None - canonical = { - *(f"{finger}_mcp_roll" for finger in _FINGERS), - *(f"{finger}_mcp_roll_side" for finger in _FINGERS), - } - return "decreasing" if name in canonical else None - - -def _right_19_auxiliary_commands( - finger: str, *, side_view: bool, target_kind: str -) -> tuple[tuple[int, int], ...]: - """Return the reviewed right-hand fan/occlusion parking pose.""" - order = ("pinky", "ring", "middle", "index") - target_index = order.index(finger) - commands: dict[int, int] = {} - for other_index, other in enumerate(order): - if other == finger: - continue - roll_motor = _MOTOR_BY_JOINT[f"{other}_mcp_roll"] - if other_index < target_index: - commands[roll_motor] = 127 if side_view else 0 - else: - commands[roll_motor] = 255 - if side_view and other_index < target_index: - commands[_MOTOR_BY_JOINT[f"{other}_mcp_pitch"]] = 0 - commands[_MOTOR_BY_JOINT[f"{other}_pip"]] = 0 - if target_kind in {"pitch", "pip"}: - commands[_MOTOR_BY_JOINT[f"{finger}_mcp_roll"]] = 127 - if target_kind == "roll": - commands[_MOTOR_BY_JOINT[f"{finger}_mcp_pitch"]] = 255 - commands[_MOTOR_BY_JOINT[f"{finger}_pip"]] = 255 - elif target_kind == "pitch": - commands[_MOTOR_BY_JOINT[f"{finger}_pip"]] = 255 - elif target_kind == "pip": - commands[_MOTOR_BY_JOINT[f"{finger}_mcp_pitch"]] = 255 - return tuple(sorted((int(index), int(value)) for index, value in commands.items())) - - -def _build_right_19_profile() -> HandCalibrationProfile: - """Build the 19-Tag/16-motion-task right-hand profile.""" - view_tags: dict[str, dict[str, int]] = { - "front": { - "front_base": 0, - "thumb_cmc": 1, - "thumb_mcp": 2, - "thumb_ip": 3, - "pinky_roll": 10, - "ring_roll": 11, - "middle_roll": 12, - "index_roll": 13, - }, - "side": { - "side_base": 4, - "ring_pip": 5, - "pinky_pip": 6, - "pinky_dip": 7, - "ring_dip": 14, - "middle_pip": 15, - "middle_dip": 16, - "index_pip": 17, - "index_dip": 18, - }, - "top": {"top_base": 8, "thumb_yaw": 9}, - } - # Four adjacent fingers inevitably occlude some Tags in the - # common open-hand pose. Startup therefore proves only the three fixed - # palm references. Every moving Tag is still mandatory, but is checked - # by its task-local visibility sweep after the other fingers have moved to - # that task's clearance pose. Requiring all 15 at one instant would make - # a physically correct 19-Tag installation impossible to start. - preflight_view_roles = { - "front": ("front_base",), - "side": ("side_base",), - "top": ("top_base",), - } - joint_specs: dict[str, JointSpec] = { - "thumb_cmc_pitch": JointSpec( - "thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc", - zero_kind="urdf_axis_chain", - ), - "thumb_cmc_roll": JointSpec( - "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc", - zero_kind="urdf_axis_chain", - ), - "thumb_cmc_yaw": JointSpec( - "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw", - zero_kind="urdf_axis_chain", - ), - "thumb_mcp": JointSpec( - "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp", - zero_kind="urdf_axis_chain", - ), - "thumb_ip": JointSpec( - "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip", - ), - } - for finger in ("index", "middle", "ring", "pinky"): - roll = f"{finger}_mcp_roll" - pitch = f"{finger}_mcp_pitch" - pip = f"{finger}_pip" - dip = f"{finger}_dip" - joint_specs[roll] = JointSpec( - roll, _MOTOR_BY_JOINT[roll], True, "front", "front_base", - f"{finger}_roll", zero_kind="urdf_axis_chain", - ) - # The side PIP Tag is on the middle phalanx after PIP. With PIP held - # at 255 it observes MCP pitch; with MCP pitch held at 255 it observes - # PIP itself. - joint_specs[pitch] = JointSpec( - pitch, _MOTOR_BY_JOINT[pitch], True, "side", "side_base", - f"{finger}_pip", zero_kind="urdf_axis_chain", - ) - joint_specs[pip] = JointSpec( - pip, _MOTOR_BY_JOINT[pip], True, "side", "side_base", - f"{finger}_pip", zero_kind="urdf_axis_chain", - ) - # DIP is passive, but no longer unobserved. The PIP sweep moves the - # complete linkage once while the middle/distal Tag pair directly - # measures DIP. Its dynamic curve is therefore visual truth; its - # absolute static URDF zero deliberately remains source CAD because an - # arbitrary distal-Tag mounting angle is indistinguishable from a DIP - # zero offset without a mechanical zero fixture. - joint_specs[dip] = JointSpec( - dip, - _MOTOR_BY_JOINT[pip], - False, - "side", - f"{finger}_pip", - f"{finger}_dip", - ) - - measurement_specs = dict(joint_specs) - axis_validation_sources: dict[str, str] = {} - for finger in ("pinky", "ring", "middle", "index"): - canonical = f"{finger}_mcp_roll" - alias = f"{canonical}_side" - measurement_specs[alias] = JointSpec( - alias, - _MOTOR_BY_JOINT[canonical], - True, - "side", - "side_base", - f"{finger}_pip", - zero_kind="axis_cross_view_validation", - ) - axis_validation_sources[canonical] = alias - - sweeps: list[SweepSpec] = [ - SweepSpec( - "front", 0, ("thumb_cmc_pitch",), "thumb_cmc_pitch_front", - ((5, 255), (10, 255)), - ), - SweepSpec("front", 5, ("thumb_cmc_roll",), "thumb_cmc_roll_front"), - SweepSpec( - "front", 15, ("thumb_mcp", "thumb_ip"), "thumb_mcp_ip_front" - ), - SweepSpec( - "top", 10, ("thumb_cmc_yaw",), "thumb_cmc_yaw_top", ((5, 145),) - ), - ] - for finger in ("pinky", "ring", "middle", "index"): - roll_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_roll"] - pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] - pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] - sweeps.extend( - ( - SweepSpec( - "front", - roll_motor, - ( - f"{finger}_mcp_roll", - f"{finger}_mcp_roll_side", - ), - f"{finger}_roll_multiview", - _right_19_auxiliary_commands( - finger, side_view=True, target_kind="roll" - ), - ), - SweepSpec( - "side", - pitch_motor, - (f"{finger}_mcp_pitch",), - f"{finger}_pitch_side", - _right_19_auxiliary_commands( - finger, side_view=True, target_kind="pitch" - ), - ), - SweepSpec( - "side", - pip_motor, - (f"{finger}_pip", f"{finger}_dip"), - f"{finger}_pip_side", - _right_19_auxiliary_commands( - finger, side_view=True, target_kind="pip" - ), - ), - ) - ) - image_joints = frozenset( - { - "thumb_cmc_pitch", - "thumb_cmc_roll", - "thumb_mcp", - "thumb_ip", - *(f"{finger}_mcp_roll" for finger in ("index", "middle", "ring", "pinky")), - } - ) - return HandCalibrationProfile( - side="right", - reference_finger="pinky", - view_tags=view_tags, - preflight_view_roles=preflight_view_roles, - joint_specs=joint_specs, - sweep_specs=tuple(sweeps), - image_trajectory_joints=image_joints, - roll_clearance_commands={}, - thumb_pitch_clearance_commands={10: 255, 5: 255}, - layout_id=G20_RIGHT_19_LAYOUT, - measurement_specs=measurement_specs, - axis_validation_sources=axis_validation_sources, - # Observe the zero-adjacent roll and pitch axes from Tag 9 while the - # existing front sweeps run. Their same-view angle identifies the - # intervening yaw zero without adding a motion task or reference URDF. - palm_axis_observers=( - PalmAxisObserver( - source_name="thumb_cmc_pitch_top_axis", - task_name="thumb_cmc_pitch_front", - view="top", - parent_role="top_base", - child_role="thumb_yaw", - model_joint="thumb_cmc_pitch", - motor_index=joint_specs["thumb_cmc_pitch"].motor_index, - ), - PalmAxisObserver( - source_name="thumb_cmc_roll_top_axis", - task_name="thumb_cmc_roll_front", - view="top", - parent_role="top_base", - child_role="thumb_yaw", - model_joint="thumb_cmc_roll", - motor_index=joint_specs["thumb_cmc_roll"].motor_index, - ), - ), - minimum_palm_orientation_sources=2, - palm_orientation_minimum_arc_rad=math.radians(10.0), - palm_orientation_maximum_command_distance_u8=64, - capabilities=frozenset( - { - "precheck_sweeps", - "steady_command_checkpoints", - "directional_zero", - "isolated_holdout", - "cross_view_roll_curve", - "stable_cross_view_cone_bias", - "measured_passive_dips", - "urdf_zero_publication", - "palm_axis_relative_motion_v3", - } - ), - precheck_sweeps=True, - steady_command_checkpoints=True, - directional_zero=True, - isolated_holdout=True, - cross_view_roll_curve=True, - stable_cross_view_cone_bias=True, - ) - - -RIGHT_19_HAND_PROFILE = _build_right_19_profile() - - -def get_hand_calibration_profile( - side: str, layout_id: str = "legacy_11" -) -> HandCalibrationProfile: - value = str(side).lower() - layout = str(layout_id).lower() - if layout in {G20_RIGHT_19_LAYOUT, G20_RIGHT_15_LAYOUT}: - if value != "right": - raise ValueError("G20 right product layout requires hand side right") - return RIGHT_19_HAND_PROFILE - if layout != "legacy_11": - raise ValueError(f"unknown G20 tag layout: {layout_id}") - if value == "left": - return LEFT_HAND_PROFILE - if value == "right": - return RIGHT_HAND_PROFILE - raise ValueError("hand side must be left or right") - - -def derive_mimic_passive_fits( - source_urdf: str | Path, - fits: Mapping[str, JointCurveFit], - *, - profile: HandCalibrationProfile, -) -> dict[str, JointCurveFit]: - """Complete legacy non-visual DIP curves using the URDF mimic contract. - - The runtime curves are relative to the baseline command, so a URDF mimic - offset cancels while the multiplier scales the measured PIP motion. The - URDF itself remains byte-for-byte protected by publication checks. - """ - result = dict(fits) - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return result - visually_measured = all( - profile.joint_specs[name].source_joint is None - for name in MIMIC_DERIVED_FINGER_DIPS - ) - if visually_measured: - missing = sorted(set(profile.joint_specs) - set(result)) - extra = sorted(set(result) - set(profile.joint_specs)) - if missing or extra: - raise ValueError( - f"runtime fit set is incomplete: missing={missing} extra={extra}" - ) - return result - path = Path(source_urdf).expanduser().resolve() - if not path.is_file(): - raise ValueError(f"source URDF does not exist: {path}") - joints = { - str(node.get("name")): node - for node in ET.parse(path).getroot().findall("joint") - } - for passive_name, source_name in MIMIC_DERIVED_FINGER_DIPS.items(): - spec = profile.joint_specs[passive_name] - if spec.source_joint != source_name: - raise ValueError(f"{passive_name} has the wrong curve source") - if source_name not in result: - raise ValueError(f"missing measured source curve {source_name}") - joint = joints.get(passive_name) - mimic = None if joint is None else joint.find("mimic") - if mimic is None or mimic.get("joint") != source_name: - raise ValueError( - f"source URDF {passive_name} must mimic {source_name}" - ) - multiplier = float(mimic.get("multiplier", "1")) - offset = float(mimic.get("offset", "0")) - if not math.isfinite(multiplier) or multiplier <= 0.0: - raise ValueError(f"source URDF {passive_name} mimic multiplier is invalid") - if not math.isfinite(offset): - raise ValueError(f"source URDF {passive_name} mimic offset is invalid") - source = result[source_name] - - def scaled(values: Sequence[float]) -> tuple[float, ...]: - return tuple(multiplier * float(value) for value in values) - - result[passive_name] = JointCurveFit( - angle_rad=scaled(source.angle_rad), - decreasing_rad=scaled(source.decreasing_rad), - increasing_rad=scaled(source.increasing_rad), - circle={ - "curve_source": source_name, - "source_urdf_mimic_multiplier": multiplier, - "source_urdf_mimic_offset_rad": offset, - "visual_measurement": False, - }, - maximum_monotonic_correction_rad=( - abs(multiplier) * source.maximum_monotonic_correction_rad - ), - maximum_hysteresis_rad=( - abs(multiplier) * source.maximum_hysteresis_rad - ), - quality={ - "mimic_multiplier": multiplier, - "mimic_offset_rad": offset, - "visual_measurement": 0.0, - }, - zero_offset_rad=multiplier * source.zero_offset_rad, - ) - if set(result) != set(profile.joint_specs): - missing = sorted(set(profile.joint_specs) - set(result)) - extra = sorted(set(result) - set(profile.joint_specs)) - raise ValueError( - f"runtime fit set is incomplete: missing={missing} extra={extra}" - ) - return result - - -def clamp_runtime_fits_to_urdf_limits( - source_urdf: str | Path, - fits: Mapping[str, JointCurveFit], - *, - endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, -) -> dict[str, JointCurveFit]: - """Saturate published curves at protected physical limits. - - Raw trajectory fits and all fit/zero diagnostics remain untouched. The - returned transfer functions are the commands a URDF/MuJoCo runtime can - actually represent safely. An endpoint-anchored joint uses the corrected - coordinate upper limit ``CAD_upper - origin_offset``; its physical maximum - remains exactly the original CAD maximum after the origin correction. - """ - path = Path(source_urdf).expanduser().resolve() - joints = { - str(node.get("name")): node - for node in ET.parse(path).getroot().findall("joint") - } - result: dict[str, JointCurveFit] = {} - endpoint_offsets = { - str(name): float(value) - for name, value in dict(endpoint_anchored_offsets_rad or {}).items() - } - for name, fit in fits.items(): - joint = joints.get(name) - limit = None if joint is None else joint.find("limit") - if ( - limit is None - or limit.get("lower") is None - or limit.get("upper") is None - ): - raise ValueError(f"source URDF joint {name} has no finite position limit") - lower = float(limit.get("lower")) - upper = float(limit.get("upper")) - if name in endpoint_offsets: - upper -= endpoint_offsets[name] - if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: - raise ValueError(f"source URDF joint {name} has invalid position limits") - - clipped_counts: dict[str, int] = {} - - def clipped(values: Sequence[float], branch: str) -> tuple[float, ...]: - source = np.asarray(values, dtype=float) - bounded = np.clip(source, lower, upper) - clipped_counts[branch] = int(np.count_nonzero(bounded != source)) - return tuple(float(value) for value in bounded) - - circle = dict(fit.circle) - bounded_angle = clipped(fit.angle_rad, "angle_rad") - bounded_decreasing = clipped(fit.decreasing_rad, "decreasing_rad") - bounded_increasing = clipped(fit.increasing_rad, "increasing_rad") - if any(clipped_counts.values()): - circle["runtime_urdf_limit_clipped_bins"] = clipped_counts - circle["runtime_urdf_limits_rad"] = [lower, upper] - result[name] = replace( - fit, - angle_rad=bounded_angle, - decreasing_rad=bounded_decreasing, - increasing_rad=bounded_increasing, - circle=circle, - ) - return result - - -# Backwards-compatible aliases keep the existing left-hand API stable. -VIEW_TAGS = LEFT_HAND_PROFILE.view_tags -JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs -SWEEP_SPECS = LEFT_HAND_PROFILE.sweep_specs -MEASURED_JOINTS = LEFT_HAND_PROFILE.measured_joints -ACTIVE_JOINTS = LEFT_HAND_PROFILE.active_joints -PASSIVE_JOINTS = LEFT_HAND_PROFILE.passive_joints -SPLAY_JOINTS: tuple[str, ...] = ( - "index_mcp_roll", - "middle_mcp_roll", - "ring_mcp_roll", - "pinky_mcp_roll", -) - -# These directly actuated motions have a fixed parent link during their sweep -# and a motion plane that is close to the active camera's image plane. Their -# projected circles are substantially more repeatable than the difference of -# two independently estimated planar-Tag PnP depths. thumb_ip remains projected -# because its front-view PnP depth is not repeatable enough for a 3-D fit. The -# side-view index_dip and oblique top-view yaw remain parent-relative full -# SE(3) measurements; their circle directions are constrained by the more -# repeatable relative-orientation screw axis in urdf_zero.py. -IMAGE_TRAJECTORY_JOINTS = LEFT_HAND_PROFILE.image_trajectory_joints - -# Keep the three unmeasured finger-roll motors away from the front camera's -# line of sight while index_mcp_roll is measured. -INDEX_ROLL_CLEARANCE_COMMANDS = LEFT_HAND_PROFILE.roll_clearance_commands - -# Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is -# measured. This keeps the moving top-view tag sufficiently front-facing. -THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = { - 5: 145, -} - - -def calibration_auxiliary_commands( - spec: SweepSpec, - *, - profile: HandCalibrationProfile = LEFT_HAND_PROFILE, -) -> dict[int, int]: - """Return motors that must remain fixed throughout one calibration task.""" - if spec.auxiliary_commands: - return {int(index): int(value) for index, value in spec.auxiliary_commands} - if spec.motor_index == 0: - return dict(profile.thumb_pitch_clearance_commands) - if spec.motor_index == profile.reference_roll_motor: - return dict(profile.roll_clearance_commands) - if spec.motor_index == 10: - return dict(THUMB_YAW_CLEARANCE_COMMANDS) - return {} - - -def build_full_hand_command( - motor_index: int, - command_u8: int, - baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, - profile: HandCalibrationProfile = LEFT_HAND_PROFILE, -) -> list[int]: - if len(baseline) != 20: - raise ValueError("baseline must contain exactly 20 values") - motor = int(motor_index) - if motor not in {spec.motor_index for spec in profile.joint_specs.values()}: - raise ValueError("motor_index is not a controlled G20 calibration channel") - command = int(command_u8) - if not 0 <= command <= 255: - raise ValueError("command_u8 must be in [0, 255]") - result = [int(value) for value in baseline] - if any(not 0 <= value <= 255 for value in result): - raise ValueError("baseline values must be in [0, 255]") - result[motor] = command - return result - - -def build_calibration_motion_command( - spec: SweepSpec, - command_u8: int, - baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, - profile: HandCalibrationProfile = LEFT_HAND_PROFILE, -) -> list[int]: - """Build a sweep command, including any required clearance pose.""" - result = build_full_hand_command( - spec.motor_index, - command_u8, - baseline=baseline, - profile=profile, - ) - for motor_index, auxiliary_command in calibration_auxiliary_commands( - spec, profile=profile - ).items(): - result[motor_index] = auxiliary_command - return result - - -def _append_single_motor_waypoint( - waypoints: list[tuple[int, ...]], current: list[int], motor: int, value: int -) -> None: - target = int(value) - if current[int(motor)] == target: - return - current[int(motor)] = target - waypoints.append(tuple(current)) - - -def build_calibration_preparation_waypoints( - spec: SweepSpec, - command_u8: int, - *, - current_command: Sequence[int], - baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, - profile: HandCalibrationProfile = LEFT_HAND_PROFILE, - parallel: bool = True, -) -> tuple[tuple[int, ...], ...]: - """Enter a task pose in the reviewed safe order. - - ``parallel`` merges the sequential entry into joint-class groups: all - neighbouring roll axes open together, then the clearance fingers flex - their PIPs together, then their MCP pitches, then any remaining auxiliary - motors, and the measured roll channel moves last. Every roll still opens - before any finger flexes, each finger's PIP still flexes before its - pitch, and the roll targets are unchanged, so no axis is forced across - its coupled mechanical boundary. - """ - if len(current_command) != 20: - raise ValueError("current_command must contain exactly 20 values") - final = build_calibration_motion_command( - spec, command_u8, baseline=baseline, profile=profile - ) - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return (tuple(final),) - current = [int(np.clip(round(float(value)), 0, 255)) for value in current_command] - controlled_motors = { - int(joint.motor_index) for joint in profile.joint_specs.values() - } - # G20 feedback channels 11..14 are reserved and remain zero in the SDK. - # Preserve their reported values in transition vectors; waiting for the - # nominal 255 baseline on a non-existent actuator would deadlock before the - # first real motor is allowed to move. - for motor in range(20): - if motor not in controlled_motors: - final[motor] = current[motor] - auxiliary = calibration_auxiliary_commands(spec, profile=profile) - finger_order = ("pinky", "ring", "middle", "index") - handled: set[int] = set() - capture_views = { - profile.record_specs[joint_name].view for joint_name in spec.joints - } - target_roll_motor: int | None = None - lower_roll_motors: list[int] = [] - if "side" in capture_views: - target_finger = str(spec.joints[0]).split("_", 1)[0] - target_roll_motor = _MOTOR_BY_JOINT[f"{target_finger}_mcp_roll"] - lower_roll_motors = list(range(9, target_roll_motor, -1)) - if parallel: - roll_group: list[int] = [] - pip_group: list[int] = [] - pitch_group: list[int] = [] - if target_roll_motor is not None: - for motor in range(6, target_roll_motor): - if auxiliary.get(motor) == 255: - roll_group.append(motor) - handled.add(motor) - for roll_motor in lower_roll_motors: - roll_group.append(roll_motor) - handled.add(roll_motor) - finger = finger_order[9 - roll_motor] - pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] - pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] - if auxiliary.get(pip_motor) == 0: - pip_group.append(pip_motor) - handled.add(pip_motor) - if auxiliary.get(pitch_motor) == 0: - pitch_group.append(pitch_motor) - handled.add(pitch_motor) - other_group = [ - motor - for motor in sorted(controlled_motors | set(auxiliary)) - if motor not in handled - and motor != spec.motor_index - and current[motor] != final[motor] - ] - waypoints: list[tuple[int, ...]] = [] - _append_class_waypoint(waypoints, current, final, roll_group) - _append_class_waypoint(waypoints, current, final, pip_group) - _append_class_waypoint(waypoints, current, final, pitch_group) - _append_class_waypoint(waypoints, current, final, other_group) - # The measured channel moves last, after the swept volume is clear. - _append_single_motor_waypoint( - waypoints, current, spec.motor_index, final[spec.motor_index] - ) - return tuple(waypoints or [tuple(final)]) - waypoints = [] - if "side" in capture_views: - # Side-view clearance never forces a neighbouring roll axis across its - # coupled mechanical boundary. Real sessions showed neighbouring - # roll axes saturating at feedback 64 (motor 9) and 58 (motor 8) when - # commanded to 0 in a coupled avoidance pose. Keep those roll axes at - # neutral 127 and remove occlusion only by flexing the neighbour. - for motor in range(6, target_roll_motor): - if auxiliary.get(motor) == 255: - _append_single_motor_waypoint( - waypoints, current, motor, final[motor] - ) - handled.add(motor) - for roll_motor in lower_roll_motors: - if roll_motor in auxiliary: - _append_single_motor_waypoint( - waypoints, current, roll_motor, final[roll_motor] - ) - handled.add(roll_motor) - finger = finger_order[9 - roll_motor] - pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] - pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] - # Once the finger has rolled away from its neighbour, flex it out - # of the camera line of sight using the reviewed PIP->pitch order. - if auxiliary.get(pip_motor) == 0: - _append_single_motor_waypoint( - waypoints, current, pip_motor, final[pip_motor] - ) - handled.add(pip_motor) - if auxiliary.get(pitch_motor) == 0: - _append_single_motor_waypoint( - waypoints, current, pitch_motor, final[pitch_motor] - ) - handled.add(pitch_motor) - # Move the measured roll only after its neighbours have flexed out of - # its swept volume. Pitch/PIP tasks leave this roll neutral throughout. - if target_roll_motor == spec.motor_index: - _append_single_motor_waypoint( - waypoints, - current, - target_roll_motor, - final[target_roll_motor], - ) - handled.add(target_roll_motor) - for motor in sorted(auxiliary): - if motor not in handled and motor != spec.motor_index: - _append_single_motor_waypoint( - waypoints, current, motor, final[motor] - ) - if spec.motor_index not in handled: - _append_single_motor_waypoint( - waypoints, current, spec.motor_index, final[spec.motor_index] - ) - if not waypoints or waypoints[-1] != tuple(final): - # Normally every difference has already been visited. Keep this - # assertion-style fallback explicit so a future profile field cannot - # silently bypass the one-motor transition policy. - differing = [ - index for index, (left, right) in enumerate(zip(current, final)) - if index in controlled_motors and left != right - ] - for motor in differing: - _append_single_motor_waypoint( - waypoints, current, motor, final[motor] - ) - return tuple(waypoints or [tuple(final)]) - - -def _append_class_waypoint( - waypoints: list[tuple[int, ...]], - current: list[int], - target: Sequence[int], - motors: Iterable[int], -) -> None: - """Move one joint-class group of motors in a single parallel waypoint.""" - moved = False - for motor in motors: - if current[int(motor)] != target[int(motor)]: - current[int(motor)] = target[int(motor)] - moved = True - if moved: - waypoints.append(tuple(current)) - - -def build_calibration_return_waypoints( - target_command: Sequence[int], - *, - current_command: Sequence[int], - profile: HandCalibrationProfile = LEFT_HAND_PROFILE, - anchor_roll_motor: int | None = None, - parallel: bool = True, -) -> tuple[tuple[int, ...], ...]: - """Leave a product-layout avoidance pose using the inverse safe sequence. - - ``parallel`` merges the reviewed sequential order into joint-class groups: - all differing rolls return to neutral together, then all differing MCP - pitches, then all differing PIPs, then the remaining controlled motors. - Every finger still unfolds only after every roll is back at neutral, each - finger's pitch still moves before its PIP, and simultaneous same-direction - roll motion removes the "one finger sweeps across parked neighbours" - stall scenario that motivated the anchored one-motor sequence. - """ - if len(current_command) != 20 or len(target_command) != 20: - raise ValueError("return commands must contain exactly 20 values") - target = [int(value) for value in target_command] - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return (tuple(target),) - current = [int(np.clip(round(float(value)), 0, 255)) for value in current_command] - controlled_motors = { - int(joint.motor_index) for joint in profile.joint_specs.values() - } - for motor in range(20): - if motor not in controlled_motors: - target[motor] = current[motor] - if parallel: - waypoints: list[tuple[int, ...]] = [] - _append_class_waypoint(waypoints, current, target, range(6, 10)) - _append_class_waypoint(waypoints, current, target, range(1, 5)) - _append_class_waypoint(waypoints, current, target, range(16, 20)) - _append_class_waypoint( - waypoints, - current, - target, - sorted( - motor - for motor in controlled_motors - if motor not in set(range(6, 10)) - | set(range(1, 5)) - | set(range(16, 20)) - ), - ) - return tuple(waypoints or [tuple(target)]) - waypoints = [] - handled: set[int] = set() - # All four roll channels return to neutral before any bent clearance - # finger is unfolded. In the reviewed product sequence the current/next - # target finger must move first, then neighbouring fingers move outward. - # Returning index first from an all-255 pinky task makes it converge into - # three still-parked fingers and can mechanically stall around command175. - roll_order: Sequence[int] = tuple(range(6, 10)) - anchor = None if anchor_roll_motor is None else int(anchor_roll_motor) - if profile.layout_id == G20_RIGHT_19_LAYOUT: - if anchor in range(6, 10): - roll_order = tuple( - sorted( - range(6, 10), - key=lambda motor: (abs(motor - anchor), -motor), - ) - ) - else: - roll_order = tuple(reversed(range(6, 10))) - for motor in roll_order: - if current[motor] != target[motor]: - _append_single_motor_waypoint( - waypoints, current, motor, target[motor] - ) - handled.add(motor) - # Reverse the entry order, one finger at a time: roll, pitch, then PIP. - for finger in ("index", "middle", "ring", "pinky"): - for motor in ( - _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"], - _MOTOR_BY_JOINT[f"{finger}_pip"], - ): - if current[motor] != target[motor]: - _append_single_motor_waypoint( - waypoints, current, motor, target[motor] - ) - handled.add(motor) - for motor in sorted(controlled_motors): - if motor not in handled and current[motor] != target[motor]: - _append_single_motor_waypoint( - waypoints, current, motor, target[motor] - ) - return tuple(waypoints or [tuple(target)]) - - -def build_calibration_speed_profile( - spec: SweepSpec, - *, - normal_speed: int, - index_roll_speed: int, - index_flex_speed: int, - profile: HandCalibrationProfile = LEFT_HAND_PROFILE, -) -> list[int]: - """Return G20's five per-finger speeds for one calibration task.""" - normal = int(normal_speed) - index_roll = int(index_roll_speed) - index_flex = int(index_flex_speed) - if not all( - 0 <= speed <= 255 for speed in (normal, index_roll, index_flex) - ): - raise ValueError("calibration speeds must be in [0, 255]") - speeds = [normal] * 5 - if spec.motor_index in range(6, 10): - speeds[spec.motor_index - 5] = index_roll - elif spec.motor_index in range(1, 5): - speeds[spec.motor_index] = index_flex - elif spec.motor_index in range(16, 20): - speeds[spec.motor_index - 15] = index_flex - return speeds - - -def _record_vector(record: Mapping[str, Any]) -> np.ndarray: - value = np.asarray(record.get("relative_translation_xyz_m"), dtype=float) - if value.shape != (3,) or not np.all(np.isfinite(value)): - raise ValueError("record relative_translation_xyz_m must contain 3 values") - return value - - -def _record_image_vector(record: Mapping[str, Any]) -> np.ndarray: - value = np.asarray(record.get("image_relative_xy_px"), dtype=float) - if value.shape != (2,) or not np.all(np.isfinite(value)): - raise ValueError("record image_relative_xy_px must contain 2 values") - return value - - -def fit_joint_center_curve( - records: Sequence[Mapping[str, Any]], - *, - maximum_plane_rms_m: float = 0.004, - maximum_radial_rms_m: float = 0.004, - minimum_radius_m: float = 0.003, - minimum_arc_rad: float = math.radians(15.0), -) -> JointCurveFit: - """Fit one command-indexed curve from a parent-frame centre trajectory.""" - samples = [dict(record) for record in records] - if len(samples) < 12: - raise ValueError("joint trajectory requires at least 12 samples") - points = np.asarray([_record_vector(record) for record in samples]) - axis, common_plane_rms = _fit_plane_axis([points]) - circle = _fit_circle_with_axis(points, axis) - circle = _orient_circle_positive(circle, samples, list(points)) - values = [_angle_for_circle(point, circle) for point in points] - curves, correction, hysteresis = _fit_joint_curve(samples, values) - quality = { - "plane_rms_m": max( - float(common_plane_rms), float(circle["plane_rms_m"]) - ), - "radial_rms_m": float(circle["radial_rms_m"]), - "radius_m": float(circle["radius_m"]), - "arc_rad": float(circle["observed_arc_rad"]), - } - failures: list[str] = [] - if quality["plane_rms_m"] > float(maximum_plane_rms_m): - failures.append("plane_rms") - if quality["radial_rms_m"] > float(maximum_radial_rms_m): - failures.append("radial_rms") - if quality["radius_m"] < float(minimum_radius_m): - failures.append("radius") - if quality["arc_rad"] < float(minimum_arc_rad): - failures.append("arc") - if failures: - raise ValueError("joint_trajectory_quality_failed:" + ",".join(failures)) - return JointCurveFit( - angle_rad=tuple(float(value) for value in curves["angle_rad"]), - decreasing_rad=tuple( - float(value) for value in curves["decreasing_rad"] - ), - increasing_rad=tuple( - float(value) for value in curves["increasing_rad"] - ), - circle=dict(circle), - maximum_monotonic_correction_rad=float(correction), - maximum_hysteresis_rad=float(hysteresis), - quality=quality, - ) - - -def _signed_image_circle_angle( - point_xy_px: Sequence[float], circle: Mapping[str, Any] -) -> float: - point = np.asarray(point_xy_px, dtype=float) - centre = np.asarray(circle["center_xy_px"], dtype=float) - reference = np.asarray(circle["reference_xy_px"], dtype=float) - vector = point - centre - if point.shape != (2,) or not np.all(np.isfinite(point)): - raise ValueError("image point must contain 2 finite values") - if float(np.linalg.norm(vector)) < 1.0e-9: - raise ValueError("image point lies at the fitted circle centre") - return float(circle["orientation_sign"]) * math.atan2( - float(reference[0] * vector[1] - reference[1] * vector[0]), - float(np.dot(reference, vector)), - ) - - -def fit_joint_image_curve( - records: Sequence[Mapping[str, Any]], - *, - maximum_radial_rms_px: float = 2.0, - maximum_radial_p95_px: float = 3.5, - minimum_radius_px: float = 20.0, - minimum_arc_rad: float = math.radians(15.0), -) -> JointCurveFit: - """Fit angular phase from a stable projected parent/child circle.""" - samples = [dict(record) for record in records] - if len(samples) < 12: - raise ValueError("joint image trajectory requires at least 12 samples") - points = np.asarray([_record_image_vector(record) for record in samples]) - centre, radius = _fit_circle(points) - radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius) - quality = { - "radial_rms_px": float(np.sqrt(np.mean(np.square(radial_error)))), - "radial_p95_px": float(np.percentile(radial_error, 95.0)), - "radius_px": float(radius), - "arc_rad": float(_trajectory_arc_rad(points, centre)), - } - failures: list[str] = [] - if quality["radial_rms_px"] > float(maximum_radial_rms_px): - failures.append("radial_rms") - if quality["radial_p95_px"] > float(maximum_radial_p95_px): - failures.append("radial_p95") - if quality["radius_px"] < float(minimum_radius_px): - failures.append("radius") - if quality["arc_rad"] < float(minimum_arc_rad): - failures.append("arc") - if failures: - raise ValueError( - "joint_image_trajectory_quality_failed:" + ",".join(failures) - ) - - endpoint_points = np.asarray( - [ - point - for record, point in zip(samples, points) - if int(record["command_u8"]) == 255 - ] - ) - if endpoint_points.size == 0: - raise ValueError("joint image trajectory is missing command 255") - reference = np.median(endpoint_points, axis=0) - centre - reference_norm = float(np.linalg.norm(reference)) - if reference_norm < 1.0e-9: - raise ValueError("joint image trajectory endpoint is degenerate") - reference /= reference_norm - circle: dict[str, Any] = { - "space": "image_2d", - "center_xy_px": [float(value) for value in centre], - "reference_xy_px": [float(value) for value in reference], - "orientation_sign": 1.0, - "radius_px": float(radius), - } - values = [_signed_image_circle_angle(point, circle) for point in points] - command_zero_values = [ - value - for record, value in zip(samples, values) - if int(record["command_u8"]) == 0 - ] - if not command_zero_values: - raise ValueError("joint image trajectory is missing command 0") - if float(np.median(command_zero_values)) < 0.0: - circle["orientation_sign"] = -1.0 - values = [-value for value in values] - curves, correction, hysteresis = _fit_joint_curve(samples, values) - return JointCurveFit( - angle_rad=tuple(float(value) for value in curves["angle_rad"]), - decreasing_rad=tuple( - float(value) for value in curves["decreasing_rad"] - ), - increasing_rad=tuple( - float(value) for value in curves["increasing_rad"] - ), - circle=circle, - maximum_monotonic_correction_rad=float(correction), - maximum_hysteresis_rad=float(hysteresis), - quality=quality, - ) - - -def fit_measured_joint_curve( - joint_name: str, - records: Sequence[Mapping[str, Any]], - *, - maximum_plane_rms_m: float = 0.004, - maximum_radial_rms_m: float = 0.004, - minimum_radius_m: float = 0.003, - minimum_arc_rad: float = math.radians(15.0), - image_maximum_radial_rms_px: float = 2.0, - image_maximum_radial_p95_px: float = 3.5, - image_minimum_radius_px: float = 20.0, - profile: HandCalibrationProfile = LEFT_HAND_PROFILE, -) -> JointCurveFit: - """Select the stable trajectory representation for one measured joint.""" - if str(joint_name) in profile.image_trajectory_joints: - return fit_joint_image_curve( - records, - maximum_radial_rms_px=image_maximum_radial_rms_px, - maximum_radial_p95_px=image_maximum_radial_p95_px, - minimum_radius_px=image_minimum_radius_px, - minimum_arc_rad=minimum_arc_rad, - ) - return fit_joint_center_curve( - records, - maximum_plane_rms_m=maximum_plane_rms_m, - maximum_radial_rms_m=maximum_radial_rms_m, - minimum_radius_m=minimum_radius_m, - minimum_arc_rad=minimum_arc_rad, - ) - - -def center_splay_curve(fit: JointCurveFit) -> tuple[JointCurveFit, int, float]: - """Centre a measured 255->0 splay travel on its angular midpoint.""" - raw = np.asarray(fit.angle_rad, dtype=float) - if raw.shape != (256,) or not np.all(np.isfinite(raw)): - raise ValueError("splay curve must contain 256 finite values") - if np.any(np.diff(raw) > 1.0e-7): - raise ValueError("raw splay curve must be non-increasing") - travel_midpoint = 0.5 * float(raw[0] + raw[255]) - if travel_midpoint <= 0.0: - raise ValueError("splay travel must be positive") - centred = raw - travel_midpoint - # Make the public endpoint symmetry exact after decimal rounding. - endpoint = round(0.5 * float(raw[0] - raw[255]), 8) - centred[0] = endpoint - centred[255] = -endpoint - centred = np.asarray([round(float(value), 8) for value in centred]) - zero_command = int(np.argmin(np.abs(centred))) - return ( - replace( - fit, - angle_rad=tuple(float(value) for value in centred), - zero_offset_rad=round(float(travel_midpoint), 8), - ), - zero_command, - round(float(travel_midpoint), 8), - ) - - -def measure_joint_vector(fit: JointCurveFit, vector_xyz_m: Sequence[float]) -> float: - """Measure one vector with a fitted model, including splay centring.""" - return float( - _angle_for_circle(vector_xyz_m, fit.circle) - fit.zero_offset_rad - ) - - -def measure_joint_observation( - fit: JointCurveFit, - *, - vector_xyz_m: Sequence[float], - image_vector_xy_px: Sequence[float], -) -> float: - """Measure an observation in the same space used to fit its curve.""" - if fit.circle.get("space") == "image_2d": - return float( - _signed_image_circle_angle(image_vector_xy_px, fit.circle) - - fit.zero_offset_rad - ) - return measure_joint_vector(fit, vector_xyz_m) - - -def compare_cross_view_roll_curves( - primary: JointCurveFit, - secondary: JointCurveFit, - *, - maximum_rms_difference_rad: float = math.radians(1.0), - maximum_branch_gap_difference_rad: float = math.radians(0.3), - allow_projection_scale: bool = False, - maximum_projection_scale_ratio: float = 1.5, -) -> dict[str, float]: - """Validate two independently fitted roll curves before axis fusion. - - The G20 right roll primary is an image-plane centre trajectory while its - side validation is a 3-D rotation trajectory. A fixed Tag mounting tilt - changes the image-plane angular scale without changing the mechanism. In - projection-scale mode, remove that one constant scale before comparing - curve shape. Direction, scale bounds, direction-dependent branch gap and - every single-view quality/holdout gate remain independent hard checks. - """ - if maximum_rms_difference_rad <= 0.0: - raise ValueError("cross-view RMS limit must be positive") - if maximum_branch_gap_difference_rad <= 0.0: - raise ValueError("cross-view branch-gap limit must be positive") - if maximum_projection_scale_ratio <= 1.0: - raise ValueError("cross-view projection-scale ratio must exceed one") - - fields: dict[str, tuple[np.ndarray, np.ndarray]] = {} - for field_name in ("angle_rad", "decreasing_rad", "increasing_rad"): - left = np.asarray(getattr(primary, field_name), dtype=float) - right = np.asarray(getattr(secondary, field_name), dtype=float) - if left.shape != (256,) or right.shape != (256,): - raise ValueError("cross-view curves must contain 256 values") - if not np.all(np.isfinite(left)) or not np.all(np.isfinite(right)): - raise ValueError("cross-view curves must be finite") - fields[field_name] = (left, right) - - primary_travel = float( - fields["angle_rad"][0][0] - fields["angle_rad"][0][255] - ) - secondary_travel = float( - fields["angle_rad"][1][0] - fields["angle_rad"][1][255] - ) - if primary_travel * secondary_travel <= 0.0: - raise ValueError("cross_view_roll_curve_direction_disagrees") - - primary_zero = int(primary.circle.get("zero_command_u8", 127)) - secondary_zero = int(secondary.circle.get("zero_command_u8", 127)) - if not 0 <= primary_zero <= 255 or not 0 <= secondary_zero <= 255: - raise ValueError("cross-view zero command must be in [0, 255]") - primary_anchor = float(primary.decreasing_rad[primary_zero]) - secondary_anchor = float(secondary.decreasing_rad[secondary_zero]) - projection_scale = 1.0 - if allow_projection_scale: - primary_values = np.concatenate( - [left - primary_anchor for left, _ in fields.values()] - ) - secondary_values = np.concatenate( - [right - secondary_anchor for _, right in fields.values()] - ) - denominator = float(secondary_values @ secondary_values) - if denominator <= np.finfo(float).eps: - raise ValueError("cross_view_roll_projection_scale_unobservable") - projection_scale = float( - (primary_values @ secondary_values) / denominator - ) - if projection_scale <= 0.0 or not math.isfinite(projection_scale): - raise ValueError("cross_view_roll_curve_direction_disagrees") - scale_ratio = max(projection_scale, 1.0 / projection_scale) - if scale_ratio > float(maximum_projection_scale_ratio): - raise ValueError( - "cross_view_roll_projection_scale_ratio_too_large:" - f"{scale_ratio:.6f}" - ) - - metrics: dict[str, float] = { - "projection_scale": projection_scale, - "projection_scale_ratio": max( - projection_scale, 1.0 / projection_scale - ), - } - for field_name, (left, right) in fields.items(): - metrics[f"raw_{field_name}_rms_difference_rad"] = float( - np.sqrt(np.mean(np.square(left - right))) - ) - if allow_projection_scale: - left = left - primary_anchor - right = projection_scale * (right - secondary_anchor) - rms = float(np.sqrt(np.mean(np.square(left - right)))) - metrics[f"{field_name}_rms_difference_rad"] = rms - if rms > maximum_rms_difference_rad: - raise ValueError( - "cross_view_roll_curve_difference_too_large:" - f"{field_name}:{math.degrees(rms):.6f}deg" - ) - metrics["travel_difference_rad"] = abs(primary_travel - secondary_travel) - primary_gap = float( - primary.increasing_rad[primary_zero] - - primary.decreasing_rad[primary_zero] - ) - secondary_gap = float( - secondary.increasing_rad[secondary_zero] - - secondary.decreasing_rad[secondary_zero] - ) - metrics["raw_baseline_branch_gap_difference_rad"] = abs( - primary_gap - secondary_gap - ) - branch_gap_difference = abs( - primary_gap - projection_scale * secondary_gap - ) - metrics["baseline_branch_gap_difference_rad"] = branch_gap_difference - # In projection-scale mode the primary is an image-circle phase while the - # secondary is a 3-D rotation angle. A fixed Tag tilt/offset changes the - # image projection of the two motion branches independently, so their - # absolute gap is not a cross-view invariant. Both views retain their - # own hard absolute-gap and four-cycle gap-range checks in the acquisition - # quality gate. Keep this value as a diagnostic, but apply the cross-view - # absolute-gap gate only when both curves live in comparable coordinates. - metrics["baseline_branch_gap_gate_applied"] = float( - not allow_projection_scale - ) - if ( - not allow_projection_scale - and branch_gap_difference > maximum_branch_gap_difference_rad - ): - raise ValueError( - "cross_view_roll_branch_gap_difference_too_large:" - f"{math.degrees(branch_gap_difference):.6f}deg" - ) - return metrics - - -def cross_view_roll_diagnostic_metrics( - primary: JointCurveFit, - secondary: JointCurveFit, -) -> dict[str, float]: - """Compare a validation-only roll view without creating a retry gate. - - G20-right publishes the front image-plane roll curve. The side PIP-link - observation supplies an independently quality-gated physical axis-line - position, but its angle includes projection and linkage effects and is - therefore diagnostic rather than a second measurement of the published - curve. Live fitting and offline replay must use the same non-blocking - comparison; otherwise a stable view bias can recall an already complete - task even though reacquiring the same motion cannot change the result. - """ - try: - return compare_cross_view_roll_curves( - primary, - secondary, - maximum_rms_difference_rad=float("inf"), - maximum_branch_gap_difference_rad=float("inf"), - allow_projection_scale=True, - maximum_projection_scale_ratio=float("inf"), - ) - except ValueError as error: - if str(error) == "cross_view_roll_curve_direction_disagrees": - return {"direction_disagrees": 1.0} - raise - - -def fit_projected_zero( - records: Sequence[Mapping[str, Any]], - *, - minimum_radius_px: float = 20.0, - minimum_arc_rad: float = math.radians(15.0), - maximum_radial_rms_px: float = 2.0, - maximum_radial_p95_px: float = 3.5, - maximum_round_difference_rad: float = math.radians(1.0), -) -> float: - """Return the image/table-projected zero from three sweep rounds.""" - samples = [dict(record) for record in records] - points = np.asarray( - [record.get("image_relative_xy_px") for record in samples], dtype=float - ) - if points.ndim != 2 or points.shape[1] != 2 or len(points) < 12: - raise ValueError("projected zero requires 2-D trajectory samples") - if not np.all(np.isfinite(points)): - raise ValueError("projected zero samples must be finite") - centre, radius = _fit_circle(points) - radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius) - radial_rms = float(np.sqrt(np.mean(np.square(radial_error)))) - radial_p95 = float(np.percentile(radial_error, 95.0)) - arc = _trajectory_arc_rad(points, centre) - if radius < minimum_radius_px: - raise ValueError("projected_zero_radius_too_small") - if arc < minimum_arc_rad: - raise ValueError("projected_zero_arc_too_small") - if radial_rms > maximum_radial_rms_px: - raise ValueError("projected_zero_radial_rms_too_large") - if radial_p95 > maximum_radial_p95_px: - raise ValueError("projected_zero_radial_p95_too_large") - - cycles = sorted({int(record["cycle"]) for record in samples}) - if len(cycles) < 3: - raise ValueError("projected zero requires three sweep rounds") - angles: list[float] = [] - for cycle in cycles: - zero_points = np.asarray( - [ - record["image_relative_xy_px"] - for record in samples - if int(record["cycle"]) == cycle - and int(record["command_u8"]) == 255 - ], - dtype=float, - ) - if zero_points.size == 0: - raise ValueError("projected zero is missing command 255") - point = np.median(zero_points, axis=0) - inward = centre - point - if float(np.linalg.norm(inward)) < 1.0e-9: - raise ValueError("projected zero point is degenerate") - angles.append( - wrap_angle_rad(math.atan2(-float(inward[1]), float(inward[0]))) - ) - if maximum_pairwise_angle_difference_rad(angles) > maximum_round_difference_rad: - raise ValueError("projected_zero_round_difference_too_large") - return round(float(circular_median_rad(angles)), 8) - - -def build_compact_payload( - *, - serial_number: str, - measured_fits: Mapping[str, JointCurveFit], - urdf_zero_offsets_rad: Mapping[str, float], - validation_errors_rad: Sequence[float], - passed: bool, - baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, - side: str = "left", - layout_id: str = "legacy_11", - zero_uncertainty_rad: Mapping[str, float] | None = None, - zero_cycle_offsets_rad: Mapping[str, Sequence[float]] | None = None, - zero_observers: Mapping[str, str] | None = None, - artifact_hashes: Mapping[str, str] | None = None, - cross_view_roll_metrics: Mapping[str, Mapping[str, float]] | None = None, - joint_dynamic_diagnostics: Mapping[str, Mapping[str, Any]] | None = None, - zero_geometry_diagnostics: Mapping[str, Any] | None = None, -) -> dict[str, Any]: - profile = get_hand_calibration_profile(side, layout_id) - expected_fits = ( - set(profile.joint_specs) - if profile.layout_id == G20_RIGHT_19_LAYOUT - else set(profile.measured_joints) - ) - if set(measured_fits) != expected_fits: - raise ValueError("measured_fits has the wrong runtime joint set") - expected_active = { - name for name, spec in profile.joint_specs.items() if spec.active - } - if set(urdf_zero_offsets_rad) != expected_active: - raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set") - if len(baseline) != 20: - raise ValueError("baseline must contain exactly 20 commands") - - if profile.layout_id == G20_RIGHT_19_LAYOUT: - return _build_right_19_compact_payload( - serial_number=serial_number, - measured_fits=measured_fits, - urdf_zero_offsets_rad=urdf_zero_offsets_rad, - validation_errors_rad=validation_errors_rad, - passed=passed, - baseline=baseline, - profile=profile, - zero_uncertainty_rad=zero_uncertainty_rad or {}, - zero_cycle_offsets_rad=zero_cycle_offsets_rad or {}, - zero_observers=zero_observers or {}, - artifact_hashes=artifact_hashes or {}, - cross_view_roll_metrics=cross_view_roll_metrics or {}, - joint_dynamic_diagnostics=joint_dynamic_diagnostics or {}, - zero_geometry_diagnostics=zero_geometry_diagnostics or {}, - ) - - joints: dict[str, dict[str, Any]] = {} - for name, spec in profile.joint_specs.items(): - source_name = spec.source_joint or name - fit = measured_fits[source_name] - joint: dict[str, Any] = { - "motor_index": int(spec.motor_index), - "angle_rad": [round(float(value), 8) for value in fit.angle_rad], - } - if spec.active: - joint["zero_command_u8"] = int(baseline[spec.motor_index]) - joint["zero_angles"] = { - "urdf_zero_offset_rad": round( - float(urdf_zero_offsets_rad[name]), 8 - ) - } - else: - joint["passive"] = True - if spec.source_joint is not None: - joint["source_joint"] = spec.source_joint - joints[name] = joint - - errors = np.abs(np.asarray(validation_errors_rad, dtype=float)) - mae = float(np.mean(errors)) if errors.size else float("nan") - p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan") - payload = { - "schema_version": 4, - "model": "G20", - "side": profile.side, - "serial_number": str(serial_number), - "angle_unit": "rad", - "command_range": [0, 255], - "baseline_command_u8": [int(value) for value in baseline], - "joints": joints, - "quality": { - "passed": bool(passed), - "validation_mae_rad": ( - None if not math.isfinite(mae) else round(mae, 8) - ), - "validation_p95_rad": ( - None if not math.isfinite(p95) else round(p95, 8) - ), - }, - } - validate_compact_payload(payload) - return payload - - -def _build_right_19_compact_payload( - *, - serial_number: str, - measured_fits: Mapping[str, JointCurveFit], - urdf_zero_offsets_rad: Mapping[str, float], - validation_errors_rad: Sequence[float], - passed: bool, - baseline: Sequence[int], - profile: HandCalibrationProfile, - zero_uncertainty_rad: Mapping[str, float], - zero_cycle_offsets_rad: Mapping[str, Sequence[float]], - zero_observers: Mapping[str, str], - artifact_hashes: Mapping[str, str], - cross_view_roll_metrics: Mapping[str, Mapping[str, float]], - joint_dynamic_diagnostics: Mapping[str, Mapping[str, Any]], - zero_geometry_diagnostics: Mapping[str, Any], -) -> dict[str, Any]: - # The public runtime artifact intentionally keeps the established schema-v4 - # contract. Directional curves, uncertainty, hashes and geometry metrics - # are calibration diagnostics and belong in calibration_summary_zh.json; - # exposing them here made it too easy for a runtime to apply a feedback - # curve or a zero offset twice. - del ( - zero_uncertainty_rad, - zero_cycle_offsets_rad, - zero_observers, - artifact_hashes, - cross_view_roll_metrics, - joint_dynamic_diagnostics, - zero_geometry_diagnostics, - ) - joints: dict[str, dict[str, Any]] = {} - for name, spec in profile.joint_specs.items(): - fit = measured_fits[name] - zero_command = int(baseline[spec.motor_index]) - decreasing = np.asarray(fit.decreasing_rad, dtype=float) - increasing = np.asarray(fit.increasing_rad, dtype=float) - if decreasing.shape != (256,) or increasing.shape != (256,): - raise ValueError(f"{name} directional curves must contain 256 values") - # A single schema-v4 curve is the steady-state midpoint of both - # approach directions. Publication separately rejects a branch gap - # above 2 degrees, bounding the midpoint's direction error to 1 degree. - curve = 0.5 * (decreasing + increasing) - curve -= float(curve[zero_command]) - joint: dict[str, Any] = { - "motor_index": int(spec.motor_index), - "angle_rad": [round(float(value), 8) for value in curve], - } - if spec.active: - joint["zero_command_u8"] = zero_command - joint["zero_angles"] = { - "urdf_zero_offset_rad": round( - float(urdf_zero_offsets_rad[name]), 8 - ) - } - else: - joint["passive"] = True - joints[name] = joint - - errors = np.abs(np.asarray(validation_errors_rad, dtype=float)) - mae = float(np.mean(errors)) if errors.size else float("nan") - p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan") - payload = { - "schema_version": 4, - "model": "G20", - "side": "right", - "serial_number": str(serial_number), - "angle_unit": "rad", - "command_range": [0, 255], - "baseline_command_u8": [int(value) for value in baseline], - "joints": joints, - "quality": { - "passed": bool(passed), - "validation_mae_rad": ( - None if not math.isfinite(mae) else round(mae, 8) - ), - "validation_p95_rad": ( - None if not math.isfinite(p95) else round(p95, 8) - ), - }, - } - validate_compact_payload(payload) - return payload - - -def infer_compact_payload_layout(payload: Mapping[str, Any]) -> str: - """Infer the layout for schema-v4 while honoring explicit newer schemas.""" - explicit = str(payload.get("layout_id", "")).strip().lower() - if explicit: - return explicit - raw_joints = payload.get("joints", {}) - independent_right = bool( - str(payload.get("side", "")).lower() == "right" - and isinstance(raw_joints, Mapping) - and raw_joints - and all( - not isinstance(value, Mapping) or "source_joint" not in value - for value in raw_joints.values() - ) - ) - return G20_RIGHT_19_LAYOUT if independent_right else "legacy_11" - - -def validate_compact_payload(payload: Mapping[str, Any]) -> None: - if payload.get("schema_version") == 5: - _validate_right_19_compact_payload(payload) - return - expected_top = { - "schema_version", "model", "side", "serial_number", "angle_unit", - "command_range", "baseline_command_u8", "joints", "quality", - } - if set(payload) != expected_top: - raise ValueError("compact calibration has unexpected top-level fields") - if payload["schema_version"] != 4: - raise ValueError("schema_version must be 4") - if payload["model"] != "G20" or payload["side"] not in {"left", "right"}: - raise ValueError("payload must describe a left or right G20") - # schema-v4 has no layout field. A right-hand payload without inherited - # source_joint fields is the independent right-hand product layout; legacy - # payloads retain their source_joint markers and remain readable. - layout_id = infer_compact_payload_layout(payload) - independent_right = layout_id == G20_RIGHT_19_LAYOUT - profile = get_hand_calibration_profile( - str(payload["side"]), - layout_id, - ) - if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: - raise ValueError("payload angle or command units are invalid") - baseline = payload["baseline_command_u8"] - if not isinstance(baseline, list) or len(baseline) != 20: - raise ValueError("baseline_command_u8 must contain 20 values") - joints = payload["joints"] - if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs): - raise ValueError("payload must contain exactly 21 G20 joints") - - for name, spec in profile.joint_specs.items(): - joint = joints[name] - allowed = {"motor_index", "angle_rad"} - allowed.add("zero_command_u8" if spec.active else "passive") - if spec.active: - allowed.add("zero_angles") - if spec.source_joint is not None and not independent_right: - allowed.add("source_joint") - if set(joint) != allowed: - raise ValueError(f"{name} has unexpected fields") - if int(joint["motor_index"]) != spec.motor_index: - raise ValueError(f"{name} has the wrong motor_index") - curve = np.asarray(joint["angle_rad"], dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"{name}.angle_rad must contain 256 finite values") - if np.any(np.diff(curve) > 1.0e-7): - raise ValueError(f"{name}.angle_rad must be non-increasing") - if spec.active: - zero = joint["zero_command_u8"] - if not isinstance(zero, int) or not 0 <= zero <= 255: - raise ValueError(f"{name}.zero_command_u8 is invalid") - zero_angles = joint.get("zero_angles") - if not isinstance(zero_angles, Mapping) or set(zero_angles) != { - "urdf_zero_offset_rad" - }: - raise ValueError( - f"{name}.zero_angles must contain urdf_zero_offset_rad" - ) - offset = float(zero_angles["urdf_zero_offset_rad"]) - if not math.isfinite(offset): - raise ValueError(f"{name}.urdf_zero_offset_rad is invalid") - if abs(float(curve[zero])) > 1.0e-6: - raise ValueError( - f"{name}.angle_rad must be zero at zero_command_u8" - ) - elif joint.get("passive") is not True: - raise ValueError(f"{name} must be marked passive") - if spec.source_joint is not None and not independent_right: - if joint.get("source_joint") != spec.source_joint: - raise ValueError(f"{name} has the wrong source_joint") - source = joints[spec.source_joint] - if joint["angle_rad"] != source["angle_rad"]: - raise ValueError(f"{name} must copy its source curve exactly") - if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]: - raise ValueError(f"{name} must copy its source zero command") - # source_joint denotes a dynamic command-angle curve source. The - # absolute URDF zero belongs to an independent motor/assembly and - # must remain per-joint unless it was independently observed. - - quality = payload["quality"] - if set(quality) != { - "passed", "validation_mae_rad", "validation_p95_rad" - }: - raise ValueError("quality must contain only compact summary fields") - - -def _validate_right_19_compact_payload(payload: Mapping[str, Any]) -> None: - expected_top = { - "schema_version", "layout_id", "model", "side", "serial_number", - "angle_unit", "command_range", "curve_input_domain", - "runtime_curve_policy", "baseline_command_u8", - "static_zero_targets", "frozen_static_zero_joints", "joints", - "cross_view_roll", "static_zero_validation", "artifacts", "quality", - } - if set(payload) != expected_top: - raise ValueError("schema v5 calibration has unexpected top-level fields") - if ( - payload["layout_id"] not in {"g20_right_19", G20_RIGHT_15_LAYOUT} - or payload["model"] != "G20" - or payload["side"] != "right" - ): - raise ValueError("schema v5 must describe a G20 right product layout") - profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) - baseline = payload["baseline_command_u8"] - if not isinstance(baseline, list) or len(baseline) != 20: - raise ValueError("baseline_command_u8 must contain 20 values") - if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: - raise ValueError("payload angle or command units are invalid") - if payload["curve_input_domain"] != "feedback_u8": - raise ValueError("schema v5 curves must be feedback-indexed") - runtime_curve_policy = payload["runtime_curve_policy"] - if runtime_curve_policy not in { - "direction_aware", - "direction_aware_canonical_zero", - }: - raise ValueError("schema v5 runtime curve policy must be direction-aware") - # v5 is a read-only legacy contract from the earlier 15-zero product. - # Keep its historical scope stable even though new v4 publications now - # calibrate thumb_mcp as the 16th active static zero. - static_targets = { - name - for name, spec in profile.joint_specs.items() - if spec.zero_kind == "urdf_axis_chain" - } - {"thumb_mcp"} - frozen = {"thumb_mcp", *profile.passive_joints} - if set(payload["static_zero_targets"]) != static_targets: - raise ValueError("schema v5 must contain exactly 15 static-zero targets") - if set(payload["frozen_static_zero_joints"]) != frozen: - raise ValueError("schema v5 frozen static-zero joint set is invalid") - static_validation = payload["static_zero_validation"] - training_cycles = static_validation.get("training_cycles", []) - holdout_cycle = static_validation.get("holdout_cycle") - expected_cycle_count = len(training_cycles) + 1 - joints = payload["joints"] - if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs): - raise ValueError("schema v5 must contain exactly 21 independent joints") - for name, spec in profile.joint_specs.items(): - joint = joints[name] - expected_fields = { - "motor_index", "zero_command_u8", "angle_rad", "decreasing_rad", - "increasing_rad", "dynamic_quality", "static_zero_status", - "zero_angles" if spec.active else "passive", - } - if set(joint) != expected_fields: - raise ValueError(f"{name} has unexpected schema v5 fields") - if int(joint["motor_index"]) != spec.motor_index: - raise ValueError(f"{name} has the wrong motor_index") - zero_command = joint["zero_command_u8"] - if zero_command != int(baseline[spec.motor_index]): - raise ValueError(f"{name} has the wrong zero command") - for field_name in ("angle_rad", "decreasing_rad", "increasing_rad"): - curve = np.asarray(joint[field_name], dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"{name}.{field_name} must contain 256 values") - if np.any(np.diff(curve) > 1.0e-7): - raise ValueError(f"{name}.{field_name} must be non-increasing") - allow_directional_offset = ( - runtime_curve_policy == "direction_aware_canonical_zero" - and canonical_zero_direction(profile, name) is not None - and field_name == "increasing_rad" - ) - if ( - not allow_directional_offset - and abs(float(curve[zero_command])) > 1.0e-6 - ): - raise ValueError(f"{name}.{field_name} must be zero at baseline") - expected_status = ( - "static_zero_calibrated" - if name in static_targets - else "cad_static_zero_retained_front_only" - if name == "thumb_mcp" - else "dynamic_measured_static_unobservable" - ) - if joint["static_zero_status"] != expected_status: - raise ValueError(f"{name} has the wrong static-zero status") - dynamic_quality = joint["dynamic_quality"] - if set(dynamic_quality) != { - "maximum_monotonic_correction_rad", - "maximum_hysteresis_rad", - "fit", - "cycle_travel_rad", - "cycle_travel_range_rad", - "baseline_hysteresis_by_cycle_rad", - "holdout_cycle", - "holdout_cycle_mae_rad", - "holdout_cycle_max_rad", - }: - raise ValueError(f"{name}.dynamic_quality fields are invalid") - if ( - dynamic_quality["holdout_cycle"] != holdout_cycle - or len(dynamic_quality["cycle_travel_rad"]) != expected_cycle_count - or len( - dynamic_quality["baseline_hysteresis_by_cycle_rad"] - ) != expected_cycle_count - ): - raise ValueError(f"{name} must record all dynamic cycles") - if spec.active: - zero = joint["zero_angles"] - if set(zero) != { - "urdf_zero_offset_rad", - "confidence_interval_95_half_width_rad", - "cycle_offsets_rad", - "observer_joint", - }: - raise ValueError(f"{name}.zero_angles fields are invalid") - if not math.isfinite(float(zero["urdf_zero_offset_rad"])): - raise ValueError(f"{name} zero offset is invalid") - if name in static_targets: - confidence = zero["confidence_interval_95_half_width_rad"] - if ( - confidence is None - or not math.isfinite(float(confidence)) - or not 0.0 <= float(confidence) <= math.radians(0.5) - ): - raise ValueError(f"{name} confidence interval is invalid") - if len(zero["cycle_offsets_rad"]) != len(training_cycles): - raise ValueError( - f"{name} must record training-only zero estimates" - ) - if not isinstance(zero["observer_joint"], str): - raise ValueError(f"{name} must record its zero observer") - if name == "thumb_mcp" and float(zero["urdf_zero_offset_rad"]) != 0.0: - raise ValueError("thumb_mcp static zero must remain source CAD") - elif joint.get("passive") is not True: - raise ValueError(f"{name} must be marked passive") - artifacts = payload["artifacts"] - expected_hashes = { - "source_urdf_sha256", "camera_extrinsics_sha256", - "corrected_urdf_sha256", - } - if set(artifacts) != expected_hashes or any( - re.fullmatch(r"[0-9a-f]{64}", str(value)) is None - for value in artifacts.values() - ): - raise ValueError("schema v5 artifact SHA-256 values are invalid") - if set(payload["cross_view_roll"]) != { - f"{finger}_mcp_roll" for finger in ("index", "middle", "ring", "pinky") - }: - raise ValueError("schema v5 requires four cross-view roll diagnostics") - if set(static_validation) != { - "training_cycles", "holdout_cycle", "axis_line_rms_m", - "axis_line_rms_by_joint_m", "observability_rank", - "observability_parameter_count", "observability_condition_number", - "offset_covariance_rad2", - }: - raise ValueError("schema v5 static-zero validation fields are invalid") - training_cycles = static_validation["training_cycles"] - holdout_cycle = static_validation["holdout_cycle"] - if ( - not isinstance(training_cycles, list) - or len(training_cycles) < 3 - or any(not isinstance(value, int) for value in training_cycles) - or not isinstance(holdout_cycle, int) - or holdout_cycle in training_cycles - ): - raise ValueError("schema v5 training/holdout cycle split is invalid") - axis_line_rms = float(static_validation["axis_line_rms_m"]) - if not math.isfinite(axis_line_rms) or axis_line_rms < 0.0: - raise ValueError("schema v5 axis-line RMS is invalid") - line_by_joint = static_validation["axis_line_rms_by_joint_m"] - if not isinstance(line_by_joint, Mapping) or any( - not math.isfinite(float(value)) or float(value) < 0.0 - for value in line_by_joint.values() - ): - raise ValueError("schema v5 per-joint axis-line RMS is invalid") - parameter_count = int(static_validation["observability_parameter_count"]) - rank = int(static_validation["observability_rank"]) - condition = float(static_validation["observability_condition_number"]) - if parameter_count != 21 or rank != parameter_count: - raise ValueError("schema v5 zero-observation Jacobian is rank deficient") - if not math.isfinite(condition) or condition < 1.0: - raise ValueError("schema v5 observability condition number is invalid") - offset_covariance = static_validation["offset_covariance_rad2"] - if set(offset_covariance) != static_targets or any( - not math.isfinite(float(value)) or float(value) < 0.0 - for value in offset_covariance.values() - ): - raise ValueError("schema v5 offset covariance is invalid") - quality = payload["quality"] - if set(quality) != { - "passed", "all_15_static_targets_required", "validation_mae_rad", - "validation_p95_rad", - } or quality["all_15_static_targets_required"] is not True: - raise ValueError("schema v5 quality summary is invalid") +sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py index 64c18b0..a030e19 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py @@ -17,6 +17,7 @@ from ...core import ( VisionRigSpec, ZeroSolvePolicy, ) +from .profile import MIMIC_DERIVED_FINGER_DIPS from ..registry import EngineBindings, RegisteredProfile from .artifacts import build_artifact_policy from .motion import ( @@ -24,7 +25,6 @@ from .motion import ( build_calibration_preparation_waypoints, build_calibration_return_waypoints, ) -from .zero_policy import MIMIC_DERIVED_FINGER_DIPS _HARD_THRESHOLD_KEYS = frozenset( @@ -45,6 +45,12 @@ def _run_cli(args: list[str] | None = None) -> None: main(args) +def _run_node(args: list[str] | None = None) -> None: + from .node import main + + main(args) + + def adapt_profile( *, key: ProfileKey, @@ -127,6 +133,15 @@ def adapt_profile( views=views, common_frame="calibration_common", extrinsic_reference_view=views[0].name, + extrinsics_quality_limits={ + "reprojection_rms_px": 1.2, + "maximum_rotation_repeatability_deg": 0.3, + "maximum_translation_repeatability_m": 0.0015, + }, + minimum_capture_counts={ + "front_side_captures": 15, + "front_top_captures": 15, + }, ), motion=MotionPolicy( tasks=tasks, @@ -196,5 +211,6 @@ def adapt_profile( preparation_waypoints=build_calibration_preparation_waypoints, return_waypoints=build_calibration_return_waypoints, cli_main=_run_cli, + node_main=_run_node, ), ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py index 932f36d..d7d5cc3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py @@ -21,6 +21,9 @@ def build_artifact_policy( } ), session_compatibility_tokens=frozenset(compatibility_tokens), + publish_corrected_urdf=( + "urdf_zero_publication" in compatibility_tokens + ), ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py new file mode 100644 index 0000000..d2de771 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/golden_regression.py @@ -0,0 +1,144 @@ +"""Read-only regression checks for the reviewed hardware sessions.""" + +from __future__ import annotations + +import argparse +import hashlib +import json +from pathlib import Path +from typing import Any + +_PASS_SESSIONS = { + "20260830_181154": "full", + "20260831_141123": "thumb", + "20260831_163843": "thumb", +} +_FAIL_SESSIONS = { + "20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"), + "20260831_142322": ("VAL-QUALITY-501", "zero_model_validation_failed"), +} + + +def _sha256_file(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _summary(session: Path) -> dict[str, Any]: + value = json.loads( + (session / "calibration_summary_zh.json").read_text(encoding="utf-8") + ) + if not isinstance(value, dict): + raise ValueError(f"invalid golden summary: {session}") + return value + + +def validate_golden_sessions( + serial_root: str | Path, + *, + replay_full_session: bool = True, +) -> dict[str, Any]: + """Prove PASS/FAIL, hashes, partial freezes, and failure non-publication.""" + root = Path(serial_root).expanduser().resolve() + results: dict[str, Any] = {} + for session_id, scope in _PASS_SESSIONS.items(): + session = root / session_id + summary = _summary(session) + if summary.get("result") != "PASS": + raise ValueError(f"golden PASS changed: {session_id}") + if summary.get("calibration_scope") != scope: + raise ValueError(f"golden scope changed: {session_id}") + artifacts = summary["artifacts"] + hashes = summary["hashes"] + calibration_json = session / artifacts["json"] + corrected_urdf = session / artifacts["urdf"] + actual_json_hash = _sha256_file(calibration_json) + actual_urdf_hash = _sha256_file(corrected_urdf) + if actual_json_hash != hashes["calibration_json_sha256"]: + raise ValueError(f"golden JSON hash changed: {session_id}") + if actual_urdf_hash != hashes["corrected_urdf_sha256"]: + raise ValueError(f"golden URDF hash changed: {session_id}") + if session_id == "20260831_141123" and len( + summary.get("preserved_certified_zero_joints", ()) + ) != 12: + raise ValueError("merged thumb no longer freezes all finger zeros") + if session_id == "20260831_163843": + payload = json.loads(calibration_json.read_text(encoding="utf-8")) + if payload.get("artifact_type") != ( + "g20_right_standalone_thumb_calibration" + ): + raise ValueError("standalone thumb artifact type changed") + results[session_id] = { + "result": "PASS", + "json_sha256": actual_json_hash, + "urdf_sha256": actual_urdf_hash, + } + + for session_id, (error_code, reason) in _FAIL_SESSIONS.items(): + session = root / session_id + summary = _summary(session) + if ( + summary.get("result") != "FAIL" + or summary.get("error_code") != error_code + or summary.get("reason") != reason + ): + raise ValueError(f"golden failure decision changed: {session_id}") + results[session_id] = { + "result": "FAIL", + "error_code": error_code, + "reason": reason, + } + + failure_ids = set(_FAIL_SESSIONS) + for pointer_name in ("latest_passed", "latest_thumb_passed"): + pointer = root / pointer_name + if pointer.exists() and pointer.resolve().name in failure_ids: + raise ValueError(f"failure session was published through {pointer_name}") + + if replay_full_session: + from .offline_replay import replay_session + + session_id = "20260830_181154" + replay = replay_session(root / session_id, write_outputs=False) + expected = results[session_id] + if replay["computed_final_json_sha256"] != expected["json_sha256"]: + raise ValueError( + "full-session replay JSON is not byte-identical: " + f"computed={replay['computed_final_json_sha256']} " + f"expected={expected['json_sha256']}" + ) + if replay["corrected_urdf_sha256"] != expected["urdf_sha256"]: + raise ValueError( + "full-session replay URDF is not byte-identical: " + f"computed={replay['corrected_urdf_sha256']} " + f"expected={expected['urdf_sha256']}" + ) + results[session_id]["offline_replay"] = "byte_identical" + return results + + +def main(args: list[str] | None = None) -> None: + parser = argparse.ArgumentParser( + description="Validate the five reviewed calibration sessions" + ) + parser.add_argument("serial_root") + parser.add_argument("--no-replay", action="store_true") + selected = parser.parse_args(args) + print( + json.dumps( + validate_golden_sessions( + selected.serial_root, + replay_full_session=not selected.no_replay, + ), + ensure_ascii=False, + indent=2, + sort_keys=True, + ) + ) + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py index 004cf51..9a5223e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py @@ -1,7 +1,7 @@ """One-release typed wrappers for the legacy 11-Tag layouts.""" from ...core import ProfileKey -from ...full_hand import get_hand_calibration_profile +from .profile import get_hand_calibration_profile from ...urdf_zero import get_zero_calibration_profile from ..registry import RegisteredProfile from ._adapter import adapt_profile diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py index 0be31a0..465bd0f 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py @@ -1,6 +1,6 @@ """Reviewed motion and safe-waypoint strategy exports.""" -from ...full_hand import ( +from .profile import ( build_calibration_motion_command, build_calibration_preparation_waypoints, build_calibration_return_waypoints, diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py new file mode 100644 index 0000000..0bf3dda --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/node.py @@ -0,0 +1,13060 @@ +"""One-command three-camera calibration for either complete G20 hand.""" + +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass, field, replace +import hashlib +import json +import math +from pathlib import Path +import random +import re +import time +import traceback +from typing import Any, Mapping, Sequence + +import numpy as np +import rclpy +from apriltag_msgs.msg import AprilTagDetectionArray +from rclpy.exceptions import ParameterUninitializedException +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.qos import qos_profile_sensor_data +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, JointState +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from ...acquisition import ( + StateSample, + TagQuality, + interpolate_state_u8, + required_resume_views, + tag_quality_is_valid, + update_pnp_reset_watchdog, +) +from ...core import ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + robust_rotation_summary, +) +from ...core.urdf import build_correction_plan +from ...extrinsics import ( + ThreeCameraExtrinsics, + camera_info_fingerprint, + load_three_camera_extrinsics, + matrix_payload, + transform_matrix, +) +from .profile import ( + G20_COMBINATION_REQUIRED_TARGET_KEYS, + G20_REFERENCE_THUMB_CMC_JOINTS, + G20_RIGHT_19_LAYOUT, + LEFT_HAND_PROFILE, + RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, + RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, + THREE_CAMERA_BASELINE_COMMAND, + HandCalibrationProfile, + JointCurveFit, + PalmAxisObserver, + SweepSpec, + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, + build_calibration_speed_profile, + build_compact_payload, + calibration_auxiliary_commands, + canonical_zero_direction, + clamp_runtime_fits_to_urdf_limits, + compare_cross_view_roll_curves, + cross_view_roll_diagnostic_metrics, + derive_mimic_passive_fits, + fit_joint_image_curve, + get_hand_calibration_profile, +) +from ...hikrobot_camera import configure_fastdds_large_image_transport +from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES +from ...pnp import ( + SquareTagGroupPoseTracker, + SquareTagPose, + SquareTagPoseTracker, +) +from ...product import get_product_calibration_contract +from ...sample_schema import ( + SampleDataContractError, + canonical_sample_record, + fitting_sample_record, + fitting_sample_records, + validate_sample_records, +) +from ...storage import append_jsonl, append_jsonl_many, atomic_write_json +from .reporting_zh import render_three_camera_status_text_zh +from .zero_solver import ( + LEFT_ZERO_PROFILE, + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, + RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, + JointAxisMeasurement, + PalmOrientationMeasurement, + UrdfKinematicModel, + ZeroCalibrationProfile, + ZeroSolveResult, + anchor_right_19_mechanical_endpoint_curves, + axis_line_cycle_rms_m, + axis_line_uses_depth_free_interpretation_plane, + baseline_hysteresis_by_cycle_rad, + circle_direction_is_constrained, + cross_view_side_line_source, + fit_joint_axis_measurement, + fit_partial_palm_orientation_measurements, + fit_rotation_joint_curve, + derive_right_19_mechanical_endpoint_offsets, + expand_right_19_thumb_zero_result_with_cad_fingers, + joint_curve_holdout_errors, + maximum_axis_line_cycle_spread_m, + measure_joint_curve_observation, + merge_right_19_thumb_zero_result, + refit_axis_line_group_with_shared_radius, + select_cross_view_roll_direction_source, + solve_urdf_zero_offsets, + get_zero_calibration_profile, + get_right_19_thumb_zero_profile, + write_zero_corrected_urdf, + with_depth_free_axis_projection, +) + + +STATE_PREFLIGHT = "PREFLIGHT" +STATE_WAIT_START = "WAIT_START" +STATE_IMPORTING_BASE = "IMPORTING_BASE" +STATE_REVALIDATING_INHERITED = "REVALIDATING_INHERITED" +STATE_RETURN_BASELINE = "RETURN_BASELINE" +STATE_PREPARE_SWEEP = "PREPARE_SWEEP" +STATE_SWEEP = "SWEEP" +STATE_FITTING = "FITTING" +STATE_VALIDATION_MOVE = "VALIDATION_MOVE" +STATE_VALIDATION_CAPTURE = "VALIDATION_CAPTURE" +STATE_PAUSED = "PAUSED" +STATE_ABORTED = "ABORTED" +STATE_COMPLETE = "COMPLETE" + +STEADY_COMMAND_CHECKPOINTS: tuple[int, ...] = ( + 255, 224, 192, 160, 128, 96, 64, 32, 0 +) + +COMBINATION_TAG_TARGETS_BY_VIEW: Mapping[ + str, tuple[tuple[str, str], ...] +] = { + "front": tuple( + (name, name) + for name in ( + "thumb_cmc_pitch", "thumb_mcp", "thumb_ip", + "index_mcp_roll", "middle_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", + ) + ), + "side": tuple( + (f"{finger}_{joint}", f"{finger}_{joint}") + for finger in ("index", "middle", "ring", "pinky") + for joint in ("pip", "dip") + ), + # The top-view Tag observes yaw but is mounted on the final CMC link, + # downstream of pitch in the URDF chain. + "top": (("thumb_cmc_yaw", "thumb_cmc_pitch"),), +} + +COMBINATION_BASE_OBSERVER_BY_VIEW = { + "front": "thumb_cmc_pitch", + "side": "index_pip", + "top": "thumb_cmc_yaw", +} + +COMBINATION_BASE_ROLE_BY_VIEW = { + "front": "front_base", + "side": "side_base", + "top": "top_base", +} + + +def combination_target_coverage( + observation_counts: Mapping[str, int], + validation_counts: Mapping[str, int], +) -> dict[str, Any]: + required = G20_COMBINATION_REQUIRED_TARGET_KEYS + observations = { + key: int(observation_counts.get(key, 0)) for key in required + } + validations = { + key: int(validation_counts.get(key, 0)) for key in required + } + missing_observations = sorted( + key for key, count in observations.items() if count < 2 + ) + missing_validations = sorted( + key for key, count in validations.items() if count < 1 + ) + return { + "required_targets": list(required), + "minimum_observations_per_target": 2, + "minimum_validations_per_target": 1, + "observation_counts": observations, + "validation_counts": validations, + "missing_observation_targets": missing_observations, + "missing_validation_targets": missing_validations, + "coverage_passed": not missing_observations and not missing_validations, + } + + +def _combination_observable_joints( + profile: HandCalibrationProfile, + view: str, + roles: Sequence[str], +) -> tuple[str, ...]: + """Return combination targets measurable from the currently visible Tags. + + Combination poses deliberately do not require all four adjacent fingers + to be visible at once. A joint is usable only when both Tags that define + its relative pose are present in the same camera frame. + """ + visible = {str(role) for role in roles} + targets = { + observation_name + for observation_name, _ in COMBINATION_TAG_TARGETS_BY_VIEW[view] + } + return tuple( + name + for name, spec in profile.record_specs.items() + if name in targets + and spec.measured + and spec.view == view + and spec.parent_role in visible + and spec.child_role in visible + ) + + +def _overall_progress( + state: str, + *, + scan_progress: float, + validation_index: int = 0, + validation_total: int = 0, +) -> float: + """Keep 100% reserved for a validated, written calibration result.""" + scan = float(np.clip(scan_progress, 0.0, 1.0)) + if state == STATE_COMPLETE: + return 1.0 + if state in {STATE_FITTING}: + return 0.92 + if state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE}: + validation = ( + 0.0 + if validation_total <= 0 + else float(np.clip(validation_index / validation_total, 0.0, 1.0)) + ) + return 0.92 + 0.07 * validation + return min(0.90, 0.90 * scan) + + +def _stamp_ns(stamp: Any) -> int: + return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) + + +def _safe_name(value: str) -> str: + safe = re.sub(r"[^A-Za-z0-9_.-]+", "_", str(value).strip()) + return safe or "UNSET" + + +def _sweep_storage_key(spec: SweepSpec) -> str | int: + """Keep legacy motor-keyed retry state while separating product tasks.""" + return spec.key if spec.task_name else int(spec.motor_index) + + +def _sweep_views( + profile: HandCalibrationProfile, spec: SweepSpec +) -> tuple[str, ...]: + """Return every camera that contributes a joint to one physical sweep.""" + views: list[str] = [] + for joint_name in spec.joints: + joint = profile.record_specs[joint_name] + if joint.view is not None and joint.view not in views: + views.append(joint.view) + return tuple(views or [spec.view]) + + +def _palm_axis_observer_for_sweep( + profile: HandCalibrationProfile, + spec: SweepSpec, + view: str, +) -> PalmAxisObserver | None: + """Return the one non-blocking palm observer attached to this task/view.""" + matches = tuple( + observer + for observer in profile.palm_axis_observers + if observer.task_name == spec.key and observer.view == str(view) + ) + if len(matches) > 1: + raise ValueError( + f"multiple palm-axis observers configured for {spec.key}:{view}" + ) + return matches[0] if matches else None + + +def _palm_axis_observer_schema( + profile: HandCalibrationProfile, +) -> list[dict[str, Any]]: + return [ + { + "source_name": item.source_name, + "task_name": item.task_name, + "view": item.view, + "parent_role": item.parent_role, + "child_role": item.child_role, + "model_joint": item.model_joint, + "motor_index": int(item.motor_index), + } + for item in profile.palm_axis_observers + ] + + +def _palm_axis_resume_policy( + profile: HandCalibrationProfile, + session_start: Mapping[str, Any], + compatibility_tokens: frozenset[str] | None = None, +) -> tuple[bool, tuple[str, ...]]: + """Validate optional palm-axis checkpoint data when a model uses it.""" + capability = "palm_axis_relative_motion_v3" + previous = {str(value) for value in session_start.get("capabilities", [])} + current = ( + frozenset(profile.capabilities) + if compatibility_tokens is None + else frozenset(compatibility_tokens) + ) + if not profile.palm_axis_observers and capability not in current: + return True, () + required_previous = set(current) - {capability} + if not required_previous.issubset(previous): + raise ValueError("resume checkpoint lacks required capabilities") + if capability in previous: + if session_start.get("palm_axis_observers") != ( + _palm_axis_observer_schema(profile) + ): + raise ValueError("resume palm-axis observer schema differs") + return True, () + # A new side-channel version changes the zero solver's observation model. + # Do not combine its fresh CMC axes with any trajectory captured under the + # previous PnP transaction semantics. This full invalidation happens only + # once; same-version checkpoints keep their normal task-level resume. + return False, tuple(spec.key for spec in profile.sweep_specs) + + +def _node_profile(node: Any) -> HandCalibrationProfile: + return getattr(node, "profile", LEFT_HAND_PROFILE) + + +def _command_names(node: Any) -> tuple[str, ...]: + profile = _node_profile(node) + names = tuple(getattr(profile, "command_names", ()) or COMMAND_NAMES) + if not names or len(set(names)) != len(names): + raise ValueError("product command names must be non-empty and unique") + return names + + +def _command_count(node: Any) -> int: + return len(_command_names(node)) + + +def _sweep_joints_for_view( + profile: HandCalibrationProfile, + spec: SweepSpec, + view: str, +) -> tuple[str, ...]: + return tuple( + joint_name + for joint_name in spec.joints + if profile.record_specs[joint_name].view == str(view) + ) + + +def resume_tasks_invalidated_by_tag_size_changes( + profile: HandCalibrationProfile, + previous_sizes_m_by_id: Mapping[int, float], + current_sizes_m_by_id: Mapping[int, float], +) -> tuple[tuple[int, ...], tuple[str, ...]]: + """Localise a corrected physical Tag size to dependent motion tasks. + + PnP translations stored in a checkpoint are already scaled by the Tag + size used during acquisition and cannot be repaired exactly afterwards. + A size-only product correction therefore invalidates every task that used + one of those Tags, while complete tasks whose required Tags are unchanged + remain valid sparse checkpoints. + """ + previous = { + int(key): float(value) + for key, value in previous_sizes_m_by_id.items() + } + current = { + int(key): float(value) + for key, value in current_sizes_m_by_id.items() + } + if set(previous) != set(current): + raise ValueError("resume Tag-size maps contain different Tag IDs") + if any( + not math.isfinite(value) or value <= 0.0 + for value in (*previous.values(), *current.values()) + ): + raise ValueError("resume Tag sizes must be finite and positive") + changed_ids = { + tag_id + for tag_id in current + if not math.isclose( + previous[tag_id], current[tag_id], rel_tol=0.0, abs_tol=1.0e-12 + ) + } + affected: list[str] = [] + base_role_by_view = { + "front": "front_base", + "side": "side_base", + "top": "top_base", + } + for spec in profile.sweep_specs: + used_ids: set[int] = set() + for view in _sweep_views(profile, spec): + used_ids.add(int(profile.view_tags[view][base_role_by_view[view]])) + for joint_name in _sweep_joints_for_view(profile, spec, view): + joint = profile.record_specs[joint_name] + for role in (joint.parent_role, joint.child_role): + if role is not None: + used_ids.add(int(profile.view_tags[view][role])) + if used_ids & changed_ids: + affected.append(spec.key) + return tuple(sorted(changed_ids)), tuple(affected) + + +def _fit_retry_joint_names( + profile: HandCalibrationProfile, + spec: SweepSpec, + failures: Sequence[Mapping[str, Any]], +) -> set[str]: + """Resolve the independently failed measurements in a physical task.""" + selected: set[str] = set() + validation_sources = profile.axis_validation_sources or {} + for failure in failures: + quality_sources = failure.get("quality_source_joints", ()) + if ( + isinstance(quality_sources, Sequence) + and not isinstance(quality_sources, (str, bytes)) + ): + selected_sources = { + str(source) + for source in quality_sources + if str(source) in spec.joints + } + if selected_sources: + selected.update(selected_sources) + continue + name = str(failure.get("joint", "")) + if str(failure.get("metric", "")) == "cross_view_roll_curve": + name = str(validation_sources.get(name, name)) + if name in spec.joints: + selected.add(name) + return selected or set(spec.joints) + + +def _isolated_axis_cycle_outliers( + axes: Sequence[np.ndarray], limit_rad: float +) -> set[int]: + """Return one unambiguous outlier outside an all-other-cycle cluster. + + At least three mutually consistent inlier cycles are required. Ambiguous + or generally scattered axes deliberately return an empty set so callers + retain the conservative full-task retry. + """ + if len(axes) < 4: + return set() + candidates: list[tuple[float, int]] = [] + for excluded in range(len(axes)): + inliers = [ + np.asarray(axis, dtype=float) + for index, axis in enumerate(axes) + if index != excluded + ] + consistent = True + maximum_inlier_difference = 0.0 + for left_index, left in enumerate(inliers): + for right in inliers[left_index + 1 :]: + difference = math.acos( + abs(float(np.clip(left @ right, -1.0, 1.0))) + ) + maximum_inlier_difference = max( + maximum_inlier_difference, difference + ) + if difference > float(limit_rad): + consistent = False + break + if not consistent: + break + if consistent: + candidates.append((maximum_inlier_difference, excluded)) + if not candidates: + return set() + candidates.sort() + best_spread, best_excluded = candidates[0] + # A gradual end-to-end drift can produce two different all-but-one + # subsets just under the formal limit. Localize only when one three-cycle + # cluster is substantially tighter than the acceptance band. + if best_spread > 0.5 * float(limit_rad): + return set() + if ( + len(candidates) > 1 + and abs(candidates[1][0] - best_spread) <= math.radians(0.01) + ): + return set() + return {best_excluded} + + +def _isolated_scalar_cycle_outlier( + values: Sequence[float], limit_rad: float +) -> set[int]: + """Return one clearly separated scalar outlier among three rounds. + + Zero-offset fits have three training cycles, so the axis helper above + cannot localize them (it deliberately requires three inliers). Use the + unique closest pair as a two-cycle consensus only when that pair is tight + and clearly better than either alternative. Symmetric drift and + generally scattered values retain the conservative full-task retry. + """ + samples = np.asarray(values, dtype=float) + limit = float(limit_rad) + if ( + samples.ndim != 1 + or samples.size != 3 + or not np.all(np.isfinite(samples)) + or not math.isfinite(limit) + or limit <= 0.0 + ): + return set() + pair_candidates = sorted( + ( + abs(float(samples[left] - samples[right])), + left, + right, + ) + for left in range(samples.size) + for right in range(left + 1, samples.size) + ) + pair_spread, left, right = pair_candidates[0] + second_pair_spread = pair_candidates[1][0] + overall_spread = pair_candidates[-1][0] + if overall_spread <= limit or pair_spread > 0.5 * limit: + return set() + # Do not arbitrarily pick one side of a gradual or nearly symmetric + # three-cycle drift. The winning pair must be materially tighter than + # the next candidate, not merely win due to floating-point noise. + if second_pair_spread - pair_spread < 0.1 * limit: + return set() + excluded = [ + index for index in range(samples.size) if index not in {left, right} + ] + if len(excluded) != 1: + return set() + outlier = excluded[0] + inlier_center = 0.5 * float(samples[left] + samples[right]) + if abs(float(samples[outlier]) - inlier_center) < 0.75 * limit: + return set() + return {outlier} + + +def _thumb_yaw_zero_repeatability_failures( + zero_result: ZeroSolveResult, + *, + maximum_cycle_range_rad: float, + maximum_confidence_half_width_rad: float, +) -> list[dict[str, Any]]: + """Apply a stricter, retry-aware publication gate to thumb CMC yaw.""" + joint_name = "thumb_cmc_yaw" + cycle_offsets = tuple( + float(value) + for value in zero_result.cycle_offsets_rad.get(joint_name, ()) + ) + failures: list[dict[str, Any]] = [] + if len(cycle_offsets) >= 2 and all( + math.isfinite(value) for value in cycle_offsets + ): + cycle_range = max(cycle_offsets) - min(cycle_offsets) + if cycle_range > float(maximum_cycle_range_rad): + failure: dict[str, Any] = { + "joint": joint_name, + "metric": "zero_cycle_offset_range_deg", + "actual": round(math.degrees(cycle_range), 6), + "limit": round( + math.degrees(maximum_cycle_range_rad), 6 + ), + "comparison": "maximum", + "cycle_offset_deg": [ + round(math.degrees(value), 6) + for value in cycle_offsets + ], + } + outliers = _isolated_scalar_cycle_outlier( + cycle_offsets, maximum_cycle_range_rad + ) + if outliers: + outlier = next(iter(outliers)) + failure["cycle"] = outlier + 1 + failure["inlier_cycles"] = [ + index + 1 + for index in range(len(cycle_offsets)) + if index != outlier + ] + failures.append(failure) + + # A range failure is more actionable: it can often identify exactly one + # acquisition cycle. Avoid adding a second, non-localized failure that + # would unnecessarily turn that retry back into a full yaw rescan. + if failures: + return failures + confidence = zero_result.offset_confidence_half_width_rad.get(joint_name) + if ( + confidence is not None + and math.isfinite(float(confidence)) + and float(confidence) > float(maximum_confidence_half_width_rad) + ): + failures.append( + { + "joint": joint_name, + "metric": "zero_confidence_95_half_width_deg", + "actual": round(math.degrees(float(confidence)), 6), + "limit": round( + math.degrees(maximum_confidence_half_width_rad), 6 + ), + "comparison": "maximum", + } + ) + return failures + + +def _previous_passed_joint_zero_offset( + session_dir: Path, serial_number: str, joint_name: str +) -> tuple[Path, float] | None: + """Read one zero from the previous passed sibling session, if present.""" + pointer = session_dir.parent / "latest_passed" + try: + previous = pointer.resolve(strict=True) + except OSError: + return None + if previous == session_dir or previous.parent != session_dir.parent: + return None + payload_path = previous / ( + f"g20_right_{_safe_name(serial_number)}_calibration.json" + ) + try: + payload = json.loads(payload_path.read_text(encoding="utf-8")) + offset = float( + payload["joints"][joint_name]["zero_angles"][ + "urdf_zero_offset_rad" + ] + ) + except (KeyError, OSError, TypeError, ValueError, json.JSONDecodeError): + return None + if not math.isfinite(offset): + return None + return previous, offset + + +def _isolated_axis_line_cycle_outliers( + measurements: Sequence[JointAxisMeasurement], limit_m: float +) -> set[int]: + """Return one cycle whose removal restores line-position repeatability. + + Four independently fitted near-parallel lines can contain one bad PnP + branch or one mechanically unsettled pass. When three cycles form a + clearly tighter cluster below the unchanged formal RMS limit, reacquire + only the excluded cycle. Ambiguous gradual drift still returns no + outlier and therefore keeps the conservative full four-cycle retry. + """ + if len(measurements) < 4 or float(limit_m) <= 0.0: + return set() + full_rms = axis_line_cycle_rms_m(measurements) + if full_rms <= float(limit_m): + return set() + candidates: list[tuple[float, int]] = [] + for excluded in range(len(measurements)): + inliers = [ + measurement + for index, measurement in enumerate(measurements) + if index != excluded + ] + inlier_rms = axis_line_cycle_rms_m(inliers) + if inlier_rms <= float(limit_m): + candidates.append((inlier_rms, excluded)) + if not candidates: + return set() + candidates.sort() + best_rms, best_excluded = candidates[0] + # Require a strong three-cycle cluster and a unique choice. This rejects + # the common gradual-drift case where excluding either endpoint happens + # to move a marginal RMS just below the gate. + if best_rms > 0.75 * float(limit_m): + return set() + if ( + len(candidates) > 1 + and candidates[1][0] - best_rms <= 0.10 * float(limit_m) + ): + return set() + return {best_excluded} + + +def _preserve_pnp_task_reference_for_sweep( + item: SweepItem, *, is_fit_retry: bool, has_precheck_anchor: bool +) -> bool: + """Keep the accepted IPPE branch after precheck and across retries.""" + return bool( + not item.precheck + and ( + item.cycle > 0 + or bool(is_fit_retry) + or bool(has_precheck_anchor) + ) + ) + + +def _requires_pnp_tracker_reset_for_sweep( + profile: HandCalibrationProfile, + item: SweepItem, + *, + is_fit_retry: bool, + preserve_retry_continuity: bool = False, +) -> bool: + """Reset G20-right PnP once per task, not once per formal cycle. + + The low-speed decreasing precheck establishes a branch with a static + endpoint window. Clearing frame-to-frame state at every later cycle made + four measurements of one rigid installation depend on four independent + planar-PnP initializations. Keep the tracker continuous for the whole + normal task transaction; a bounded retry remains a deliberately fresh + initialization while retaining the accepted task reference. + """ + if item.direction != DIRECTION_DECREASING: + return False + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return True + if is_fit_retry and preserve_retry_continuity: + # A dependent retry (currently the top-view thumb-yaw axis pair) + # replaces motion samples inside an already accepted task-relative + # PnP generation. Resetting here can move the two planar Tags to a + # different, internally smooth pose branch and shift every yaw zero + # by several degrees. Keep the exact live tracker/reference while + # replacing only the localized physical acquisition. + return False + if is_fit_retry: + return True + if profile.precheck_sweeps: + return bool(item.precheck) + return not item.precheck and item.cycle == 0 + + +def _fixed_base_role(view: str) -> str: + return { + "front": "front_base", + "side": "side_base", + "top": "top_base", + }[str(view)] + + +def _sweep_uses_locked_base_reference( + profile: HandCalibrationProfile, + spec: SweepSpec, + view: str, +) -> bool: + """Use one session-fixed base pose wherever task geometry needs it. + + Pinky/ring flexion is required to expose the middle/index side Tag, but + that same safe pose physically covers front Tag 0. The clearance remains + parked through that finger's roll, pitch and PIP tasks, including stages + where front is only an inactive status camera. Tag 0 is therefore locked + from baseline for every four-finger task; any moving Tag used by the + actual capture remains live and mandatory. + + Top Tag 8 has a different contract: it remains live and visible, but its + pose is frozen after preflight. Pitch and roll are collected in separate + tasks and jointly define thumb yaw. Re-solving a near-frontal planar base + Tag independently in those tasks expresses the two axes in subtly + different frames and turns image-position-dependent IPPE tilt into a yaw + zero shift. One session reference makes any fixed pose bias common to + both axes, where it cancels geometrically. + """ + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return False + selected_view = str(view) + if selected_view == "front": + return spec.key.startswith( + ("pinky_", "ring_", "middle_", "index_") + ) + return bool( + selected_view == "top" + and spec.key + in { + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + "thumb_cmc_yaw_top", + } + ) + + +def _maximum_corner_drift_px( + reference: Sequence[Sequence[float]], + current: Sequence[Sequence[float]], +) -> float: + """Return the largest same-corner displacement for one fixed Tag.""" + reference_array = np.asarray(reference, dtype=float) + current_array = np.asarray(current, dtype=float) + if ( + reference_array.shape != (4, 2) + or current_array.shape != (4, 2) + or not np.all(np.isfinite(reference_array)) + or not np.all(np.isfinite(current_array)) + ): + return float("inf") + return float( + np.max(np.linalg.norm(current_array - reference_array, axis=1)) + ) + + +def _selected_pose_qualities( + selected: Mapping[str, SquareTagPose], + live_qualities: Mapping[str, TagQuality], + *, + locked_base_role: str | None, + locked_base_quality: TagQuality | None, +) -> dict[str, TagQuality]: + """Attach PnP residuals without pretending a cached Tag is live. + + A hidden fixed palm Tag has no entry in ``live_qualities``. Its cached + pose can still be selected together with the live moving Tag, so quality + propagation must explicitly use the quality captured at baseline instead + of indexing a missing live detection. + """ + updated = dict(live_qualities) + for role, pose in selected.items(): + source = ( + locked_base_quality + if role == locked_base_role + else live_qualities.get(role) + ) + if source is None: + raise KeyError(f"selected role has no quality source: {role}") + updated[role] = TagQuality( + hamming=source.hamming, + decision_margin=source.decision_margin, + edge_pixels=source.edge_pixels, + reprojection_error_px=pose.reprojection_error_px, + ) + return updated + + +def _frames_for_joint( + frames: Sequence[Any], joint_name: str +) -> list[Any]: + """Filter an asynchronous multi-camera buffer to one joint observation.""" + return [ + frame + for frame in frames + if ( + not frame.joint_quaternions_xyzw + and not frame.joint_vectors_xyz_m + and not frame.image_vectors_xy_px + and not frame.parent_poses_common + and not frame.child_poses_common + ) + or ( + joint_name in frame.joint_quaternions_xyzw + and joint_name in frame.joint_vectors_xyz_m + and joint_name in frame.image_vectors_xy_px + and joint_name in frame.parent_poses_common + and joint_name in frame.child_poses_common + ) + ] + + +def _frames_cover_sweep_joints( + frames: Sequence[Any], spec: SweepSpec, minimum_per_joint: int +) -> bool: + minimum = max(1, int(minimum_per_joint)) + return all( + len(_frames_for_joint(frames, joint_name)) >= minimum + for joint_name in spec.joints + ) + + +def _records_have_formal_sweep_coverage( + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + joint_names: Sequence[str], + *, + repetitions: int, + minimum_bins: int, + maximum_bin_gap: int, +) -> bool: + """Return whether every persisted direction has formal bin coverage.""" + expected_groups = { + (cycle, direction) + for cycle in range(int(repetitions)) + for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) + } + if not joint_names or not expected_groups: + return False + for joint_name in joint_names: + grouped: dict[tuple[int, str], set[int]] = {} + for record in records_by_joint.get(joint_name, ()): + direction = str(record.get("direction", "")) + if direction not in { + DIRECTION_DECREASING, + DIRECTION_INCREASING, + }: + continue + command = int( + np.clip( + np.rint( + float( + record.get( + "command_u8", + record.get("feedback_u8", -1), + ) + ) + ), + 0, + 255, + ) + ) + grouped.setdefault( + (int(record.get("cycle", -1)), direction), set() + ).add(command) + if not expected_groups.issubset(grouped): + return False + for key in expected_groups: + commands = sorted(grouped[key]) + if ( + not commands + or commands[0] != 0 + or commands[-1] != 255 + or len(commands) < int(minimum_bins) + or max(np.diff(commands), default=0) > int(maximum_bin_gap) + ): + return False + return True + + +def _frames_cover_sweep_motion( + frames: Sequence[Any], + spec: SweepSpec, + *, + motor_index: int, + minimum_per_joint: int, + minimum_span_u8: float, +) -> bool: + for joint_name in spec.joints: + selected = _frames_for_joint(frames, joint_name) + if len(selected) < int(minimum_per_joint): + return False + values = [float(frame.state_u8[motor_index]) for frame in selected] + if max(values) - min(values) < float(minimum_span_u8): + return False + return True + + +def _joint_failure_reason( + reason: str, spec: SweepSpec, joint_name: str +) -> str: + return ( + str(reason) + if len(spec.joints) == 1 + else f"{reason}:{joint_name}" + ) + + +def _steady_checkpoint_commands( + profile: HandCalibrationProfile, item: "SweepItem" +) -> tuple[int, ...]: + """Return first-training-round steady commands after the start pose.""" + if ( + profile.layout_id != G20_RIGHT_19_LAYOUT + or item.precheck + or item.cycle != 0 + ): + return () + ordered = ( + STEADY_COMMAND_CHECKPOINTS + if item.direction == DIRECTION_DECREASING + else tuple(reversed(STEADY_COMMAND_CHECKPOINTS)) + ) + if int(ordered[0]) != item.start_u8 or int(ordered[-1]) != item.target_u8: + raise RuntimeError("steady command checkpoint direction is inconsistent") + return tuple(int(value) for value in ordered[1:]) + + +RESUMABLE_SAMPLE_KINDS = frozenset( + {"sample", "baseline_hold_sample", "steady_command_sample"} +) + +RECALIBRATION_SCOPES = frozenset({"full", "thumb", "fingers"}) + + +def recalibration_task_keys( + profile: HandCalibrationProfile, scope: str +) -> tuple[str, ...]: + """Return tasks that must be freshly acquired for a partial session.""" + selected = str(scope).strip().lower() + if selected not in RECALIBRATION_SCOPES: + raise ValueError( + "recalibration_scope must be one of: " + + ", ".join(sorted(RECALIBRATION_SCOPES)) + ) + if selected == "full": + return () + if ( + profile.side != "right" + or profile.layout_id != G20_RIGHT_19_LAYOUT + ): + raise ValueError( + "partial recalibration is supported only for the G20 right " + "19-Tag product" + ) + thumb_tasks = tuple( + spec.key + for spec in profile.sweep_specs + if any(str(name).startswith("thumb_") for name in spec.joints) + ) + if len(thumb_tasks) != 4: + raise ValueError( + "G20 right thumb recalibration must resolve exactly four tasks" + ) + if selected == "thumb": + return thumb_tasks + thumb_task_set = set(thumb_tasks) + return tuple( + spec.key + for spec in profile.sweep_specs + if spec.key not in thumb_task_set + ) + + +def recalibration_quality_joints( + profile: HandCalibrationProfile, scope: str +) -> tuple[str, ...]: + """Return joints whose quality is decided by freshly acquired data.""" + selected = str(scope).strip().lower() + if selected == "full": + return tuple(profile.measured_joints) + # Reuse the scope validation and product guard in the task selector. + recalibration_task_keys(profile, selected) + want_thumb = selected == "thumb" + return tuple( + name + for name in profile.measured_joints + if str(name).startswith("thumb_") == want_thumb + ) + + +def build_standalone_thumb_payload( + *, + profile: HandCalibrationProfile, + serial_number: str, + measured_fits: Mapping[str, JointCurveFit], + thumb_offsets_rad: Mapping[str, float], + validation_errors_rad: Sequence[float], + baseline_command_u8: Sequence[int], + source_urdf_sha256: str, + camera_extrinsics_sha256: str, + corrected_urdf_sha256: str, +) -> dict[str, Any]: + """Build a non-runtime artifact for a fully standalone thumb solve.""" + if profile.side != "right" or profile.layout_id != G20_RIGHT_19_LAYOUT: + raise ValueError("standalone thumb payload requires G20 right 19-Tag") + expected_fits = set(recalibration_quality_joints(profile, "thumb")) + if set(measured_fits) != expected_fits: + raise ValueError("standalone thumb fits have the wrong joint set") + expected_offsets = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + offsets = { + str(name): float(value) for name, value in thumb_offsets_rad.items() + } + if set(offsets) != expected_offsets or any( + not math.isfinite(value) for value in offsets.values() + ): + raise ValueError("standalone thumb offsets must contain four finite zeros") + if len(baseline_command_u8) != 20: + raise ValueError("standalone thumb baseline must contain 20 commands") + joints: dict[str, dict[str, Any]] = {} + for name in sorted(expected_fits): + fit = measured_fits[name] + curve = np.asarray(fit.angle_rad, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"standalone thumb curve is invalid: {name}") + spec = profile.joint_specs[name] + item: dict[str, Any] = { + "motor_index": int(spec.motor_index), + "angle_rad": [round(float(value), 8) for value in curve], + } + if name in offsets: + item["zero_command_u8"] = int( + baseline_command_u8[spec.motor_index] + ) + item["zero_angles"] = { + "urdf_zero_offset_rad": round(offsets[name], 8) + } + else: + item["passive"] = True + joints[name] = item + absolute_errors = np.abs( + np.asarray(validation_errors_rad, dtype=float) + ) + if absolute_errors.size == 0 or not np.all(np.isfinite(absolute_errors)): + raise ValueError("standalone thumb validation errors are missing") + return { + "schema_version": 1, + "artifact_type": "g20_right_standalone_thumb_calibration", + "model": "G20", + "side": "right", + "serial_number": str(serial_number), + "angle_unit": "rad", + "command_range": [0, 255], + "baseline_command_u8": [int(value) for value in baseline_command_u8], + "non_thumb_zero_policy": "source_cad_unchanged", + "joints": joints, + "quality": { + "passed": True, + "validation_mae_rad": round(float(np.mean(absolute_errors)), 8), + "validation_p95_rad": round( + float(np.percentile(absolute_errors, 95.0)), 8 + ), + }, + "hashes": { + "source_urdf_sha256": str(source_urdf_sha256), + "camera_extrinsics_sha256": str(camera_extrinsics_sha256), + "corrected_urdf_sha256": str(corrected_urdf_sha256), + }, + } + + +def _partial_scope_frozen_zero_offsets( + profile: HandCalibrationProfile, + scope: str, + payload: Mapping[str, Any], +) -> dict[str, float]: + """Return certified non-target zeros that a partial run must preserve.""" + selected = str(scope).strip().lower() + if selected == "full": + return {} + if selected not in {"thumb", "fingers"}: + raise ValueError(f"unsupported partial recalibration scope: {scope}") + joints = payload.get("joints") + if not isinstance(joints, Mapping): + raise ValueError("base calibration payload is missing joints") + freeze_thumb = selected == "fingers" + result: dict[str, float] = {} + for name in get_zero_calibration_profile( + profile.side, profile.layout_id + ).direct_zero_joints: + is_thumb = str(name).startswith("thumb_") + if is_thumb != freeze_thumb: + continue + try: + value = float( + joints[name]["zero_angles"]["urdf_zero_offset_rad"] + ) + except (KeyError, TypeError, ValueError) as error: + raise ValueError( + f"base calibration is missing certified zero for {name}" + ) from error + if not math.isfinite(value): + raise ValueError(f"base calibration zero is invalid for {name}") + result[name] = value + expected = 4 if freeze_thumb else 12 + if len(result) != expected: + raise ValueError( + f"partial {selected} scope must freeze exactly {expected} zeros" + ) + return result + + +def _normalise_legacy_split_roll_resume_rows( + profile: HandCalibrationProfile, + rows: Sequence[Mapping[str, Any]], +) -> tuple[dict[str, Any], ...]: + """Map the former front/side roll tasks onto one multiview checkpoint. + + This lets a calibration that was already running during the task-layout + upgrade donate all fully completed data if it later needs to resume. The + two historical motions remain independent observations; they are never + presented as timestamp-synchronised frames or accepted unless both joint + record sets pass the normal completeness checks. + """ + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return tuple(dict(row) for row in rows) + aliases = { + f"{finger}_roll_{view}": f"{finger}_roll_multiview" + for finger in ("pinky", "ring", "middle", "index") + for view in ("front", "side") + } + normalised: list[dict[str, Any]] = [] + for source in rows: + row = dict(source) + old_task = str(row.get("task_name", "")) + replacement = aliases.get(old_task) + if ( + replacement is not None + and row.get("kind") != "synchronised_frame" + ): + row["task_name"] = replacement + row["resume_source_task_name"] = old_task + normalised.append(row) + return tuple(normalised) + + +def _discard_automatically_retried_sweep_rows( + rows: Sequence[Mapping[str, Any]], +) -> tuple[dict[str, Any], ...]: + """Replay automatic-sweep retry tombstones before checkpoint selection. + + An automatic acquisition retry clears that cycle/direction from the + online in-memory stores. The raw JSONL is append-only, so the superseded + steady checkpoints remain physically present before the + ``automatic_sweep_retry`` event. Treat that event as a write-ahead + tombstone: records before it must never be merged with the replacement + acquisition written afterwards. + + Older sessions did not include ``task_name`` on retry events. Their + motor/cycle/direction and joint list still identify the invalidated + acquisition unambiguously. New sessions include both task and attempt + for a stronger durable identity. + """ + materialised = tuple(dict(row) for row in rows) + retry_events = [ + (index, row) + for index, row in enumerate(materialised) + if str(row.get("kind", "")) == "automatic_sweep_retry" + ] + if not retry_events: + return materialised + + durable_kinds = set(RESUMABLE_SAMPLE_KINDS) | { + "palm_axis_sample", + "synchronised_frame", + } + + def invalidated( + row_index: int, row: Mapping[str, Any] + ) -> bool: + if str(row.get("kind", "")) not in durable_kinds: + return False + row_task = str(row.get("task_name", "")) + row_cycle = int(row.get("cycle", -999)) + row_direction = str(row.get("direction", "")) + row_motor = int(row.get("motor_index", -1)) + row_joints = { + str(name) + for name in ( + row.get("joint"), + row.get("model_joint"), + *(row.get("joints", ()) or ()), + ) + if name + } + for retry_index, retry in retry_events: + if retry_index <= row_index: + continue + if int(retry.get("cycle", -998)) != row_cycle: + continue + if str(retry.get("direction", "")) != row_direction: + continue + retry_task = str(retry.get("task_name", "")) + if retry_task and row_task != retry_task: + continue + retry_motor = int(retry.get("motor_index", -1)) + if ( + row_motor >= 0 + and retry_motor >= 0 + and row_motor != retry_motor + ): + continue + retry_joints = { + str(name) for name in (retry.get("joints", ()) or ()) if name + } + if ( + row_joints + and retry_joints + and row_joints.isdisjoint(retry_joints) + ): + continue + return True + return False + + return tuple( + row + for index, row in enumerate(materialised) + if not invalidated(index, row) + ) + + +def _latest_resume_rows( + rows: Sequence[Mapping[str, Any]], + *, + kind: str, + minimum_sweep_bins: int = 0, +) -> dict[tuple[str, str, int, str], list[dict[str, Any]]]: + """Select the latest *complete* persisted attempt for each group. + + An interrupted retry must not hide the previous complete scan. This is + especially important for the nine steady checkpoints, which are written + before the continuous trajectory starts. + """ + selected = [ + dict(row) + for row in rows + if row.get("kind") == kind + and row.get("task_name") + and row.get("joint") + and row.get("direction") in {DIRECTION_DECREASING, DIRECTION_INCREASING} + ] + grouped: dict[ + tuple[str, str, int, str], dict[int, list[dict[str, Any]]] + ] = {} + for row in selected: + key = ( + str(row["task_name"]), + str(row["joint"]), + int(row.get("cycle", -1)), + str(row["direction"]), + ) + grouped.setdefault(key, {}).setdefault( + int(row.get("attempt", 1)), [] + ).append(row) + result: dict[tuple[str, str, int, str], list[dict[str, Any]]] = {} + for key, attempts in grouped.items(): + for _attempt, candidate in sorted( + attempts.items(), reverse=True + ): + if kind == "steady_command_sample": + requested = { + int(row.get("requested_command_u8", -1)) + for row in candidate + } + complete = requested == set(STEADY_COMMAND_CHECKPOINTS) + elif kind == "sample": + feedback_bins = { + int(round(float(row.get("feedback_u8", -1)))) + for row in candidate + } + complete = ( + 0 in feedback_bins + and 255 in feedback_bins + and len(feedback_bins) >= int(minimum_sweep_bins) + ) + else: + complete = bool(candidate) + if complete: + result[key] = candidate + break + return result + + +def _unresolved_fit_failure_tasks( + profile: HandCalibrationProfile, + rows: Sequence[Mapping[str, Any]], +) -> set[str]: + """Return tasks whose newest complete acquisition was still rejected. + + A newer model may deliberately retire a metric for a constrained joint. + Such historical failures are safe to replay because the final fit will + evaluate the imported raw samples against all current quality gates. + """ + sample_attempts: dict[str, int] = {} + for row in rows: + task_name = str(row.get("task_name", "")) + if row.get("kind") == "sample" and task_name: + sample_attempts[task_name] = max( + sample_attempts.get(task_name, 0), + int(row.get("attempt", 1)), + ) + zero_profile = get_zero_calibration_profile( + profile.side, profile.layout_id + ) + unresolved: set[str] = set() + for row in rows: + if row.get("kind") != "fit_failure": + continue + task_name = str(row.get("task_name", "")) + if not task_name: + matches = [ + spec + for spec in profile.sweep_specs + if spec.view == str(row.get("view", "")) + and spec.motor_index == int(row.get("motor_index", -1)) + and set(spec.joints) + == {str(name) for name in row.get("joints", [])} + ] + if len(matches) != 1: + continue + task_name = matches[0].key + failure_attempt = int(row.get("attempt", 1)) + if failure_attempt < sample_attempts.get(task_name, 0): + continue + failures = [ + item + for item in row.get("failures", []) + if isinstance(item, Mapping) + ] + def retired(item: Mapping[str, Any]) -> bool: + metric = str(item.get("metric", "")) + joint_name = str(item.get("joint", "")) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and metric == "rotation_orthogonal_rms_deg" + and joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + ): + return True + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and metric in { + "hysteresis_deg", + "command_direction_gap_deg", + } + ): + # Product curves now gate direction dependence with settled + # feedback-domain samples and final-cycle holdout. Historical + # failures that mixed velocity lag or firmware tracking + # deadband with mechanical hysteresis are safe to revalidate. + return True + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and joint_name + in RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS + and metric == "axis_pose_line_rms_mm" + ): + # Product v19 originally treated the correlated per-frame + # pose-line residual of an incorrectly scaled passive-DIP Tag as the + # uncertainty of its fitted axis centre. Current code + # revalidates those complete raw trajectories using radial, + # cross-cycle and holdout gates, so the old rejection itself + # must not force another eight-direction scan. + return True + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and metric == "cross_view_roll_curve" + and joint_name in (profile.axis_validation_sources or {}) + ): + # The side roll alias is a validation-only observation in the + # current product model. Its raw samples are still + # revalidated through side-line radial/cycle/holdout gates, + # but a historical front/side curve-shape warning must not + # make a complete task look unfinished on every restart. + return True + return metric in { + "rotation_circle_axis_difference_deg", + "axis_plane_rms_mm", + } and circle_direction_is_constrained( + joint_name, + zero_profile.constrained_circle_joints, + ) + + retired_by_current_model = bool(failures) and all( + retired(item) for item in failures + ) + if not retired_by_current_model: + unresolved.add(task_name) + return unresolved + + +def resumable_completed_task_prefix( + profile: HandCalibrationProfile, + repetitions: int, + baseline: Sequence[int], + rows: Sequence[Mapping[str, Any]], + *, + minimum_sweep_bins: int = 32, + allow_sparse: bool = False, +) -> tuple[tuple[str, ...], tuple[dict[str, Any], ...]]: + """Return quality-preserving complete tasks and their raw records. + + A partially written task is never reused. Every joint, all four cycles, + both directions, both endpoints, the two nine-point command curves and + any required directional baseline holds must be present before the whole + task becomes a durable restart checkpoint. The compatibility default + returns only a contiguous prefix; product resume can opt into sparse + recovery so a failed early task does not discard independent later tasks. + """ + rows = _discard_automatically_retried_sweep_rows( + _normalise_legacy_split_roll_resume_rows(profile, rows) + ) + samples = _latest_resume_rows( + rows, + kind="sample", + minimum_sweep_bins=minimum_sweep_bins, + ) + baselines = _latest_resume_rows(rows, kind="baseline_hold_sample") + checkpoints = _latest_resume_rows(rows, kind="steady_command_sample") + directions = (DIRECTION_DECREASING, DIRECTION_INCREASING) + expected_checkpoints = set(STEADY_COMMAND_CHECKPOINTS) + unresolved_fit_failures = _unresolved_fit_failure_tasks(profile, rows) + completed: list[str] = [] + selected_group_keys: set[tuple[str, tuple[str, str, int, str]]] = set() + + for spec in profile.sweep_specs: + if spec.key in unresolved_fit_failures: + if allow_sparse: + continue + break + complete = True + task_group_keys: set[ + tuple[str, tuple[str, str, int, str]] + ] = set() + for joint_name in spec.joints: + joint = profile.record_specs[joint_name] + needs_baseline_hold = int(baseline[joint.motor_index]) not in { + 0, + 255, + } + for cycle in range(int(repetitions)): + for direction in directions: + key = (spec.key, joint_name, cycle, direction) + sample_rows = samples.get(key, []) + feedback_bins = { + int(round(float(row.get("feedback_u8", -1)))) + for row in sample_rows + } + if ( + len(feedback_bins) < int(minimum_sweep_bins) + or 0 not in feedback_bins + or 255 not in feedback_bins + ): + complete = False + break + task_group_keys.add(("sample", key)) + if needs_baseline_hold: + if not baselines.get(key): + complete = False + break + task_group_keys.add(("baseline_hold_sample", key)) + if not complete: + break + if not complete: + break + for direction in directions: + key = (spec.key, joint_name, 0, direction) + requested = { + int(row.get("requested_command_u8", -1)) + for row in checkpoints.get(key, []) + } + if requested != expected_checkpoints: + complete = False + break + task_group_keys.add(("steady_command_sample", key)) + if not complete: + break + if not complete: + if allow_sparse: + continue + break + completed.append(spec.key) + selected_group_keys.update(task_group_keys) + + reusable: list[dict[str, Any]] = [] + grouped = { + "sample": samples, + "baseline_hold_sample": baselines, + "steady_command_sample": checkpoints, + } + for kind, key in selected_group_keys: + reusable.extend(grouped[kind][key]) + completed_set = set(completed) + latest_task_attempt = { + (key[0], key[2], key[3]): max( + int(row.get("attempt", 1)) for row in sample_rows + ) + for key, sample_rows in samples.items() + if key[0] in completed_set and sample_rows + } + reusable.extend( + dict(row) + for row in rows + if row.get("kind") == "synchronised_frame" + and str(row.get("task_name", "")) in completed_set + and int(row.get("attempt", 1)) + == latest_task_attempt.get( + ( + str(row.get("task_name", "")), + int(row.get("cycle", -1)), + str(row.get("direction", "")), + ), + -1, + ) + ) + reusable.extend( + dict(row) + for row in rows + if row.get("kind") == "palm_axis_sample" + and str(row.get("task_name", "")) in completed_set + and int(row.get("attempt", 1)) + == latest_task_attempt.get( + ( + str(row.get("task_name", "")), + int(row.get("cycle", -1)), + str(row.get("direction", "")), + ), + -1, + ) + ) + reusable.sort( + key=lambda row: ( + profile.sweep_specs.index( + next(spec for spec in profile.sweep_specs if spec.key == row["task_name"]) + ), + int(row.get("cycle", -1)), + 0 if row.get("direction") == DIRECTION_DECREASING else 1, + 0 if row.get("kind") == "synchronised_frame" else 1, + str(row.get("joint", "")), + float(row.get("feedback_u8", row.get("requested_command_u8", 0))), + ) + ) + return tuple(completed), tuple(reusable) + + +def _file_sha256(path: str | Path) -> str: + digest = hashlib.sha256() + with Path(path).open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _publish_import_status(node: Any) -> None: + """Publish synchronous import progress when running inside a ROS node.""" + publisher = getattr(node, "_publish_status", None) + if callable(publisher): + publisher(time.monotonic()) + + +def _steady_records_in_feedback_domain( + records: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + """Re-index settled poses by feedback for mechanical backlash checks. + + Runtime curves remain indexed by requested command. This companion + representation separates intrinsic hysteresis from the firmware's + direction-dependent command/feedback deadband. Endpoint records have + already passed their motor-specific reach gate, so they are snapped to + the exact endpoints required by the curve fitter. + """ + return fitting_sample_records( + records, + domain="feedback", + allow_legacy_command=True, + snap_requested_endpoints=True, + ) + + +def _classify_cross_view_roll_hysteresis( + front_values_deg: Sequence[float], + side_values_deg: Sequence[float], + *, + limit_deg: float, +) -> str: + """Classify a diagnostic without turning it into a formal acceptance.""" + front = np.asarray(front_values_deg, dtype=float) + side = np.asarray(side_values_deg, dtype=float) + if front.size == 0 or side.size == 0: + raise ValueError("cross-view diagnostic requires both camera results") + front_max = float(np.max(front)) + side_max = float(np.max(side)) + if front_max <= limit_deg and side_max <= limit_deg: + return "both_views_within_formal_hysteresis_limit" + if front_max > limit_deg and side_max > limit_deg: + return "both_views_confirm_direction_dependent_pose" + if front_max > limit_deg: + return "front_only_difference_check_roll_tag_bracket_or_front_pnp" + return "side_only_difference_check_side_tag_chain_or_side_pnp" + + +def _fit_failure_is_systematic( + failures: Sequence[Mapping[str, Any]], repetitions: int +) -> bool: + """Identify a repeatable model conflict that more motion cannot repair.""" + if not failures or int(repetitions) <= 1: + return False + if all(bool(item.get("systematic", False)) for item in failures): + return True + if any( + str(item.get("metric", "")) + != "rotation_circle_axis_difference_deg" + for item in failures + ): + return False + cycles = {int(item.get("cycle", -1)) for item in failures} + if cycles != set(range(1, int(repetitions) + 1)): + return False + actual = np.asarray([float(item["actual"]) for item in failures]) + limits = np.asarray([float(item["limit"]) for item in failures]) + if ( + not np.all(np.isfinite(actual)) + or not np.all(np.isfinite(limits)) + or np.any(limits <= 0.0) + ): + return False + # Every independent round is far outside the limit and agrees with the + # others. This is a fixed geometry/PnP model conflict, not random capture + # loss, so two full four-round retries would only reproduce it. + return bool( + np.all(actual >= 1.5 * limits) + and float(np.ptp(actual)) <= max(0.5, 0.2 * float(np.mean(actual))) + ) + + +def _cross_view_curve_failure( + primary_name: str, + validation_name: str, + primary: JointCurveFit, + validation: JointCurveFit, + *, + scope: str, + maximum_rms_difference_rad: float, + maximum_branch_gap_difference_rad: float, + allow_projection_scale: bool = False, + maximum_projection_scale_ratio: float = 1.5, + cycle_fits: Sequence[tuple[JointCurveFit, JointCurveFit]] = (), +) -> dict[str, Any] | None: + """Return a localized, retry-aware cross-view failure. + + A stable offset repeated in every cycle is an installation/model conflict; + reacquiring the same four motions cannot change it. A single bad cycle is + instead localized so only that side-view cycle is reacquired. + """ + try: + compare_cross_view_roll_curves( + primary, + validation, + maximum_rms_difference_rad=maximum_rms_difference_rad, + maximum_branch_gap_difference_rad=( + maximum_branch_gap_difference_rad + ), + allow_projection_scale=allow_projection_scale, + maximum_projection_scale_ratio=( + maximum_projection_scale_ratio + ), + ) + return None + except ValueError as error: + reason = str(error) + + metrics: dict[str, float] = {} + if reason != "cross_view_roll_curve_direction_disagrees": + metrics = compare_cross_view_roll_curves( + primary, + validation, + maximum_rms_difference_rad=float("inf"), + maximum_branch_gap_difference_rad=float("inf"), + allow_projection_scale=allow_projection_scale, + maximum_projection_scale_ratio=float("inf"), + ) + failure: dict[str, Any] = { + "joint": primary_name, + "metric": "cross_view_roll_curve", + "reason": reason, + "scope": scope, + "quality_source_joints": [validation_name], + } + metric_key: str | None = None + metric_limit: float | None = None + metric_in_degrees = True + if reason.startswith("cross_view_roll_curve_difference_too_large:"): + field_name = reason.split(":", 2)[1] + metric_key = f"{field_name}_rms_difference_rad" + metric_limit = float(maximum_rms_difference_rad) + elif reason.startswith( + "cross_view_roll_branch_gap_difference_too_large:" + ): + metric_key = "baseline_branch_gap_difference_rad" + metric_limit = float(maximum_branch_gap_difference_rad) + elif reason.startswith( + "cross_view_roll_projection_scale_ratio_too_large:" + ): + metric_key = "projection_scale_ratio" + metric_limit = float(maximum_projection_scale_ratio) + metric_in_degrees = False + + cycle_values_rad: list[float] = [] + cycle_direction_disagrees: list[bool] = [] + for primary_cycle, validation_cycle in cycle_fits: + try: + cycle_metrics = compare_cross_view_roll_curves( + primary_cycle, + validation_cycle, + maximum_rms_difference_rad=float("inf"), + maximum_branch_gap_difference_rad=float("inf"), + allow_projection_scale=allow_projection_scale, + maximum_projection_scale_ratio=float("inf"), + ) + except ValueError as cycle_error: + if str(cycle_error) != "cross_view_roll_curve_direction_disagrees": + raise + cycle_metrics = {} + if metric_key is not None: + value = cycle_metrics.get(metric_key) + if value is not None: + cycle_values_rad.append(float(value)) + primary_travel = float( + primary_cycle.angle_rad[0] - primary_cycle.angle_rad[255] + ) + validation_travel = float( + validation_cycle.angle_rad[0] + - validation_cycle.angle_rad[255] + ) + cycle_direction_disagrees.append( + primary_travel * validation_travel <= 0.0 + ) + + if metric_key is not None and metric_limit is not None: + actual_value = float(metrics[metric_key]) + rendered_actual = ( + math.degrees(actual_value) + if metric_in_degrees + else actual_value + ) + rendered_limit = ( + math.degrees(metric_limit) + if metric_in_degrees + else metric_limit + ) + failure.update( + { + "actual": round(rendered_actual, 6), + "limit": round(rendered_limit, 6), + "comparison": "maximum", + "unit": "deg" if metric_in_degrees else "ratio", + } + ) + if cycle_values_rad: + values = np.asarray(cycle_values_rad, dtype=float) + cycle_key = ( + "cycle_values_deg" + if metric_in_degrees + else "cycle_values_ratio" + ) + failure[cycle_key] = [ + round( + math.degrees(value) if metric_in_degrees else value, + 6, + ) + for value in values + ] + over = np.flatnonzero(values > metric_limit) + if ( + over.size == 1 + and np.all( + np.delete(values, over[0]) <= 0.9 * metric_limit + ) + ): + failure["cycle"] = int(over[0]) + 1 + # All cycles tell the same near-threshold story: this is a fixed + # view/mount/model bias, not random data that another scan heals. + if ( + np.all(values >= 0.85 * metric_limit) + and float(np.ptp(values)) <= 0.35 * metric_limit + ): + failure["systematic"] = True + failure["classification"] = ( + "stable_cross_view_installation_or_model_bias" + ) + elif reason == "cross_view_roll_curve_direction_disagrees" and ( + cycle_direction_disagrees + and all(cycle_direction_disagrees) + ): + failure["systematic"] = True + failure["classification"] = "stable_cross_view_direction_conflict" + return failure + + +def _fit_failure_repeats_branch_clusters( + previous: Sequence[Mapping[str, Any]], + current: Sequence[Mapping[str, Any]], +) -> bool: + """Detect the same cross-cycle PnP branch split on two full retries.""" + + def travel_failure( + failures: Sequence[Mapping[str, Any]], + ) -> Mapping[str, Any] | None: + return next( + ( + item + for item in failures + if str(item.get("metric")) == "cycle_travel_range_deg" + and isinstance(item.get("cycle_travel_deg"), Sequence) + ), + None, + ) + + left = travel_failure(previous) + right = travel_failure(current) + if left is None or right is None: + return False + left_values = np.asarray(left["cycle_travel_deg"], dtype=float) + right_values = np.asarray(right["cycle_travel_deg"], dtype=float) + if ( + left_values.shape != right_values.shape + or left_values.size < 3 + or not np.all(np.isfinite(left_values)) + or not np.all(np.isfinite(right_values)) + or np.max(np.abs(left_values - right_values)) > 1.0 + ): + return False + limit = float(right.get("limit", 0.0)) + if limit <= 0.0 or float(np.ptp(right_values)) <= limit: + return False + + previous_by_metric = { + (str(item.get("joint")), str(item.get("metric"))): item + for item in previous + if "actual" in item and "limit" in item + } + stable_model_failures = 0 + for item in current: + key = (str(item.get("joint")), str(item.get("metric"))) + old = previous_by_metric.get(key) + if old is None or key[1] == "cycle_travel_range_deg": + continue + actual = float(item.get("actual", float("nan"))) + old_actual = float(old.get("actual", float("nan"))) + item_limit = float(item.get("limit", 0.0)) + if ( + np.isfinite(actual) + and np.isfinite(old_actual) + and item_limit > 0.0 + and actual > item_limit + and old_actual > item_limit + and abs(actual - old_actual) <= max(0.25, 0.1 * item_limit) + ): + stable_model_failures += 1 + return stable_model_failures >= 1 + + +def _robust_pose_payload( + poses: Sequence[Mapping[str, Sequence[float]]], +) -> dict[str, list[float]]: + if not poses: + raise ValueError("at least one pose is required") + translation = np.median( + np.asarray([pose["translation_xyz_m"] for pose in poses], dtype=float), + axis=0, + ) + quaternion = robust_rotation_summary( + [pose["quaternion_xyzw"] for pose in poses] + )[0] + return { + "translation_xyz_m": [float(value) for value in translation], + "quaternion_xyzw": [float(value) for value in quaternion], + } + + +def _view_pairs( + view: str, reference_finger: str = "index" +) -> tuple[tuple[str, str], ...]: + if view == "front": + return ( + ("front_base", "thumb_cmc"), + ("thumb_cmc", "thumb_mcp"), + ("thumb_mcp", "thumb_ip"), + ("front_base", f"{reference_finger}_roll"), + ) + if view == "side": + return ( + ("side_base", "thumb_cmc"), + ("side_base", f"{reference_finger}_mcp"), + (f"{reference_finger}_mcp", f"{reference_finger}_pip"), + (f"{reference_finger}_pip", f"{reference_finger}_dip"), + ) + if view == "top": + return (("top_base", "thumb_yaw"),) + raise ValueError(f"unknown view {view}") + + +@dataclass(frozen=True) +class FrameObservation: + stamp_ns: int + received_at: float + view: str + state_u8: tuple[float, ...] + state_sync_error_ns: int + joint_vectors_xyz_m: Mapping[str, tuple[float, float, float]] + image_vectors_xy_px: Mapping[str, tuple[float, float]] + joint_quaternions_xyzw: Mapping[str, tuple[float, float, float, float]] + parent_poses_common: Mapping[str, Mapping[str, list[float]]] + child_poses_common: Mapping[str, Mapping[str, list[float]]] + joint_reprojection_error_px: Mapping[str, float] + + +@dataclass(frozen=True) +class SweepItem: + spec: SweepSpec + cycle: int + direction: str + precheck: bool = False + + @property + def start_u8(self) -> int: + return 255 if self.direction == DIRECTION_DECREASING else 0 + + @property + def target_u8(self) -> int: + return 0 if self.direction == DIRECTION_DECREASING else 255 + + +def _build_sweep_plan( + profile: HandCalibrationProfile, repetitions: int +) -> tuple[SweepItem, ...]: + """Build one deterministic qualification-and-measurement transaction. + + Each product task owns a low-speed outbound/return qualification followed + by fixed-order formal cycles. Keeping this construction in one place + prevents resume, progress and runtime transitions from silently inventing + different task lifecycles. + """ + plan: list[SweepItem] = [] + for spec in profile.sweep_specs: + if profile.precheck_sweeps: + plan.extend( + SweepItem(spec, -1, direction, precheck=True) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ) + plan.extend( + SweepItem(spec, cycle, direction) + for cycle in range(int(repetitions)) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ) + return tuple(plan) + + +def _sweep_plan_transition( + profile: HandCalibrationProfile, + completed: SweepItem, + following: SweepItem, +) -> str: + """Return the only allowed boundary between two planned sweep items.""" + if completed.spec != following.spec: + return "task_change" + # Both qualification directions and the qualification-to-formal handoff + # are one continuous transaction. The physical endpoint and task + # clearance pose are unchanged, so a baseline detour can only discard + # good observations and perturb the PnP branch. + if completed.precheck: + return "immediate_reverse" + if completed.direction == DIRECTION_INCREASING: + # Formal cycles deliberately re-establish the same mechanical approach + # between repetitions so hysteresis measurements remain comparable. + return "cycle_reset" + return "immediate_reverse" + + +@dataclass(frozen=True) +class ValidationItem: + spec: SweepSpec + command_u8: int + + +@dataclass(frozen=True) +class CombinationValidationItem: + name: str + label_zh: str + command_u8: tuple[int, ...] + + +def _combination_validation_items( + baseline: Sequence[int], +) -> tuple[CombinationValidationItem, ...]: + if len(baseline) != 20: + raise ValueError("combination baseline must contain 20 commands") + base = [int(value) for value in baseline] + + def item( + name: str, label: str, changes: Mapping[int, int] + ) -> CombinationValidationItem: + values = list(base) + for index, value in changes.items(): + values[int(index)] = int(value) + return CombinationValidationItem(name, label, tuple(values)) + + return ( + item("all_open", "全开", {}), + item("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}), + item("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}), + item("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}), + item("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}), + item("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}), + item( + "half_grip", + "四指半握", + {1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160}, + ), + item( + "light_pinch", + "轻捏", + {0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176}, + ), + ) + + +def _combination_joint_angles( + profile: HandCalibrationProfile, + measured_fits: Mapping[str, JointCurveFit], + command_u8: Sequence[int], + motor_directions: Sequence[str], +) -> dict[str, float]: + """Evaluate a combination pose on the branch used to reach it. + + Using ``angle_rad`` here averages the increasing and decreasing branches; + on a serial chain (especially the thumb) those half-hysteresis errors + accumulate and can reject an otherwise correct pose. + """ + if len(command_u8) != 20 or len(motor_directions) != 20: + raise ValueError("combination commands and directions require 20 motors") + angles: dict[str, float] = {} + for name, spec in profile.joint_specs.items(): + target = int(command_u8[spec.motor_index]) + fit = measured_fits[name] + direction = str(motor_directions[spec.motor_index]) + if direction == DIRECTION_DECREASING: + curve = fit.decreasing_rad + elif direction == DIRECTION_INCREASING: + curve = fit.increasing_rad + else: + raise ValueError(f"invalid combination direction: {direction}") + angles[name] = float(curve[target]) + return angles + + +def _combination_motor_directions( + items: Sequence[CombinationValidationItem], + pose_index: int, + baseline_u8: Sequence[int], +) -> tuple[str, ...]: + """Reconstruct the direction in which every motor reached a pose. + + Pose zero establishes the all-open Tag mounts. Each later pose starts + from that baseline. A motor moved by an earlier pose returns along the + opposite branch and stays on that branch until another pose moves it. + """ + if len(baseline_u8) != 20: + raise ValueError("combination baseline must contain 20 motors") + if pose_index < 0 or pose_index >= len(items): + raise ValueError("combination pose index is out of range") + baseline = tuple(int(value) for value in baseline_u8) + directions = [DIRECTION_DECREASING] * 20 + for previous in items[1:pose_index]: + for motor, target in enumerate(previous.command_u8): + if int(target) < baseline[motor]: + directions[motor] = DIRECTION_INCREASING + elif int(target) > baseline[motor]: + directions[motor] = DIRECTION_DECREASING + current = items[pose_index] + for motor, target in enumerate(current.command_u8): + if int(target) < baseline[motor]: + directions[motor] = DIRECTION_DECREASING + elif int(target) > baseline[motor]: + directions[motor] = DIRECTION_INCREASING + return tuple(directions) + + +def _model_link_in_observer_base( + observer_base_common: Sequence[Sequence[float]], + model_base_common: Sequence[Sequence[float]], + model_link_from_base: Sequence[Sequence[float]], +) -> np.ndarray: + """Express a URDF link in the fixed palm Tag's coordinate frame.""" + observer = np.asarray(observer_base_common, dtype=float) + model_base = np.asarray(model_base_common, dtype=float) + link = np.asarray(model_link_from_base, dtype=float) + if any(value.shape != (4, 4) for value in (observer, model_base, link)): + raise ValueError("combination transforms must be 4x4 matrices") + return np.linalg.inv(observer) @ model_base @ link + + +@dataclass +class ViewRuntime: + name: str + tag_size_m: float + preflight_frames: int + tracker: SquareTagPoseTracker + group_tracker: SquareTagGroupPoseTracker + view_tags: Mapping[str, int] = field(default_factory=dict) + tag_sizes_m_by_role: Mapping[str, float] = field(default_factory=dict) + preflight_roles: tuple[str, ...] = () + role_by_id: dict[int, str] = field(default_factory=dict) + group_trackers: dict[ + tuple[str, ...], SquareTagGroupPoseTracker + ] = field(default_factory=dict) + current_required_roles: tuple[str, ...] = () + camera_matrix: np.ndarray | None = None + camera_info_valid: bool = False + camera_frame: str = "" + image_width: int = 0 + image_height: int = 0 + intrinsics_sha256: str = "" + extrinsics_valid: bool = False + valid_flags: deque[bool] = field(default_factory=deque) + detection_times: deque[float] = field(default_factory=deque) + # Task-scoped capture validity: only frames judged against an active + # task's required roles are counted. The rolling valid_flags window is + # cleared whenever the required-role set changes, so right after a task + # finishes it silently switches to scoring idle frames against the full + # preflight role set — the wrong measurement for a per-task data gate. + task_valid_frames: int = 0 + task_total_frames: int = 0 + latest_tag_quality: dict[str, TagQuality] = field(default_factory=dict) + latest_pnp_rejections: dict[str, str] = field(default_factory=dict) + latest_group_pnp_reason: str = "" + latest_group_missing_candidate_roles: tuple[str, ...] = () + # Task-scoped counters preserve the reasons that occurred before the last + # camera frame. A final ``group_initializing:5/8`` frame must not hide a + # recurring per-Tag rejection or a completed-window geometry rejection. + pnp_rejection_counts: dict[str, int] = field(default_factory=dict) + group_pnp_rejection_counts: dict[str, int] = field(default_factory=dict) + pnp_initialization_progress: tuple[int, int] | None = None + latest_pnp_valid: bool = False + last_pnp_diagnostic_signature: tuple[Any, ...] | None = None + pnp_invalid_since: float | None = None + pnp_reset_count: int = 0 + last_message_at: float = 0.0 + last_valid_at: float = 0.0 + fixed_base_observations: deque[ + tuple[SquareTagPose, tuple[float, float], TagQuality] + ] = field(default_factory=deque) + fixed_base_corner_observations: deque[np.ndarray] = field( + default_factory=deque + ) + locked_base_pose: SquareTagPose | None = None + locked_base_center_xy_px: tuple[float, float] | None = None + locked_base_corners_xy: tuple[ + tuple[float, float], + tuple[float, float], + tuple[float, float], + tuple[float, float], + ] | None = None + locked_base_quality: TagQuality | None = None + locked_base_corner_drift_count: int = 0 + latest_locked_base_corner_drift_px: float = 0.0 + + def __post_init__(self) -> None: + self.role_by_id = { + tag_id: role for role, tag_id in self.view_tags.items() + } + self.valid_flags = deque(maxlen=self.preflight_frames) + self.detection_times = deque(maxlen=self.preflight_frames) + self.fixed_base_observations = deque(maxlen=self.preflight_frames) + self.fixed_base_corner_observations = deque( + maxlen=self.preflight_frames + ) + if not self.preflight_roles: + self.preflight_roles = tuple(self.view_tags) + if any(role not in self.view_tags for role in self.preflight_roles): + raise ValueError("preflight roles must be configured for the view") + if any(role not in self.view_tags for role in self.tag_sizes_m_by_role): + raise ValueError("Tag-size roles must be configured for the view") + if any( + not math.isfinite(float(size)) or float(size) <= 0.0 + for size in self.tag_sizes_m_by_role.values() + ): + raise ValueError("Tag sizes must be finite and positive") + self.current_required_roles = tuple(self.preflight_roles) + self.group_trackers[tuple(self.preflight_roles)] = self.group_tracker + + @property + def roles(self) -> tuple[str, ...]: + return tuple(self.view_tags) + + @property + def valid_rate(self) -> float: + if not self.valid_flags: + return 0.0 + return float(sum(self.valid_flags) / len(self.valid_flags)) + + @property + def detection_hz(self) -> float: + if len(self.detection_times) < 2: + return 0.0 + elapsed = self.detection_times[-1] - self.detection_times[0] + return 0.0 if elapsed <= 0.0 else (len(self.detection_times) - 1) / elapsed + + +class G20ThreeCameraCalibrationNode(Node): + """Own all hand commands while observing three fixed Hikrobot views.""" + + def __init__(self) -> None: + super().__init__("g20_calibration") + self._declare_parameters() + self._load_parameters() + + self.session_dir.mkdir(parents=True, exist_ok=True) + self.raw_path = self.session_dir / "raw_samples.jsonl" + self.final_path = self.session_dir / ( + f"g20_{self.hand_type}_{_safe_name(self.serial_number)}_calibration.json" + ) + + self.views = { + name: self._make_view_runtime(name) + for name in ("front", "side", "top") + } + self.latest_state_u8: tuple[float, ...] = () + self.last_state_at = 0.0 + self.state_history: deque[StateSample] = deque(maxlen=1200) + self.state_receive_times: deque[float] = deque(maxlen=300) + self.latest_hand_info: dict[str, Any] = {} + self.commanded_speed_profile: tuple[int, ...] = () + self.speed_commanded_at = 0.0 + + self.state = STATE_PREFLIGHT + self.reason = "waiting_for_devices_and_sdk" + self.sample_schema_version = 1 + self.base_import_progress: dict[str, Any] = {} + self.paused_reason = "" + self.started = False + # Startup has two deliberately separate gates. Camera/SDK health is + # enough to let us recover the hand from an interrupted pose; fixed + # palm Tags are checked only after that recovery reaches baseline. + self.startup_baseline_recovered = False + self.abort_original_reason = "" + self.baseline_after = "" + self.position_hold_since: float | None = None + self.motion_stage_started_at = 0.0 + self.return_waypoints: deque[tuple[int, ...]] = deque() + self.preparation_waypoints: deque[tuple[int, ...]] = deque() + self.preparation_command_u8: tuple[int, ...] = tuple( + self.baseline_command + ) + + self.sweep_items: list[SweepItem] = [] + self.sweep_index = 0 + self.active_sweep: SweepItem | None = None + # Owns required Tag roles and PnP continuity across the brief + # active_sweep=None interval used for same-task cycle resets. + self.pnp_task_spec: SweepSpec | None = None + self.sweep_frames: list[FrameObservation] = [] + # Roll joints use command 127 as their zero. A frame acquired while + # merely passing 127 contains velocity/latency error and is not a + # valid backlash observation. Keep a separate buffer populated only + # after the motor has reached 127 and the commanded mid-sweep hold has + # started. + self.sweep_baseline_frames: list[FrameObservation] = [] + self.sweep_baseline_pending = False + self.sweep_baseline_hold_since: float | None = None + self.sweep_checkpoint_commands: deque[int] = deque() + self.sweep_checkpoint_mode = False + self.sweep_checkpoint_target_u8: int | None = None + self.sweep_checkpoint_hold_since: float | None = None + self.sweep_checkpoint_frames: list[FrameObservation] = [] + # Keep synchronised endpoint observations acquired while the motor is + # held at the sweep start. If these frames are discarded and the + # target is commanded immediately, a fast motor can leave the endpoint + # before the next camera/state pair arrives. + self.sweep_start_frames: list[FrameObservation] = [] + # A completed direction already supplies fresh, synchronised frames at + # the physical endpoint where the immediately reversed direction + # starts. Stage those frames across the state transition instead of + # discarding them and waiting again at the unchanged pose. + self.carried_sweep_start_frames: list[FrameObservation] = [] + self.sweep_started_at = 0.0 + self.sweep_last_valid_at = 0.0 + self.sweep_last_valid_at_by_view: dict[str, float] = {} + self.sweep_endpoint_since: float | None = None + self.sweep_detection_total_frames = 0 + self.sweep_detection_valid_frames = 0 + self.sweep_detection_total_by_view: dict[str, int] = {} + self.sweep_detection_valid_by_view: dict[str, int] = {} + self.precheck_speed_metrics: dict[ + str, dict[str, dict[str, float | int]] + ] = {} + self.formal_speed_scales: dict[str, float] = {} + self.motion_progress_reference_error_u8 = float("inf") + self.motion_last_progress_at = 0.0 + self.motion_stall_details: dict[str, Any] = {} + self.retry_sweep_spec: SweepSpec | None = None + self.retry_resume_index: int | None = None + self.retry_sweep_items: list[SweepItem] = [] + self.retry_joint_names: set[str] = set() + # Some fitted quantities are observed by data collected during other + # physical tasks. In particular, right-hand thumb yaw zero comes from + # the two top-view side channels captured during CMC pitch and roll. + self.retry_source_failure_task_key: str | None = None + self.retry_source_task_keys: tuple[str, ...] = () + self.retry_cycle_override: set[int] = set() + self.retry_preserve_pnp_continuity = False + self.active_sweep_is_fit_retry = False + self.fit_failure: dict[str, Any] = {} + self.fit_failure_history_by_task: dict[ + str, list[list[dict[str, Any]]] + ] = {} + self.cross_view_roll_diagnostic_result: dict[str, Any] = {} + self.sweep_attempts: dict[str | int, int] = { + _sweep_storage_key(spec): 1 for spec in self.profile.sweep_specs + } + self.sweep_retry_counts: dict[tuple[str | int, int, str], int] = {} + self.motion_retry_counts: dict[str, int] = {} + self.validation_retry_counts: dict[tuple[str | int, int], int] = {} + self.records_by_joint: dict[str, list[dict[str, Any]]] = { + name: [] for name in self.profile.record_joints + } + self.baseline_records_by_joint: dict[str, list[dict[str, Any]]] = { + name: [] for name in self.profile.record_joints + } + self.command_records_by_joint: dict[str, list[dict[str, Any]]] = { + name: [] for name in self.profile.record_joints + } + self.palm_axis_records_by_source: dict[ + str, list[dict[str, Any]] + ] = { + observer.source_name: [] + for observer in self.profile.palm_axis_observers + } + + self.measured_fits: dict[str, JointCurveFit] = {} + self.axis_measurements: list[JointAxisMeasurement] = [] + self.palm_orientation_measurements: list[ + PalmOrientationMeasurement + ] = [] + self.palm_orientation_rejections: dict[str, str] = {} + self.zero_result: ZeroSolveResult | None = None + self.validated_endpoint_zero_offsets_rad: dict[str, float] = {} + self.corrected_urdf_path: Path | None = None + self.fit_quality_passed = False + + self.validation_items: list[ValidationItem] = [] + self.validation_index = 0 + self.active_validation: ValidationItem | None = None + self.active_validation_direction: str | None = None + self.validation_frames_buffer: list[FrameObservation] = [] + self.validation_errors_rad: list[float] = [] + self.validation_stage_started_at = 0.0 + self.combination_validation_items: list[CombinationValidationItem] = [] + self.combination_validation_index = 0 + self.active_combination_validation: CombinationValidationItem | None = None + self.combination_validation_frames_buffer: dict[ + str, list[FrameObservation] + ] = {view: [] for view in ("front", "side", "top")} + self.combination_validation_completed = False + self.combination_tag_mounts: dict[str, np.ndarray] = {} + self.combination_tag_observation_counts: dict[str, int] = {} + self.combination_tag_validation_counts: dict[str, int] = {} + self.combination_position_errors_m: list[float] = [] + self.combination_orientation_errors_rad: list[float] = [] + self.completed_payload: dict[str, Any] | None = None + + self.command_publisher = self.create_publisher( + JointState, self.command_topic, 1 + ) + self.setting_publisher = self.create_publisher( + String, self.setting_topic, 10 + ) + self.status_publisher = self.create_publisher(String, "~/status", 10) + self.status_text_publisher = self.create_publisher( + String, "~/status_text", 10 + ) + self.create_subscription( + JointState, self.state_topic, self._state_callback, 10 + ) + self.create_subscription( + String, self.info_topic, self._info_callback, 10 + ) + for name, runtime in self.views.items(): + del runtime + self.create_subscription( + CameraInfo, + self.camera_info_topics[name], + lambda message, view=name: self._camera_info_callback( + view, message + ), + qos_profile_sensor_data, + ) + self.create_subscription( + AprilTagDetectionArray, + self.detections_topics[name], + lambda message, view=name: self._detections_callback( + view, message + ), + qos_profile_sensor_data, + ) + + self.create_service(Trigger, "~/start", self._start_callback) + self.create_service(Trigger, "~/pause", self._pause_callback) + self.create_service(Trigger, "~/resume", self._resume_callback) + self.create_service(Trigger, "~/abort", self._abort_callback) + self.last_status_publish = 0.0 + self.timer = self.create_timer(0.05, self._timer_callback) + self.get_logger().info( + f"Three-camera calibration session: {self.session_dir}" + ) + + def _declare_parameters(self) -> None: + self.declare_parameter("model", "G20") + self.declare_parameter("hand_type", "left") + self.declare_parameter("tag_layout", "legacy_11") + self.declare_parameter("serial_number", "UNSET") + self.declare_parameter("session_dir", "calibration_output/session") + self.declare_parameter("resume_raw_samples_path", "") + self.declare_parameter("recalibration_scope", "full") + self.declare_parameter("camera_extrinsics_file", "") + self.declare_parameter("source_urdf_path", "") + self.declare_parameter("source_urdf_expected_sha256", "") + self.declare_parameter("corrected_urdf_output_dir", "") + self.declare_parameter("commands_enabled", True) + self.declare_parameter("command_topic", "/g20/cb_left_hand_control_cmd") + self.declare_parameter("state_topic", "/g20/cb_left_hand_state") + self.declare_parameter("info_topic", "/g20/cb_left_hand_info") + self.declare_parameter( + "setting_topic", "/g20/cb_hand_setting_cmd" + ) + for view in ("front", "side", "top"): + self.declare_parameter(f"{view}_camera_serial", "") + self.declare_parameter( + f"{view}_camera_info_topic", + f"/g20_calibration/{view}/camera/camera_info", + ) + self.declare_parameter( + f"{view}_detections_topic", + f"/g20_calibration/{view}/apriltag/detections", + ) + self.declare_parameter("tag_size_m", 0.016) + self.declare_parameter( + "tag_size_override_ids", Parameter.Type.INTEGER_ARRAY + ) + self.declare_parameter( + "tag_size_overrides_m", Parameter.Type.DOUBLE_ARRAY + ) + self.declare_parameter( + "baseline_command_u8", list(THREE_CAMERA_BASELINE_COMMAND) + ) + self.declare_parameter("normal_calibration_speed", 15) + self.declare_parameter("index_roll_calibration_speed", 5) + self.declare_parameter("index_flex_calibration_speed", 10) + self.declare_parameter("adaptive_formal_speed_enabled", True) + self.declare_parameter("adaptive_formal_speed_max_scale", 1.5) + self.declare_parameter("adaptive_formal_speed_minimum_bins", 64) + self.declare_parameter("adaptive_formal_speed_maximum_bin_gap", 8) + self.declare_parameter("speed_setting_settle_seconds", 0.25) + self.declare_parameter("parallel_pose_transitions", True) + self.declare_parameter("repetitions", 3) + self.declare_parameter("g20_right_19_repetitions", 4) + self.declare_parameter("preflight_frames", 60) + self.declare_parameter("minimum_detection_rate", 0.95) + self.declare_parameter("minimum_detection_hz", 15.0) + self.declare_parameter("minimum_feedback_hz", 25.0) + self.declare_parameter("maximum_hamming", 0) + self.declare_parameter("minimum_decision_margin", 30.0) + self.declare_parameter("minimum_edge_pixels", 30.0) + self.declare_parameter("pnp_maximum_reprojection_error_px", 1.5) + self.declare_parameter("pnp_reprojection_tie_px", 1.5) + self.declare_parameter("pnp_maximum_pose_jump_deg", 35.0) + self.declare_parameter("pnp_maximum_translation_jump_m", 0.04) + self.declare_parameter("pnp_maximum_tag_tilt_deg", 75.0) + self.declare_parameter("pnp_tracker_reset_seconds", 5.0) + self.declare_parameter("pnp_group_initialization_frames", 8) + self.declare_parameter("pnp_group_normal_alignment_scale_deg", 5.0) + self.declare_parameter("pnp_group_maximum_normal_alignment_deg", 15.0) + self.declare_parameter("fixed_base_maximum_corner_drift_px", 2.0) + self.declare_parameter("fixed_base_movement_confirmation_frames", 5) + self.declare_parameter("thumb_ip_pnp_coupling_multiplier", 1.03) + self.declare_parameter("thumb_ip_pnp_coupling_scale_deg", 3.0) + self.declare_parameter( + "thumb_ip_pnp_maximum_coupling_residual_deg", 7.5 + ) + self.declare_parameter("top_pnp_invalid_reset_seconds", 1.0) + self.declare_parameter("maximum_state_image_skew_ms", 50.0) + self.declare_parameter("axis_maximum_plane_rms_m", 0.003) + self.declare_parameter("passive_axis_maximum_plane_rms_m", 0.004) + self.declare_parameter("axis_maximum_radial_rms_m", 0.003) + self.declare_parameter("axis_maximum_pose_line_rms_m", 0.001) + self.declare_parameter("axis_maximum_rotation_circle_difference_deg", 1.0) + self.declare_parameter( + "active_maximum_rotation_orthogonal_rms_deg", 2.5 + ) + self.declare_parameter( + "passive_maximum_rotation_orthogonal_rms_deg", 7.5 + ) + self.declare_parameter("zero_maximum_axis_cycle_difference_deg", 0.75) + self.declare_parameter( + "thumb_yaw_maximum_zero_cycle_difference_deg", 0.5 + ) + self.declare_parameter( + "thumb_yaw_maximum_confidence_half_width_deg", 0.75 + ) + self.declare_parameter( + "thumb_yaw_cross_session_diagnostic_deg", 0.75 + ) + self.declare_parameter("zero_maximum_axis_cone_mismatch_deg", 5.0) + self.declare_parameter( + "zero_maximum_observability_condition_number", 1.0e10 + ) + self.declare_parameter("zero_maximum_offset_deg", 20.0) + self.declare_parameter("zero_finger_maximum_offset_deg", 3.0) + self.declare_parameter("mechanical_endpoint_maximum_offset_deg", 5.0) + self.declare_parameter("endpoint_tolerance_u8", 2.0) + self.declare_parameter( + "synchronised_endpoint_tolerance_margin_u8", 1.0 + ) + self.declare_parameter( + "steady_checkpoint_command_feedback_tolerance_u8", 8.0 + ) + self.declare_parameter( + "steady_checkpoint_maximum_feedback_range_u8", 2.0 + ) + self.declare_parameter("thumb_yaw_zero_endpoint_tolerance_u8", 4.0) + self.declare_parameter( + "right_thumb_yaw_255_endpoint_tolerance_u8", 5.0 + ) + self.declare_parameter("pinky_pip_zero_endpoint_tolerance_u8", 5.0) + self.declare_parameter("endpoint_hold_seconds", 0.5) + self.declare_parameter("baseline_hold_seconds", 0.5) + self.declare_parameter("minimum_baseline_hold_frames", 10) + self.declare_parameter("task_precheck_hold_seconds", 2.0) + self.declare_parameter("position_timeout_seconds", 30.0) + self.declare_parameter("sweep_timeout_seconds", 90.0) + self.declare_parameter("motor_stall_timeout_seconds", 5.0) + self.declare_parameter("motor_stall_startup_grace_seconds", 1.0) + self.declare_parameter("motor_stall_minimum_progress_u8", 1.0) + self.declare_parameter("invalid_timeout_seconds", 3.0) + self.declare_parameter("minimum_sweep_frames", 40) + self.declare_parameter("minimum_state_span_u8", 240.0) + self.declare_parameter("minimum_sweep_bins", 32) + self.declare_parameter("maximum_bin_gap", 16) + self.declare_parameter("automatic_sweep_retry_limit", 3) + self.declare_parameter("automatic_fit_retry_limit", 2) + self.declare_parameter("automatic_motion_retry_limit", 2) + self.declare_parameter("cross_view_roll_diagnostic_finger", "") + self.declare_parameter("provisional_warning_ratio", 1.25) + self.declare_parameter("retry_minimum_speed", 3) + self.declare_parameter("retry_speed_scales", [0.8, 0.6, 0.5]) + self.declare_parameter( + "retry_endpoint_hold_seconds", [0.75, 1.0, 1.25] + ) + self.declare_parameter("trajectory_maximum_plane_rms_m", 0.004) + self.declare_parameter("trajectory_maximum_radial_rms_m", 0.004) + self.declare_parameter("trajectory_minimum_radius_m", 0.003) + self.declare_parameter("trajectory_minimum_arc_deg", 15.0) + self.declare_parameter("image_trajectory_maximum_radial_rms_px", 2.0) + self.declare_parameter("image_trajectory_maximum_radial_p95_px", 3.5) + self.declare_parameter("image_trajectory_minimum_radius_px", 20.0) + self.declare_parameter( + "trajectory_maximum_cycle_travel_difference_deg", 3.0 + ) + self.declare_parameter( + "passive_maximum_cycle_travel_difference_deg", 10.0 + ) + self.declare_parameter("maximum_monotonic_correction_deg", 2.0) + self.declare_parameter("maximum_hysteresis_deg", 5.0) + self.declare_parameter("baseline_maximum_hysteresis_deg", 0.5) + self.declare_parameter( + "directional_zero_maximum_branch_gap_deg", 1.5 + ) + self.declare_parameter( + "directional_zero_maximum_branch_gap_range_deg", 0.3 + ) + self.declare_parameter( + "cross_view_roll_maximum_branch_gap_difference_deg", 0.3 + ) + self.declare_parameter( + "cross_view_roll_maximum_shape_rms_deg", 1.25 + ) + self.declare_parameter( + "cross_view_roll_maximum_projection_scale_ratio", 1.5 + ) + self.declare_parameter( + "cross_view_roll_maximum_axis_difference_deg", 15.0 + ) + self.declare_parameter("passive_maximum_monotonic_correction_deg", 3.0) + self.declare_parameter("passive_maximum_hysteresis_deg", 7.5) + self.declare_parameter("command_maximum_direction_gap_deg", 2.0) + self.declare_parameter("validation_enabled", False) + self.declare_parameter("combination_validation_enabled", True) + self.declare_parameter("combination_validation_frames", 10) + self.declare_parameter("combination_maximum_position_p95_m", 0.003) + self.declare_parameter("combination_maximum_orientation_p95_deg", 2.0) + self.declare_parameter("validation_command_count", 3) + self.declare_parameter("validation_frames", 10) + self.declare_parameter("validation_seed", 20260804) + self.declare_parameter("validation_timeout_seconds", 20.0) + self.declare_parameter("maximum_validation_mae_deg", 1.0) + self.declare_parameter("maximum_validation_p95_deg", 2.0) + self.declare_parameter("maximum_validation_error_deg", 1.5) + self.declare_parameter("zero_maximum_confidence_half_width_deg", 0.5) + + def _load_parameters(self) -> None: + def value(name: str) -> Any: + return self.get_parameter(name).value + + def optional_array(name: str) -> Sequence[Any]: + # An empty YAML sequence has no element type. With a typed ROS 2 + # declaration it can therefore remain NOT_SET and ``.value`` + # raises instead of returning an empty list. These overrides are + # optional, so preserve the default-size semantics in that case. + try: + result = value(name) + except ParameterUninitializedException: + return () + return () if result is None else result + + self.model = str(value("model")).strip().upper() + self.hand_type = str(value("hand_type")).lower() + self.tag_layout = str(value("tag_layout")).lower() + product_contract = get_product_calibration_contract( + self.model, self.hand_type, self.tag_layout + ) + self.profile = product_contract.profile + self.zero_profile = product_contract.zero_profile + self.calibration_profile = product_contract.typed_profile + self.serial_number = str(value("serial_number")) + if self.serial_number == "UNSET": + raise ValueError("serial_number is required") + self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() + resume_value = str(value("resume_raw_samples_path")).strip() + self.resume_raw_samples_path = ( + None + if not resume_value + else Path(resume_value).expanduser().resolve() + ) + self.recalibration_scope = str( + value("recalibration_scope") + ).strip().lower() + self.recalibration_task_keys = recalibration_task_keys( + self.profile, self.recalibration_scope + ) + self.standalone_thumb_calibration = bool( + self.recalibration_scope == "thumb" + and self.resume_raw_samples_path is None + ) + if ( + self.recalibration_scope == "fingers" + and self.resume_raw_samples_path is None + ): + raise ValueError( + "finger recalibration requires resume_raw_samples_path from " + "a passed thumb or complete session" + ) + self.partial_scope_fixed_zero_offsets_rad: dict[str, float] = {} + if ( + self.recalibration_scope != "full" + and self.resume_raw_samples_path is not None + ): + assert self.resume_raw_samples_path is not None + base_payload_path = self.resume_raw_samples_path.parent / ( + f"g20_right_{_safe_name(self.serial_number)}_calibration.json" + ) + try: + base_payload = json.loads( + base_payload_path.read_text(encoding="utf-8") + ) + except (OSError, json.JSONDecodeError) as error: + raise ValueError( + "partial recalibration base calibration JSON is invalid" + ) from error + if not isinstance(base_payload, Mapping): + raise ValueError( + "partial recalibration base calibration JSON must be an object" + ) + self.partial_scope_fixed_zero_offsets_rad = ( + _partial_scope_frozen_zero_offsets( + self.profile, + self.recalibration_scope, + base_payload, + ) + ) + self.resumed_task_keys: tuple[str, ...] = () + self.resume_source_session = "" + self.camera_extrinsics_file = Path( + str(value("camera_extrinsics_file")) + ).expanduser().resolve() + self.source_urdf_path = Path( + str(value("source_urdf_path")) + ).expanduser().resolve() + self.source_urdf_expected_sha256 = str( + value("source_urdf_expected_sha256") + ).strip().lower() + output_value = str(value("corrected_urdf_output_dir")) + self.corrected_urdf_output_dir = ( + Path(output_value).expanduser().resolve() + if output_value + else self.source_urdf_path.parent + ) + self.camera_serials = { + view: str(value(f"{view}_camera_serial")) + for view in ("front", "side", "top") + } + self.extrinsics: ThreeCameraExtrinsics | None = None + self.extrinsics_error = "" + try: + self.extrinsics = load_three_camera_extrinsics( + self.camera_extrinsics_file + ) + except Exception as error: + self.extrinsics_error = str(error) + self.commands_enabled = bool(value("commands_enabled")) + self.command_topic = str(value("command_topic")) + self.state_topic = str(value("state_topic")) + self.info_topic = str(value("info_topic")) + if self.hand_type == "right": + if self.command_topic == "/g20/cb_left_hand_control_cmd": + self.command_topic = "/g20/cb_right_hand_control_cmd" + if self.state_topic == "/g20/cb_left_hand_state": + self.state_topic = "/g20/cb_right_hand_state" + if self.info_topic == "/g20/cb_left_hand_info": + self.info_topic = "/g20/cb_right_hand_info" + self.setting_topic = str(value("setting_topic")) + self.camera_info_topics = { + view: str(value(f"{view}_camera_info_topic")) + for view in ("front", "side", "top") + } + self.detections_topics = { + view: str(value(f"{view}_detections_topic")) + for view in ("front", "side", "top") + } + self.tag_size_m = float(value("tag_size_m")) + override_ids = tuple( + int(item) for item in optional_array("tag_size_override_ids") + ) + override_sizes = tuple( + float(item) for item in optional_array("tag_size_overrides_m") + ) + if len(override_ids) != len(override_sizes): + raise ValueError( + "tag_size_override_ids and tag_size_overrides_m must have " + "the same length" + ) + if len(set(override_ids)) != len(override_ids): + raise ValueError("tag_size_override_ids must be unique") + configured_tag_ids = { + int(tag_id) + for tags in self.profile.view_tags.values() + for tag_id in tags.values() + } + if ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and any(tag_id not in configured_tag_ids for tag_id in override_ids) + ): + raise ValueError("Tag-size override contains an unconfigured Tag ID") + if ( + not math.isfinite(self.tag_size_m) + or self.tag_size_m <= 0.0 + or any( + not math.isfinite(size) or size <= 0.0 + for size in override_sizes + ) + ): + raise ValueError("Tag sizes must be finite and positive") + self.tag_sizes_m_by_id = { + tag_id: self.tag_size_m for tag_id in configured_tag_ids + } + self.tag_sizes_m_by_id.update( + (tag_id, size) + for tag_id, size in zip(override_ids, override_sizes) + if tag_id in configured_tag_ids + ) + self.baseline_command = tuple( + int(item) for item in value("baseline_command_u8") + ) + self.return_command_u8 = tuple(self.baseline_command) + self.normal_calibration_speed = int(value("normal_calibration_speed")) + self.index_roll_calibration_speed = int( + value("index_roll_calibration_speed") + ) + self.index_flex_calibration_speed = int( + value("index_flex_calibration_speed") + ) + self.adaptive_formal_speed_enabled = bool( + value("adaptive_formal_speed_enabled") + ) + self.adaptive_formal_speed_max_scale = float( + value("adaptive_formal_speed_max_scale") + ) + self.adaptive_formal_speed_minimum_bins = int( + value("adaptive_formal_speed_minimum_bins") + ) + self.adaptive_formal_speed_maximum_bin_gap = int( + value("adaptive_formal_speed_maximum_bin_gap") + ) + self.speed_setting_settle_seconds = float( + value("speed_setting_settle_seconds") + ) + self.repetitions = int(value("repetitions")) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + self.repetitions = int(value("g20_right_19_repetitions")) + self.preflight_frames = int(value("preflight_frames")) + self.minimum_detection_rate = float(value("minimum_detection_rate")) + self.minimum_detection_hz = float(value("minimum_detection_hz")) + self.minimum_feedback_hz = float(value("minimum_feedback_hz")) + self.maximum_hamming = int(value("maximum_hamming")) + self.minimum_decision_margin = float(value("minimum_decision_margin")) + self.minimum_edge_pixels = float(value("minimum_edge_pixels")) + self.pnp_maximum_reprojection_error_px = float( + value("pnp_maximum_reprojection_error_px") + ) + self.pnp_reprojection_tie_px = float(value("pnp_reprojection_tie_px")) + self.pnp_maximum_pose_jump_rad = math.radians( + float(value("pnp_maximum_pose_jump_deg")) + ) + self.pnp_maximum_translation_jump_m = float( + value("pnp_maximum_translation_jump_m") + ) + self.pnp_maximum_tag_tilt_rad = math.radians( + float(value("pnp_maximum_tag_tilt_deg")) + ) + self.pnp_tracker_reset_seconds = float(value("pnp_tracker_reset_seconds")) + self.pnp_group_initialization_frames = int( + value("pnp_group_initialization_frames") + ) + self.pnp_group_normal_alignment_scale_rad = math.radians( + float(value("pnp_group_normal_alignment_scale_deg")) + ) + self.pnp_group_maximum_normal_alignment_rad = math.radians( + float(value("pnp_group_maximum_normal_alignment_deg")) + ) + self.fixed_base_maximum_corner_drift_px = float( + value("fixed_base_maximum_corner_drift_px") + ) + self.fixed_base_movement_confirmation_frames = int( + value("fixed_base_movement_confirmation_frames") + ) + self.thumb_ip_pnp_coupling_multiplier = float( + value("thumb_ip_pnp_coupling_multiplier") + ) + self.thumb_ip_pnp_coupling_scale_rad = math.radians( + float(value("thumb_ip_pnp_coupling_scale_deg")) + ) + self.thumb_ip_pnp_maximum_coupling_residual_rad = math.radians( + float(value("thumb_ip_pnp_maximum_coupling_residual_deg")) + ) + self.top_pnp_invalid_reset_seconds = float( + value("top_pnp_invalid_reset_seconds") + ) + self.maximum_state_image_skew_ns = int( + float(value("maximum_state_image_skew_ms")) * 1_000_000.0 + ) + self.axis_maximum_plane_rms_m = float( + value("axis_maximum_plane_rms_m") + ) + self.passive_axis_maximum_plane_rms_m = float( + value("passive_axis_maximum_plane_rms_m") + ) + self.axis_maximum_radial_rms_m = float( + value("axis_maximum_radial_rms_m") + ) + self.axis_maximum_pose_line_rms_m = float( + value("axis_maximum_pose_line_rms_m") + ) + self.axis_maximum_rotation_circle_difference_rad = math.radians( + float(value("axis_maximum_rotation_circle_difference_deg")) + ) + self.active_maximum_rotation_orthogonal_rms_rad = math.radians( + float(value("active_maximum_rotation_orthogonal_rms_deg")) + ) + self.passive_maximum_rotation_orthogonal_rms_rad = math.radians( + float(value("passive_maximum_rotation_orthogonal_rms_deg")) + ) + self.zero_maximum_axis_cycle_difference_rad = math.radians( + float(value("zero_maximum_axis_cycle_difference_deg")) + ) + self.thumb_yaw_maximum_zero_cycle_difference_rad = math.radians( + float(value("thumb_yaw_maximum_zero_cycle_difference_deg")) + ) + self.thumb_yaw_maximum_confidence_half_width_rad = math.radians( + float(value("thumb_yaw_maximum_confidence_half_width_deg")) + ) + self.thumb_yaw_cross_session_diagnostic_rad = math.radians( + float(value("thumb_yaw_cross_session_diagnostic_deg")) + ) + self.zero_maximum_axis_cone_mismatch_rad = math.radians( + float(value("zero_maximum_axis_cone_mismatch_deg")) + ) + self.zero_maximum_observability_condition_number = float( + value("zero_maximum_observability_condition_number") + ) + self.zero_maximum_offset_rad = math.radians( + float(value("zero_maximum_offset_deg")) + ) + self.zero_finger_maximum_offset_rad = math.radians( + float(value("zero_finger_maximum_offset_deg")) + ) + self.mechanical_endpoint_maximum_offset_rad = math.radians( + float(value("mechanical_endpoint_maximum_offset_deg")) + ) + self.zero_joint_maximum_offsets_rad = {} + self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) + self.synchronised_endpoint_tolerance_margin_u8 = float( + value("synchronised_endpoint_tolerance_margin_u8") + ) + self.steady_checkpoint_command_feedback_tolerance_u8 = float( + value("steady_checkpoint_command_feedback_tolerance_u8") + ) + self.steady_checkpoint_maximum_feedback_range_u8 = float( + value("steady_checkpoint_maximum_feedback_range_u8") + ) + self.thumb_yaw_zero_endpoint_tolerance_u8 = float( + value("thumb_yaw_zero_endpoint_tolerance_u8") + ) + self.right_thumb_yaw_255_endpoint_tolerance_u8 = float( + value("right_thumb_yaw_255_endpoint_tolerance_u8") + ) + self.pinky_pip_zero_endpoint_tolerance_u8 = float( + value("pinky_pip_zero_endpoint_tolerance_u8") + ) + self.endpoint_hold_seconds = float(value("endpoint_hold_seconds")) + self.baseline_hold_seconds = float(value("baseline_hold_seconds")) + self.minimum_baseline_hold_frames = int( + value("minimum_baseline_hold_frames") + ) + self.task_precheck_hold_seconds = float( + value("task_precheck_hold_seconds") + ) + self.position_timeout_seconds = float(value("position_timeout_seconds")) + self.parallel_pose_transitions = bool( + value("parallel_pose_transitions") + ) + self.sweep_timeout_seconds = float(value("sweep_timeout_seconds")) + self.motor_stall_timeout_seconds = float( + value("motor_stall_timeout_seconds") + ) + self.motor_stall_startup_grace_seconds = float( + value("motor_stall_startup_grace_seconds") + ) + self.motor_stall_minimum_progress_u8 = float( + value("motor_stall_minimum_progress_u8") + ) + self.invalid_timeout_seconds = float(value("invalid_timeout_seconds")) + self.minimum_sweep_frames = int(value("minimum_sweep_frames")) + self.minimum_state_span_u8 = float(value("minimum_state_span_u8")) + self.minimum_sweep_bins = int(value("minimum_sweep_bins")) + self.maximum_bin_gap = int(value("maximum_bin_gap")) + self.automatic_sweep_retry_limit = int( + value("automatic_sweep_retry_limit") + ) + self.automatic_fit_retry_limit = int( + value("automatic_fit_retry_limit") + ) + self.cross_view_roll_diagnostic_finger = str( + value("cross_view_roll_diagnostic_finger") + ).strip().lower() + self.automatic_motion_retry_limit = int( + value("automatic_motion_retry_limit") + ) + self.provisional_warning_ratio = float( + value("provisional_warning_ratio") + ) + self.retry_minimum_speed = int(value("retry_minimum_speed")) + self.retry_speed_scales = tuple( + float(item) for item in value("retry_speed_scales") + ) + self.retry_endpoint_hold_seconds = tuple( + float(item) for item in value("retry_endpoint_hold_seconds") + ) + self.trajectory_maximum_plane_rms_m = float( + value("trajectory_maximum_plane_rms_m") + ) + self.trajectory_maximum_radial_rms_m = float( + value("trajectory_maximum_radial_rms_m") + ) + self.trajectory_minimum_radius_m = float( + value("trajectory_minimum_radius_m") + ) + self.trajectory_minimum_arc_rad = math.radians( + float(value("trajectory_minimum_arc_deg")) + ) + self.image_trajectory_maximum_radial_rms_px = float( + value("image_trajectory_maximum_radial_rms_px") + ) + self.image_trajectory_maximum_radial_p95_px = float( + value("image_trajectory_maximum_radial_p95_px") + ) + self.image_trajectory_minimum_radius_px = float( + value("image_trajectory_minimum_radius_px") + ) + self.trajectory_maximum_cycle_travel_difference_rad = math.radians( + float(value("trajectory_maximum_cycle_travel_difference_deg")) + ) + self.passive_maximum_cycle_travel_difference_rad = math.radians( + float(value("passive_maximum_cycle_travel_difference_deg")) + ) + self.maximum_monotonic_correction_rad = math.radians( + float(value("maximum_monotonic_correction_deg")) + ) + self.maximum_hysteresis_rad = math.radians( + float(value("maximum_hysteresis_deg")) + ) + self.baseline_maximum_hysteresis_rad = math.radians( + float(value("baseline_maximum_hysteresis_deg")) + ) + self.directional_zero_maximum_branch_gap_rad = math.radians( + float(value("directional_zero_maximum_branch_gap_deg")) + ) + self.directional_zero_maximum_branch_gap_range_rad = math.radians( + float(value("directional_zero_maximum_branch_gap_range_deg")) + ) + self.cross_view_roll_maximum_branch_gap_difference_rad = math.radians( + float(value("cross_view_roll_maximum_branch_gap_difference_deg")) + ) + self.cross_view_roll_maximum_shape_rms_rad = math.radians( + float(value("cross_view_roll_maximum_shape_rms_deg")) + ) + self.cross_view_roll_maximum_projection_scale_ratio = float( + value("cross_view_roll_maximum_projection_scale_ratio") + ) + self.cross_view_roll_maximum_axis_difference_rad = math.radians( + float(value("cross_view_roll_maximum_axis_difference_deg")) + ) + self.passive_maximum_monotonic_correction_rad = math.radians( + float(value("passive_maximum_monotonic_correction_deg")) + ) + self.passive_maximum_hysteresis_rad = math.radians( + float(value("passive_maximum_hysteresis_deg")) + ) + self.command_maximum_direction_gap_rad = math.radians( + float(value("command_maximum_direction_gap_deg")) + ) + self.validation_enabled = bool(value("validation_enabled")) + self.combination_validation_enabled = bool( + value("combination_validation_enabled") + ) + if self.standalone_thumb_calibration: + # A standalone thumb artifact has no four-finger curves or + # Cartesian combination model to validate. + self.combination_validation_enabled = False + self.combination_validation_frames = int( + value("combination_validation_frames") + ) + self.combination_maximum_position_p95_m = float( + value("combination_maximum_position_p95_m") + ) + self.combination_maximum_orientation_p95_rad = math.radians( + float(value("combination_maximum_orientation_p95_deg")) + ) + self.validation_command_count = int(value("validation_command_count")) + self.validation_frames = int(value("validation_frames")) + self.validation_seed = int(value("validation_seed")) + self.validation_timeout_seconds = float( + value("validation_timeout_seconds") + ) + self.maximum_validation_mae_rad = math.radians( + float(value("maximum_validation_mae_deg")) + ) + self.maximum_validation_p95_rad = math.radians( + float(value("maximum_validation_p95_deg")) + ) + self.maximum_validation_error_rad = math.radians( + float(value("maximum_validation_error_deg")) + ) + self.zero_maximum_confidence_half_width_rad = math.radians( + float(value("zero_maximum_confidence_half_width_deg")) + ) + if len(self.baseline_command) != self.profile.command_count: + raise ValueError( + "baseline_command_u8 length does not match the registered " + f"product command schema ({self.profile.command_count})" + ) + if any(value < 0 or value > 255 for value in self.baseline_command): + raise ValueError("baseline_command_u8 values must be in [0, 255]") + if self.baseline_command != tuple(self.profile.baseline_command): + raise ValueError( + "baseline_command_u8 differs from the registered product " + "calibration baseline" + ) + if any(not serial for serial in self.camera_serials.values()): + raise ValueError("front/side/top camera serial parameters are required") + if not self.source_urdf_path.is_file(): + raise ValueError( + f"source_urdf_path does not exist: {self.source_urdf_path}" + ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + if re.fullmatch( + r"[0-9a-f]{64}", self.source_urdf_expected_sha256 + ) is None: + raise ValueError( + "g20_right_19 requires source_urdf_expected_sha256 from " + "the CAD/hardware owner; unconfirmed source CAD cannot " + "produce a formal corrected URDF" + ) + actual_source_hash = _file_sha256(self.source_urdf_path) + if actual_source_hash != self.source_urdf_expected_sha256: + raise ValueError( + "source_urdf_path SHA-256 does not match the confirmed " + "G20 CAD hash" + ) + source_model = UrdfKinematicModel(self.source_urdf_path) + thumb_ip_model = source_model.joints.get("thumb_ip") + if ( + thumb_ip_model is None + or thumb_ip_model.mimic_joint != "thumb_mcp" + or abs(thumb_ip_model.mimic_offset) > 1.0e-9 + ): + raise ValueError( + "source URDF thumb_ip mimic structure is invalid" + ) + source_name = self.source_urdf_path.name.lower() + opposite = "right" if self.hand_type == "left" else "left" + if f"g20_{opposite}" in source_name: + raise ValueError( + "source_urdf_path hand side does not match hand_type" + ) + if ( + "zero_calibrated" in self.source_urdf_path.stem.lower() + or re.search( + r"calibrated_20\d{6}", + self.source_urdf_path.stem.lower(), + ) + ): + raise ValueError( + "source_urdf_path must be the original CAD URDF" + ) + if not 0 <= self.normal_calibration_speed <= 255: + raise ValueError("normal_calibration_speed must be in [0, 255]") + if not 0 <= self.index_roll_calibration_speed <= 255: + raise ValueError("index_roll_calibration_speed must be in [0, 255]") + if not 0 <= self.index_flex_calibration_speed <= 255: + raise ValueError("index_flex_calibration_speed must be in [0, 255]") + if not 1.0 <= self.adaptive_formal_speed_max_scale <= 2.0: + raise ValueError( + "adaptive_formal_speed_max_scale must be in [1, 2]" + ) + if ( + self.adaptive_formal_speed_minimum_bins + < self.minimum_sweep_bins + ): + raise ValueError( + "adaptive_formal_speed_minimum_bins cannot be below the " + "formal minimum_sweep_bins" + ) + if not ( + 1 + <= self.adaptive_formal_speed_maximum_bin_gap + <= self.maximum_bin_gap + ): + raise ValueError( + "adaptive_formal_speed_maximum_bin_gap must be in " + "[1, maximum_bin_gap]" + ) + if self.speed_setting_settle_seconds < 0.0: + raise ValueError("speed_setting_settle_seconds must be non-negative") + if not 0.0 <= self.endpoint_tolerance_u8 <= 10.0: + raise ValueError("endpoint_tolerance_u8 must be in [0, 10]") + if not 0.0 <= self.synchronised_endpoint_tolerance_margin_u8 <= 4.0: + raise ValueError( + "synchronised_endpoint_tolerance_margin_u8 must be in [0, 4]" + ) + if not ( + self.endpoint_tolerance_u8 + <= self.steady_checkpoint_command_feedback_tolerance_u8 + <= 16.0 + ): + raise ValueError( + "steady_checkpoint_command_feedback_tolerance_u8 must be " + "between endpoint_tolerance_u8 and 16" + ) + if not 0.0 < self.steady_checkpoint_maximum_feedback_range_u8 <= 4.0: + raise ValueError( + "steady_checkpoint_maximum_feedback_range_u8 must be in (0, 4]" + ) + if not ( + self.endpoint_tolerance_u8 + <= self.thumb_yaw_zero_endpoint_tolerance_u8 + <= 10.0 + ): + raise ValueError( + "thumb_yaw_zero_endpoint_tolerance_u8 must be between the " + "default endpoint tolerance and 10" + ) + if not ( + self.endpoint_tolerance_u8 + <= self.right_thumb_yaw_255_endpoint_tolerance_u8 + <= 10.0 + ): + raise ValueError( + "right_thumb_yaw_255_endpoint_tolerance_u8 must be between " + "the default endpoint tolerance and 10" + ) + if self.top_pnp_invalid_reset_seconds <= 0.0: + raise ValueError("top_pnp_invalid_reset_seconds must be positive") + if not 3 <= self.pnp_group_initialization_frames <= 30: + raise ValueError( + "pnp_group_initialization_frames must be in [3, 30]" + ) + if not 0.5 <= self.fixed_base_maximum_corner_drift_px <= 10.0: + raise ValueError( + "fixed_base_maximum_corner_drift_px must be in [0.5, 10]" + ) + if not 1 <= self.fixed_base_movement_confirmation_frames <= 30: + raise ValueError( + "fixed_base_movement_confirmation_frames must be in [1, 30]" + ) + if not ( + self.endpoint_tolerance_u8 + <= self.pinky_pip_zero_endpoint_tolerance_u8 + <= 8.0 + ): + raise ValueError( + "pinky_pip_zero_endpoint_tolerance_u8 must be between " + "endpoint_tolerance_u8 and 8" + ) + if self.motor_stall_timeout_seconds <= 0.0: + raise ValueError("motor_stall_timeout_seconds must be positive") + if not 0.0 <= self.motor_stall_startup_grace_seconds <= 5.0: + raise ValueError("motor_stall_startup_grace_seconds must be in [0, 5]") + if not 0.0 < self.motor_stall_minimum_progress_u8 <= 10.0: + raise ValueError( + "motor_stall_minimum_progress_u8 must be in (0, 10]" + ) + if not all( + value > 0.0 + for value in ( + self.image_trajectory_maximum_radial_rms_px, + self.image_trajectory_maximum_radial_p95_px, + self.image_trajectory_minimum_radius_px, + self.pnp_group_normal_alignment_scale_rad, + self.pnp_group_maximum_normal_alignment_rad, + self.thumb_ip_pnp_coupling_multiplier, + self.thumb_ip_pnp_coupling_scale_rad, + self.thumb_ip_pnp_maximum_coupling_residual_rad, + self.axis_maximum_plane_rms_m, + self.passive_axis_maximum_plane_rms_m, + self.axis_maximum_pose_line_rms_m, + self.zero_maximum_axis_cycle_difference_rad, + self.thumb_yaw_maximum_zero_cycle_difference_rad, + self.thumb_yaw_maximum_confidence_half_width_rad, + self.thumb_yaw_cross_session_diagnostic_rad, + self.zero_maximum_axis_cone_mismatch_rad, + self.zero_maximum_offset_rad, + self.zero_finger_maximum_offset_rad, + self.mechanical_endpoint_maximum_offset_rad, + self.maximum_validation_error_rad, + self.zero_maximum_confidence_half_width_rad, + self.task_precheck_hold_seconds, + self.active_maximum_rotation_orthogonal_rms_rad, + self.passive_maximum_rotation_orthogonal_rms_rad, + self.trajectory_maximum_cycle_travel_difference_rad, + self.passive_maximum_cycle_travel_difference_rad, + self.baseline_maximum_hysteresis_rad, + self.directional_zero_maximum_branch_gap_rad, + self.directional_zero_maximum_branch_gap_range_rad, + self.cross_view_roll_maximum_branch_gap_difference_rad, + self.cross_view_roll_maximum_shape_rms_rad, + self.passive_maximum_monotonic_correction_rad, + self.passive_maximum_hysteresis_rad, + self.command_maximum_direction_gap_rad, + ) + ): + raise ValueError("trajectory quality thresholds must be positive") + if ( + self.thumb_yaw_maximum_zero_cycle_difference_rad + > self.zero_maximum_axis_cycle_difference_rad + ): + raise ValueError( + "thumb_yaw_maximum_zero_cycle_difference_deg must not " + "exceed zero_maximum_axis_cycle_difference_deg" + ) + if ( + self.thumb_yaw_maximum_confidence_half_width_rad + > self.zero_maximum_confidence_half_width_rad + ): + raise ValueError( + "thumb_yaw_maximum_confidence_half_width_deg must not " + "exceed zero_maximum_confidence_half_width_deg" + ) + if not 1.0 < self.cross_view_roll_maximum_projection_scale_ratio <= 2.0: + raise ValueError( + "cross-view roll projection-scale ratio must be in (1, 2]" + ) + if self.zero_finger_maximum_offset_rad > self.zero_maximum_offset_rad: + raise ValueError( + "zero_finger_maximum_offset_deg cannot exceed " + "zero_maximum_offset_deg" + ) + if ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and self.repetitions < 4 + ): + raise ValueError( + "g20_right_19 requires at least 4 repetitions: at least 3 " + "training cycles and one isolated holdout" + ) + if ( + self.profile.layout_id != G20_RIGHT_19_LAYOUT + and self.repetitions != 3 + ): + raise ValueError( + "legacy three-camera calibration requires exactly 3 repetitions" + ) + if self.preflight_frames < 10 or self.minimum_sweep_bins < 3: + raise ValueError("preflight_frames or minimum_sweep_bins is too small") + if self.minimum_feedback_hz <= 0.0: + raise ValueError("minimum_feedback_hz must be positive") + if self.minimum_baseline_hold_frames < 3: + raise ValueError("minimum_baseline_hold_frames must be at least 3") + if not 0 <= self.automatic_sweep_retry_limit <= 5: + raise ValueError("automatic_sweep_retry_limit must be in [0, 5]") + if not 0 <= self.automatic_fit_retry_limit <= 2: + raise ValueError("automatic_fit_retry_limit must be in [0, 2]") + if not 0 <= self.automatic_motion_retry_limit <= 5: + raise ValueError("automatic_motion_retry_limit must be in [0, 5]") + if self.cross_view_roll_diagnostic_finger not in { + "", "pinky", "ring", "middle", "index" + }: + raise ValueError( + "cross_view_roll_diagnostic_finger must be empty or one of " + "pinky/ring/middle/index" + ) + if ( + self.cross_view_roll_diagnostic_finger + and self.profile.layout_id != G20_RIGHT_19_LAYOUT + ): + raise ValueError( + "cross-view roll diagnostic requires g20_right_19" + ) + if not 1.0 <= self.provisional_warning_ratio <= 2.0: + raise ValueError("provisional_warning_ratio must be in [1, 2]") + if not 1 <= self.retry_minimum_speed <= 255: + raise ValueError("retry_minimum_speed must be in [1, 255]") + if ( + len(self.retry_speed_scales) != self.automatic_sweep_retry_limit + or any(not 0.0 < item <= 1.0 for item in self.retry_speed_scales) + ): + raise ValueError("retry_speed_scales must match the sweep retry limit") + if ( + len(self.retry_endpoint_hold_seconds) + != self.automatic_sweep_retry_limit + or any(item < self.endpoint_hold_seconds for item in self.retry_endpoint_hold_seconds) + ): + raise ValueError( + "retry endpoint holds must match retries and not shorten the hold" + ) + if not 1 <= self.validation_command_count <= 10: + raise ValueError("validation_command_count must be in [1, 10]") + if self.validation_frames < 3: + raise ValueError("validation_frames must be at least 3") + if self.combination_validation_frames < 3: + raise ValueError("combination_validation_frames must be at least 3") + if ( + self.combination_maximum_position_p95_m <= 0.0 + or self.combination_maximum_orientation_p95_rad <= 0.0 + ): + raise ValueError("combination pose validation thresholds must be positive") + + def _make_group_pose_tracker( + self, name: str, roles: Sequence[str] + ) -> SquareTagGroupPoseTracker: + role_tuple = tuple(str(role) for role in roles) + role_set = set(role_tuple) + adjacent_pairs = tuple( + pair + for pair in dict.fromkeys( + (spec.parent_role, spec.child_role) + for spec in self.profile.record_specs.values() + if spec.measured + and spec.view == name + and spec.parent_role is not None + and spec.child_role is not None + ) + if pair[0] in role_set and pair[1] in role_set + ) + # Do not impose an absolute face-normal relationship between the + # fixed palm Tag and an articulated finger Tag. That angle depends on + # the physical Tag bracket and the endpoint joint pose; it is not a + # product invariant. The old "nearly parallel" prior repeatedly + # cleared otherwise stable 8-frame initialisation windows for ID4/ID5 + # on ring MCP flexion. Multi-frame relative rigidity, reprojection + # error and the task reference retained across cycles provide the + # valid branch evidence without assuming a particular mounting plane. + normal_alignment_pairs: tuple[tuple[str, str], ...] = () + thumb_mcp_ip_roles = {"thumb_cmc", "thumb_mcp", "thumb_ip"} + thumb_mcp_ip_coupling = () + if name == "front" and thumb_mcp_ip_roles.issubset(role_set): + thumb_mcp_ip_coupling = ( + ( + "thumb_cmc", + "thumb_mcp", + "thumb_mcp", + "thumb_ip", + self.thumb_ip_pnp_coupling_multiplier, + ), + ) + return SquareTagGroupPoseTracker( + roles=role_tuple, + adjacent_pairs=adjacent_pairs, + maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, + maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, + relative_rotation_scale_rad=math.radians(5.0), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=self.pnp_tracker_reset_seconds, + # Five-tag front preflight would make the exhaustive multi-frame + # search unnecessarily expensive. Four-tag side groups remain + # small enough and need the robust initializer so the fixed base + # Tag can anchor the three reference-finger IPPE branches. + initialization_frames=( + 1 + if len(role_tuple) > 4 + else self.pnp_group_initialization_frames + ), + normal_alignment_pairs=normal_alignment_pairs, + normal_alignment_scale_rad=( + self.pnp_group_normal_alignment_scale_rad + ), + maximum_normal_alignment_rad=( + self.pnp_group_maximum_normal_alignment_rad + if normal_alignment_pairs + else None + ), + # The source-URDF thumb_ip mimic ratio is used only to disambiguate + # the two planar-IPPE candidates while motor 15 drives MCP and IP + # together. If no candidate matches this weak prior, selection + # falls back to visual continuity instead of dropping the frame. + # Accepted Tag poses still fit both visual curves independently. + coupled_rotation_pairs=thumb_mcp_ip_coupling, + coupled_rotation_scale_rad=self.thumb_ip_pnp_coupling_scale_rad, + maximum_coupled_rotation_residual_rad=( + self.thumb_ip_pnp_maximum_coupling_residual_rad + if thumb_mcp_ip_coupling + else None + ), + ) + + def _make_view_runtime(self, name: str) -> ViewRuntime: + tracker = SquareTagPoseTracker( + maximum_reprojection_error_px=self.pnp_maximum_reprojection_error_px, + reprojection_tie_px=self.pnp_reprojection_tie_px, + maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, + maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, + maximum_tag_tilt_rad=self.pnp_maximum_tag_tilt_rad, + reset_after_seconds=self.pnp_tracker_reset_seconds, + ) + preflight_roles = tuple(self.profile.preflight_view_roles[name]) + if getattr(self, "standalone_thumb_calibration", False): + base_role = _fixed_base_role(name) + preflight_roles = tuple( + role + for role in self.profile.view_tags[name] + if role == base_role or role.startswith("thumb_") + ) + group = self._make_group_pose_tracker( + name, + preflight_roles, + ) + return ViewRuntime( + name=name, + tag_size_m=self.tag_size_m, + preflight_frames=self.preflight_frames, + tracker=tracker, + group_tracker=group, + view_tags=self.profile.view_tags[name], + tag_sizes_m_by_role={ + role: self.tag_sizes_m_by_id[int(tag_id)] + for role, tag_id in self.profile.view_tags[name].items() + }, + preflight_roles=preflight_roles, + ) + + def _required_roles_for_view(self, view: str) -> tuple[str, ...]: + """Return only the roles required by the active task in this view.""" + runtime = self.views[view] + if getattr(self, "active_combination_validation", None) is not None: + # Combination validation consumes every reliable visible target, + # but only the fixed palm reference is a blocking requirement. + # Four adjacent fingers cannot expose all of their Tags to one + # side camera at the same instant. + return tuple(runtime.preflight_roles) + active_spec: SweepSpec | None = None + if ( + self.active_sweep is not None + and view in _sweep_views(_node_profile(self), self.active_sweep.spec) + ): + active_spec = self.active_sweep.spec + elif ( + self.active_validation is not None + and view in _sweep_views( + self.profile, self.active_validation.spec + ) + ): + active_spec = self.active_validation.spec + elif ( + self.retry_sweep_spec is not None + and view in _sweep_views(_node_profile(self), self.retry_sweep_spec) + ): + active_spec = self.retry_sweep_spec + elif ( + getattr(self, "pnp_task_spec", None) is not None + and view + in _sweep_views( + _node_profile(self), self.pnp_task_spec + ) + ): + active_spec = self.pnp_task_spec + if active_spec is None: + return tuple(runtime.preflight_roles) + required: set[str] = set() + for joint_name in _sweep_joints_for_view( + self.profile, active_spec, view + ): + joint = self.profile.record_specs[joint_name] + if joint.parent_role is not None: + required.add(joint.parent_role) + if joint.child_role is not None: + required.add(joint.child_role) + # Every task keeps its view's fixed palm Tag. Besides suppressing an + # unanchored IPPE mirror branch, this makes the thumb MCP/IP task obey + # its explicit front 0/1/2/3 visibility contract. + base_role = { + "front": "front_base", + "side": "side_base", + "top": "top_base", + }[view] + required.add(base_role) + return tuple(role for role in runtime.roles if role in required) + + def _locked_base_role_for_active_capture(self, view: str) -> str | None: + runtime = self.views[view] + if ( + getattr(runtime, "locked_base_pose", None) is None + or getattr(runtime, "locked_base_center_xy_px", None) is None + or getattr(runtime, "locked_base_quality", None) is None + ): + return None + if getattr(self, "active_combination_validation", None) is not None: + return _fixed_base_role(view) + spec: SweepSpec | None = None + if self.active_sweep is not None: + spec = self.active_sweep.spec + elif self.active_validation is not None: + spec = self.active_validation.spec + elif self.retry_sweep_spec is not None: + spec = self.retry_sweep_spec + elif getattr(self, "pnp_task_spec", None) is not None: + spec = self.pnp_task_spec + if spec is None or not _sweep_uses_locked_base_reference( + _node_profile(self), spec, view + ): + return None + return _fixed_base_role(view) + + def _live_required_roles_for_view( + self, view: str, required_roles: Sequence[str] + ) -> tuple[str, ...]: + locked = self._locked_base_role_for_active_capture(view) + # Front Tag 0 may be intentionally hidden by finger clearance. Top + # Tag 8 must remain visible so its raw corners can prove that the + # session reference has not moved, even though its PnP pose is frozen. + if str(view) == "top": + return tuple(required_roles) + return tuple(role for role in required_roles if role != locked) + + def _lock_fixed_base_references(self) -> bool: + """Freeze robust palm references captured at the confirmed baseline.""" + minimum = min(30, int(self.preflight_frames)) + pending = False + for view, runtime in self.views.items(): + if runtime.locked_base_pose is not None: + continue + observations = list(runtime.fixed_base_observations) + if len(observations) < minimum: + pending = True + continue + poses = [item[0] for item in observations] + centres = np.asarray([item[1] for item in observations], dtype=float) + qualities = [item[2] for item in observations] + translation = np.median( + np.asarray( + [pose.translation_xyz_m for pose in poses], dtype=float + ), + axis=0, + ) + quaternion = robust_rotation_summary( + [pose.quaternion_xyzw for pose in poses] + )[0] + reprojection = float( + np.percentile( + [pose.reprojection_error_px for pose in poses], 95.0 + ) + ) + runtime.locked_base_pose = SquareTagPose( + tuple(float(value) for value in quaternion), + tuple(float(value) for value in translation), + reprojection, + ) + runtime.locked_base_center_xy_px = tuple( + float(value) for value in np.median(centres, axis=0) + ) + corner_observations = list( + getattr(runtime, "fixed_base_corner_observations", ()) + ) + if len(corner_observations) >= minimum: + locked_corners = np.median( + np.asarray(corner_observations, dtype=float), axis=0 + ) + runtime.locked_base_corners_xy = tuple( + tuple(float(value) for value in point) + for point in locked_corners + ) + else: + runtime.locked_base_corners_xy = None + runtime.locked_base_corner_drift_count = 0 + runtime.latest_locked_base_corner_drift_px = 0.0 + runtime.locked_base_quality = TagQuality( + hamming=max(item.hamming for item in qualities), + decision_margin=min(item.decision_margin for item in qualities), + edge_pixels=min(item.edge_pixels for item in qualities), + reprojection_error_px=reprojection, + ) + translation_rms_m = float( + np.sqrt( + np.mean( + np.sum( + ( + np.asarray( + [pose.translation_xyz_m for pose in poses], + dtype=float, + ) + - translation + ) + ** 2, + axis=1, + ) + ) + ) + ) + rotation_p95_deg = math.degrees( + float( + np.percentile( + [ + ( + Rotation.from_quat(quaternion).inv() + * Rotation.from_quat(pose.quaternion_xyzw) + ).magnitude() + for pose in poses + ], + 95.0, + ) + ) + ) + append_jsonl( + self.raw_path, + { + "kind": "fixed_base_reference_locked", + "view": view, + "role": _fixed_base_role(view), + "tag_id": runtime.view_tags[_fixed_base_role(view)], + "sample_count": len(observations), + "translation_rms_m": round(translation_rms_m, 9), + "rotation_p95_deg": round(rotation_p95_deg, 6), + "reprojection_p95_px": round(reprojection, 6), + "corner_reference_xy": ( + None + if runtime.locked_base_corners_xy is None + else [ + list(point) + for point in runtime.locked_base_corners_xy + ] + ), + }, + ) + return not pending and all( + runtime.locked_base_pose is not None + for runtime in self.views.values() + ) + + def _group_tracker_for_roles( + self, runtime: ViewRuntime, roles: Sequence[str] + ) -> SquareTagGroupPoseTracker: + key = tuple(str(role) for role in roles) + group = runtime.group_trackers.get(key) + if group is None: + group = self._make_group_pose_tracker(runtime.name, key) + runtime.group_trackers[key] = group + return group + + @staticmethod + def _reset_view_trackers( + runtime: ViewRuntime, *, preserve_task_reference: bool = False + ) -> None: + runtime.tracker.reset() + for group in runtime.group_trackers.values(): + group.reset( + preserve_task_reference=preserve_task_reference + ) + + @staticmethod + def _reset_view_pnp_diagnostics(runtime: ViewRuntime) -> None: + """Clear PnP evidence only when a new capture scope begins.""" + runtime.latest_pnp_rejections.clear() + runtime.latest_group_pnp_reason = "" + runtime.latest_group_missing_candidate_roles = () + runtime.pnp_rejection_counts.clear() + runtime.group_pnp_rejection_counts.clear() + runtime.pnp_initialization_progress = None + runtime.latest_pnp_valid = False + runtime.last_pnp_diagnostic_signature = None + + def _record_group_pnp_candidate_event( + self, + *, + runtime: ViewRuntime, + stamp_ns: int, + required_roles: Sequence[str], + pose_roles: Sequence[str], + corners_by_role: Mapping[str, np.ndarray], + qualities: Mapping[str, TagQuality], + matched_tracking: tuple[tuple[float, ...], int] | None, + selected: Mapping[str, SquareTagPose] | None, + pnp_rejections: Mapping[str, str], + group_pnp_reason: str, + ) -> None: + """Persist bounded, replayable evidence for group candidate decisions. + + Valid FrameObservation records intentionally contain only accepted + poses. Without a separate event, a deterministic PnP dropout leaves + no corners, per-candidate tilt, reprojection error or missing role in + raw_samples.jsonl, making the next run guess at the cause. Record one + event per eight-count motor bucket and decision signature. This is + dense enough to locate a repeatable geometry boundary without writing + every 30 Hz rejected camera message. + """ + item = self.active_sweep + if ( + item is None + or self.state not in {STATE_PREPARE_SWEEP, STATE_SWEEP} + or runtime.name not in _sweep_views(_node_profile(self), item.spec) + ): + return + candidate_diagnostics = { + role: dict( + runtime.tracker.last_candidate_diagnostics_by_role.get( + role, {} + ) + ) + for role in pose_roles + if runtime.tracker.last_candidate_diagnostics_by_role.get(role) + } + oblique_group_roles = tuple( + role + for role, diagnostics in candidate_diagnostics.items() + if int(diagnostics.get("reprojection_candidate_count", 0)) > 0 + and int( + diagnostics.get("independent_tilt_candidate_count", 0) + ) + == 0 + ) + if ( + not group_pnp_reason + and not pnp_rejections + and not oblique_group_roles + ): + return + if selected is not None and not oblique_group_roles: + return + state_u8 = ( + matched_tracking[0] + if matched_tracking is not None + else self.latest_state_u8 + ) + motor_value = ( + None + if len(state_u8) != 20 + else float(state_u8[item.spec.motor_index]) + ) + motor_bucket = ( + -1 + if motor_value is None or not math.isfinite(motor_value) + else int(np.clip(math.floor(motor_value / 8.0), 0, 31)) + ) + missing_roles = tuple(runtime.latest_group_missing_candidate_roles) + signature = ( + item.spec.key, + item.cycle, + item.direction, + motor_bucket, + selected is not None, + str(group_pnp_reason), + tuple( + sorted( + (str(key), str(value)) + for key, value in pnp_rejections.items() + ) + ), + missing_roles, + oblique_group_roles, + ) + if runtime.last_pnp_diagnostic_signature == signature: + return + runtime.last_pnp_diagnostic_signature = signature + append_jsonl( + self.raw_path, + { + "kind": "group_pnp_candidate_event", + "outcome": ( + "selected_with_oblique_group_constraint" + if selected is not None + else "group_pose_rejected" + ), + "view": runtime.name, + "task_name": item.spec.key, + "motor_index": item.spec.motor_index, + "cycle": item.cycle, + "direction": item.direction, + "feedback_u8": motor_value, + "feedback_bucket_u8": ( + None if motor_bucket < 0 else motor_bucket * 8 + ), + "image_stamp_ns": int(stamp_ns), + "required_roles": list(required_roles), + "required_tag_ids": { + role: runtime.view_tags[role] + for role in required_roles + }, + "group_pnp_reason": str(group_pnp_reason), + "group_missing_candidate_roles": list(missing_roles), + "pnp_rejections": dict(pnp_rejections), + "oblique_group_roles": list(oblique_group_roles), + "candidate_diagnostics": candidate_diagnostics, + "tag_quality": { + role: { + "hamming": int(quality.hamming), + "decision_margin": float(quality.decision_margin), + "mean_edge_pixels": float(quality.edge_pixels), + } + for role, quality in qualities.items() + if role in required_roles + }, + "corners_xy": { + role: np.asarray(corners, dtype=float).tolist() + for role, corners in corners_by_role.items() + if role in required_roles + }, + "camera_intrinsics_sha256": runtime.intrinsics_sha256, + "camera_matrix": ( + None + if runtime.camera_matrix is None + else runtime.camera_matrix.tolist() + ), + }, + ) + + def _camera_info_callback(self, view: str, message: CameraInfo) -> None: + runtime = self.views[view] + valid = ( + int(message.width) > 0 + and int(message.height) > 0 + and len(message.p) == 12 + and float(message.p[0]) > 0.0 + and float(message.p[5]) > 0.0 + ) + matrix = None + if valid: + matrix = np.asarray(message.p, dtype=float).reshape(3, 4)[:, :3] + valid = bool( + np.all(np.isfinite(matrix)) + and matrix[0, 0] > 0.0 + and matrix[1, 1] > 0.0 + ) + if ( + matrix is not None + and runtime.camera_matrix is not None + and not np.allclose(matrix, runtime.camera_matrix) + ): + self._reset_view_trackers(runtime) + runtime.camera_info_valid = valid + runtime.camera_matrix = matrix if valid else None + runtime.camera_frame = str(message.header.frame_id) + runtime.image_width = int(message.width) + runtime.image_height = int(message.height) + runtime.intrinsics_sha256 = ( + "" + if matrix is None + else camera_info_fingerprint( + width=message.width, + height=message.height, + camera_matrix=message.k, + distortion=message.d, + rectification=message.r, + projection=message.p, + ) + ) + runtime.extrinsics_valid = False + if valid and self.extrinsics is not None: + runtime.extrinsics_valid = self.extrinsics.camera_matches( + view, + serial_number=self.camera_serials[view], + width=runtime.image_width, + height=runtime.image_height, + intrinsics_sha256=runtime.intrinsics_sha256, + ) + + def _state_callback(self, message: JointState) -> None: + command_names = _command_names(self) + if len(message.position) != len(command_names): + return + if ( + len(message.name) == len(command_names) + and set(message.name) == set(command_names) + ): + lookup = dict(zip(message.name, message.position)) + state = tuple(float(lookup[name]) for name in command_names) + else: + state = tuple(float(value) for value in message.position) + stamp = _stamp_ns(message.header.stamp) + if stamp <= 0: + stamp = int(self.get_clock().now().nanoseconds) + self.latest_state_u8 = state + self.last_state_at = time.monotonic() + self.state_receive_times.append(self.last_state_at) + if not self.state_history or stamp > self.state_history[-1].stamp_ns: + self.state_history.append(StateSample(stamp, state)) + + def _info_callback(self, message: String) -> None: + try: + value = json.loads(message.data) + except json.JSONDecodeError: + return + if isinstance(value, dict): + self.latest_hand_info = value + + def _quality_valid(self, quality: TagQuality, *, include_pnp: bool) -> bool: + return tag_quality_is_valid( + quality, + maximum_hamming=self.maximum_hamming, + minimum_decision_margin=self.minimum_decision_margin, + minimum_edge_pixels=self.minimum_edge_pixels, + maximum_reprojection_error_px=( + self.pnp_maximum_reprojection_error_px if include_pnp else None + ), + ) + + def _detections_callback( + self, view: str, message: AprilTagDetectionArray + ) -> None: + runtime = self.views[view] + now = time.monotonic() + stamp = _stamp_ns(message.header.stamp) + required_roles = self._required_roles_for_view(view) + locked_base_role = self._locked_base_role_for_active_capture(view) + live_required_roles = self._live_required_roles_for_view( + view, required_roles + ) + palm_observer: PalmAxisObserver | None = None + if ( + self.active_sweep is not None + and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} + ): + palm_observer = _palm_axis_observer_for_sweep( + _node_profile(self), self.active_sweep.spec, view + ) + palm_observer_roles = ( + () + if palm_observer is None + else (palm_observer.parent_role, palm_observer.child_role) + ) + combination_capture = bool( + self.active_combination_validation is not None + and self.state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE} + ) + if required_roles != runtime.current_required_roles: + runtime.current_required_roles = required_roles + runtime.valid_flags.clear() + runtime.detection_times.clear() + self._reset_view_pnp_diagnostics(runtime) + runtime.task_valid_frames = 0 + runtime.task_total_frames = 0 + self._reset_view_trackers(runtime) + qualities: dict[str, TagQuality] = {} + corners_by_role: dict[str, np.ndarray] = {} + centres_by_role: dict[str, np.ndarray] = {} + for detection in message.detections: + role = runtime.role_by_id.get(int(detection.id)) + if role is None: + continue + corners = np.asarray( + [[float(point.x), float(point.y)] for point in detection.corners], + dtype=float, + ) + if corners.shape != (4, 2): + continue + edges = np.linalg.norm(corners - np.roll(corners, -1, axis=0), axis=1) + qualities[role] = TagQuality( + hamming=int(detection.hamming), + decision_margin=float(detection.decision_margin), + edge_pixels=float(np.mean(edges)), + ) + corners_by_role[role] = corners + centres_by_role[role] = np.mean(corners, axis=0) + + if ( + view == "top" + and locked_base_role is not None + and locked_base_role in corners_by_role + and runtime.locked_base_corners_xy is not None + ): + corner_drift = _maximum_corner_drift_px( + runtime.locked_base_corners_xy, + corners_by_role[locked_base_role], + ) + runtime.latest_locked_base_corner_drift_px = corner_drift + if corner_drift > self.fixed_base_maximum_corner_drift_px: + runtime.locked_base_corner_drift_count += 1 + else: + runtime.locked_base_corner_drift_count = 0 + if ( + runtime.locked_base_corner_drift_count + >= self.fixed_base_movement_confirmation_frames + ): + append_jsonl( + self.raw_path, + { + "kind": "fixed_base_reference_moved", + "view": view, + "role": locked_base_role, + "tag_id": runtime.view_tags[locked_base_role], + "corner_drift_px": round(corner_drift, 6), + "maximum_corner_drift_px": ( + self.fixed_base_maximum_corner_drift_px + ), + "confirmation_frames": ( + runtime.locked_base_corner_drift_count + ), + "task_name": ( + "" + if self.active_sweep is None + else self.active_sweep.spec.key + ), + }, + ) + self._pause("fixed_base_reference_moved") + return + + pose_roles = ( + tuple( + role + for role in runtime.roles + if role in qualities + and role != locked_base_role + and self._quality_valid(qualities[role], include_pnp=False) + ) + if combination_capture + else tuple( + dict.fromkeys( + ( + *live_required_roles, + *( + role + for role in palm_observer_roles + if role in qualities + and role != locked_base_role + and self._quality_valid( + qualities[role], include_pnp=False + ) + ), + ) + ) + ) + ) + if locked_base_role is not None: + # The fixed pose is injected below. A live top base remains a + # detection/movement requirement but is never re-solved into the + # pitch/roll/yaw geometry after the session reference is locked. + pose_roles = tuple( + role for role in pose_roles if role != locked_base_role + ) + observable_combination_joints = ( + _combination_observable_joints( + self.profile, + view, + ( + *pose_roles, + *((locked_base_role,) if locked_base_role else ()), + ), + ) + if combination_capture + else () + ) + detection_good = bool( + all(role in qualities for role in live_required_roles) + and all( + self._quality_valid(qualities[role], include_pnp=False) + for role in live_required_roles + ) + and ( + not combination_capture + or bool(observable_combination_joints) + ) + ) + tracking_command_u8: int | None = None + tracking_direction: str | None = None + matched_tracking: tuple[tuple[float, ...], int] | None = None + if ( + self.active_sweep is not None + and ( + view in _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + or palm_observer is not None + ) + and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} + ): + matched_tracking = interpolate_state_u8( + list(self.state_history), + stamp, + maximum_skew_ns=self.maximum_state_image_skew_ns, + ) + if matched_tracking is not None: + matched_tracking_state = matched_tracking[0] + if ( + self.state == STATE_SWEEP + or self._motion_command_reached( + self.active_sweep.spec, + self.active_sweep.start_u8, + matched_tracking_state, + ) + ): + # During PREPARE, learn only the final held start pose. + # In particular a roll task approaches 255 from its 127 + # baseline; caching that preparation motion as the formal + # decreasing trajectory would contaminate the same-command + # return reference before the real scan overwrites it. + tracking_command_u8 = int( + np.clip( + np.rint( + matched_tracking_state[ + self.active_sweep.spec.motor_index + ] + ), + 0, + 255, + ) + ) + tracking_direction = self.active_sweep.direction + selected: dict[str, SquareTagPose] | None = None + pnp_rejections: dict[str, str] = {} + group_pnp_reason = "" + if detection_good and runtime.camera_matrix is not None: + candidates: dict[str, tuple[SquareTagPose, ...]] = {} + individual_selected: dict[str, SquareTagPose] = {} + for role in pose_roles: + pose, rejection = runtime.tracker.estimate( + role, + corners_by_role[role], + tag_size_m=float( + runtime.tag_sizes_m_by_role.get( + role, runtime.tag_size_m + ) + ), + camera_matrix=runtime.camera_matrix, + stamp_ns=stamp, + ) + if pose is not None: + individual_selected[role] = pose + candidates[role] = runtime.tracker.last_candidates_by_role.get( + role, () + ) + # A group-constrained task deliberately retains an oblique + # candidate that the independent per-Tag selector would not + # use. Report a rejection only if candidate generation itself + # failed; otherwise the group tracker owns the decision. + if rejection and ( + combination_capture or not candidates[role] + ): + pnp_rejections[role] = rejection + if combination_capture: + # Each visible target is tracked independently. This avoids + # an exponential all-Tag IPPE search and, more importantly, + # lets an occluded neighbouring finger remain optional. + selected = individual_selected + else: + if locked_base_role is not None: + assert runtime.locked_base_pose is not None + candidates[locked_base_role] = ( + runtime.locked_base_pose, + ) + group_tracker = self._group_tracker_for_roles( + runtime, required_roles + ) + selected, group_pnp_reason = group_tracker.select( + candidates, + stamp_ns=stamp, + trajectory_command_u8=tracking_command_u8, + trajectory_direction=tracking_direction, + ) + runtime.latest_group_missing_candidate_roles = ( + group_tracker.last_missing_roles + ) + if selected is not None and palm_observer_roles: + selected = dict(selected) + for role in palm_observer_roles: + pose = individual_selected.get(role) + quality = qualities.get(role) + if pose is None or quality is None: + continue + pose_quality = TagQuality( + hamming=quality.hamming, + decision_margin=quality.decision_margin, + edge_pixels=quality.edge_pixels, + reprojection_error_px=pose.reprojection_error_px, + ) + if self._quality_valid( + pose_quality, include_pnp=True + ): + selected[role] = pose + if selected is not None: + qualities = _selected_pose_qualities( + selected, + qualities, + locked_base_role=locked_base_role, + locked_base_quality=runtime.locked_base_quality, + ) + # Preserve exactly which Tags were present in this camera message. + # The cached palm reference participates in geometry and quality + # checks below, but must never be reported as a live detection. + live_qualities = { + role: quality + for role, quality in qualities.items() + if role in corners_by_role + } + if selected is not None and locked_base_role is not None: + assert runtime.locked_base_pose is not None + assert runtime.locked_base_center_xy_px is not None + assert runtime.locked_base_quality is not None + selected = dict(selected) + selected[locked_base_role] = runtime.locked_base_pose + centres_by_role[locked_base_role] = np.asarray( + runtime.locked_base_center_xy_px, dtype=float + ) + qualities[locked_base_role] = runtime.locked_base_quality + valid = bool( + selected is not None + and all(role in selected for role in required_roles) + and all( + self._quality_valid(qualities[role], include_pnp=True) + for role in ( + selected + if combination_capture + else required_roles + ) + ) + and ( + not combination_capture + or bool( + _combination_observable_joints( + self.profile, view, tuple(selected) + ) + ) + ) + ) + if ( + self.state == STATE_SWEEP + and self.active_sweep is not None + and view in _sweep_views(_node_profile(self), self.active_sweep.spec) + ): + self.sweep_detection_total_frames += 1 + total_by_view = getattr( + self, "sweep_detection_total_by_view", None + ) + if total_by_view is None: + self.sweep_detection_total_by_view = {} + total_by_view = self.sweep_detection_total_by_view + total_by_view[view] = total_by_view.get(view, 0) + 1 + if valid: + self.sweep_detection_valid_frames += 1 + valid_by_view = getattr( + self, "sweep_detection_valid_by_view", None + ) + if valid_by_view is None: + self.sweep_detection_valid_by_view = {} + valid_by_view = self.sweep_detection_valid_by_view + valid_by_view[view] = valid_by_view.get(view, 0) + 1 + runtime.latest_pnp_rejections = pnp_rejections + runtime.latest_group_pnp_reason = group_pnp_reason + if not group_pnp_reason: + runtime.latest_group_missing_candidate_roles = () + runtime.latest_pnp_valid = valid + task_pnp_capture = bool( + self.active_sweep is not None + and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} + and view in _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + ) + if task_pnp_capture: + for role, rejection in pnp_rejections.items(): + key = f"{role}:{rejection}" + runtime.pnp_rejection_counts[key] = ( + runtime.pnp_rejection_counts.get(key, 0) + 1 + ) + if group_pnp_reason.startswith("group_initializing:"): + try: + progress = group_pnp_reason.split(":", 1)[1] + accepted, required = progress.split("/", 1) + runtime.pnp_initialization_progress = ( + int(accepted), + int(required), + ) + except (IndexError, TypeError, ValueError): + runtime.pnp_initialization_progress = None + elif group_pnp_reason: + runtime.group_pnp_rejection_counts[group_pnp_reason] = ( + runtime.group_pnp_rejection_counts.get( + group_pnp_reason, 0 + ) + + 1 + ) + elif selected is not None and runtime.pnp_initialization_progress: + required = runtime.pnp_initialization_progress[1] + runtime.pnp_initialization_progress = (required, required) + self._record_group_pnp_candidate_event( + runtime=runtime, + stamp_ns=stamp, + required_roles=required_roles, + pose_roles=pose_roles, + corners_by_role=corners_by_role, + qualities=qualities, + matched_tracking=matched_tracking, + selected=selected, + pnp_rejections=pnp_rejections, + group_pnp_reason=group_pnp_reason, + ) + if view == "top": + runtime.pnp_invalid_since, reset_due = update_pnp_reset_watchdog( + detection_good=bool( + detection_good and runtime.camera_matrix is not None + ), + pnp_valid=valid, + now=now, + invalid_since=runtime.pnp_invalid_since, + reset_after_seconds=self.top_pnp_invalid_reset_seconds, + ) + if reset_due: + self._reset_view_trackers(runtime) + runtime.pnp_reset_count += 1 + self.get_logger().warning( + "Top-view PnP was continuously invalid for " + f"{self.top_pnp_invalid_reset_seconds:.1f}s; reset trackers " + f"(count={runtime.pnp_reset_count}, " + f"group_reason={group_pnp_reason or 'none'}, " + f"tag_rejections={pnp_rejections})" + ) + if ( + valid + and selected is not None + and self.state == STATE_PREFLIGHT + and self.started + and self.startup_baseline_recovered + ): + base_role = _fixed_base_role(view) + if ( + base_role in selected + and base_role in centres_by_role + and base_role in live_qualities + ): + runtime.fixed_base_observations.append( + ( + selected[base_role], + tuple( + float(value) + for value in centres_by_role[base_role] + ), + live_qualities[base_role], + ) + ) + runtime.fixed_base_corner_observations.append( + np.asarray(corners_by_role[base_role], dtype=float).copy() + ) + runtime.latest_tag_quality = live_qualities + runtime.last_message_at = now + runtime.detection_times.append(now) + runtime.valid_flags.append(valid) + task_sweep_capture = bool( + self.state == STATE_SWEEP + and self.active_sweep is not None + and view in _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + ) + if ( + runtime.current_required_roles + != tuple(runtime.preflight_roles) + and task_sweep_capture + ): + # Attempt-level validity is a motion-trajectory metric. PREPARE + # deliberately rejects the first N frames while the group PnP + # tracker initializes; counting those expected frames made clean + # side tasks report about 94% and triggered full four-round + # rescans despite 100% visibility prechecks and dense trajectories. + runtime.task_total_frames += 1 + if valid: + runtime.task_valid_frames += 1 + if not valid or selected is None: + return + runtime.last_valid_at = now + + # Preflight and motion-only states need pose quality/status, but do not + # consume FrameObservation objects. Building transforms and payloads + # for every measured joint on all three 30 Hz streams was pure work + # and could starve the detection subscriptions. Only materialize an + # observation for the one view used by an active capture stage. + sweep_capture = bool( + self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} + and self.active_sweep is not None + and view in _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + ) + palm_axis_capture = bool( + self.state == STATE_SWEEP + and self.active_sweep is not None + and not self.active_sweep.precheck + and self.active_sweep.cycle >= 0 + and palm_observer is not None + ) + validation_capture = bool( + self.state == STATE_VALIDATION_CAPTURE + and ( + ( + self.active_validation is not None + and view in _sweep_views( + self.profile, self.active_validation.spec + ) + ) + or self.active_combination_validation is not None + ) + ) + if not sweep_capture and not validation_capture and not palm_axis_capture: + return + + matched = matched_tracking + if matched is None: + matched = interpolate_state_u8( + list(self.state_history), + stamp, + maximum_skew_ns=self.maximum_state_image_skew_ns, + ) + if matched is None: + return + state_u8, sync_error_ns = matched + joint_vectors: dict[str, tuple[float, float, float]] = {} + image_vectors: dict[str, tuple[float, float]] = {} + joint_quaternions: dict[str, tuple[float, float, float, float]] = {} + parent_poses_common: dict[str, Mapping[str, list[float]]] = {} + child_poses_common: dict[str, Mapping[str, list[float]]] = {} + joint_reprojection: dict[str, float] = {} + if self.extrinsics is None or not runtime.extrinsics_valid: + return + front_from_view = self.extrinsics.transform(view) + if palm_axis_capture and palm_observer is not None: + self._record_palm_axis_sample( + palm_observer, + selected, + front_from_view, + state_u8, + sync_error_ns=int(sync_error_ns), + stamp_ns=stamp, + ) + if not sweep_capture and not validation_capture: + return + for name, spec in self.profile.record_specs.items(): + if not spec.measured or spec.view != view: + continue + assert spec.parent_role is not None and spec.child_role is not None + if ( + spec.parent_role not in selected + or spec.child_role not in selected + ): + continue + parent = selected[spec.parent_role] + child = selected[spec.child_role] + parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) + relative = parent_rotation.inv().apply( + np.asarray(child.translation_xyz_m, dtype=float) + - np.asarray(parent.translation_xyz_m, dtype=float) + ) + image_relative = ( + centres_by_role[spec.child_role] + - centres_by_role[spec.parent_role] + ) + parent_matrix = front_from_view @ transform_matrix( + parent.translation_xyz_m, parent.quaternion_xyzw + ) + child_matrix = front_from_view @ transform_matrix( + child.translation_xyz_m, child.quaternion_xyzw + ) + relative_rotation = Rotation.from_matrix( + parent_matrix[:3, :3] + ).inv() * Rotation.from_matrix(child_matrix[:3, :3]) + joint_vectors[name] = tuple(float(value) for value in relative) + image_vectors[name] = tuple(float(value) for value in image_relative) + joint_quaternions[name] = tuple( + float(value) for value in relative_rotation.as_quat() + ) + parent_poses_common[name] = matrix_payload(parent_matrix) + child_poses_common[name] = matrix_payload(child_matrix) + joint_reprojection[name] = max( + float(parent.reprojection_error_px), + float(child.reprojection_error_px), + ) + observation = FrameObservation( + stamp_ns=stamp, + received_at=now, + view=view, + state_u8=state_u8, + state_sync_error_ns=int(sync_error_ns), + joint_vectors_xyz_m=joint_vectors, + image_vectors_xy_px=image_vectors, + joint_quaternions_xyzw=joint_quaternions, + parent_poses_common=parent_poses_common, + child_poses_common=child_poses_common, + joint_reprojection_error_px=joint_reprojection, + ) + self._accept_frame(observation) + + def _record_palm_axis_sample( + self, + observer: PalmAxisObserver, + selected: Mapping[str, SquareTagPose], + common_from_view: np.ndarray, + state_u8: Sequence[float], + *, + sync_error_ns: int, + stamp_ns: int, + ) -> None: + """Buffer one direction-only sample without touching sweep validity.""" + item = self.active_sweep + if ( + item is None + or item.spec.key != observer.task_name + or observer.parent_role not in selected + or observer.child_role not in selected + ): + return + parent = selected[observer.parent_role] + child = selected[observer.child_role] + parent_matrix = common_from_view @ transform_matrix( + parent.translation_xyz_m, parent.quaternion_xyzw + ) + child_matrix = common_from_view @ transform_matrix( + child.translation_xyz_m, child.quaternion_xyzw + ) + relative_rotation = ( + Rotation.from_matrix(parent_matrix[:3, :3]).inv() + * Rotation.from_matrix(child_matrix[:3, :3]) + ) + feedback = float(state_u8[observer.motor_index]) + durable = canonical_sample_record({ + "kind": "palm_axis_sample", + "attempt": self.sweep_attempts.get( + _sweep_storage_key(item.spec), 1 + ), + "task_name": item.spec.key, + "view": observer.view, + "source_joint": observer.source_name, + "model_joint": observer.model_joint, + "motor_index": int(observer.motor_index), + "cycle": int(item.cycle), + "direction": item.direction, + "requested_command_u8": int(item.target_u8), + "feedback_u8": feedback, + "relative_quaternion_xyzw": [ + float(value) for value in relative_rotation.as_quat() + ], + "parent_pose_common": matrix_payload(parent_matrix), + "child_pose_common": matrix_payload(child_matrix), + "state_u8": [float(value) for value in state_u8], + "state_image_sync_error_ms": round( + abs(int(sync_error_ns)) / 1_000_000.0, 6 + ), + "pnp_reprojection_error_px": round( + max( + float(parent.reprojection_error_px), + float(child.reprojection_error_px), + ), + 6, + ), + "image_stamp_ns": int(stamp_ns), + }) + record = fitting_sample_record(durable) + self.palm_axis_records_by_source.setdefault( + observer.source_name, [] + ).append(record) + + def _persist_palm_axis_samples(self, item: SweepItem) -> None: + """Commit the side channel only after its authoritative sweep passes.""" + observer = next( + ( + candidate + for candidate in self.profile.palm_axis_observers + if candidate.task_name == item.spec.key + ), + None, + ) + if observer is None: + return + attempt = int( + self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) + ) + rows = [ + record + for record in self.palm_axis_records_by_source.get( + observer.source_name, () + ) + if int(record.get("attempt", 1)) == attempt + and int(record.get("cycle", -1)) == int(item.cycle) + and str(record.get("direction", "")) == item.direction + ] + append_jsonl_many( + self.raw_path, + (canonical_sample_record(record) for record in rows), + ) + + def _accept_frame(self, observation: FrameObservation) -> None: + if self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: + if observation.view not in _sweep_views( + _node_profile(self), self.active_sweep.spec + ): + return + if not self._motion_command_reached( + self.active_sweep.spec, + self.active_sweep.start_u8, + observation.state_u8, + ): + return + self.sweep_start_frames.append(observation) + maximum_start_frames = max( + 1, int(getattr(self, "preflight_frames", 30)) + ) * len( + _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + ) + if len(self.sweep_start_frames) > maximum_start_frames: + del self.sweep_start_frames[:-maximum_start_frames] + return + if self.state == STATE_SWEEP and self.active_sweep is not None: + if observation.view not in _sweep_views( + self.profile, self.active_sweep.spec + ): + return + motor = self.active_sweep.spec.motor_index + value = float(observation.state_u8[motor]) + if not -3.0 <= value <= 258.0: + return + if not self._motion_command_reached( + self.active_sweep.spec, + int(np.clip(np.rint(value), 0, 255)), + observation.state_u8, + ): + return + self.sweep_frames.append(observation) + checkpoint_target = getattr( + self, "sweep_checkpoint_target_u8", None + ) + if ( + checkpoint_target is not None + and getattr(self, "sweep_checkpoint_hold_since", None) + is not None + and self._steady_checkpoint_reached( + self.active_sweep.spec, + int(checkpoint_target), + observation.state_u8, + ) + ): + self.sweep_checkpoint_frames.append(observation) + if ( + getattr(self, "sweep_baseline_pending", False) + and getattr(self, "sweep_baseline_hold_since", None) + is not None + and observation.received_at + - float(self.sweep_baseline_hold_since) + >= float(self.baseline_hold_seconds) + and self._motion_command_reached( + self.active_sweep.spec, + int( + self.baseline_command[ + self.active_sweep.spec.motor_index + ] + ), + observation.state_u8, + ) + ): + self.sweep_baseline_frames.append(observation) + self.sweep_last_valid_at = observation.received_at + last_by_view = getattr( + self, "sweep_last_valid_at_by_view", None + ) + if last_by_view is None: + self.sweep_last_valid_at_by_view = {} + last_by_view = self.sweep_last_valid_at_by_view + last_by_view[observation.view] = observation.received_at + return + if ( + self.state == STATE_VALIDATION_CAPTURE + and self.active_validation is not None + and observation.view in _sweep_views( + self.profile, self.active_validation.spec + ) + ): + if self._motion_command_reached( + self.active_validation.spec, + self.active_validation.command_u8, + observation.state_u8, + ): + self.validation_frames_buffer.append(observation) + return + if ( + self.state == STATE_VALIDATION_CAPTURE + and self.active_combination_validation is not None + and self._command_vector_reached( + self.active_combination_validation.command_u8 + ) + ): + self.combination_validation_frames_buffer[ + observation.view + ].append(observation) + + def _view_ready(self, runtime: ViewRuntime, now: float) -> bool: + minimum_frames = min(30, self.preflight_frames) + return bool( + runtime.camera_info_valid + and runtime.extrinsics_valid + and len(runtime.valid_flags) >= minimum_frames + and runtime.valid_rate >= self.minimum_detection_rate + and runtime.detection_hz >= self.minimum_detection_hz + and now - runtime.last_message_at <= 1.0 + ) + + def _all_preflight_ready(self, now: float) -> bool: + feedback_hz = ( + 0.0 + if len(self.state_receive_times) < 2 + or self.state_receive_times[-1] <= self.state_receive_times[0] + else (len(self.state_receive_times) - 1) + / (self.state_receive_times[-1] - self.state_receive_times[0]) + ) + return bool( + self.extrinsics is not None + and len(self.latest_state_u8) == 20 + and now - self.last_state_at <= 1.0 + and feedback_hz >= self.minimum_feedback_hz + and all(self._view_ready(runtime, now) for runtime in self.views.values()) + ) + + def _all_devices_ready(self, now: float) -> bool: + """Check only hardware transport needed for a safe baseline return. + + Tag visibility must not gate this check: an interrupted calibration can + leave the fingers in a pose that occludes a fixed palm Tag. Requiring + that Tag before commanding baseline would create a startup deadlock. + """ + feedback_hz = ( + 0.0 + if len(self.state_receive_times) < 2 + or self.state_receive_times[-1] <= self.state_receive_times[0] + else (len(self.state_receive_times) - 1) + / (self.state_receive_times[-1] - self.state_receive_times[0]) + ) + return bool( + self.extrinsics is not None + and len(self.latest_state_u8) == 20 + and now - self.last_state_at <= 1.0 + and feedback_hz >= self.minimum_feedback_hz + and all( + runtime.camera_info_valid + and runtime.extrinsics_valid + and runtime.last_message_at > 0.0 + and now - runtime.last_message_at <= 1.0 + for runtime in self.views.values() + ) + ) + + def _finish_startup_baseline_recovery(self) -> None: + """Start a fresh fixed-palm-Tag window at confirmed baseline.""" + for runtime in self.views.values(): + runtime.valid_flags.clear() + runtime.detection_times.clear() + observations = getattr(runtime, "fixed_base_observations", None) + if observations is not None: + observations.clear() + corner_observations = getattr( + runtime, "fixed_base_corner_observations", None + ) + if corner_observations is not None: + corner_observations.clear() + runtime.locked_base_pose = None + runtime.locked_base_center_xy_px = None + runtime.locked_base_corners_xy = None + runtime.locked_base_quality = None + runtime.locked_base_corner_drift_count = 0 + runtime.latest_locked_base_corner_drift_px = 0.0 + self._reset_view_trackers(runtime) + self.startup_baseline_recovered = True + self.state = STATE_PREFLIGHT + self.reason = "waiting_for_baseline_tags_after_recovery" + + def _active_view_for_resume(self) -> str | None: + if self.active_sweep is not None: + return self.active_sweep.spec.view + if self.active_validation is not None: + return self.active_validation.spec.view + if self.retry_sweep_spec is not None: + return self.retry_sweep_spec.view + return None + + def _active_views_for_resume(self) -> tuple[str, ...]: + if self.active_sweep is not None: + return _sweep_views(_node_profile(self), self.active_sweep.spec) + if self.active_validation is not None: + return _sweep_views(_node_profile(self), self.active_validation.spec) + if self.retry_sweep_spec is not None: + return _sweep_views(_node_profile(self), self.retry_sweep_spec) + return required_resume_views(None) + + def _resume_preflight_ready(self, now: float) -> bool: + if ( + len(self.latest_state_u8) != _command_count(self) + or now - self.last_state_at > 1.0 + ): + return False + return all( + self._view_ready(self.views[view], now) + for view in self._active_views_for_resume() + ) + + def _prepare_failed_sweep_retry(self) -> SweepSpec: + """Discard only failed measurements/cycles and queue two directions.""" + if self.retry_sweep_spec is None: + raise RuntimeError("no failed sweep is pending retry") + spec = self.retry_sweep_spec + repetitions = int(getattr(self, "repetitions", 3)) + cycles = set(getattr(self, "retry_cycles", set(range(repetitions)))) + profile = _node_profile(self) + source_task_keys = ( + tuple(getattr(self, "retry_source_task_keys", ())) + if getattr(self, "retry_source_failure_task_key", None) == spec.key + else () + ) + source_key_set = set(source_task_keys) + source_specs = [ + candidate + for candidate in profile.sweep_specs + if candidate.key in source_key_set + ] + # Yaw zero is a same-view relationship between the pitch and roll + # axes. A retry must remain in the original top-camera PnP reference + # generation; otherwise the retry changes the datum it is intended to + # verify. Ordinary single-task retries still reset their camera to + # recover from a genuinely bad branch. + self.retry_preserve_pnp_continuity = bool(source_specs) + data_specs = source_specs or [spec] + retry_joint_names = set(getattr(self, "retry_joint_names", set())) + if source_specs: + retry_joint_names = { + joint_name + for candidate in source_specs + for joint_name in candidate.joints + } + else: + retry_joint_names = retry_joint_names or set(spec.joints) + retry_joint_names.intersection_update(spec.joints) + if not retry_joint_names: + retry_joint_names = set(spec.joints) + self.retry_joint_names = retry_joint_names + baseline_records_by_joint = getattr( + self, "baseline_records_by_joint", {} + ) + for joint_name in retry_joint_names: + self.records_by_joint[joint_name][:] = [ + record + for record in self.records_by_joint[joint_name] + if int(record.get("cycle", -1)) not in cycles + ] + if joint_name in baseline_records_by_joint: + baseline_records_by_joint[joint_name][:] = [ + record + for record in baseline_records_by_joint[joint_name] + if int(record.get("cycle", -1)) not in cycles + ] + command_records = getattr(self, "command_records_by_joint", {}) + if joint_name in command_records: + command_records[joint_name][:] = [ + record + for record in command_records[joint_name] + if int(record.get("cycle", -1)) not in cycles + ] + data_task_keys = {candidate.key for candidate in data_specs} + observers = [ + item + for item in profile.palm_axis_observers + if item.task_name in data_task_keys + ] + for observer in observers: + records = self.palm_axis_records_by_source.get( + observer.source_name, [] + ) + records[:] = [ + record + for record in records + if int(record.get("cycle", -1)) not in cycles + ] + all_items = getattr(self, "sweep_items", []) + self.retry_sweep_items = [ + item + for item in all_items + if item.spec in data_specs and item.cycle in cycles + ] + if not self.retry_sweep_items: + self.retry_sweep_items = [ + SweepItem(candidate, cycle, direction) + for candidate in data_specs + for cycle in sorted(cycles) + for direction in ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + ) + ] + # A planar Tag PnP tracker can occasionally remain on a continuous but + # biased pose branch for an otherwise clean sweep. Starting a full + # fit retry with that tracker state intact tends to reproduce the same + # orientation residual, so reset only the affected camera before the + # retry. Raw samples remain logged and all final quality gates stay + # unchanged. + reset_trackers = getattr(self, "_reset_view_trackers", None) + retry_views = { + profile.record_specs[name].view + for name in retry_joint_names + } + for observer in observers: + retry_views.add(observer.view) + for view in retry_views: + if view is None: + continue + runtime = getattr(self, "views", {}).get(view) + if runtime is None or reset_trackers is None: + continue + if self.retry_preserve_pnp_continuity: + # Frame-quality counters are attempt-local even though the + # geometric PnP state deliberately remains continuous. + runtime.task_valid_frames = 0 + runtime.task_total_frames = 0 + continue + if _node_profile(self).layout_id == G20_RIGHT_19_LAYOUT: + reset_trackers(runtime, preserve_task_reference=True) + else: + reset_trackers(runtime) + runtime.pnp_invalid_since = None + runtime.pnp_reset_count += 1 + # Detection quality is attempt-local. Carrying the previous + # attempt into this retry made its percentage describe two PnP + # branches instead of the data currently being admitted. + runtime.task_valid_frames = 0 + runtime.task_total_frames = 0 + storage_key = _sweep_storage_key(spec) + self.sweep_attempts[storage_key] = ( + self.sweep_attempts.get(storage_key, 1) + 1 + ) + source_attempts: dict[str, int] = {} + for source_spec in source_specs: + source_key = _sweep_storage_key(source_spec) + self.sweep_attempts[source_key] = ( + self.sweep_attempts.get(source_key, 1) + 1 + ) + source_attempts[str(source_spec.key)] = self.sweep_attempts[ + source_key + ] + invalidated_storage_keys = { + storage_key, + *(_sweep_storage_key(candidate) for candidate in source_specs), + } + for key in list(getattr(self, "sweep_retry_counts", {})): + if key[0] in invalidated_storage_keys: + self.sweep_retry_counts.pop(key, None) + append_jsonl( + self.raw_path, + { + "kind": "retry", + "view": spec.view, + **({"task_name": spec.key} if spec.task_name else {}), + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "joints_to_rescan": sorted(retry_joint_names), + **( + {"source_task_names": list(source_task_keys)} + if source_task_keys + else {} + ), + **( + {"source_attempts": source_attempts} + if source_attempts + else {} + ), + **( + {"pnp_reference_policy": "preserve_existing_generation"} + if self.retry_preserve_pnp_continuity + else {} + ), + "attempt": self.sweep_attempts[storage_key], + "reason": self.paused_reason, + "cycles": [cycle + 1 for cycle in sorted(cycles)], + }, + ) + self.fit_failure = {} + G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) + getattr(self, "motion_stall_details", {}).clear() + return spec + + def _invalidate_fitted_calibration_state(self) -> None: + """Invalidate every artifact derived from the current motion records. + + A task rescan changes the observation set, so curves, zero offsets and + validation diagnostics must be treated as one generation. Keeping + this transition in one place prevents final publication from mixing a + newly collected task with state left by an earlier fit attempt. + """ + self.measured_fits = {} + self.axis_measurements = [] + self.palm_orientation_measurements = [] + self.palm_orientation_rejections = {} + self.zero_result = None + self.validated_endpoint_zero_offsets_rad = {} + self.corrected_urdf_path = None + self.fit_quality_passed = False + self.validation_errors_rad = [] + self.cross_view_roll_metrics = {} + self.validation_only_fits = {} + self.joint_dynamic_diagnostics = {} + + def _endpoint_zero_offsets_for_publication(self) -> dict[str, float]: + """Return the endpoint anchors belonging to the validated zero fit.""" + stored = { + str(name): float(value) + for name, value in getattr( + self, "validated_endpoint_zero_offsets_rad", {} + ).items() + } + if self.profile.layout_id != G20_RIGHT_19_LAYOUT: + if stored: + raise RuntimeError( + "validated_endpoint_zero_state_unexpected_for_layout" + ) + return {} + expected = set(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS) + if getattr(self, "standalone_thumb_calibration", False): + expected = { + name for name in expected if name.startswith("thumb_") + } + actual = set(stored) + if actual != expected: + missing = ",".join(sorted(expected - actual)) or "-" + extra = ",".join(sorted(actual - expected)) or "-" + raise RuntimeError( + "validated_endpoint_zero_state_incomplete:" + f"missing={missing};extra={extra}" + ) + if not all(math.isfinite(value) for value in stored.values()): + raise RuntimeError("validated_endpoint_zero_state_non_finite") + if self.zero_result is None: + raise RuntimeError("URDF zero solution is missing") + solved = self.zero_result.direct_offsets_rad + diverged = sorted( + name + for name, value in stored.items() + if name not in solved + or not math.isclose( + value, + float(solved[name]), + rel_tol=0.0, + abs_tol=1.0e-12, + ) + ) + if diverged: + raise RuntimeError( + "validated_endpoint_zero_state_diverged:" + + ",".join(diverged) + ) + return stored + + def _restore_durable_task_checkpoint(self) -> int: + """Restore independently validated complete tasks from a failed session.""" + source = self.resume_raw_samples_path + if source is None: + return 0 + if not source.is_file(): + raise RuntimeError(f"resume raw samples do not exist: {source}") + if source.resolve() == self.raw_path.resolve(): + raise RuntimeError("resume raw samples must come from an older session") + rows: list[dict[str, Any]] = [] + source_size = max(1, int(source.stat().st_size)) + bytes_read = 0 + with source.open("rb") as stream: + for line_number, raw_line in enumerate(stream, start=1): + bytes_read += len(raw_line) + if not raw_line.strip(): + continue + try: + line = raw_line.decode("utf-8") + value = json.loads(line) + except (UnicodeDecodeError, json.JSONDecodeError) as error: + raise RuntimeError( + f"resume raw JSON is invalid at line {line_number}" + ) from error + if isinstance(value, dict): + rows.append(value) + if line_number % 1000 == 0: + self.base_import_progress = { + "phase": "reading", + "records_read": len(rows), + "bytes_read": bytes_read, + "total_bytes": source_size, + "fraction": min(1.0, bytes_read / source_size), + } + self.reason = "reading_base_session_records" + _publish_import_status(self) + starts = [row for row in rows if row.get("kind") == "session_start"] + if len(starts) != 1: + raise RuntimeError("resume raw must contain exactly one session_start") + start = starts[0] + previous_tag_sizes = { + int(tag_id): float(size) + for tag_id, size in dict( + start.get("tag_sizes_m_by_id", {}) + ).items() + } + current_tag_sizes = dict(getattr(self, "tag_sizes_m_by_id", {})) + try: + ( + palm_axis_schema_compatible, + palm_axis_invalidated_tasks, + ) = _palm_axis_resume_policy( + self.profile, + start, + getattr( + getattr( + getattr(self, "calibration_profile", None), + "artifacts", + None, + ), + "session_compatibility_tokens", + frozenset(self.profile.capabilities), + ), + ) + except ValueError as error: + raise RuntimeError( + "resume checkpoint algorithm capabilities differ" + ) from error + if ( + str(start.get("model", "G20")).upper() + != str(getattr(self, "model", "G20")).upper() + or str(start.get("hand_type")) != self.hand_type + or str(start.get("tag_layout")) != self.profile.layout_id + or start.get("view_tags") + != {view: dict(tags) for view, tags in self.profile.view_tags.items()} + or tuple(int(value) for value in start.get("baseline_command_u8", [])) + != tuple(self.baseline_command) + or str(start.get("source_urdf_sha256", "")) + != _file_sha256(self.source_urdf_path) + ): + raise RuntimeError( + "resume checkpoint geometry, algorithm capabilities, Tag " + "layout, baseline or source URDF differs" + ) + try: + changed_tag_size_ids, size_invalidated_tasks = ( + resume_tasks_invalidated_by_tag_size_changes( + self.profile, + previous_tag_sizes, + current_tag_sizes, + ) + ) + except ValueError as error: + raise RuntimeError( + "resume checkpoint Tag-size geometry differs incompatibly" + ) from error + invalidated_task_set = set(size_invalidated_tasks) + invalidated_task_set.update(palm_axis_invalidated_tasks) + scope_invalidated_tasks = tuple( + getattr(self, "recalibration_task_keys", ()) or () + ) + invalidated_task_set.update(scope_invalidated_tasks) + if invalidated_task_set: + rows = [ + row + for row in rows + if str(row.get("task_name", "")) not in invalidated_task_set + ] + completed, reusable = resumable_completed_task_prefix( + self.profile, + self.repetitions, + self.baseline_command, + rows, + minimum_sweep_bins=self.minimum_sweep_bins, + allow_sparse=True, + ) + validate_sample_records( + row + for row in reusable + if str(row.get("kind", "")) in RESUMABLE_SAMPLE_KINDS + or row.get("kind") == "palm_axis_sample" + ) + self.state = STATE_REVALIDATING_INHERITED + self.reason = "revalidating_inherited_tasks" + self.base_import_progress = { + "phase": "revalidating", + "records_read": len(rows), + "completed_tasks": 0, + "total_tasks": len(completed), + "fraction": 0.0, + } + _publish_import_status(self) + for durable in reusable: + kind = str(durable["kind"]) + if kind == "synchronised_frame": + continue + if kind == "palm_axis_sample": + source_name = str(durable.get("source_joint", "")) + if source_name not in self.palm_axis_records_by_source: + raise RuntimeError( + "resume checkpoint contains unknown palm-axis source " + + source_name + ) + record = fitting_sample_record(durable) + self.palm_axis_records_by_source[source_name].append(record) + continue + joint_name = str(durable["joint"]) + if joint_name not in self.records_by_joint: + raise RuntimeError( + f"resume checkpoint contains unknown joint {joint_name}" + ) + if kind not in { + "sample", "baseline_hold_sample", "steady_command_sample" + }: + continue + record = fitting_sample_record(durable) + if kind == "sample": + self.records_by_joint[joint_name].append(record) + elif kind == "baseline_hold_sample": + self.baseline_records_by_joint[joint_name].append(record) + else: + self.command_records_by_joint[joint_name].append(record) + completed, dropped = G20ThreeCameraCalibrationNode._revalidate_imported_tasks( + self, completed, allow_sparse=True + ) + completed_set = set(completed) + reusable = tuple( + row + for row in reusable + if str(row.get("task_name", "")) in completed_set + ) + # Attempts are durable identities, not counters local to one process. + # Without this floor a resumed task whose accepted data used attempt 2 + # would label its next retry as attempt 2 again. The raw log then + # contained two different acquisitions with the same identity and an + # offline replay merged them into one invalid trajectory. + attempt_floor_by_task: dict[str, int] = {} + for row in reusable: + task_key = str(row.get("task_name", "")) + if task_key not in completed_set: + continue + try: + attempt = max(1, int(row.get("attempt", 1))) + except (TypeError, ValueError): + attempt = 1 + attempt_floor_by_task[task_key] = max( + attempt_floor_by_task.get(task_key, 1), attempt + ) + attempts = getattr(self, "sweep_attempts", None) + if attempts is None: + self.sweep_attempts = { + _sweep_storage_key(spec): 1 + for spec in self.profile.sweep_specs + } + attempts = self.sweep_attempts + for spec in self.profile.sweep_specs: + if spec.key not in completed_set: + continue + storage_key = _sweep_storage_key(spec) + attempts[storage_key] = max( + int(attempts.get(storage_key, 1)), + attempt_floor_by_task.get(spec.key, 1), + ) + 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 + if spec.key not in completed_set + ] + append_jsonl( + self.raw_path, + { + "kind": "resume_checkpoint_import", + "source_session": source.parent.name, + "completed_task_count": len(completed), + "completed_task_keys": list(completed), + "discarded_incomplete_task": ( + missing_tasks[0] if missing_tasks else None + ), + "pending_task_keys": missing_tasks, + "revalidation_dropped_tasks": dropped, + "tag_size_changed_ids": list(changed_tag_size_ids), + "tag_size_invalidated_task_keys": list(size_invalidated_tasks), + "palm_axis_schema_compatible": palm_axis_schema_compatible, + "palm_axis_invalidated_task_keys": list( + palm_axis_invalidated_tasks + ), + "recalibration_scope": str( + getattr(self, "recalibration_scope", "full") + ), + "scope_invalidated_task_keys": list( + scope_invalidated_tasks + ), + "imported_record_count": len(reusable), + "imported_attempt_floor_by_task": attempt_floor_by_task, + "source_raw_samples_sha256": _file_sha256(source), + }, + ) + append_jsonl_many(self.raw_path, reusable) + return len(completed) + + def _revalidate_imported_tasks( + self, completed: Sequence[str], *, allow_sparse: bool = False + ) -> tuple[list[str], list[dict[str, Any]]]: + """Re-apply the final hard gates to imported tasks at import time. + + A task that only provisionally passed (warning band) in its source + session would otherwise survive until the final fit re-applies the + unmodified thresholds after every other task is collected, forcing + the session to go back and re-scan it at the very end. Failing + tasks are dropped here. Sparse product resume independently checks + later complete tasks so only the failed task has to be re-collected; + the compatibility mode still drops the remaining suffix. + """ + def discard_task_records(task_key: str) -> None: + spec = next( + ( + candidate + for candidate in self.profile.sweep_specs + if candidate.key == task_key + ), + None, + ) + if spec is None: + return + for joint_name in spec.joints: + for store in ( + self.records_by_joint, + self.baseline_records_by_joint, + self.command_records_by_joint, + ): + if joint_name in store: + store[joint_name].clear() + observer = next( + ( + item + for item in self.profile.palm_axis_observers + if item.task_name == task_key + ), + None, + ) + if observer is not None: + self.palm_axis_records_by_source[ + observer.source_name + ].clear() + + accepted: list[str] = [] + dropped: list[dict[str, Any]] = [] + for task_index, task_key in enumerate(completed, start=1): + if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED: + total = max(1, len(completed)) + self.base_import_progress.update( + { + "current_task": task_key, + "completed_tasks": task_index - 1, + "total_tasks": len(completed), + "fraction": (task_index - 1) / total, + } + ) + _publish_import_status(self) + spec = next( + ( + item.spec + for item in self.sweep_items + if item.spec.key == task_key + ), + None, + ) + failures = ( + G20ThreeCameraCalibrationNode._provisional_fit_failures( + self, spec, include_view_validity=False + ) + if spec is not None + else [{"metric": "missing_task_spec"}] + ) + if failures: + dropped.append( + { + "task": task_key, + "failures": [ + { + key: value + for key, value in failure.items() + if key in {"joint", "metric", "actual", "limit"} + } + for failure in failures[:6] + ], + } + ) + # Make the rejection visible before checking later tasks. + # Some fits intentionally depend on an earlier reference + # task (for example every non-reference finger roll uses the + # pinky roll direction). Delaying this clear until the end + # let a dependent task pass with records that were about to + # be removed, producing a checkpoint that failed only on the + # next restart. + discard_task_records(task_key) + if allow_sparse: + continue + break + accepted.append(task_key) + if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED: + self.base_import_progress.update( + { + "current_task": "", + "completed_tasks": len(completed), + "total_tasks": len(completed), + "fraction": 1.0, + } + ) + _publish_import_status(self) + if dropped and not allow_sparse: + accepted_set = set(accepted) + for task_key in completed: + if task_key not in accepted_set: + discard_task_records(task_key) + if isinstance(completed, tuple): + accepted = tuple(accepted) + return accepted, dropped + + def _start_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state != STATE_WAIT_START: + response.success = False + response.message = f"not ready: state={self.state} reason={self.reason}" + return response + if not self.commands_enabled: + response.success = False + response.message = "commands_enabled=false; preview cannot move the hand" + return response + if self.command_publisher.get_subscription_count() < 1: + response.success = False + response.message = "hand SDK is not subscribed to the command topic" + return response + if self.setting_publisher.get_subscription_count() < 1: + response.success = False + response.message = "hand SDK is not subscribed to the setting topic" + return response + if len(self.get_publishers_info_by_topic(self.command_topic)) > 1: + response.success = False + response.message = "another node is publishing hand commands" + return response + self.started = True + self.startup_baseline_recovered = False + self.sweep_items = [] + self.pnp_task_spec = None + selected_sweep_specs = list(self.profile.sweep_specs) + if self.standalone_thumb_calibration: + standalone_task_keys = set(self.recalibration_task_keys) + selected_sweep_specs = [ + spec + for spec in selected_sweep_specs + if spec.key in standalone_task_keys + ] + if self.cross_view_roll_diagnostic_finger: + selected_sweep_specs = [ + spec + for spec in selected_sweep_specs + if spec.key + == f"{self.cross_view_roll_diagnostic_finger}_roll_multiview" + ] + if len(selected_sweep_specs) != 1: + raise RuntimeError( + "cross-view diagnostic could not resolve multiview task" + ) + selected_profile = replace( + self.profile, + sweep_specs=tuple(selected_sweep_specs), + ) + self.sweep_items.extend( + _build_sweep_plan(selected_profile, self.repetitions) + ) + self.sweep_index = 0 + self.retry_sweep_spec = None + self.retry_resume_index = None + self.retry_sweep_items.clear() + self.retry_joint_names.clear() + self.retry_source_failure_task_key = None + self.retry_source_task_keys = () + self.retry_cycle_override.clear() + self.retry_preserve_pnp_continuity = False + self.active_sweep_is_fit_retry = False + self.fit_failure = {} + self.fit_failure_history_by_task.clear() + self.cross_view_roll_diagnostic_result = {} + self.motion_stall_details.clear() + self.sweep_attempts = { + _sweep_storage_key(spec): 1 for spec in self.profile.sweep_specs + } + self.sweep_retry_counts.clear() + self.motion_retry_counts.clear() + self.validation_retry_counts.clear() + self.precheck_speed_metrics.clear() + self.formal_speed_scales.clear() + self.records_by_joint = { + name: [] for name in self.profile.record_joints + } + self.baseline_records_by_joint = { + name: [] for name in self.profile.record_joints + } + self.command_records_by_joint = { + name: [] for name in self.profile.record_joints + } + self.palm_axis_records_by_source = { + observer.source_name: [] + for observer in self.profile.palm_axis_observers + } + G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) + self.combination_validation_items.clear() + self.combination_validation_index = 0 + self.active_combination_validation = None + self.combination_validation_completed = False + self.combination_tag_mounts.clear() + self.combination_tag_observation_counts.clear() + self.combination_tag_validation_counts.clear() + self.combination_position_errors_m.clear() + self.combination_orientation_errors_rad.clear() + for frames in self.combination_validation_frames_buffer.values(): + frames.clear() + self.completed_payload = None + self.sweep_frames.clear() + self.sweep_baseline_frames.clear() + self.sweep_baseline_pending = False + self.sweep_baseline_hold_since = None + self.sweep_start_frames.clear() + self.carried_sweep_start_frames.clear() + append_jsonl( + self.raw_path, + { + "kind": "session_start", + "model": self.model, + "hand_type": self.hand_type, + "tag_layout": self.profile.layout_id, + "sample_schema_version": self.sample_schema_version, + "recalibration_scope": self.recalibration_scope, + "recalibration_task_keys": list( + self.recalibration_task_keys + ), + "command_names": list(_command_names(self)), + # Retained only as an old-session serialization token. Live + # decisions use the typed motion/measurement policies. + "capabilities": sorted( + getattr( + getattr( + getattr(self, "calibration_profile", None), + "artifacts", + None, + ), + "session_compatibility_tokens", + frozenset(self.profile.capabilities), + ) + ), + "reference_finger": self.profile.reference_finger, + "view_tags": { + view: dict(tags) + for view, tags in self.profile.view_tags.items() + }, + "palm_axis_observers": _palm_axis_observer_schema( + self.profile + ), + "tag_family": "36h11", + "tag_size_m": float(self.tag_size_m), + "tag_sizes_m_by_id": { + str(tag_id): float(self.tag_sizes_m_by_id[tag_id]) + for tag_id in sorted(self.tag_sizes_m_by_id) + }, + "baseline_command_u8": [ + int(value) for value in self.baseline_command + ], + "camera_extrinsics_file": str(self.camera_extrinsics_file), + "source_urdf_path": str(self.source_urdf_path), + "source_urdf_sha256": _file_sha256(self.source_urdf_path), + "curve_input_domain": ( + "requested_command_u8" + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else "command_u8" + ), + "baseline_hysteresis_source": ( + "dedicated_mid_sweep_hold" + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else "trajectory_endpoint" + ), + "directional_zero_policy": ( + "canonical_decreasing_255_to_127" + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else "independent_direction_rezero" + ), + "thumb_ip_pnp_coupling": { + "usage": "candidate_prior_with_visual_fallback", + }, + "cross_view_roll_diagnostic_finger": ( + self.cross_view_roll_diagnostic_finger + ), + "training_cycles": list(range(self.repetitions - 1)), + "validation_cycle": self.repetitions - 1, + "adaptive_formal_speed": { + "enabled": bool(self.adaptive_formal_speed_enabled), + "maximum_scale": float( + self.adaptive_formal_speed_max_scale + ), + "minimum_bins": int( + self.adaptive_formal_speed_minimum_bins + ), + "maximum_bin_gap": int( + self.adaptive_formal_speed_maximum_bin_gap + ), + }, + }, + ) + try: + if self.resume_raw_samples_path is not None: + 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()) + restored_tasks = self._restore_durable_task_checkpoint() + except Exception as error: + self.started = False + response.success = False + response.message = f"CFG-RESUME-009:{error}" + return response + self._begin_return_baseline("startup_tag_preflight") + response.success = True + response.message = ( + "three-camera calibration started" + if restored_tasks == 0 + else f"calibration resumed with {restored_tasks} completed tasks" + ) + return response + + def _pause_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state in {STATE_COMPLETE, STATE_ABORTED, STATE_PREFLIGHT, STATE_WAIT_START}: + response.success = False + response.message = f"cannot pause from {self.state}" + return response + self._pause("operator_pause") + response.success = True + response.message = "calibration paused and current pose held" + return response + + def _resume_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state != STATE_PAUSED: + response.success = False + response.message = "calibration is not paused" + return response + if self.paused_reason == "zero_model_validation_failed": + response.success = False + response.message = ( + "zero/URDF model validation failed; samples are retained but " + "resume cannot repair this result by rescanning. Inspect the " + "reported zero guards and start a new session after correcting " + "the camera/tag/URDF geometry" + ) + return response + if self.paused_reason == "fixed_base_reference_moved": + response.success = False + response.message = ( + "the locked top base reference moved during acquisition; " + "start a new session so preflight can establish a new " + "reference before any motion" + ) + return response + if self.paused_reason == "cross_view_roll_diagnostic_complete": + response.success = False + response.message = ( + "cross-view roll diagnostic is complete and URDF publication " + "is locked; inspect the front/side result and start a formal " + "session only after choosing the mechanical or vision remedy" + ) + return response + now = time.monotonic() + if not self._resume_preflight_ready(now): + response.success = False + required = ",".join(self._active_views_for_resume()) + response.message = ( + f"resume preflight is not ready; required_views={required}" + ) + return response + for view in self._active_views_for_resume(): + runtime = self.views[view] + self._reset_view_trackers(runtime) + runtime.pnp_invalid_since = None + self.sweep_frames.clear() + self.sweep_start_frames.clear() + self.validation_frames_buffer.clear() + if self.retry_sweep_spec is not None: + self._prepare_failed_sweep_retry() + self._begin_return_baseline("resume_sweep") + response.success = True + response.message = ( + "calibration resumed; only the failed six-direction joint " + "sweep will restart" + ) + return response + if self.active_sweep is not None and self.sweep_index < len(self.sweep_items): + self._begin_return_baseline("resume_sweep") + elif self.active_combination_validation is not None: + item = self.active_combination_validation + active = { + "kind": "combination_validation", + "pose_name": item.name, + "label_zh": item.label_zh, + "pose_index": self.combination_validation_index + 1, + "pose_total": len(self.combination_validation_items), + "requested_command_u8": list(item.command_u8), + "valid_frames_by_view": { + view: len(frames) + for view, frames in self.combination_validation_frames_buffer.items() + }, + "automatic_retry_count": self.validation_retry_counts.get( + ("combination", self.combination_validation_index), 0 + ), + "automatic_retry_limit": self.automatic_sweep_retry_limit, + } + elif self.active_validation is not None: + self._begin_return_baseline("resume_validation") + else: + self._begin_return_baseline("next_sweep") + response.success = True + response.message = "calibration resumed; active step will restart" + return response + + def _abort_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + self._publish_speed_profile(self._normal_speed_profile()) + unsafe_to_return = bool( + self.motion_stall_details + or "stall" in str(self.reason) + or len(self.latest_state_u8) != 20 + or time.monotonic() - self.last_state_at > 1.0 + ) + if unsafe_to_return: + self._publish_hold_current() + self.state = STATE_ABORTED + self.reason = "operator_abort_held_current_for_safety" + else: + self.abort_original_reason = str(self.reason or "operator_abort") + self._begin_return_baseline("abort") + response.success = True + response.message = ( + "calibration aborted and current pose held" + if unsafe_to_return + else "calibration stopping after safe baseline return" + ) + return response + + def _publish_command(self, values: list[int]) -> None: + if not self.commands_enabled: + return + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + message.name = list(_command_names(self)) + message.position = [float(value) for value in values] + self.command_publisher.publish(message) + + def _publish_speed_profile(self, speeds: list[int]) -> None: + if not self.commands_enabled: + return + profile = tuple(int(value) for value in speeds) + if profile == self.commanded_speed_profile: + return + message = String() + message.data = json.dumps( + { + "setting_cmd": "set_speed", + "params": { + "hand_type": self.hand_type, + "speed": list(profile), + }, + } + ) + self.setting_publisher.publish(message) + self.commanded_speed_profile = profile + self.speed_commanded_at = time.monotonic() + + def _normal_speed_profile(self) -> list[int]: + return [self.normal_calibration_speed] * 5 + + def _transition_speed_profile( + self, target_command: Sequence[int] + ) -> list[int]: + """Use conservative joint-class speeds for one safe pose waypoint.""" + speeds = list(self._normal_speed_profile()) + current = getattr(self, "latest_state_u8", ()) + if len(current) != 20 or len(target_command) != 20: + return speeds + roll_speed = int( + getattr(self, "index_roll_calibration_speed", min(speeds)) + ) + flex_speed = int( + getattr(self, "index_flex_calibration_speed", min(speeds)) + ) + for motor, (actual, target) in enumerate( + zip(current, target_command) + ): + if abs(float(actual) - float(target)) <= 0.5: + continue + if motor in range(6, 10): + slot = motor - 5 + speeds[slot] = min(speeds[slot], roll_speed) + elif motor in range(1, 5): + slot = motor + speeds[slot] = min(speeds[slot], flex_speed) + elif motor in range(16, 20): + slot = motor - 15 + speeds[slot] = min(speeds[slot], flex_speed) + return speeds + + @staticmethod + def _speed_slot_for_spec(spec: SweepSpec) -> int: + motor = int(spec.motor_index) + if motor in {0, 5, 10, 15}: + return 0 + if motor in range(1, 5): + return motor + if motor in range(6, 10): + return motor - 5 + if motor in range(16, 20): + return motor - 15 + raise ValueError(f"unsupported G20 calibration motor: {motor}") + + def _base_speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return build_calibration_speed_profile( + spec, + normal_speed=self.normal_calibration_speed, + index_roll_speed=self.index_roll_calibration_speed, + index_flex_speed=self.index_flex_calibration_speed, + profile=profile, + ) + + def _speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: + speeds = G20ThreeCameraCalibrationNode._base_speed_profile_for_spec( + self, spec + ) + active = getattr(self, "active_sweep", None) + precheck = bool(active is not None and active.precheck) + fit_retry = bool(getattr(self, "active_sweep_is_fit_retry", False)) + if not precheck and not fit_retry: + scale = float( + getattr(self, "formal_speed_scales", {}).get(spec.key, 1.0) + ) + slot = G20ThreeCameraCalibrationNode._speed_slot_for_spec(spec) + speeds[slot] = min( + 255, int(math.floor(float(speeds[slot]) * scale + 1e-9)) + ) + retry = 0 + if active is not None: + key = ( + _sweep_storage_key(active.spec), + active.cycle, + active.direction, + ) + retry = self.sweep_retry_counts.get(key, 0) + if retry: + scale = self.retry_speed_scales[min(retry, len(self.retry_speed_scales)) - 1] + speeds = [ + max(self.retry_minimum_speed, int(round(speed * scale))) + for speed in speeds + ] + return speeds + + def _record_precheck_speed_metric( + self, + item: SweepItem, + *, + bin_count: int, + maximum_bin_gap: int, + valid_frames: int, + ) -> None: + """Use the accepted low-speed precheck to bound formal-scan speed.""" + key = ( + _sweep_storage_key(item.spec), item.cycle, item.direction + ) + retry = int(getattr(self, "sweep_retry_counts", {}).get(key, 0)) + base_speeds = ( + G20ThreeCameraCalibrationNode._base_speed_profile_for_spec( + self, item.spec + ) + ) + actual_speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec( + self, item.spec + ) + slot = G20ThreeCameraCalibrationNode._speed_slot_for_spec(item.spec) + base_speed = int(base_speeds[slot]) + actual_speed = int(actual_speeds[slot]) + acquisition_scale = ( + 1.0 + if base_speed <= 0 + else float(actual_speed) / float(base_speed) + ) + metrics = getattr(self, "precheck_speed_metrics", None) + if metrics is None: + self.precheck_speed_metrics = {} + metrics = self.precheck_speed_metrics + by_direction = metrics.setdefault(item.spec.key, {}) + by_direction[item.direction] = { + "bin_count": int(bin_count), + "maximum_bin_gap": int(maximum_bin_gap), + "valid_frames": int(valid_frames), + "retry": retry, + "acquisition_speed": actual_speed, + "acquisition_speed_scale": acquisition_scale, + } + required = { + DIRECTION_DECREASING, + DIRECTION_INCREASING, + } + if not required.issubset(by_direction): + return + + profile = _node_profile(self) + configured_enabled = bool( + getattr(self, "adaptive_formal_speed_enabled", False) + ) + # G20 right repeatability is evaluated across independent sessions. + # Letting camera-dependent precheck density alter the physical scan + # speed changes velocity lag and backlash, so two runs of the same hand + # no longer measure the same process. Keep the reviewed base speed for + # every product task; adaptive speed remains available to legacy tools. + stability_speed_lock = profile.layout_id == G20_RIGHT_19_LAYOUT + enabled = configured_enabled and not stability_speed_lock + # MCP roll is both the fastest native mechanism and the one guarded by + # the strict 0.5-degree baseline backlash limit. Field data at speed + # 7 exceeded that limit after a clean speed-5 precheck, so sampling + # density alone is not sufficient evidence to accelerate roll. + eligible = int(item.spec.motor_index) not in range(6, 10) + selected_scale = 1.0 + if enabled and eligible and base_speed > 0: + caps: list[float] = [] + target_bins = int(self.adaptive_formal_speed_minimum_bins) + target_gap = int(self.adaptive_formal_speed_maximum_bin_gap) + for direction in sorted(required): + metric = by_direction[direction] + observed_scale = float(metric["acquisition_speed_scale"]) + caps.append( + observed_scale + * float(metric["bin_count"]) + / float(target_bins) + ) + caps.append( + observed_scale + * float(target_gap) + / float(max(1, int(metric["maximum_bin_gap"]))) + ) + selected_scale = max( + 1.0, + min(float(self.adaptive_formal_speed_max_scale), *caps), + ) + selected_speed = min( + 255, + int(math.floor(float(base_speed) * selected_scale + 1e-9)), + ) + if selected_speed <= base_speed or base_speed <= 0: + selected_speed = base_speed + selected_scale = 1.0 + else: + selected_scale = float(selected_speed) / float(base_speed) + scales = getattr(self, "formal_speed_scales", None) + if scales is None: + self.formal_speed_scales = {} + scales = self.formal_speed_scales + scales[item.spec.key] = selected_scale + raw_path = getattr(self, "raw_path", None) + if raw_path is not None: + append_jsonl( + raw_path, + { + "kind": "task_formal_speed_selected", + "task_name": item.spec.key, + "motor_index": int(item.spec.motor_index), + "enabled": enabled, + "configured_enabled": configured_enabled, + "stability_speed_lock": stability_speed_lock, + "eligible": eligible, + "ineligible_reason": ( + "g20_right_deterministic_acquisition_speed" + if stability_speed_lock + else "roll_baseline_hysteresis_sensitive" + if configured_enabled and not eligible + else "" + ), + "base_speed": base_speed, + "formal_speed": selected_speed, + "formal_speed_scale": round(selected_scale, 6), + "minimum_retained_bins": int( + self.adaptive_formal_speed_minimum_bins + ), + "maximum_retained_bin_gap": int( + self.adaptive_formal_speed_maximum_bin_gap + ), + "precheck": { + direction: dict(by_direction[direction]) + for direction in sorted(required) + }, + }, + ) + + def _active_endpoint_hold_seconds(self) -> float: + if self.active_sweep is None: + return float(self.endpoint_hold_seconds) + if self.active_sweep.precheck: + return max( + float(self.endpoint_hold_seconds), + float(self.task_precheck_hold_seconds), + ) + key = ( + _sweep_storage_key(self.active_sweep.spec), + self.active_sweep.cycle, + self.active_sweep.direction, + ) + retry = self.sweep_retry_counts.get(key, 0) + if retry <= 0: + return float(self.endpoint_hold_seconds) + return float( + self.retry_endpoint_hold_seconds[ + min(retry, len(self.retry_endpoint_hold_seconds)) - 1 + ] + ) + + def _publish_hold_current(self) -> None: + if len(self.latest_state_u8) != 20: + return + values = [ + int(np.clip(np.rint(value), 0, 255)) + for value in self.latest_state_u8 + ] + self._publish_command(values) + + def _snap_reached_state_to_command( + self, + current_state: Sequence[float], + target_command: Sequence[int], + ) -> tuple[int, ...]: + """Remove no-op transition steps already inside feedback tolerance.""" + if len(current_state) != 20 or len(target_command) != 20: + raise ValueError("state and target command must contain 20 values") + result = [ + int(np.clip(np.rint(value), 0, 255)) for value in current_state + ] + controlled = { + int(spec.motor_index) for spec in self.profile.joint_specs.values() + } + for index in controlled: + target = int(target_command[index]) + if abs(float(current_state[index]) - target) <= ( + G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, target + ) + ): + result[index] = target + return tuple(result) + + def _begin_return_baseline(self, after: str) -> None: + self.baseline_after = str(after) + if str(after) not in {"next_cycle", "resume_sweep"}: + # A mechanical task boundary is also the PnP ownership boundary. + # Same-task cycle resets and localized fit recovery retain it. + self.pnp_task_spec = None + target_command = ( + G20ThreeCameraCalibrationNode._return_command_for_transition( + self, after + ) + ) + current_state = getattr(self, "latest_state_u8", ()) + if len(current_state) != 20: + current_state = target_command + else: + current_state = self._snap_reached_state_to_command( + current_state, target_command + ) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + self.return_waypoints = deque( + build_calibration_return_waypoints( + target_command, + current_command=current_state, + profile=profile, + anchor_roll_motor=( + G20ThreeCameraCalibrationNode._return_anchor_roll_motor( + self + ) + ), + parallel=self.parallel_pose_transitions, + ) + ) + self.return_command_u8 = self.return_waypoints.popleft() + self.position_hold_since = None + self.motion_stage_started_at = time.monotonic() + getattr(self, "motion_stall_details", {}).clear() + self._reset_motion_progress( + self.motion_stage_started_at, self._baseline_error_u8() + ) + self.state = STATE_RETURN_BASELINE + self.reason = ( + "holding_same_finger_clearance_before_next_task" + if str(after) == "next_task_same_finger" + else f"return_baseline_before_{after}" + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, self.return_command_u8 + ) + ) + self._publish_command(list(self.return_command_u8)) + + def _return_anchor_roll_motor(self) -> int | None: + """Resolve the finger that must leave a fan pose first.""" + retry_items = getattr(self, "retry_sweep_items", []) + item = retry_items[0] if retry_items else None + if item is None: + items = getattr(self, "sweep_items", []) + index = int(getattr(self, "sweep_index", len(items))) + if index < len(items): + item = items[index] + if item is None: + item = getattr(self, "active_sweep", None) + if item is None: + return None + motor = int(item.spec.motor_index) + if motor in range(6, 10): + return motor + if motor in range(1, 5): + return motor + 5 + if motor in range(16, 20): + return motor - 10 + return None + + @staticmethod + def _four_finger_task_group(spec: SweepSpec) -> str | None: + """Return the finger whose consecutive tasks share one avoidance pose.""" + for joint_name in spec.joints: + finger = str(joint_name).split("_", 1)[0] + if finger in {"pinky", "ring", "middle", "index"}: + return finger + return None + + def _next_pending_sweep_item(self) -> SweepItem | None: + retry_items = getattr(self, "retry_sweep_items", []) + if retry_items: + return retry_items[0] + items = getattr(self, "sweep_items", []) + index = int(getattr(self, "sweep_index", len(items))) + return items[index] if index < len(items) else None + + def _advance_past_resumed_sweeps(self) -> None: + """Skip every independently revalidated task restored from disk.""" + completed = set(getattr(self, "resumed_task_keys", ()) or ()) + items = getattr(self, "sweep_items", []) + while ( + self.sweep_index < len(items) + and items[self.sweep_index].spec.key in completed + ): + self.sweep_index += 1 + + def _transition_after_completed_spec(self, spec: SweepSpec) -> str: + """Keep clearance parked between consecutive tasks of one finger.""" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item(self) + current_group = ( + G20ThreeCameraCalibrationNode._four_finger_task_group(spec) + ) + next_group = ( + None + if next_item is None + else G20ThreeCameraCalibrationNode._four_finger_task_group( + next_item.spec + ) + ) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and current_group is not None + and current_group == next_group + ): + return "next_task_same_finger" + return "next_sweep" + + def _return_command_for_transition(self, after: str) -> tuple[int, ...]: + """Choose a global or task-local safe transition target.""" + if str(after) in {"next_cycle", "next_task_same_finger"}: + next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item( + self + ) + if next_item is None: + return tuple(self.baseline_command) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return tuple( + build_calibration_motion_command( + next_item.spec, + int(self.baseline_command[next_item.spec.motor_index]), + baseline=self.baseline_command, + profile=profile, + ) + ) + if str(after) == "next_sweep": + merged = ( + G20ThreeCameraCalibrationNode._cross_group_return_command( + self + ) + ) + if merged is not None: + return merged + if str(after) not in {"retry_sweep", "resume_sweep"}: + return tuple(self.baseline_command) + retry_items = getattr(self, "retry_sweep_items", []) + item = retry_items[0] if retry_items else getattr(self, "active_sweep", None) + if item is None: + return tuple(self.baseline_command) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + # A four-finger retry immediately repeats the same task. Preserve its + # reviewed occlusion-clearance pose and return only to the queued + # direction start; a global baseline would unfold the parked fingers + # and make the next preparation flex them again. The retry setup has + # already reset every contributing PnP tracker. + keep_task_clearance = bool( + item.spec.motor_index == 10 + or ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and G20ThreeCameraCalibrationNode._four_finger_task_group( + item.spec + ) + is not None + ) + ) + if not keep_task_clearance: + return tuple(self.baseline_command) + return tuple( + build_calibration_motion_command( + item.spec, + item.start_u8, + baseline=self.baseline_command, + profile=profile, + ) + ) + + def _cross_group_return_command(self) -> tuple[int, ...] | None: + """Keep already-positioned clearance motors through a finger change. + + Crossing from one finger's task group to the next used to unfold every + auxiliary motor back to baseline and then re-flex it for the next + avoidance pose. Motors the next group still wants at a non-baseline + value and that are already in place (within their endpoint deadband) + now simply stay put; everything else still returns to baseline, so the + reviewed roll-before-unfold and open-before-flex orderings keep + holding and only genuinely unused clearance unfolds. + """ + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return None + next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item( + self + ) + if next_item is None: + return None + current_state = getattr(self, "latest_state_u8", ()) + if len(current_state) != 20: + return None + baseline = [int(value) for value in self.baseline_command] + # Match what the next preparation will actually command: the measured + # motor enters at its sweep start, not at the baseline value. + next_start = int( + getattr( + next_item, + "start_u8", + baseline[int(next_item.spec.motor_index)], + ) + ) + next_final = [ + int(value) + for value in build_calibration_motion_command( + next_item.spec, + next_start, + baseline=baseline, + profile=profile, + ) + ] + controlled = { + int(spec.motor_index) for spec in profile.joint_specs.values() + } + target = list(baseline) + for motor in sorted(controlled): + next_value = next_final[motor] + if next_value == baseline[motor]: + continue + actual = float(current_state[motor]) + tolerance = ( + G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, motor, next_value + ) + ) + if abs(actual - next_value) <= tolerance: + target[motor] = next_value + return tuple(target) + + def _current_return_command(self) -> tuple[int, ...]: + command = getattr(self, "return_command_u8", self.baseline_command) + return tuple(int(value) for value in command) + + def _baseline_reached(self) -> bool: + if len(self.latest_state_u8) != 20: + return False + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return_command = G20ThreeCameraCalibrationNode._current_return_command( + self + ) + indices = sorted( + {spec.motor_index for spec in profile.joint_specs.values()} + ) + return all( + abs(float(self.latest_state_u8[index]) - return_command[index]) + <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(return_command[index]) + ) + for index in indices + ) + + def _baseline_error_details(self) -> dict[str, float | int | str]: + if len(self.latest_state_u8) != 20: + return { + "stage": "return_baseline", + "motor_index": -1, + "target_u8": float("nan"), + "actual_u8": float("nan"), + "error_u8": float("inf"), + "tolerance_u8": float(self.endpoint_tolerance_u8), + } + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return_command = G20ThreeCameraCalibrationNode._current_return_command( + self + ) + indices = sorted({spec.motor_index for spec in profile.joint_specs.values()}) + values: list[dict[str, float | int | str]] = [] + for index in indices: + target = float(return_command[index]) + actual = float(self.latest_state_u8[index]) + tolerance = ( + G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(target) + ) + ) + values.append( + { + "stage": "return_baseline", + "motor_index": int(index), + "target_u8": target, + "actual_u8": actual, + "error_u8": abs(actual - target), + "tolerance_u8": tolerance, + } + ) + outside = [ + item + for item in values + if float(item["error_u8"]) > float(item["tolerance_u8"]) + ] + candidates = outside or values + return max( + candidates, + key=lambda item: ( + float(item["error_u8"]) - float(item["tolerance_u8"]), + float(item["error_u8"]), + ), + ) + + def _baseline_error_u8(self) -> float: + if len(self.latest_state_u8) != 20: + return float("inf") + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + return_command = G20ThreeCameraCalibrationNode._current_return_command( + self + ) + indices = {spec.motor_index for spec in profile.joint_specs.values()} + return max( + abs( + float(self.latest_state_u8[index]) + - float(return_command[index]) + ) + for index in indices + ) + + def _command_vector_reached(self, command: Sequence[int]) -> bool: + if len(self.latest_state_u8) != 20 or len(command) != 20: + return False + controlled = { + int(spec.motor_index) for spec in self.profile.joint_specs.values() + } + return all( + abs(float(self.latest_state_u8[index]) - float(command[index])) + <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(command[index]) + ) + for index in controlled + ) + + def _command_vector_error_u8(self, command: Sequence[int]) -> float: + if len(self.latest_state_u8) != 20 or len(command) != 20: + return float("inf") + controlled = { + int(spec.motor_index) for spec in self.profile.joint_specs.values() + } + return max( + abs(float(self.latest_state_u8[index]) - float(command[index])) + for index in controlled + ) + + def _command_vector_error_details( + self, command: Sequence[int], context: str + ) -> dict[str, float | int | str]: + """Identify the actual controlled motor blocking one safe waypoint.""" + if len(self.latest_state_u8) != 20 or len(command) != 20: + return { + "stage": str(context), + "motor_index": -1, + "target_u8": float("nan"), + "actual_u8": float("nan"), + "error_u8": float("inf"), + "tolerance_u8": float(self.endpoint_tolerance_u8), + } + controlled = sorted( + {int(spec.motor_index) for spec in self.profile.joint_specs.values()} + ) + values: list[dict[str, float | int | str]] = [] + for index in controlled: + target = float(command[index]) + actual = float(self.latest_state_u8[index]) + tolerance = G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(target) + ) + values.append( + { + "stage": str(context), + "motor_index": index, + "target_u8": target, + "actual_u8": actual, + "error_u8": abs(actual - target), + "tolerance_u8": float(tolerance), + } + ) + outside = [ + item + for item in values + if float(item["error_u8"]) > float(item["tolerance_u8"]) + ] + return max( + outside or values, + key=lambda item: ( + float(item["error_u8"]) - float(item["tolerance_u8"]), + float(item["error_u8"]), + ), + ) + + def _motion_command_reached( + self, + spec: SweepSpec, + command_u8: int, + state_u8: tuple[float, ...] | None = None, + ) -> bool: + """Check the swept motor and all clearance motors together.""" + state = self.latest_state_u8 if state_u8 is None else state_u8 + if len(state) != 20: + return False + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + expected = build_calibration_motion_command( + spec, + command_u8, + baseline=self.baseline_command, + profile=profile, + ) + indices = {spec.motor_index} + indices.update(calibration_auxiliary_commands(spec, profile=profile)) + return all( + abs(float(state[index]) - expected[index]) + <= ( + self._endpoint_tolerance_for_spec(spec, command_u8) + if index == spec.motor_index + else G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, expected[index] + ) + ) + for index in indices + ) + + def _steady_checkpoint_reached( + self, + spec: SweepSpec, + command_u8: int, + state_u8: tuple[float, ...] | None = None, + ) -> bool: + """Accept a settled command/feedback offset without hiding a stall. + + The checkpoint exists specifically to identify requested-command to + feedback/angle offsets, so applying the ordinary ±2-u8 equality gate + here is circular. Only the swept motor gets the bounded calibration + deadband; every clearance motor keeps its normal strict tolerance. + """ + state = self.latest_state_u8 if state_u8 is None else state_u8 + if len(state) != 20: + return False + expected = build_calibration_motion_command( + spec, + command_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + swept_tolerance = max( + float(self.steady_checkpoint_command_feedback_tolerance_u8), + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + self, spec, command_u8 + ), + ) + if ( + abs(float(state[spec.motor_index]) - expected[spec.motor_index]) + > swept_tolerance + ): + return False + return all( + abs(float(state[index]) - expected[index]) + <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, expected[index] + ) + for index in calibration_auxiliary_commands( + spec, profile=self.profile + ) + ) + + def _steady_checkpoint_feedback_is_stable( + self, + spec: SweepSpec, + frames: Sequence[FrameObservation], + ) -> bool: + """Require the held feedback range to be small before recording it.""" + if len(frames) < 3: + return False + values = [ + float(frame.state_u8[spec.motor_index]) for frame in frames + ] + return bool( + max(values) - min(values) + <= float(self.steady_checkpoint_maximum_feedback_range_u8) + ) + + def _motion_command_error_u8( + self, + spec: SweepSpec, + command_u8: int, + state_u8: tuple[float, ...] | None = None, + ) -> float: + """Return the worst swept/auxiliary motor error for stall detection.""" + state = self.latest_state_u8 if state_u8 is None else state_u8 + if len(state) != 20: + return float("inf") + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + expected = build_calibration_motion_command( + spec, + command_u8, + baseline=self.baseline_command, + profile=profile, + ) + indices = {spec.motor_index} + indices.update(calibration_auxiliary_commands(spec, profile=profile)) + return max( + abs(float(state[index]) - float(expected[index])) + for index in indices + ) + + def _motion_command_error_details( + self, + spec: SweepSpec, + command_u8: int, + context: str, + ) -> dict[str, float | int | str]: + """Identify the swept or clearance motor blocking a motion.""" + if len(self.latest_state_u8) != 20: + return { + "stage": str(context), + "motor_index": -1, + "target_u8": float(command_u8), + "actual_u8": float("nan"), + "error_u8": float("inf"), + "tolerance_u8": float(self.endpoint_tolerance_u8), + } + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + expected = build_calibration_motion_command( + spec, + command_u8, + baseline=self.baseline_command, + profile=profile, + ) + indices = {spec.motor_index} + indices.update(calibration_auxiliary_commands(spec, profile=profile)) + values: list[dict[str, float | int | str]] = [] + for index in indices: + target = float(expected[index]) + actual = float(self.latest_state_u8[index]) + tolerance = ( + G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( + self, spec, command_u8 + ) + if index == spec.motor_index + else G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, index, int(target) + ) + ) + values.append( + { + "stage": str(context), + "motor_index": int(index), + "target_u8": target, + "actual_u8": actual, + "error_u8": abs(actual - target), + "tolerance_u8": float(tolerance), + } + ) + outside = [ + item + for item in values + if float(item["error_u8"]) > float(item["tolerance_u8"]) + ] + return max( + outside or values, + key=lambda item: ( + float(item["error_u8"]) - float(item["tolerance_u8"]), + float(item["error_u8"]), + ), + ) + + def _reset_motion_progress(self, now: float, error_u8: float) -> None: + self.motion_progress_reference_error_u8 = float(error_u8) + self.motion_last_progress_at = float(now) + + def _pause_if_motion_stalled( + self, + *, + now: float, + error_u8: float, + context: str, + details: Mapping[str, Any] | None = None, + ) -> bool: + """Pause immediately when feedback stops moving toward its target.""" + error = float(error_u8) + if not math.isfinite(error): + return False + reference = float( + getattr(self, "motion_progress_reference_error_u8", float("inf")) + ) + last_progress = float(getattr(self, "motion_last_progress_at", now)) + minimum_progress = float( + getattr(self, "motor_stall_minimum_progress_u8", 1.0) + ) + if not math.isfinite(reference): + self._reset_motion_progress(now, error) + return False + if reference - error >= minimum_progress: + self._reset_motion_progress(now, error) + return False + motion_started_value = getattr(self, "motion_stage_started_at", None) + startup_grace = float( + getattr(self, "motor_stall_startup_grace_seconds", 1.0) + ) + if ( + motion_started_value is not None + and now - float(motion_started_value) < startup_grace + ): + return False + timeout = float(getattr(self, "motor_stall_timeout_seconds", 8.0)) + if now - last_progress < timeout: + return False + detail_payload = {} if details is None else dict(details) + reason_parts = ["motor_state_stalled", str(context)] + for key in ("motor_index", "target_u8", "actual_u8", "tolerance_u8"): + if key in detail_payload: + reason_parts.append(f"{key}={detail_payload[key]}") + reason_parts.append(f"timeout_seconds={timeout:.3f}") + reason_parts.append(f"error_u8={error:.3f}") + reason = ":".join(reason_parts) + self.motion_stall_details = { + "kind": "motion_stall", + "stage": str(context), + "error_u8": error, + "timeout_seconds": timeout, + **detail_payload, + } + append_jsonl( + self.raw_path, + { + "kind": "mechanical_motion_stall", + "context": context, + "error_u8": error, + "timeout_seconds": timeout, + "minimum_progress_u8": minimum_progress, + "state_u8": [float(value) for value in self.latest_state_u8], + "details": detail_payload, + }, + ) + self._pause(reason) + return True + + def _active_sweep_timeout_seconds(self) -> float: + """Scale retry timeout so a deliberately slower retry can finish.""" + if self.active_sweep is None: + return float(self.sweep_timeout_seconds) + key = ( + _sweep_storage_key(self.active_sweep.spec), + self.active_sweep.cycle, + self.active_sweep.direction, + ) + retry = self.sweep_retry_counts.get(key, 0) + if retry <= 0: + return float(self.sweep_timeout_seconds) + scale = self.retry_speed_scales[ + min(retry, len(self.retry_speed_scales)) - 1 + ] + return float(self.sweep_timeout_seconds) / float(scale) + + def _endpoint_tolerance_for_spec( + self, spec: SweepSpec, endpoint_u8: int + ) -> float: + """Return the measured feedback deadband for one sweep endpoint.""" + if spec.motor_index == 10 and int(endpoint_u8) == 0: + return float( + getattr( + self, + "thumb_yaw_zero_endpoint_tolerance_u8", + self.endpoint_tolerance_u8, + ) + ) + if spec.motor_index == 19 and int(endpoint_u8) == 0: + return float( + getattr( + self, + "pinky_pip_zero_endpoint_tolerance_u8", + self.endpoint_tolerance_u8, + ) + ) + return G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( + self, spec.motor_index, int(endpoint_u8) + ) + + def _synchronised_endpoint_tolerance_for_spec( + self, spec: SweepSpec, endpoint_u8: int + ) -> float: + """Return the endpoint bin deadband for image-timestamped feedback. + + Motion completion deliberately keeps the physical endpoint deadband. + A synchronised image observation uses interpolated feedback at the + camera timestamp and can trail that latest feedback by a fraction of + one u8 count. One additional count prevents a reached endpoint from + being discarded solely at this continuous-to-integer bin boundary. + """ + mechanical_tolerance = self._endpoint_tolerance_for_spec( + spec, endpoint_u8 + ) + return mechanical_tolerance + float( + getattr(self, "synchronised_endpoint_tolerance_margin_u8", 1.0) + ) + + def _motor_endpoint_tolerance( + self, motor_index: int, endpoint_u8: int + ) -> float: + """Return a side/motor-specific feedback deadband when established.""" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if int(motor_index) == 10 and int(endpoint_u8) == 0: + return float( + getattr( + self, + "thumb_yaw_zero_endpoint_tolerance_u8", + 4.0, + ) + ) + if ( + profile.side == "right" + and int(motor_index) == 10 + and int(endpoint_u8) == 255 + ): + return float( + getattr( + self, + "right_thumb_yaw_255_endpoint_tolerance_u8", + 5.0, + ) + ) + if ( + profile.side == "right" + and int(motor_index) == 19 + and int(endpoint_u8) == 0 + ): + return float( + getattr( + self, + "pinky_pip_zero_endpoint_tolerance_u8", + 5.0, + ) + ) + return float(self.endpoint_tolerance_u8) + + def _requires_mid_sweep_baseline_hold(self, item: SweepItem) -> bool: + """Return whether this formal sweep crosses a non-endpoint zero. + + The four MCP-roll motors use command 127 as zero. Their zero-pose + hysteresis must be measured after settling at 127 from each direction, + not inferred from frames captured while the motor is moving through + 127. Endpoint-zero joints already have static start/end holds. + """ + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if ( + profile.layout_id != G20_RIGHT_19_LAYOUT + or item.precheck + or item.cycle < 0 + ): + return False + baseline = int(self.baseline_command[item.spec.motor_index]) + return baseline not in {item.start_u8, item.target_u8} + + def _start_next_sweep(self) -> None: + carried_start_frames = list( + getattr(self, "carried_sweep_start_frames", ()) + ) + if hasattr(self, "carried_sweep_start_frames"): + self.carried_sweep_start_frames.clear() + retry_items = getattr(self, "retry_sweep_items", []) + if retry_items: + self.active_sweep = retry_items.pop(0) + self.active_sweep_is_fit_retry = True + else: + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) + if self.sweep_index >= len(self.sweep_items): + self.active_sweep = None + self._begin_return_baseline("fit") + return + self.active_sweep = self.sweep_items[self.sweep_index] + self.active_sweep_is_fit_retry = False + item = self.active_sweep + profile = _node_profile(self) + if profile.layout_id == G20_RIGHT_19_LAYOUT: + self.pnp_task_spec = item.spec + is_fit_retry = bool( + getattr(self, "active_sweep_is_fit_retry", False) + ) + if _requires_pnp_tracker_reset_for_sweep( + profile, + item, + is_fit_retry=is_fit_retry, + preserve_retry_continuity=bool( + getattr(self, "retry_preserve_pnp_continuity", False) + ), + ): + reset_trackers = getattr(self, "_reset_view_trackers", None) + preserve_task_reference = _preserve_pnp_task_reference_for_sweep( + item, + is_fit_retry=is_fit_retry, + has_precheck_anchor=( + profile.layout_id == G20_RIGHT_19_LAYOUT + ), + ) + reset_views = set(_sweep_views(profile, item.spec)) + observer = next( + ( + candidate + for candidate in profile.palm_axis_observers + if candidate.task_name == item.spec.key + ), + None, + ) + if observer is not None: + reset_views.add(observer.view) + if is_fit_retry: + scoped = set(getattr(self, "retry_joint_names", set())) + if scoped: + reset_views = { + _node_profile(self).record_specs[name].view + for name in scoped + } + reset_views.discard(None) + if observer is not None: + reset_views.add(observer.view) + for view in reset_views: + runtime = getattr(self, "views", {}).get(view) + if runtime is None or reset_trackers is None: + continue + # A normal G20-right task enters here only for its first + # decreasing precheck. All formal cycles keep this exact + # frame-to-frame branch instead of independently selecting a + # new stable IPPE solution. A fit retry deliberately starts + # fresh but retains the task-relative anchor. + if preserve_task_reference: + reset_trackers( + runtime, preserve_task_reference=True + ) + else: + reset_trackers(runtime) + self._reset_view_pnp_diagnostics(runtime) + runtime.pnp_invalid_since = None + runtime.pnp_reset_count += 1 + raw_path = getattr(self, "raw_path", None) + if raw_path is not None: + append_jsonl( + raw_path, + { + "kind": "pnp_task_initialization", + "view": view, + "task_name": item.spec.key, + "motor_index": item.spec.motor_index, + "cycle": item.cycle, + "fit_retry": is_fit_retry, + "task_reference_preserved": ( + preserve_task_reference + ), + }, + ) + getattr(self, "sweep_frames", []).clear() + getattr(self, "sweep_baseline_frames", []).clear() + self.sweep_baseline_pending = False + self.sweep_baseline_hold_since = None + getattr(self, "sweep_start_frames", []).clear() + getattr(self, "sweep_start_frames", []).extend(carried_start_frames) + self.sweep_detection_total_frames = 0 + self.sweep_detection_valid_frames = 0 + self.sweep_detection_total_by_view = {} + self.sweep_detection_valid_by_view = {} + self.position_hold_since = None + self.motion_stage_started_at = time.monotonic() + current_state = getattr(self, "latest_state_u8", ()) + if len(current_state) != 20: + current_state = self.baseline_command + else: + preparation_target = build_calibration_motion_command( + item.spec, + item.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + current_state = self._snap_reached_state_to_command( + current_state, preparation_target + ) + self.preparation_waypoints = deque( + build_calibration_preparation_waypoints( + item.spec, + item.start_u8, + current_command=current_state, + baseline=self.baseline_command, + profile=self.profile, + parallel=self.parallel_pose_transitions, + ) + ) + self.preparation_command_u8 = self.preparation_waypoints.popleft() + self._reset_motion_progress( + self.motion_stage_started_at, + self._command_vector_error_u8(self.preparation_command_u8), + ) + self.state = STATE_PREPARE_SWEEP + self.reason = ( + f"prepare_{item.spec.view}_motor_{item.spec.motor_index}_" + f"cycle_{item.cycle}_{item.direction}" + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, self.preparation_command_u8 + ) + ) + self._publish_command(list(self.preparation_command_u8)) + + def _stage_immediate_reverse_start_frames( + self, completed: SweepItem, following: SweepItem + ) -> int: + """Carry a proven terminal endpoint into an immediate reverse pass.""" + carried = getattr(self, "carried_sweep_start_frames", None) + if carried is None: + self.carried_sweep_start_frames = [] + carried = self.carried_sweep_start_frames + carried.clear() + if ( + completed.spec != following.spec + or completed.target_u8 != following.start_u8 + or completed.direction == following.direction + ): + return 0 + tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, following.spec, following.start_u8 + ) + ) + limit_per_view = max(1, int(getattr(self, "preflight_frames", 30))) + selected: list[FrameObservation] = [] + for view in _sweep_views(_node_profile(self), following.spec): + view_frames = [ + frame + for frame in self.sweep_frames + if frame.view == view + and abs( + float(frame.state_u8[following.spec.motor_index]) + - following.start_u8 + ) + <= tolerance + ] + selected.extend(view_frames[-limit_per_view:]) + selected.sort(key=lambda frame: frame.stamp_ns) + carried.extend(selected) + if selected and getattr(self, "raw_path", None) is not None: + append_jsonl( + self.raw_path, + { + "kind": "sweep_endpoint_frames_carried", + "task_name": completed.spec.key, + "from_cycle": completed.cycle, + "from_direction": completed.direction, + "to_cycle": following.cycle, + "to_direction": following.direction, + "endpoint_u8": following.start_u8, + "valid_frames": len(selected), + "valid_frames_by_joint": { + name: len(_frames_for_joint(selected, name)) + for name in following.spec.joints + }, + }, + ) + return len(selected) + + def _begin_active_sweep(self, now: float) -> None: + assert self.active_sweep is not None + self.sweep_frames.clear() + self.sweep_frames.extend(self.sweep_start_frames) + self.sweep_start_frames.clear() + if not hasattr(self, "sweep_baseline_frames"): + self.sweep_baseline_frames = [] + self.sweep_baseline_frames.clear() + self.sweep_baseline_pending = ( + G20ThreeCameraCalibrationNode._requires_mid_sweep_baseline_hold( + self, self.active_sweep + ) + ) + self.sweep_baseline_hold_since = None + self.sweep_started_at = now + self.sweep_last_valid_at = now + capture_views = _sweep_views( + _node_profile(self), self.active_sweep.spec + ) + self.sweep_last_valid_at_by_view = { + view: now for view in capture_views + } + self.sweep_endpoint_since = None + self.sweep_detection_total_frames = 0 + self.sweep_detection_valid_frames = 0 + self.sweep_detection_total_by_view = { + view: 0 for view in capture_views + } + self.sweep_detection_valid_by_view = { + view: 0 for view in capture_views + } + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + self.sweep_checkpoint_commands = deque( + _steady_checkpoint_commands(profile, self.active_sweep) + ) + self.sweep_checkpoint_mode = bool(self.sweep_checkpoint_commands) + self.sweep_checkpoint_target_u8 = None + self.sweep_checkpoint_hold_since = None + if not hasattr(self, "sweep_checkpoint_frames"): + self.sweep_checkpoint_frames = [] + self.sweep_checkpoint_frames.clear() + if self.sweep_checkpoint_commands: + # PREPARE_SWEEP already held the start endpoint long enough to be + # a steady command-domain observation. + if hasattr(self, "command_records_by_joint"): + self._record_command_checkpoint( + self.active_sweep, + self.active_sweep.start_u8, + list(self.sweep_frames), + ) + self._reset_motion_progress( + now, + self._motion_command_error_u8( + self.active_sweep.spec, self.active_sweep.target_u8 + ), + ) + self.state = STATE_SWEEP + self.reason = ( + "collecting_dedicated_baseline_hold" + if self.sweep_baseline_pending + else "collecting_timestamp_synchronised_tag_centres" + ) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + target_u8 = ( + int(self.baseline_command[self.active_sweep.spec.motor_index]) + if self.sweep_baseline_pending + else ( + self.sweep_checkpoint_commands.popleft() + if self.sweep_checkpoint_commands + else self.active_sweep.target_u8 + ) + ) + if not self.sweep_baseline_pending and self.sweep_checkpoint_mode: + self.sweep_checkpoint_target_u8 = int(target_u8) + self._publish_command( + build_calibration_motion_command( + self.active_sweep.spec, + target_u8, + baseline=self.baseline_command, + profile=profile, + ) + ) + + def _record_command_checkpoint( + self, + item: SweepItem, + requested_command_u8: int, + frames: Sequence[FrameObservation], + ) -> None: + """Persist one steady requested-command/feedback/vision observation.""" + motor = int(item.spec.motor_index) + record_joint_names = tuple(item.spec.joints) + if getattr(self, "active_sweep_is_fit_retry", False): + scoped = set(getattr(self, "retry_joint_names", set())) + if scoped: + record_joint_names = tuple( + name for name in item.spec.joints if name in scoped + ) + for joint_name in record_joint_names: + selected = _frames_for_joint(frames, joint_name) + if len(selected) < 3: + raise RuntimeError( + f"steady command checkpoint {joint_name} has fewer than 3 frames" + ) + feedback = float( + np.median([float(frame.state_u8[motor]) for frame in selected]) + ) + state = np.median( + np.asarray( + [frame.state_u8 for frame in selected], dtype=float + ), + axis=0, + ) + durable = canonical_sample_record({ + "kind": "steady_command_sample", + "attempt": self.sweep_attempts.get( + _sweep_storage_key(item.spec), 1 + ), + "task_name": item.spec.key, + "view": self.profile.record_specs[joint_name].view, + "joint": joint_name, + "motor_index": motor, + "cycle": item.cycle, + "direction": item.direction, + "requested_command_u8": int(requested_command_u8), + "feedback_u8": round(feedback, 6), + "image_stamp_ns": int( + np.median([frame.stamp_ns for frame in selected]) + ), + "relative_quaternion_xyzw": [ + float(value) + for value in robust_rotation_summary( + [ + frame.joint_quaternions_xyzw[joint_name] + for frame in selected + ] + )[0] + ], + "relative_translation_xyz_m": [ + float(value) + for value in np.median( + np.asarray( + [ + frame.joint_vectors_xyz_m[joint_name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + ], + "image_relative_xy_px": [ + float(value) + for value in np.median( + np.asarray( + [ + frame.image_vectors_xy_px[joint_name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + ], + "parent_pose_common": _robust_pose_payload( + [frame.parent_poses_common[joint_name] for frame in selected] + ), + "child_pose_common": _robust_pose_payload( + [frame.child_poses_common[joint_name] for frame in selected] + ), + "state_u8": [float(value) for value in state], + "valid_frames": len(selected), + }) + record = fitting_sample_record(durable) + self.command_records_by_joint[joint_name].append(record) + append_jsonl(self.raw_path, durable) + + def _publish_next_checkpoint(self, now: float) -> bool: + """Advance a first-round steady scan; return False at its endpoint.""" + if not self.sweep_checkpoint_commands: + self.sweep_checkpoint_target_u8 = None + # The final checkpoint is also the normal sweep endpoint. Leave + # checkpoint mode here so _advance can run its common endpoint + # hold/completion path on this and subsequent timer ticks. Keeping + # the mode latched made the next tick treat the intentionally + # cleared target as an internal error. + self.sweep_checkpoint_mode = False + return False + assert self.active_sweep is not None + target = int(self.sweep_checkpoint_commands.popleft()) + self.sweep_checkpoint_target_u8 = target + self.sweep_checkpoint_hold_since = None + self.sweep_checkpoint_frames.clear() + self.sweep_endpoint_since = None + self.motion_stage_started_at = now + self._reset_motion_progress( + now, self._motion_command_error_u8(self.active_sweep.spec, target) + ) + self._publish_command( + build_calibration_motion_command( + self.active_sweep.spec, + target, + baseline=self.baseline_command, + profile=self.profile, + ) + ) + return True + + def _sweep_spec_start_index(self, spec: SweepSpec) -> int: + return next( + index + for index, item in enumerate(self.sweep_items) + if item.spec == spec + ) + + def _fit_joint_records( + self, + joint_name: str, + records: list[dict[str, Any]], + *, + relaxed: bool = False, + ) -> JointCurveFit: + del relaxed + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + zero_command = int( + self.baseline_command[ + profile.record_specs[joint_name].motor_index + ] + ) + model_records = G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, joint_name, records + ) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and ( + joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + or ( + joint_name.endswith("_mcp_roll") + and not joint_name.startswith("thumb_") + ) + ) + ): + return fit_joint_image_curve( + model_records, + maximum_radial_rms_px=( + self.image_trajectory_maximum_radial_rms_px + ), + maximum_radial_p95_px=( + self.image_trajectory_maximum_radial_p95_px + ), + minimum_radius_px=self.image_trajectory_minimum_radius_px, + minimum_arc_rad=self.trajectory_minimum_arc_rad, + ) + return fit_rotation_joint_curve( + model_records, + zero_command_u8=zero_command, + canonical_zero_direction=canonical_zero_direction( + profile, joint_name + ), + ) + + def _hysteresis_axis_for_fit( + self, + joint_name: str, + records: Sequence[Mapping[str, Any]], + fit: JointCurveFit, + ) -> Sequence[float]: + """Return a physical rotation axis for directional zero checks. + + End-on finger flexion uses its much more stable projected circle for + the dynamic curve. Baseline hysteresis is still a pose-domain check, + so fit a separate diagnostic rotation axis instead of accidentally + treating the image circle as a 3-D pose model. + """ + axis = fit.circle.get("axis_xyz") + if axis is not None: + return axis + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + zero_command = int( + self.baseline_command[ + profile.record_specs[joint_name].motor_index + ] + ) + diagnostic_fit = fit_rotation_joint_curve( + G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, joint_name, records + ), + zero_command_u8=zero_command, + canonical_zero_direction=canonical_zero_direction( + profile, joint_name + ), + ) + return diagnostic_fit.circle["axis_xyz"] + + def _records_with_baseline_holds( + self, + joint_name: str, + records: Sequence[Mapping[str, Any]], + ) -> list[dict[str, Any]]: + """Add settled mid-range zero poses to directional model fitting.""" + samples = [dict(record) for record in records] + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + if canonical_zero_direction(profile, joint_name) is None: + return samples + included_groups = { + (int(record["cycle"]), str(record["direction"])) + for record in samples + } + holds = getattr(self, "baseline_records_by_joint", {}).get( + joint_name, () + ) + samples.extend( + dict(record) + for record in holds + if ( + int(record["cycle"]), str(record["direction"]) + ) in included_groups + ) + return samples + + def _fit_axis_measurement_raw( + self, joint_name: str, cycle: int + ) -> JointAxisMeasurement: + constraint: Sequence[float] | None = None + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) + upstream_joint = { + # These neighbouring axes are parallel in the fixed source URDF. + # A small planar Tag's monocular PnP orientation can have a stable + # field-dependent bias, so use the well-observed upstream axis + # direction and let each Tag-centre trajectory independently fit + # its physical axis line. This changes no URDF geometry and is + # valid only because the source-URDF axes are deliberately locked. + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + **{ + f"{finger}_pip": f"{finger}_mcp_pitch" + for finger in ("index", "middle", "ring", "pinky") + }, + **{ + f"{finger}_dip": f"{finger}_pip" + for finger in ("index", "middle", "ring", "pinky") + }, + }.get(joint_name) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and joint_name.endswith("_mcp_roll") + and not joint_name.endswith("_mcp_roll_side") + and not joint_name.startswith("thumb_") + ): + reference_roll = f"{profile.reference_finger}_mcp_roll" + if joint_name != reference_roll: + # The four finger roll axes are exactly parallel in the + # product URDF. The reference-finger task is acquired first, + # so reuse its measured common-frame direction for the other + # three fingers. This removes session-to-session planar-PnP + # branch tilt without changing any fitted axis-line position. + upstream_joint = reference_roll + if upstream_joint is not None: + # Resolve recursively so thumb_ip receives the already constrained + # thumb_mcp direction (and index_dip the constrained index_pip + # direction), rather than reintroducing the raw PnP orientation at + # the last passive joint. + upstream = ( + G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + self, upstream_joint, cycle + ) + ) + constraint = upstream.axis_common_xyz + spec = profile.record_specs[joint_name] + extrinsics = getattr(self, "extrinsics", None) + view_normal = None + if extrinsics is not None: + view_transform = extrinsics.transform(spec.view) + view_normal = view_transform[:3, :3] @ np.asarray( + [0.0, 0.0, 1.0], dtype=float + ) + measurement_records = ( + G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, + joint_name, + self.records_by_joint[joint_name], + ) + ) + measurement = fit_joint_axis_measurement( + joint_name, + measurement_records, + cycle=cycle, + zero_command_u8=int( + self.baseline_command[spec.motor_index] + ), + axis_common_constraint=constraint, + constrained_circle_joints=( + zero_profile.constrained_circle_joints + | ({joint_name} if joint_name.endswith("_side") else set()) + ), + view_normal_common_xyz=view_normal, + canonical_zero_direction=canonical_zero_direction( + profile, joint_name + ), + ) + sweep_spec = next( + sweep + for sweep in profile.sweep_specs + if joint_name in sweep.joints + ) + condition_command = build_calibration_motion_command( + sweep_spec, + int(self.baseline_command[spec.motor_index]), + baseline=self.baseline_command, + profile=profile, + ) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + measurement = replace( + measurement, + condition_command_u8=tuple( + float(value) for value in condition_command + ), + ) + if extrinsics is None: + return measurement + return with_depth_free_axis_projection( + replace( + measurement, + view_normal_common_xyz=tuple( + float(value) for value in view_normal + ), + ), + extrinsics.transform(spec.view)[:3, 3], + ) + + def _fit_axis_measurement( + self, joint_name: str, cycle: int + ) -> JointAxisMeasurement: + """Fit one axis, fusing a separately accepted cross-view estimate.""" + primary = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + self, joint_name, cycle + ) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + validation_sources = profile.axis_validation_sources or {} + validation_name = validation_sources.get(joint_name) + if validation_name is None: + return primary + validation_records = self.records_by_joint.get(validation_name, []) + if not any( + int(record.get("cycle", -1)) == int(cycle) + for record in validation_records + ): + # Front roll is provisionally checked before the independent side + # task has been acquired. Fusion is allowed only in the final fit + # after both complete raw datasets have independently passed. + return primary + secondary = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( + self, validation_name, cycle + ) + primary_axis = np.asarray(primary.axis_common_xyz, dtype=float) + secondary_axis = np.asarray(secondary.axis_common_xyz, dtype=float) + if float(primary_axis @ secondary_axis) < 0.0: + secondary_axis = -secondary_axis + axis_difference = math.acos( + float(np.clip(primary_axis @ secondary_axis, -1.0, 1.0)) + ) + primary_point = np.asarray(primary.point_common_xyz_m, dtype=float) + secondary_point = np.asarray(secondary.point_common_xyz_m, dtype=float) + line_distance = float( + np.linalg.norm( + np.cross(secondary_point - primary_point, primary_axis) + ) + ) + validation_spec = profile.record_specs[validation_name] + if validation_spec.zero_kind == "axis_cross_view_validation": + # This Tag is attached to the downstream PIP link. During MCP + # roll its orientation contains linkage/pitch coupling and is not + # an independent observation of the published MCP angle or axis + # direction. It remains the physical axis-line-position source; + # its own radial, line-repeatability, visibility and trajectory + # quality gates are all applied before reaching this point. + decision = ( + "diagnostic_only_gross_pose_disagreement_use_primary" + if axis_difference + > getattr( + self, + "cross_view_roll_maximum_axis_difference_rad", + math.radians(15.0), + ) + else "diagnostic_only_pose_disagreement_use_primary" + ) + G20ThreeCameraCalibrationNode._record_cross_view_roll_axis_diagnostic( + self, + joint_name, + cycle, + primary=primary, + secondary=secondary, + axis_difference_rad=axis_difference, + line_distance_m=line_distance, + decision=decision, + ) + extrinsics = getattr(self, "extrinsics", None) + if extrinsics is None or validation_spec.view is None: + raise ValueError( + f"{validation_name}: cross-view camera geometry is unavailable" + ) + primary_view = profile.record_specs[joint_name].view + if primary_view is None: + raise ValueError( + f"{joint_name}: primary point source has no view" + ) + point_camera_center = extrinsics.transform(primary_view)[:3, 3] + point_ray = primary_point - point_camera_center + interpretation_plane_normal = np.cross( + point_ray, primary_axis + ) + interpretation_plane_normal /= np.linalg.norm( + interpretation_plane_normal + ) + return replace( + primary, + point_common_xyz_m=tuple( + float(value) for value in primary.point_common_xyz_m + ), + axis_point_source=( + "front_interpretation_plane_cross_view_validated" + ), + pose_axis_line_rms_m=primary.pose_axis_line_rms_m, + pose_axis_line_source_joints=(joint_name,), + axis_point_camera_center_common_xyz_m=tuple( + float(value) + for value in point_camera_center + ), + axis_point_interpretation_plane_normal_common_xyz=tuple( + float(value) + for value in interpretation_plane_normal + ), + ) + # Monocular planar-tag orientation on the side view carries a + # systematic line-of-sight IPPE bias of several degrees through roll + # sweeps (tags tilted ~13-20 deg from the ray), which sub-pixel + # reprojection cannot expose. Session 20260820_105535 showed a + # stable 11.4 deg front/side disagreement from exactly this effect, + # so sub-degree cross-view agreement is not achievable at this + # geometry. Disagreement above the fusion gate now falls back to the + # trusted front-only axis with a recorded diagnostic instead of + # failing the joint; only a gross direction error (wrong-link or a + # loose Tag that changes the observed motion axis) still fails here. + gross_axis_limit = getattr( + self, + "cross_view_roll_maximum_axis_difference_rad", + math.radians(15.0), + ) + # Do not apply a universal front/side line-distance gate here. The + # front roll link rides the splay screw, so its pure-revolute pose fit + # is a displaced pseudo-line whose offset varies by finger (about + # 15 mm for pinky and 37 mm for ring on the same fixed setup). It is + # not the physical line ultimately published. Retain the distance in + # diagnostics; validate the selected side physical line through its + # own radial quality, cycle RMS and final URDF geometry instead. + if axis_difference > gross_axis_limit: + raise ValueError( + "cross_view_roll_axis_gross_disagreement:" + f"{math.degrees(axis_difference):.6f}deg," + f"{1000.0 * line_distance:.6f}mm" + ) + if ( + axis_difference > self.zero_maximum_axis_cycle_difference_rad + or line_distance > self.axis_maximum_pose_line_rms_m + ): + selected_direction = primary + decision = "skip_fusion_use_primary" + zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) + observer_name = next( + ( + observer + for observer, parent in zero_profile.axis_parent_joint.items() + if parent == joint_name + ), + None, + ) + observer_records = ( + [] + if observer_name is None + else self.records_by_joint.get(observer_name, []) + ) + if observer_records: + model = UrdfKinematicModel(self.source_urdf_path) + parent_axis, _ = model.axis_line( + joint_name, zero_offsets={}, joint_angles={} + ) + observer_axis, _ = model.axis_line( + observer_name, zero_offsets={}, joint_angles={} + ) + expected_cone = math.acos( + abs( + float( + np.clip(parent_axis @ observer_axis, -1.0, 1.0) + ) + ) + ) + + def cone_residual( + candidate_axis: np.ndarray, + measured_observer_axis: np.ndarray, + ) -> float: + measured_cone = math.acos( + abs( + float( + np.clip( + candidate_axis @ measured_observer_axis, + -1.0, + 1.0, + ) + ) + ) + ) + return abs(measured_cone - expected_cone) + + # A zero rotates the downstream axis around this parent and + # cannot change their mutual cone angle. Select the camera + # once for the complete repeated-sweep group. Per-cycle + # selection is invalid because a residual close to the gate + # can alternate views and turn their fixed systematic bias + # into a false cycle-axis spread. + group_cycles = tuple(range(int(self.repetitions))) + required_names = ( + joint_name, + validation_name, + observer_name, + ) + group_is_complete = all( + any( + int(record.get("cycle", -1)) == group_cycle + for record in self.records_by_joint.get(name, ()) + ) + for name in required_names + for group_cycle in group_cycles + ) + group_source = "primary" + if group_is_complete: + group_signature = tuple( + ( + name, + group_cycle, + len(cycle_records), + max( + ( + int(record.get("attempt", 0)) + for record in cycle_records + ), + default=0, + ), + max( + ( + int(record.get("image_stamp_ns", 0)) + for record in cycle_records + ), + default=0, + ), + ) + for name in required_names + for group_cycle in group_cycles + for cycle_records in ( + [ + record + for record in self.records_by_joint.get( + name, () + ) + if int(record.get("cycle", -1)) + == group_cycle + ], + ) + ) + cache_key = ( + joint_name, + validation_name, + observer_name, + group_signature, + ) + source_cache = getattr( + self, "_cross_view_roll_group_source_cache", {} + ) + cached_source = source_cache.get(cache_key) + if cached_source is not None: + group_source = str(cached_source) + else: + primary_residuals: list[float] = [] + secondary_residuals: list[float] = [] + fit_raw = ( + G20ThreeCameraCalibrationNode + ._fit_axis_measurement_raw + ) + try: + for group_cycle in group_cycles: + group_primary = fit_raw( + self, joint_name, group_cycle + ) + group_secondary = fit_raw( + self, validation_name, group_cycle + ) + group_observer = fit_raw( + self, observer_name, group_cycle + ) + observer_axis_common = np.asarray( + group_observer.axis_common_xyz, + dtype=float, + ) + primary_residuals.append( + cone_residual( + np.asarray( + group_primary.axis_common_xyz, + dtype=float, + ), + observer_axis_common, + ) + ) + secondary_residuals.append( + cone_residual( + np.asarray( + group_secondary.axis_common_xyz, + dtype=float, + ), + observer_axis_common, + ) + ) + except (KeyError, ValueError): + primary_residuals = [] + secondary_residuals = [] + group_source = ( + select_cross_view_roll_direction_source( + primary_residuals, + secondary_residuals, + self.zero_maximum_axis_cone_mismatch_rad, + ) + ) + source_cache[cache_key] = group_source + if len(source_cache) > 32: + source_cache.pop(next(iter(source_cache))) + self._cross_view_roll_group_source_cache = source_cache + if group_source == "secondary": + selected_direction = replace( + primary, + axis_common_xyz=tuple( + float(value) for value in secondary_axis + ), + axis_direction_source=( + "cross_view_cone_selected_secondary" + ), + ) + decision = ( + "use_secondary_zero_invariant_cone_consensus" + ) + G20ThreeCameraCalibrationNode._record_cross_view_roll_axis_diagnostic( + self, + joint_name, + cycle, + primary=primary, + secondary=secondary, + axis_difference_rad=axis_difference, + line_distance_m=line_distance, + decision=decision, + ) + # The front roll link rides the splay screw: its orientation + # tracks the rotation faithfully, but its centre trajectory + # carries the screw translation, displacing the fitted axis + # line by ~21 mm from the finger's physical MCP axis. The side + # PIP-link circle recovers that physical line. Keep the one + # direction selected consistently for all cycles and take the + # line position from the side view. + return replace( + selected_direction, + point_common_xyz_m=tuple( + float(value) + for value in secondary.point_common_xyz_m + ), + axis_point_source="side_circle_cross_view", + pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, + pose_axis_line_source_joints=(validation_name,), + ) + primary_variance = max( + primary.radial_rms_m ** 2 + primary.pose_axis_line_rms_m ** 2, + 1.0e-12, + ) + secondary_variance = max( + secondary.radial_rms_m ** 2 + secondary.pose_axis_line_rms_m ** 2, + 1.0e-12, + ) + primary_weight = 1.0 / primary_variance + secondary_weight = 1.0 / secondary_variance + total_weight = primary_weight + secondary_weight + fused_axis = ( + primary_weight * primary_axis + + secondary_weight * secondary_axis + ) + fused_axis /= np.linalg.norm(fused_axis) + # Axis-line points have a free coordinate along the axis. Fuse only + # the observable perpendicular displacement and retain the primary + # point's along-axis gauge. + delta = secondary_point - primary_point + perpendicular_delta = delta - fused_axis * float(delta @ fused_axis) + fused_point = ( + primary_point + + secondary_weight / total_weight * perpendicular_delta + ) + return replace( + primary, + axis_common_xyz=tuple(float(value) for value in fused_axis), + point_common_xyz_m=tuple(float(value) for value in fused_point), + plane_rms_m=max(primary.plane_rms_m, secondary.plane_rms_m), + radial_rms_m=max(primary.radial_rms_m, secondary.radial_rms_m), + pose_axis_line_rms_m=max( + primary.pose_axis_line_rms_m, + secondary.pose_axis_line_rms_m, + line_distance, + ), + pose_axis_line_source_joints=(joint_name, validation_name), + axis_direction_source="cross_view_weighted_fusion", + ) + + def _refit_cross_view_axis_line_group( + self, + joint_name: str, + measurements: Sequence[JointAxisMeasurement], + ) -> list[JointAxisMeasurement]: + """Apply the repeated-cycle physical-radius constraint when eligible.""" + group = list(measurements) + sources = { + source + for measurement in group + if (source := cross_view_side_line_source(measurement)) is not None + } + if ( + len(group) != int(self.repetitions) + or len(sources) != 1 + or not all( + cross_view_side_line_source(measurement) is not None + for measurement in group + ) + or any( + axis_line_uses_depth_free_interpretation_plane(measurement) + for measurement in group + ) + ): + return group + source = next(iter(sources)) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + source_spec = profile.record_specs[source] + source_records = G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, + source, + self.records_by_joint[source], + ) + return list( + refit_axis_line_group_with_shared_radius( + group, + source_records, + zero_command_u8=int( + self.baseline_command[source_spec.motor_index] + ), + canonical_zero_direction=canonical_zero_direction( + profile, source + ), + ) + ) + + def _record_cross_view_roll_axis_diagnostic( + self, + joint_name: str, + cycle: int, + *, + primary: Any, + secondary: Any, + axis_difference_rad: float, + line_distance_m: float, + decision: str = "skip_fusion_use_primary", + ) -> None: + """Log the deterministic cross-view direction decision.""" + record = { + "kind": "cross_view_roll_axis_diagnostic", + "joint": joint_name, + "cycle": int(cycle), + "decision": str(decision), + "axis_difference_deg": round( + math.degrees(axis_difference_rad), 6 + ), + "line_distance_mm": round(1000.0 * line_distance_m, 6), + "primary_axis_common_xyz": [ + round(float(value), 9) for value in primary.axis_common_xyz + ], + "secondary_axis_common_xyz": [ + round(float(value), 9) for value in secondary.axis_common_xyz + ], + "primary_axis_direction_source": primary.axis_direction_source, + } + try: + append_jsonl(self.raw_path, record) + except Exception as error: + logger_factory = getattr(self, "get_logger", None) + if callable(logger_factory): + logger_factory().warning( + f"failed to record cross-view roll diagnostic: {error}" + ) + logger_factory = getattr(self, "get_logger", None) + if callable(logger_factory): + selected_text = ( + "using the side axis selected by the zero-invariant cone" + if decision.startswith( + "use_secondary_zero_invariant_cone" + ) + else "keeping the front-only axis" + ) + logger_factory().warning( + f"{joint_name} cycle {cycle}: front/side roll axes disagree " + f"by {math.degrees(axis_difference_rad):.2f} deg; skipping " + f"fusion and {selected_text}" + ) + + def _provisional_fit_failures( + self, spec: SweepSpec, *, include_view_validity: bool = True + ) -> list[dict[str, Any]]: + """Check full-pose curve and 3-D axis after one six-way task.""" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) + failures: list[dict[str, Any]] = [] + if include_view_validity and hasattr(self, "views"): + for view in _sweep_views(profile, spec): + runtime = self.views[view] + view_joints = _sweep_joints_for_view(profile, spec, view) + validation_only_view = bool(view_joints) and all( + profile.record_specs[name].zero_kind + == "axis_cross_view_validation" + for name in view_joints + ) + retry_scope = set( + getattr(self, "retry_joint_names", set()) + if getattr(self, "retry_sweep_spec", None) == spec + else () + ) + if retry_scope and not retry_scope.intersection(view_joints): + # This view's accepted records were intentionally kept; + # only the independently failed view is being measured. + continue + valid_rate = float(runtime.valid_rate) + # Prefer the task-scoped counters: after the last sweep the + # required-role set flips back to the full preflight set and + # the rolling window restarts on idle frames, which is not + # what a per-task capture-quality gate should measure. + total = int(getattr(runtime, "task_total_frames", 0)) + if total > 0: + valid_rate = ( + float(getattr(runtime, "task_valid_frames", 0)) + / total + ) + imported_without_capture = ( + total == 0 + and spec.key + in set(getattr(self, "resumed_task_keys", ()) or ()) + ) + dense_trajectory_coverage = bool( + profile.layout_id == G20_RIGHT_19_LAYOUT + and _records_have_formal_sweep_coverage( + self.records_by_joint, + view_joints, + repetitions=self.repetitions, + minimum_bins=int( + getattr(self, "minimum_sweep_bins", 32) + ), + maximum_bin_gap=int( + getattr(self, "maximum_bin_gap", 16) + ), + ) + ) + if ( + not imported_without_capture + and valid_rate < self.minimum_detection_rate + and not validation_only_view + and not dense_trajectory_coverage + ): + failures.append( + { + "joint": view_joints[0], + "view": view, + "metric": "tag_valid_rate_percent", + "actual": round(100.0 * valid_rate, 3), + "limit": round( + 100.0 * self.minimum_detection_rate, 3 + ), + "comparison": "minimum", + "task_valid_frames": int( + getattr(runtime, "task_valid_frames", 0) + ), + "task_total_frames": total, + } + ) + for joint_name in spec.joints: + records = self.records_by_joint[joint_name] + sync_p95 = float( + np.percentile( + [ + float(record.get("state_image_sync_error_ms", 0.0)) + for record in records + ], + 95.0, + ) + ) + sync_limit_ms = ( + getattr(self, "maximum_state_image_skew_ns", 50_000_000) + / 1_000_000.0 + ) + if sync_p95 > sync_limit_ms: + failures.append( + { + "joint": joint_name, + "metric": "state_image_sync_p95_ms", + "actual": round(sync_p95, 6), + "limit": round(sync_limit_ms, 6), + "comparison": "maximum", + } + ) + try: + fit = self._fit_joint_records( + joint_name, + records, + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "trajectory_fit", + "reason": str(error), + } + ) + continue + if profile.steady_command_checkpoints: + command_store = getattr( + self, "command_records_by_joint", None + ) + if command_store is not None: + command_records = [ + dict(record) + for record in command_store.get(joint_name, ()) + if int(record.get("cycle", 0)) == 0 + ] + if len(command_records) < 18: + failures.append( + { + "joint": joint_name, + "metric": "steady_command_checkpoints", + "actual": len(command_records), + "limit": 18, + "comparison": "minimum", + } + ) + else: + try: + self._fit_joint_records( + joint_name, command_records + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "command_trajectory_fit", + "reason": str(error), + } + ) + else: + feedback_fit = self._fit_joint_records( + joint_name, + _steady_records_in_feedback_domain( + command_records + ), + ) + command_gap = float( + feedback_fit.maximum_hysteresis_rad + ) + command_gap_limit = float( + self.command_maximum_direction_gap_rad + ) + if command_gap > command_gap_limit: + failures.append( + { + "joint": joint_name, + "metric": ( + "feedback_direction_gap_deg" + ), + "actual": round( + math.degrees(command_gap), 6 + ), + "limit": round( + math.degrees(command_gap_limit), 6 + ), + "comparison": "maximum", + } + ) + try: + baseline_hysteresis = baseline_hysteresis_by_cycle_rad( + G20ThreeCameraCalibrationNode._baseline_hysteresis_records( + self, joint_name, records + ), + zero_command_u8=int( + self.baseline_command[ + profile.record_specs[joint_name].motor_index + ] + ), + axis_xyz=( + G20ThreeCameraCalibrationNode + ._hysteresis_axis_for_fit( + self, joint_name, records, fit + ) + ), + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "baseline_hysteresis", + "reason": str(error), + } + ) + continue + maximum_baseline_hysteresis = max(baseline_hysteresis) + canonical_direction = canonical_zero_direction( + profile, joint_name + ) + if canonical_direction is not None: + validation_only_pose = ( + profile.record_specs[joint_name].zero_kind + == "axis_cross_view_validation" + ) + branch_gap_range = ( + maximum_baseline_hysteresis + - min(baseline_hysteresis) + ) + branch_gap_limit = getattr( + self, + "directional_zero_maximum_branch_gap_rad", + math.radians(1.5), + ) + branch_gap_range_limit = getattr( + self, + "directional_zero_maximum_branch_gap_range_rad", + math.radians(0.3), + ) + if maximum_baseline_hysteresis > branch_gap_limit: + failures.append( + { + "joint": joint_name, + "metric": "baseline_directional_gap_deg", + "actual": round( + math.degrees( + maximum_baseline_hysteresis + ), + 6, + ), + "limit": round( + math.degrees(branch_gap_limit), 6 + ), + "comparison": "maximum", + "cycle_values_deg": [ + round(math.degrees(value), 6) + for value in baseline_hysteresis + ], + } + ) + if ( + branch_gap_range > branch_gap_range_limit + and not validation_only_pose + ): + failures.append( + { + "joint": joint_name, + "metric": ( + "baseline_directional_gap_range_deg" + ), + "actual": round( + math.degrees(branch_gap_range), 6 + ), + "limit": round( + math.degrees(branch_gap_range_limit), 6 + ), + "comparison": "maximum", + "cycle_values_deg": [ + round(math.degrees(value), 6) + for value in baseline_hysteresis + ], + } + ) + elif ( + branch_gap_range > branch_gap_range_limit + and validation_only_pose + ): + # This side-view alias is never published as a joint + # angle. Its orientation is reconstructed from a + # near-grazing planar Tag, so IPPE pose drift can move + # the apparent branch gap between rounds even when + # the image trajectory and the authoritative front + # axis are repeatable. Keep the production threshold + # as an audit reference, but do not rescan identical + # motion based on a non-published pose component. + append_jsonl( + self.raw_path, + { + "kind": "validation_only_quality_diagnostic", + "joint": joint_name, + "metric": ( + "baseline_directional_gap_range_deg" + ), + "actual": round( + math.degrees(branch_gap_range), 6 + ), + "reference_limit": round( + math.degrees(branch_gap_range_limit), 6 + ), + "cycle_values_deg": [ + round(math.degrees(value), 6) + for value in baseline_hysteresis + ], + "decision": "diagnostic_only", + }, + ) + elif ( + maximum_baseline_hysteresis + > self.baseline_maximum_hysteresis_rad + ): + failures.append( + { + "joint": joint_name, + "metric": "baseline_hysteresis_deg", + "actual": round( + math.degrees(maximum_baseline_hysteresis), 6 + ), + "limit": round( + math.degrees( + self.baseline_maximum_hysteresis_rad + ), + 6, + ), + "comparison": "maximum", + "cycle_values_deg": [ + round(math.degrees(value), 6) + for value in baseline_hysteresis + ], + } + ) + if fit.circle.get("space") == "image_2d": + checks = ( + ( + "image_radial_rms_px", + float(fit.quality["radial_rms_px"]), + self.image_trajectory_maximum_radial_rms_px, + "maximum", + ), + ( + "image_radial_p95_px", + float(fit.quality["radial_p95_px"]), + self.image_trajectory_maximum_radial_p95_px, + "maximum", + ), + ( + "image_radius_px", + float(fit.quality["radius_px"]), + self.image_trajectory_minimum_radius_px, + "minimum", + ), + ( + "arc_deg", + math.degrees(float(fit.quality["arc_rad"])), + math.degrees(self.trajectory_minimum_arc_rad), + "minimum", + ), + ) + else: + checks = ( + ( + "rotation_orthogonal_rms_deg", + math.degrees( + float( + fit.quality["rotation_orthogonal_rms_rad"] + ) + ), + math.degrees( + self.active_maximum_rotation_orthogonal_rms_rad + if profile.record_specs[joint_name].active + else self.passive_maximum_rotation_orthogonal_rms_rad + ), + "maximum", + ), + ( + "arc_deg", + math.degrees(float(fit.quality["arc_rad"])), + math.degrees(self.trajectory_minimum_arc_rad), + "minimum", + ), + ) + joint_spec = profile.record_specs[joint_name] + monotonic_limit = ( + self.maximum_monotonic_correction_rad + if joint_spec.active + else self.passive_maximum_monotonic_correction_rad + ) + hysteresis_limit = ( + self.maximum_hysteresis_rad + if joint_spec.active + else self.passive_maximum_hysteresis_rad + ) + checks = checks + ( + ( + "monotonic_correction_deg", + math.degrees(fit.maximum_monotonic_correction_rad), + math.degrees(monotonic_limit), + "maximum", + ), + ) + if not profile.directional_zero: + checks = checks + ( + ( + "hysteresis_deg", + math.degrees(fit.maximum_hysteresis_rad), + math.degrees(hysteresis_limit), + "maximum", + ), + ) + for metric, actual, limit, comparison in checks: + failed = ( + actual > limit + if comparison == "maximum" + else actual < limit + ) + if failed: + failures.append( + { + "joint": joint_name, + "metric": metric, + "actual": round(float(actual), 6), + "limit": round(float(limit), 6), + "comparison": comparison, + } + ) + + cycle_travels: list[float] = [] + cycle_axes: list[np.ndarray] = [] + cycle_axis_sources: list[str] = [] + cycle_axis_measurements: list[JointAxisMeasurement] = [] + for cycle in range(self.repetitions): + cycle_records = [ + record + for record in self.records_by_joint[joint_name] + if int(record["cycle"]) == cycle + ] + try: + cycle_fit = self._fit_joint_records( + joint_name, cycle_records + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "cycle_fit", + "cycle": cycle + 1, + "reason": str(error), + } + ) + continue + cycle_travels.append( + abs( + float(cycle_fit.angle_rad[0]) + - float(cycle_fit.angle_rad[255]) + ) + ) + try: + axis = self._fit_axis_measurement(joint_name, cycle) + except Exception as error: + reason = str(error) + failure: dict[str, Any] = { + "joint": joint_name, + "metric": "axis_fit", + "cycle": cycle + 1, + "reason": reason, + } + if reason.startswith( + "cross_view_roll_axis_gross_disagreement:" + ): + validation_name = ( + profile.axis_validation_sources or {} + ).get(joint_name) + if validation_name in spec.joints: + # A gross direction disagreement does not identify + # which independently observed view is wrong. A + # retry must therefore reacquire both sources. + failure["quality_source_joints"] = [ + joint_name, + validation_name, + ] + failures.append(failure) + continue + cycle_axes.append(np.asarray(axis.axis_common_xyz, dtype=float)) + cycle_axis_sources.append(axis.axis_direction_source) + cycle_axis_measurements.append(axis) + plane_limit = ( + self.axis_maximum_plane_rms_m + if joint_spec.active + else self.passive_axis_maximum_plane_rms_m + ) + axis_checks = [ + ( + "axis_radial_rms_mm", + 1000.0 * axis.radial_rms_m, + 1000.0 * self.axis_maximum_radial_rms_m, + ), + ] + if ( + joint_spec.zero_kind != "axis_cross_view_validation" + and cross_view_side_line_source(axis) is None + ): + # A cross-view fallback deliberately takes its direction + # from the front and its line position from the side. The + # side Tag's monocular orientation is not the published + # direction, so its per-frame ideal-revolute residual is + # not an accuracy measure for the combined line. Those + # lines are checked for independent-cycle position + # repeatability below, at this same threshold. + axis_checks.append( + ( + "axis_pose_line_rms_mm", + 1000.0 * axis.pose_axis_line_rms_m, + 1000.0 * self.axis_maximum_pose_line_rms_m, + ) + ) + circle_is_constrained = circle_direction_is_constrained( + joint_name, zero_profile.constrained_circle_joints + ) + if not circle_is_constrained: + axis_checks.extend( + [ + ( + "axis_plane_rms_mm", + 1000.0 * axis.plane_rms_m, + 1000.0 * plane_limit, + ), + ] + ) + if not circle_is_constrained: + axis_checks.append( + ( + "rotation_circle_axis_difference_deg", + math.degrees( + axis.rotation_circle_axis_difference_rad + ), + math.degrees( + self.axis_maximum_rotation_circle_difference_rad + ), + ) + ) + # With a trusted orientation-constrained axis, plane_rms is + # scatter *along* that infinite axis. It cannot change the + # axis line or joint zero, so only the perpendicular/radial + # error is an admissibility check for constrained circles. + for metric, actual, limit in axis_checks: + if actual > limit: + failure_joint = joint_name + quality_sources: tuple[str, ...] = () + if metric == "axis_pose_line_rms_mm": + quality_sources = tuple( + str(source) + for source in getattr( + axis, + "pose_axis_line_source_joints", + (), + ) + if str(source) in spec.joints + ) + if len(quality_sources) == 1: + failure_joint = quality_sources[0] + failure = { + "joint": failure_joint, + "metric": metric, + "cycle": cycle + 1, + "actual": round(float(actual), 6), + "limit": round(float(limit), 6), + "comparison": "maximum", + } + if quality_sources: + failure["quality_source_joints"] = list( + quality_sources + ) + if failure_joint != joint_name: + failure["model_joint"] = joint_name + failures.append(failure) + side_line_sources = { + source + for measurement in cycle_axis_measurements + if ( + source := cross_view_side_line_source(measurement) + ) is not None + } + if side_line_sources: + try: + cycle_axis_measurements = ( + G20ThreeCameraCalibrationNode + ._refit_cross_view_axis_line_group( + self, joint_name, cycle_axis_measurements + ) + ) + except Exception as error: + source = next(iter(side_line_sources)) + failures.append( + { + "joint": source, + "model_joint": joint_name, + "quality_source_joints": [source], + "metric": "axis_line_shared_radius_fit", + "reason": str(error), + } + ) + side_line_sources = { + source + for measurement in cycle_axis_measurements + if ( + source := cross_view_side_line_source(measurement) + ) is not None + } + if ( + len(cycle_axis_measurements) == self.repetitions + and len(side_line_sources) == 1 + and all( + cross_view_side_line_source(measurement) is not None + for measurement in cycle_axis_measurements + ) + and not any( + axis_line_uses_depth_free_interpretation_plane(measurement) + for measurement in cycle_axis_measurements + ) + ): + line_rms = axis_line_cycle_rms_m( + cycle_axis_measurements + ) + if line_rms > self.axis_maximum_pose_line_rms_m: + source = next(iter(side_line_sources)) + outliers = _isolated_axis_line_cycle_outliers( + cycle_axis_measurements, + self.axis_maximum_pose_line_rms_m, + ) + failure = { + "joint": source, + "model_joint": joint_name, + "quality_source_joints": [source], + "metric": "axis_line_cycle_rms_mm", + "actual": round(1000.0 * line_rms, 6), + "limit": round( + 1000.0 * self.axis_maximum_pose_line_rms_m, + 6, + ), + "maximum_pairwise_mm": round( + 1000.0 + * maximum_axis_line_cycle_spread_m( + cycle_axis_measurements + ), + 6, + ), + "comparison": "maximum", + } + if outliers: + outlier = next(iter(outliers)) + failure["cycle"] = outlier + 1 + failure["inlier_cycles"] = [ + index + 1 + for index in range(len(cycle_axis_measurements)) + if index != outlier + ] + failures.append(failure) + if len(cycle_travels) == self.repetitions: + travel_range = max(cycle_travels) - min(cycle_travels) + cycle_limit = ( + self.trajectory_maximum_cycle_travel_difference_rad + if joint_spec.active + else self.passive_maximum_cycle_travel_difference_rad + ) + if travel_range > cycle_limit: + failures.append( + { + "joint": joint_name, + "metric": "cycle_travel_range_deg", + "actual": round(math.degrees(travel_range), 6), + "limit": round(math.degrees(cycle_limit), 6), + "comparison": "maximum", + "cycle_travel_deg": [ + round(math.degrees(value), 6) + for value in cycle_travels + ], + } + ) + if ( + len(cycle_axes) == self.repetitions + and not all( + source == "upstream_constraint" + for source in cycle_axis_sources + ) + ): + maximum_difference = 0.0 + for left in cycle_axes: + for right in cycle_axes: + maximum_difference = max( + maximum_difference, + math.acos( + abs(float(np.clip(left @ right, -1.0, 1.0))) + ), + ) + if ( + maximum_difference + > self.zero_maximum_axis_cycle_difference_rad + and joint_spec.zero_kind + == "axis_cross_view_validation" + ): + # As above, the side alias' independently reconstructed + # pose axis is not the axis written to the URDF. The + # published front direction and the side translation + # circle retain their own strict cross-round gates. + append_jsonl( + self.raw_path, + { + "kind": "validation_only_quality_diagnostic", + "joint": joint_name, + "metric": "axis_cycle_difference_deg", + "actual": round( + math.degrees(maximum_difference), 6 + ), + "reference_limit": round( + math.degrees( + self.zero_maximum_axis_cycle_difference_rad + ), + 6, + ), + "decision": "diagnostic_only", + }, + ) + elif ( + maximum_difference + > self.zero_maximum_axis_cycle_difference_rad + ): + outliers = _isolated_axis_cycle_outliers( + cycle_axes, + self.zero_maximum_axis_cycle_difference_rad, + ) + failure = { + "joint": joint_name, + "metric": "axis_cycle_difference_deg", + "actual": round(math.degrees(maximum_difference), 6), + "limit": round( + math.degrees( + self.zero_maximum_axis_cycle_difference_rad + ), + 6, + ), + "comparison": "maximum", + } + if outliers: + outlier = next(iter(outliers)) + failure["cycle"] = outlier + 1 + failure["inlier_cycles"] = [ + index + 1 + for index in range(len(cycle_axes)) + if index != outlier + ] + failures.append(failure) + if profile.cross_view_roll_curve: + for primary_name, validation_name in ( + profile.axis_validation_sources or {} + ).items(): + if not {primary_name, validation_name}.issubset(spec.joints): + continue + try: + cycle_fits = [ + ( + self._fit_joint_records( + primary_name, + [ + record + for record in self.records_by_joint[ + primary_name + ] + if int(record.get("cycle", -1)) == cycle + ], + ), + self._fit_joint_records( + validation_name, + [ + record + for record in self.records_by_joint[ + validation_name + ] + if int(record.get("cycle", -1)) == cycle + ], + ), + ) + for cycle in range(self.repetitions) + ] + training_cycles = set(range(max(1, self.repetitions - 1))) + scopes = ( + ( + "training", + [ + record + for record in self.records_by_joint[primary_name] + if int(record.get("cycle", -1)) + in training_cycles + ], + [ + record + for record in self.records_by_joint[ + validation_name + ] + if int(record.get("cycle", -1)) + in training_cycles + ], + ), + ( + "all_cycles", + self.records_by_joint[primary_name], + self.records_by_joint[validation_name], + ), + ) + for scope, primary_records, validation_records in scopes: + failure = _cross_view_curve_failure( + primary_name, + validation_name, + self._fit_joint_records( + primary_name, primary_records + ), + self._fit_joint_records( + validation_name, validation_records + ), + scope=scope, + maximum_rms_difference_rad=( + self.cross_view_roll_maximum_shape_rms_rad + ), + maximum_branch_gap_difference_rad=( + self.cross_view_roll_maximum_branch_gap_difference_rad + ), + allow_projection_scale=True, + maximum_projection_scale_ratio=( + self.cross_view_roll_maximum_projection_scale_ratio + ), + cycle_fits=cycle_fits, + ) + if failure is not None: + append_jsonl( + self.raw_path, + { + "kind": ( + "validation_only_quality_diagnostic" + ), + **failure, + "decision": "diagnostic_only", + }, + ) + # One structured result contains all per-cycle + # evidence; avoid duplicate training/final errors. + break + except Exception as error: + append_jsonl( + self.raw_path, + { + "kind": "validation_only_quality_diagnostic", + "joint": primary_name, + "metric": "cross_view_roll_curve", + "reason": str(error), + "quality_source_joints": [validation_name], + "decision": "diagnostic_only", + }, + ) + return failures + + def _cross_view_roll_diagnostic_role( + self, spec: SweepSpec + ) -> str | None: + finger = str( + getattr(self, "cross_view_roll_diagnostic_finger", "") + ) + if not finger: + return None + if spec.key == f"{finger}_roll_multiview": + return "multiview" + return None + + @staticmethod + def _only_baseline_hysteresis_failures( + failures: Sequence[Mapping[str, Any]], + ) -> bool: + return bool(failures) and all( + str(item.get("metric", "")) + in { + "baseline_hysteresis_deg", + "baseline_directional_gap_deg", + "baseline_directional_gap_range_deg", + } + for item in failures + ) + + def _roll_baseline_hysteresis_degrees( + self, joint_name: str + ) -> list[float]: + records = self.records_by_joint[joint_name] + fit = self._fit_joint_records(joint_name, records) + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + spec = profile.record_specs[joint_name] + values = baseline_hysteresis_by_cycle_rad( + G20ThreeCameraCalibrationNode._baseline_hysteresis_records( + self, joint_name, records + ), + zero_command_u8=int( + self.baseline_command[spec.motor_index] + ), + axis_xyz=fit.circle["axis_xyz"], + ) + return [round(math.degrees(value), 6) for value in values] + + def _complete_cross_view_roll_diagnostic( + self, + side_spec: SweepSpec, + side_quality_failures: Sequence[Mapping[str, Any]] = (), + ) -> None: + finger = self.cross_view_roll_diagnostic_finger + front_joint = f"{finger}_mcp_roll" + side_joint = f"{front_joint}_side" + front_values = self._roll_baseline_hysteresis_degrees(front_joint) + side_values = self._roll_baseline_hysteresis_degrees(side_joint) + limit_deg = math.degrees(self.baseline_maximum_hysteresis_rad) + interpretation = _classify_cross_view_roll_hysteresis( + front_values, side_values, limit_deg=limit_deg + ) + result = { + "kind": "cross_view_roll_diagnostic", + "finger": finger, + "motor_index": side_spec.motor_index, + "front_joint": front_joint, + "side_joint": side_joint, + "front_by_cycle_deg": front_values, + "side_by_cycle_deg": side_values, + "front_maximum_deg": max(front_values), + "side_maximum_deg": max(side_values), + "formal_limit_deg": limit_deg, + "interpretation": interpretation, + "side_quality_failures": [ + dict(item) for item in side_quality_failures + ], + "publication_locked": True, + } + self.cross_view_roll_diagnostic_result = result + self.fit_failure = result + append_jsonl(self.raw_path, result) + self._pause("cross_view_roll_diagnostic_complete") + + def _pause_for_provisional_fit_failure( + self, + spec: SweepSpec, + failures: list[dict[str, Any]], + *, + allow_warning: bool = False, + ) -> bool: + diagnostic_role = ( + G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( + self, spec + ) + ) + if ( + diagnostic_role == "front" + and G20ThreeCameraCalibrationNode._only_baseline_hysteresis_failures( + failures + ) + ): + append_jsonl( + self.raw_path, + { + "kind": "cross_view_roll_front_failure_deferred", + "finger": self.cross_view_roll_diagnostic_finger, + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "failures": failures, + "publication_locked": True, + }, + ) + self.reason = "cross_view_roll_front_failure_deferred" + return False + + def is_warning(item: Mapping[str, Any]) -> bool: + if "actual" not in item or "limit" not in item: + return False + actual = float(item["actual"]) + limit = float(item["limit"]) + ratio = float(getattr(self, "provisional_warning_ratio", 1.0)) + if limit <= 0.0: + return False + if item.get("comparison") == "minimum": + return actual >= limit / ratio + return actual <= limit * ratio + + if allow_warning and failures and all(is_warning(item) for item in failures): + band_attempt = int( + self.sweep_attempts.get(_sweep_storage_key(spec), 1) + ) + retry_limit = int( + getattr(self, "automatic_fit_retry_limit", 0) + ) + # The final fit re-applies the unmodified hard thresholds to the + # same records. Letting even a tiny overrun continue would make + # the final fit recall this task after every later joint has been + # collected. Consume every remaining retry here; when the budget + # is exhausted, the common failure path below pauses in place. + append_jsonl( + self.raw_path, + { + "kind": ( + "provisional_fit_warning_rescan" + if band_attempt <= retry_limit + else "provisional_fit_warning_retry_exhausted" + ), + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "attempt": band_attempt, + "attempt_limit": retry_limit + 1, + "warning_ratio": float( + getattr( + self, "provisional_warning_ratio", 1.0 + ), + ), + "failures": failures, + }, + ) + + attempt = self.sweep_attempts.get(_sweep_storage_key(spec), 1) + self.retry_resume_index = self.sweep_index + self.retry_sweep_spec = spec + self.retry_joint_names = _fit_retry_joint_names( + _node_profile(self), spec, failures + ) + source_task_keys = ( + tuple(getattr(self, "retry_source_task_keys", ())) + if getattr(self, "retry_source_failure_task_key", None) == spec.key + else () + ) + source_key_set = set(source_task_keys) + source_specs = [ + candidate + for candidate in _node_profile(self).sweep_specs + if candidate.key in source_key_set + ] + if source_specs: + self.retry_joint_names = { + joint_name + for candidate in source_specs + for joint_name in candidate.joints + } + localized_cycles = { + int(item["cycle"]) - 1 + for item in failures + if item.get("cycle") is not None + } + cycle_override = set( + getattr(self, "retry_cycle_override", set()) + ) + if source_specs and cycle_override: + # These source tasks jointly define one PnP-relative zero. Mixing + # cycles acquired before and after a tracker/reference reset can + # make a clean branch change look like a physical zero change. + # Reacquire the whole source holdout set as one generation. + self.retry_cycles = cycle_override + elif failures and all( + item.get("cycle") is not None + or str(item.get("metric", "")).startswith("third_cycle_") + for item in failures + ): + if any( + str(item.get("metric", "")).startswith("third_cycle_") + for item in failures + ): + localized_cycles.add(self.repetitions - 1) + self.retry_cycles = localized_cycles + else: + self.retry_cycles = set(range(self.repetitions)) + history_by_task = getattr( + self, "fit_failure_history_by_task", None + ) + if history_by_task is None: + self.fit_failure_history_by_task = {} + history_by_task = self.fit_failure_history_by_task + task_history = history_by_task.setdefault(spec.key, []) + repeated_branch_clusters = bool( + task_history + and _fit_failure_repeats_branch_clusters( + task_history[-1], failures + ) + ) + task_history.append([dict(item) for item in failures]) + systematic = bool( + _fit_failure_is_systematic(failures, self.repetitions) + or repeated_branch_clusters + ) + fit_retry_limit = int( + getattr(self, "automatic_fit_retry_limit", 0) + ) + self.fit_failure = { + "kind": "fit_failure", + "view": spec.view, + "task_name": spec.key, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "joints_to_rescan": sorted(self.retry_joint_names), + **( + {"source_task_names": list(source_task_keys)} + if source_task_keys + else {} + ), + "attempt": attempt, + "fit_attempt": attempt, + "fit_attempt_limit": fit_retry_limit + 1, + "automatic_retry_count": max(0, attempt - 1), + "automatic_retry_limit": fit_retry_limit, + "cycles_to_rescan": [ + cycle + 1 for cycle in sorted(self.retry_cycles) + ], + "directions_to_rescan": ( + 0 + if systematic + else 2 + * len(self.retry_cycles) + * max(1, len(source_specs)) + ), + "recoverable_by_rescan": not systematic, + "repeated_branch_clusters": repeated_branch_clusters, + "failures": failures, + } + append_jsonl( + self.raw_path, + { + "kind": "fit_failure", + **self.fit_failure, + }, + ) + if systematic: + self._pause( + "joint_fit_repeated_branch_failure" + if repeated_branch_clusters + else "joint_fit_systematic_failure" + ) + return True + if attempt <= fit_retry_limit: + self.paused_reason = "joint_fit_check_failed" + self.reason = "automatic_retry_joint_fit_check_failed" + self._prepare_failed_sweep_retry() + self._begin_return_baseline("resume_sweep") + return True + self._pause("joint_fit_check_failed") + return True + + def _pause_for_zero_model_failure( + self, zero_result: ZeroSolveResult + ) -> None: + """Stop once for a model/zero failure; repeated motion cannot fix it.""" + line_errors_m = dict( + getattr(zero_result, "validation_line_error_by_joint_m", {}) + ) + axis_line_rms_m = float(getattr(zero_result, "axis_line_rms_m", 0.0)) + axis_line_limit_m = float( + getattr(self, "axis_maximum_pose_line_rms_m", 0.0) + ) + observability_rank = int(getattr(zero_result, "observability_rank", 0)) + observability_parameter_count = int( + getattr(zero_result, "observability_parameter_count", 0) + ) + observability_condition_number = float( + getattr(zero_result, "observability_condition_number", 0.0) + ) + failures: list[dict[str, Any]] = [] + for name, reason in zero_result.failure_reasons.items(): + failure: dict[str, Any] = { + "joint": name, + "metric": "zero_guard", + "reason": reason, + } + if reason == "zero_offset_exceeds_configured_limit": + configured_limit = self.zero_joint_maximum_offsets_rad.get( + name, + ( + self.zero_finger_maximum_offset_rad + if name.startswith( + ("index_", "middle_", "ring_", "pinky_") + ) + else self.zero_maximum_offset_rad + ), + ) + failure["actual_deg"] = round( + math.degrees(zero_result.direct_offsets_rad[name]), 6 + ) + failure["limit_deg"] = round( + math.degrees(configured_limit), 6 + ) + failures.append(failure) + failures.extend( + { + "joint": observer, + "metric": "third_cycle_axis_holdout_deg", + "actual": round(math.degrees(error), 6), + "limit": math.degrees(self.maximum_validation_p95_rad), + "comparison": "maximum", + } + for observer, error in ( + zero_result.validation_error_by_joint_rad.items() + ) + if error > self.maximum_validation_p95_rad + ) + failures.extend( + { + "joint": observer, + "metric": "validation_axis_line_residual_mm", + "actual": round(1000.0 * float(error), 6), + "limit": round( + 1000.0 * axis_line_limit_m, 6 + ), + "comparison": "maximum", + } + for observer, error in ( + line_errors_m.items() + ) + if error > axis_line_limit_m + ) + direct_joint = next( + iter(zero_result.failure_reasons), + next( + iter(zero_result.validation_error_by_joint_rad), + "thumb_cmc_pitch", + ), + ) + spec = next( + ( + item + for item in self.profile.sweep_specs + if direct_joint in item.joints + ), + self.profile.sweep_specs[0], + ) + self.retry_sweep_spec = None + self.retry_resume_index = None + self.retry_cycles = set() + self.retry_joint_names = set() + self.fit_failure = { + "kind": "zero_model_failure", + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "directions_to_rescan": 0, + "recoverable_by_rescan": False, + "failures": failures, + "zero_geometry": { + "axis_line_rms_mm": round( + 1000.0 * axis_line_rms_m, 6 + ), + "axis_line_rms_limit_mm": round( + 1000.0 * axis_line_limit_m, 6 + ), + "validation_line_error_by_joint_mm": { + name: round(1000.0 * float(value), 6) + for name, value in ( + line_errors_m.items() + ) + }, + "observability_rank": observability_rank, + "observability_parameter_count": observability_parameter_count, + "observability_condition_number": observability_condition_number, + }, + } + append_jsonl( + self.raw_path, + {"kind": "zero_model_failure", **self.fit_failure}, + ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + atomic_write_json( + self.session_dir / "g20_right_19_failed_diagnostics.json", + { + "schema_version": 5, + "layout_id": G20_RIGHT_19_LAYOUT, + "passed": False, + "reason": "zero_model_validation_failed", + "failure_reasons": dict(zero_result.failure_reasons), + "axis_line_rms_m": axis_line_rms_m, + "axis_line_rms_limit_m": axis_line_limit_m, + "validation_line_error_by_joint_m": dict( + line_errors_m + ), + "validation_error_by_joint_rad": dict( + zero_result.validation_error_by_joint_rad + ), + "direct_offsets_rad": dict(zero_result.direct_offsets_rad), + "offset_confidence_95_half_width_rad": dict( + zero_result.offset_confidence_half_width_rad + ), + "observability": { + "rank": observability_rank, + "parameter_count": observability_parameter_count, + "condition_number": observability_condition_number, + }, + "raw_samples": str(self.raw_path), + "source_urdf": str(self.source_urdf_path), + "camera_extrinsics": str( + self.camera_extrinsics_file + ), + "urdf_published": False, + }, + ) + self._pause("zero_model_validation_failed") + + def _retry_active_sweep_or_pause(self, reason: str) -> None: + """Retry a recoverable acquisition failure without weakening gates.""" + if self.active_sweep is None: + self._pause(reason) + return + item = self.active_sweep + key = ( + _sweep_storage_key(item.spec), item.cycle, item.direction + ) + retries = self.sweep_retry_counts.get(key, 0) + retry_limit = getattr(self, "automatic_sweep_retry_limit", 0) + if retries >= retry_limit: + self._pause(reason) + return + retries += 1 + self.sweep_retry_counts[key] = retries + observer = next( + ( + candidate + for candidate in _node_profile(self).palm_axis_observers + if candidate.task_name == item.spec.key + ), + None, + ) + retry_views = set(_sweep_views(_node_profile(self), item.spec)) + if observer is not None: + retry_views.add(observer.view) + for view in retry_views: + runtime = getattr(self, "views", {}).get(view) + if runtime is None: + continue + self._reset_view_trackers(runtime) + runtime.pnp_reset_count += 1 + speed_scales = getattr( + self, "retry_speed_scales", (0.8, 0.6, 0.5) + ) + endpoint_holds = getattr( + self, "retry_endpoint_hold_seconds", (0.75, 1.0, 1.25) + ) + # Write the invalidation before mutating the authoritative in-memory + # stores. Restart replays this event as a tombstone, so a crash can + # neither resurrect the rejected sweep nor merge it with its + # replacement acquisition. + append_jsonl( + self.raw_path, + { + "kind": "automatic_sweep_retry", + "task_name": item.spec.key, + "attempt": int( + getattr(self, "sweep_attempts", {}).get( + _sweep_storage_key(item.spec), 1 + ) + ), + "view": item.spec.view, + "motor_index": item.spec.motor_index, + "joints": list(item.spec.joints), + "cycle": item.cycle, + "direction": item.direction, + "retry": retries, + "retry_limit": retry_limit, + "reason": str(reason), + "speed_scale": float( + speed_scales[ + min(retries, len(speed_scales)) - 1 + ] + ), + "endpoint_hold_seconds": float( + endpoint_holds[ + min(retries, len(endpoint_holds)) - 1 + ] + ), + }, + ) + command_records = getattr(self, "command_records_by_joint", {}) + for joint_name in item.spec.joints: + if joint_name in command_records: + command_records[joint_name] = [ + record + for record in command_records[joint_name] + if not ( + str(record.get("task_name")) == item.spec.key + and int(record.get("cycle", -999)) == item.cycle + and str(record.get("direction")) == item.direction + ) + ] + if observer is not None: + records = self.palm_axis_records_by_source.get( + observer.source_name, [] + ) + records[:] = [ + record + for record in records + if not ( + int(record.get("cycle", -999)) == item.cycle + and str(record.get("direction", "")) == item.direction + ) + ] + getattr(self, "sweep_frames", []).clear() + getattr(self, "sweep_start_frames", []).clear() + self.reason = f"automatic_retry_{reason}" + self._begin_return_baseline("retry_sweep") + + def _record_dedicated_baseline_hold(self, item: SweepItem) -> None: + """Persist one settled baseline observation for one approach path.""" + if not G20ThreeCameraCalibrationNode._requires_mid_sweep_baseline_hold( + self, item + ): + return + frames = list(getattr(self, "sweep_baseline_frames", [])) + minimum_frames = int(getattr(self, "minimum_baseline_hold_frames", 10)) + motor = item.spec.motor_index + command = int(self.baseline_command[motor]) + attempt = self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) + record_joint_names = tuple(item.spec.joints) + if getattr(self, "active_sweep_is_fit_retry", False): + scoped = set(getattr(self, "retry_joint_names", set())) + if scoped: + record_joint_names = tuple( + name for name in item.spec.joints if name in scoped + ) + for joint_name in record_joint_names: + selected = _frames_for_joint(frames, joint_name) + if len(selected) < minimum_frames: + raise RuntimeError( + "dedicated baseline hold for " + f"{joint_name} has {len(selected)} valid frames; " + f"at least {minimum_frames} required" + ) + state = np.median( + np.asarray( + [frame.state_u8 for frame in selected], dtype=float + ), + axis=0, + ) + quaternion = robust_rotation_summary( + [ + frame.joint_quaternions_xyzw[joint_name] + for frame in selected + ] + )[0] + relative_translation = np.median( + np.asarray( + [ + frame.joint_vectors_xyz_m[joint_name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + image_relative = np.median( + np.asarray( + [ + frame.image_vectors_xy_px[joint_name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + durable = canonical_sample_record({ + "kind": "baseline_hold_sample", + "attempt": attempt, + "task_name": item.spec.key, + "view": self.profile.record_specs[joint_name].view, + "joint": joint_name, + "motor_index": motor, + "cycle": item.cycle, + "direction": item.direction, + "requested_command_u8": command, + "feedback_u8": round(float(state[motor]), 6), + "image_stamp_ns": int( + np.median([frame.stamp_ns for frame in selected]) + ), + "relative_quaternion_xyzw": [ + float(value) for value in quaternion + ], + "relative_translation_xyz_m": [ + float(value) for value in relative_translation + ], + "image_relative_xy_px": [ + float(value) for value in image_relative + ], + "parent_pose_common": _robust_pose_payload( + [ + frame.parent_poses_common[joint_name] + for frame in selected + ] + ), + "child_pose_common": _robust_pose_payload( + [ + frame.child_poses_common[joint_name] + for frame in selected + ] + ), + "state_u8": [float(value) for value in state], + "state_image_sync_error_ms": round( + float( + np.percentile( + [ + abs(frame.state_sync_error_ns) + for frame in selected + ], + 95.0, + ) + / 1_000_000.0 + ), + 6, + ), + "pnp_reprojection_error_px": round( + float( + np.percentile( + [ + frame.joint_reprojection_error_px[joint_name] + for frame in selected + ], + 95.0, + ) + ), + 6, + ), + "valid_frames": len(selected), + "hold_seconds": float(self.baseline_hold_seconds), + }) + record = fitting_sample_record(durable) + self.baseline_records_by_joint[joint_name].append(record) + if item.cycle == 0: + self.command_records_by_joint[joint_name].append(record) + append_jsonl(self.raw_path, durable) + + def _baseline_hysteresis_records( + self, + joint_name: str, + trajectory_records: Sequence[Mapping[str, Any]], + ) -> Sequence[Mapping[str, Any]]: + """Use dedicated settled records for non-endpoint baseline joints.""" + profile = getattr(self, "profile", LEFT_HAND_PROFILE) + spec = profile.record_specs[joint_name] + baseline = int(self.baseline_command[spec.motor_index]) + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and baseline not in {0, 255} + ): + return self.baseline_records_by_joint.get(joint_name, []) + return trajectory_records + + def _finish_active_sweep(self) -> None: + assert self.active_sweep is not None + item = self.active_sweep + motor = item.spec.motor_index + start_tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, item.spec, item.start_u8 + ) + ) + target_tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, item.spec, item.target_u8 + ) + ) + record_joint_names = tuple(item.spec.joints) + if getattr(self, "active_sweep_is_fit_retry", False): + scoped = set(getattr(self, "retry_joint_names", set())) + if scoped: + record_joint_names = tuple( + name for name in item.spec.joints if name in scoped + ) + + def observation_bins( + joint_name: str, + ) -> dict[int, list[FrameObservation]]: + bins: dict[int, list[FrameObservation]] = {} + frames = _frames_for_joint(self.sweep_frames, joint_name) + for frame in frames: + state = float(frame.state_u8[motor]) + if abs(state - item.start_u8) <= start_tolerance: + command = item.start_u8 + elif abs(state - item.target_u8) <= target_tolerance: + command = item.target_u8 + else: + command = int(np.clip(np.rint(state), 0, 255)) + bins.setdefault(command, []).append(frame) + return bins + + joint_bins: dict[str, dict[int, list[FrameObservation]]] = {} + for joint_name in record_joint_names: + bins = observation_bins(joint_name) + commands = sorted(bins) + if not commands or commands[0] != 0 or commands[-1] != 255: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "sweep_missing_endpoint_bin", item.spec, joint_name + ) + ) + return + joint_bins[joint_name] = bins + if item.precheck: + # This low-speed pass is a visibility/safety check at 0, 127 and + # 255, not a source for the command-angle curve. Its full-sweep + # detection rate is counted directly in the camera callback. A + # raw-frame rate below the nominal threshold is not itself a data + # defect when synchronised valid poses still cover the complete + # trajectory densely: detector flicker then changes only how many + # duplicate observations exist, not what motion is observable. + for joint_name, bins in joint_bins.items(): + commands = sorted(bins) + if min(abs(command - 127) for command in commands) > 2: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "task_precheck_missing_command_127", + item.spec, + joint_name, + ) + ) + return + detection_rates: dict[str, float] = {} + total_by_view = getattr( + self, "sweep_detection_total_by_view", {} + ) + valid_by_view = getattr( + self, "sweep_detection_valid_by_view", {} + ) + capture_views = _sweep_views(_node_profile(self), item.spec) + profile = _node_profile(self) + trajectory_coverage_by_view: dict[str, bool] = {} + for view in capture_views: + view_joint_bins = [ + joint_bins[name] + for name in _sweep_joints_for_view( + profile, item.spec, view + ) + if name in joint_bins + ] + trajectory_coverage_by_view[view] = bool(view_joint_bins) and all( + len(bins) >= self.minimum_sweep_bins + and max(np.diff(sorted(bins)), default=0) + <= self.maximum_bin_gap + for bins in view_joint_bins + ) + detection_rate_below_threshold_views: list[str] = [] + trajectory_coverage_override_views: list[str] = [] + for view in capture_views: + if view in total_by_view: + total = int(total_by_view.get(view, 0)) + valid = int(valid_by_view.get(view, 0)) + elif len(capture_views) == 1: + # Compatibility for legacy sessions and focused unit + # harnesses created before per-camera counters existed. + total = int( + getattr(self, "sweep_detection_total_frames", 0) + ) + valid = int( + getattr(self, "sweep_detection_valid_frames", 0) + ) + else: + total = 0 + valid = 0 + rate = 0.0 if total <= 0 else valid / total + detection_rates[view] = rate + if rate < self.minimum_detection_rate: + detection_rate_below_threshold_views.append(view) + if trajectory_coverage_by_view.get(view, False): + trajectory_coverage_override_views.append(view) + else: + self._retry_active_sweep_or_pause( + f"task_precheck_detection_rate_too_low:{view}" + ) + return + minimum_bin_count = min( + len(bins) for bins in joint_bins.values() + ) + precheck_maximum_bin_gap = max( + int(max(np.diff(sorted(bins)), default=0)) + for bins in joint_bins.values() + ) + minimum_valid_frames = min( + sum(len(frames) for frames in bins.values()) + for bins in joint_bins.values() + ) + G20ThreeCameraCalibrationNode._record_precheck_speed_metric( + self, + item, + bin_count=minimum_bin_count, + maximum_bin_gap=precheck_maximum_bin_gap, + valid_frames=minimum_valid_frames, + ) + append_jsonl( + self.raw_path, + { + "kind": "task_visibility_precheck", + "task_name": item.spec.key, + "view": item.spec.view, + "capture_views": list( + _sweep_views(_node_profile(self), item.spec) + ), + "motor_index": motor, + "direction": item.direction, + "checked_commands_u8": [0, 127, 255], + "bin_count": minimum_bin_count, + "maximum_bin_gap": precheck_maximum_bin_gap, + "detection_rate": round(min(detection_rates.values()), 6), + "detection_rate_by_view": { + view: round(rate, 6) + for view, rate in detection_rates.items() + }, + "minimum_detection_rate": round( + float(self.minimum_detection_rate), 6 + ), + "detection_rate_below_threshold_views": ( + detection_rate_below_threshold_views + ), + "trajectory_coverage_by_view": ( + trajectory_coverage_by_view + ), + "trajectory_coverage_override_views": ( + trajectory_coverage_override_views + ), + "passed": True, + }, + ) + if not getattr(self, "active_sweep_is_fit_retry", False): + self.sweep_index += 1 + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps( + self + ) + self.active_sweep = None + self.active_sweep_is_fit_retry = False + if self.sweep_index >= len(self.sweep_items): + self._begin_return_baseline("fit") + elif self.sweep_items[self.sweep_index].spec == item.spec: + following = self.sweep_items[self.sweep_index] + transition = _sweep_plan_transition( + self.profile, item, following + ) + if transition == "cycle_reset": + self._begin_return_baseline("next_cycle") + else: + G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( + self, + item, + following, + ) + self._start_next_sweep() + else: + self._begin_return_baseline( + G20ThreeCameraCalibrationNode._transition_after_completed_spec( + self, item.spec + ) + ) + return + + for joint_name, bins in joint_bins.items(): + commands = sorted(bins) + if len(commands) < self.minimum_sweep_bins: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "sweep_bins_too_few", item.spec, joint_name + ) + ) + return + if max(np.diff(commands), default=0) > self.maximum_bin_gap: + self._retry_active_sweep_or_pause( + _joint_failure_reason( + "sweep_bin_gap_too_large", item.spec, joint_name + ) + ) + return + + stored_joint_names = tuple(joint_bins) + + def synchronised_frame_payload( + frame: FrameObservation, + ) -> dict[str, Any]: + names = [ + name + for name in stored_joint_names + if name in frame.joint_quaternions_xyzw + ] + return { + "kind": "synchronised_frame", + "attempt": self.sweep_attempts.get( + _sweep_storage_key(item.spec), 1 + ), + "task_name": item.spec.key, + "view": frame.view, + "motor_index": motor, + "joints": names, + "cycle": item.cycle, + "direction": item.direction, + "requested_command_u8": int( + self.sweep_checkpoint_target_u8 + if self.sweep_checkpoint_target_u8 is not None + else item.target_u8 + ), + "feedback_u8": round(float(frame.state_u8[motor]), 6), + "image_stamp_ns": int(frame.stamp_ns), + "actual_state_u8": [ + float(value) for value in frame.state_u8 + ], + "state_image_sync_error_ms": round( + abs(frame.state_sync_error_ns) / 1_000_000.0, 6 + ), + "relative_quaternion_xyzw": { + name: list(frame.joint_quaternions_xyzw[name]) + for name in names + }, + "parent_pose_common": { + name: frame.parent_poses_common[name] + for name in names + }, + "child_pose_common": { + name: frame.child_poses_common[name] + for name in names + }, + "pnp_reprojection_error_px": { + name: frame.joint_reprojection_error_px[name] + for name in names + }, + } + append_jsonl_many( + self.raw_path, + ( + synchronised_frame_payload(frame) + for frame in self.sweep_frames + if any( + name in frame.joint_quaternions_xyzw + for name in stored_joint_names + ) + ), + ) + + for joint_name in stored_joint_names: + bins = joint_bins[joint_name] + commands = sorted(bins) + for command in commands: + frames = bins[command] + vector = np.median( + np.asarray( + [frame.joint_vectors_xyz_m[joint_name] for frame in frames] + ), + axis=0, + ) + image_vector = np.median( + np.asarray( + [frame.image_vectors_xy_px[joint_name] for frame in frames] + ), + axis=0, + ) + relative_quaternion = robust_rotation_summary( + [ + frame.joint_quaternions_xyzw[joint_name] + for frame in frames + ] + )[0] + parent_pose = _robust_pose_payload( + [ + frame.parent_poses_common[joint_name] + for frame in frames + ] + ) + child_pose = _robust_pose_payload( + [ + frame.child_poses_common[joint_name] + for frame in frames + ] + ) + state = np.median( + np.asarray([frame.state_u8 for frame in frames], dtype=float), + axis=0, + ) + durable = canonical_sample_record({ + "kind": "sample", + "attempt": self.sweep_attempts.get( + _sweep_storage_key(item.spec), 1 + ), + "task_name": item.spec.key, + "view": self.profile.record_specs[joint_name].view, + "joint": joint_name, + "motor_index": motor, + "cycle": item.cycle, + "direction": item.direction, + "requested_command_u8": int( + self.sweep_checkpoint_target_u8 + if self.sweep_checkpoint_target_u8 is not None + else item.target_u8 + ), + "feedback_u8": int(command), + "image_stamp_ns": int( + np.median([frame.stamp_ns for frame in frames]) + ), + "relative_translation_xyz_m": [ + float(value) for value in vector + ], + "image_relative_xy_px": [ + float(value) for value in image_vector + ], + "relative_quaternion_xyzw": [ + float(value) for value in relative_quaternion + ], + "parent_pose_common": parent_pose, + "child_pose_common": child_pose, + "state_u8": [float(value) for value in state], + "state_image_sync_error_ms": round( + float( + np.percentile( + [ + abs(frame.state_sync_error_ns) + for frame in frames + ], + 95.0, + ) + / 1_000_000.0 + ), + 6, + ), + "pnp_reprojection_error_px": round( + float( + np.percentile( + [ + frame.joint_reprojection_error_px[joint_name] + for frame in frames + ], + 95.0, + ) + ), + 6, + ), + "valid_frames": len(frames), + }) + record = fitting_sample_record(durable) + self.records_by_joint[joint_name].append(record) + append_jsonl(self.raw_path, durable) + + G20ThreeCameraCalibrationNode._persist_palm_axis_samples(self, item) + G20ThreeCameraCalibrationNode._record_dedicated_baseline_hold( + self, item + ) + previous_spec = item.spec + was_fit_retry = bool( + getattr(self, "active_sweep_is_fit_retry", False) + ) + if not was_fit_retry: + self.sweep_index += 1 + G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) + self.active_sweep = None + self.active_sweep_is_fit_retry = False + if was_fit_retry: + if self.retry_sweep_items: + following = self.retry_sweep_items[0] + transition = _sweep_plan_transition( + self.profile, item, following + ) + if transition == "cycle_reset": + self._begin_return_baseline("next_cycle") + else: + G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( + self, + item, + following, + ) + self._start_next_sweep() + return + # All physical source items for a dependent retry are complete. + # Clear the routing before checking the final source task itself, + # so an ordinary source-task failure retries only that task. + self.retry_source_failure_task_key = None + self.retry_source_task_keys = () + self.retry_cycle_override.clear() + self.retry_preserve_pnp_continuity = False + failures = self._provisional_fit_failures(previous_spec) + diagnostic_role = ( + G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( + self, previous_spec + ) + ) + if diagnostic_role in {"side", "multiview"}: + G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( + self, previous_spec, failures + ) + return + if failures and self._pause_for_provisional_fit_failure( + previous_spec, failures, allow_warning=True + ): + return + self.retry_sweep_spec = None + self.retry_resume_index = None + self.retry_cycles = set() + self.retry_joint_names = set() + self.fit_failure = {} + if self.sweep_index >= len(self.sweep_items): + self._begin_return_baseline("fit") + else: + self._begin_return_baseline( + G20ThreeCameraCalibrationNode._transition_after_completed_spec( + self, previous_spec + ) + ) + return + spec_complete = bool( + self.sweep_index >= len(self.sweep_items) + or self.sweep_items[self.sweep_index].spec != previous_spec + ) + if spec_complete: + failures = self._provisional_fit_failures(previous_spec) + diagnostic_role = ( + G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( + self, previous_spec + ) + ) + if diagnostic_role in {"side", "multiview"}: + G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( + self, previous_spec, failures + ) + return + if failures and self._pause_for_provisional_fit_failure( + previous_spec, failures, allow_warning=True + ): + return + if self.retry_sweep_spec == previous_spec: + resume_index = self.retry_resume_index + self.retry_sweep_spec = None + self.retry_resume_index = None + self.retry_cycles = set() + self.retry_joint_names = set() + if resume_index is not None: + self.sweep_index = resume_index + self.fit_failure = {} + if self.sweep_index >= len(self.sweep_items): + self._begin_return_baseline("fit") + elif self.sweep_items[self.sweep_index].spec != previous_spec: + self._begin_return_baseline( + G20ThreeCameraCalibrationNode._transition_after_completed_spec( + self, previous_spec + ) + ) + elif _sweep_plan_transition( + self.profile, + item, + self.sweep_items[self.sweep_index], + ) == "cycle_reset": + # Keep the task's complete avoidance pose between its three + # repetitions. Only the active joint returns to its standard-side + # baseline (roll: 255->127); global unfolding is deferred until + # the task really changes. + self._begin_return_baseline("next_cycle") + else: + G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( + self, + item, + self.sweep_items[self.sweep_index], + ) + self._start_next_sweep() + + def _fit_all_curves(self) -> None: + G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) + self.state = STATE_FITTING + self.reason = "fitting_3d_axes_and_urdf_zero_offsets" + # Fitting is deliberately synchronous so no capture callback can + # mutate the observation set midway through a model fit. Announce + # the state before entering the CPU-bound section: the outer product + # runner then applies its fitting-specific watchdog instead of + # mistaking a blocked ROS timer for a motion communication failure. + fitting_started_at = time.monotonic() + self.last_status_publish = fitting_started_at + self._publish_status(fitting_started_at) + if int(getattr(self, "sample_schema_version", 0)) >= 1: + for store_name, store in ( + ("trajectory", self.records_by_joint), + ("baseline", self.baseline_records_by_joint), + ("steady", self.command_records_by_joint), + ("palm_axis", self.palm_axis_records_by_source), + ): + try: + validate_sample_records( + record + for records in store.values() + for record in records + ) + except SampleDataContractError as error: + raise SampleDataContractError( + f"{error};store={store_name}" + ) from error + # Provisional checks have a warning band so collection can continue, + # but the final fit always re-applies the unmodified hard thresholds. + fresh_task_keys = set( + getattr(self, "recalibration_task_keys", ()) + ) + for spec in self.profile.sweep_specs: + if fresh_task_keys and spec.key not in fresh_task_keys: + continue + failures = self._provisional_fit_failures(spec) + if failures: + self._pause_for_provisional_fit_failure( + spec, failures, allow_warning=False + ) + return + training_fits: dict[str, JointCurveFit] = {} + holdout_by_joint: dict[str, tuple[float, ...]] = {} + measured: dict[str, JointCurveFit] = {} + validation_cycle = self.repetitions - 1 + training_cycles = tuple(range(validation_cycle)) + training_cycle_set = set(training_cycles) + quality_joint_names = set( + recalibration_quality_joints( + self.profile, + getattr(self, "recalibration_scope", "full"), + ) + ) + standalone_thumb_calibration = bool( + getattr(self, "standalone_thumb_calibration", False) + ) + fit_joint_names = tuple( + name + for name in self.profile.measured_joints + if ( + not standalone_thumb_calibration + or name in quality_joint_names + ) + ) + for name in fit_joint_names: + training_records = [ + record + for record in self.records_by_joint[name] + if int(record["cycle"]) in training_cycle_set + ] + holdout_records = [ + record + for record in self.records_by_joint[name] + if int(record["cycle"]) == validation_cycle + ] + holdout_model_records = ( + G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, name, holdout_records + ) + ) + zero_command = int( + self.baseline_command[ + self.profile.joint_specs[name].motor_index + ] + ) + training_fits[name] = self._fit_joint_records( + name, training_records + ) + holdout_by_joint[name] = joint_curve_holdout_errors( + training_fits[name], + holdout_model_records, + zero_command_u8=zero_command, + ) + measured[name] = self._fit_joint_records( + name, self.records_by_joint[name] + ) + command_fits: dict[str, JointCurveFit] = dict(training_fits) + command_feedback_fits: dict[str, JointCurveFit] = dict( + training_fits + ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + command_fits = {} + command_feedback_fits = {} + for name in fit_joint_names: + if name in G20_REFERENCE_THUMB_CMC_JOINTS: + # a609d521's dense feedback-indexed relative-rotation + # curve is the validated transfer function for the three + # CMC axes. Do not replace it with nine planar PnP + # checkpoints; those points were the source of the later + # thumb regression and combination-pose false failures. + command_fits[name] = measured[name] + continue + command_records = [ + dict(record) + for record in self.command_records_by_joint[name] + if int(record["cycle"]) == 0 + ] + if len(command_records) < 18: + failed_spec = next( + spec + for spec in self.profile.sweep_specs + if name in spec.joints + ) + self._pause_for_provisional_fit_failure( + failed_spec, + [ + { + "joint": name, + "metric": "steady_command_checkpoints", + "actual": len(command_records), + "limit": 18, + "comparison": "minimum", + } + ], + ) + return + zero_command = int( + self.baseline_command[ + self.profile.joint_specs[name].motor_index + ] + ) + if name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS: + command_fits[name] = fit_joint_image_curve( + command_records, + maximum_radial_rms_px=( + self.image_trajectory_maximum_radial_rms_px + ), + maximum_radial_p95_px=( + self.image_trajectory_maximum_radial_p95_px + ), + minimum_radius_px=( + self.image_trajectory_minimum_radius_px + ), + minimum_arc_rad=self.trajectory_minimum_arc_rad, + ) + else: + command_fits[name] = fit_rotation_joint_curve( + command_records, + zero_command_u8=zero_command, + canonical_zero_direction=canonical_zero_direction( + self.profile, name + ), + ) + command_feedback_fits[name] = self._fit_joint_records( + name, + _steady_records_in_feedback_domain(command_records), + ) + self.cross_view_roll_metrics: dict[str, dict[str, float]] = {} + self.validation_only_fits: dict[str, JointCurveFit] = {} + for name, validation_name in ( + self.profile.axis_validation_sources or {} + ).items(): + if ( + name not in training_fits + or validation_name not in quality_joint_names + ): + continue + validation_training_records = [ + record + for record in self.records_by_joint[validation_name] + if int(record["cycle"]) in training_cycle_set + ] + validation_holdout_records = [ + record + for record in self.records_by_joint[validation_name] + if int(record["cycle"]) == validation_cycle + ] + validation_holdout_model_records = ( + G20ThreeCameraCalibrationNode._records_with_baseline_holds( + self, + validation_name, + validation_holdout_records, + ) + ) + validation_training_fit = self._fit_joint_records( + validation_name, validation_training_records + ) + validation_fit = self._fit_joint_records( + validation_name, self.records_by_joint[validation_name] + ) + # Any optional post-fit random validation must query the same + # frozen training model that passed the isolated holdout. + self.validation_only_fits[validation_name] = ( + validation_training_fit + ) + # The structured task-level check above records threshold + # overruns with per-cycle evidence. At publication time this + # secondary view remains diagnostic: the primary front curve and + # the side physical axis line retain their independent hard + # quality gates. Reuse the same policy as offline replay instead + # of reintroducing an unlocalized retry from a second code path. + training_metrics = cross_view_roll_diagnostic_metrics( + training_fits[name], validation_training_fit + ) + final_metrics = cross_view_roll_diagnostic_metrics( + measured[name], validation_fit + ) + validation_errors = joint_curve_holdout_errors( + validation_training_fit, + validation_holdout_model_records, + zero_command_u8=int( + self.baseline_command[ + self.profile.record_specs[validation_name].motor_index + ] + ), + ) + holdout_by_joint[validation_name] = validation_errors + self.cross_view_roll_metrics[name] = { + **{ + f"training_{key}": float(value) + for key, value in training_metrics.items() + }, + **{ + f"final_{key}": float(value) + for key, value in final_metrics.items() + }, + } + self.joint_dynamic_diagnostics: dict[str, dict[str, Any]] = {} + for name in fit_joint_names: + cycle_travel: list[float] = [] + for cycle in range(self.repetitions): + cycle_fit = self._fit_joint_records( + name, + [ + record + for record in self.records_by_joint[name] + if int(record["cycle"]) == cycle + ], + ) + cycle_travel.append( + abs( + float(cycle_fit.angle_rad[0]) + - float(cycle_fit.angle_rad[255]) + ) + ) + holdout = np.abs( + np.asarray(holdout_by_joint[name], dtype=float) + ) + baseline_hysteresis = baseline_hysteresis_by_cycle_rad( + G20ThreeCameraCalibrationNode._baseline_hysteresis_records( + self, name, self.records_by_joint[name] + ), + zero_command_u8=int( + self.baseline_command[ + self.profile.joint_specs[name].motor_index + ] + ), + axis_xyz=( + G20ThreeCameraCalibrationNode._hysteresis_axis_for_fit( + self, + name, + self.records_by_joint[name], + measured[name], + ) + ), + ) + self.joint_dynamic_diagnostics[name] = { + "cycle_travel_rad": [ + round(float(value), 8) for value in cycle_travel + ], + "cycle_travel_range_rad": round( + max(cycle_travel) - min(cycle_travel), 8 + ), + "baseline_hysteresis_by_cycle_rad": [ + round(float(value), 8) + for value in baseline_hysteresis + ], + "holdout_cycle": validation_cycle, + "holdout_cycle_mae_rad": round(float(np.mean(holdout)), 8), + "holdout_cycle_max_rad": round(float(np.max(holdout)), 8), + } + axes: list[JointAxisMeasurement] = [] + axis_profile = ( + get_right_19_thumb_zero_profile() + if ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and self.recalibration_scope == "thumb" + ) + else self.zero_profile + ) + for name in axis_profile.axis_joints: + group = [ + self._fit_axis_measurement(name, cycle) + for cycle in range(self.repetitions) + ] + axes.extend(self._refit_cross_view_axis_line_group(name, group)) + + try: + palm_orientation_measurements, palm_orientation_rejections = ( + fit_partial_palm_orientation_measurements( + sources=self.profile.palm_orientation_sources, + records_by_joint=self.palm_axis_records_by_source, + motor_by_source=self.profile.palm_axis_motor_by_source, + baseline_command_u8=self.baseline_command, + cycles=range(self.repetitions), + minimum_sources=int( + self.profile.minimum_palm_orientation_sources + ), + minimum_arc_rad=( + self.profile.palm_orientation_minimum_arc_rad + ), + maximum_rotation_orthogonal_rms_rad=( + self.active_maximum_rotation_orthogonal_rms_rad + ), + maximum_command_distance_u8=( + self.profile + .palm_orientation_maximum_command_distance_u8 + ), + ) + ) + except ValueError as error: + failure = str(error) + self.palm_orientation_rejections = {"coverage": failure} + self.fit_failure = { + "task": "palm_orientation_side_channel", + "failures": [ + { + "joint": "palm_orientation", + "metric": "palm_orientation_coverage", + "reason": failure, + } + ], + "directions_to_rescan": 0, + } + append_jsonl( + self.raw_path, + { + "kind": "palm_orientation_optional_source_diagnostic", + "rejections": {"coverage": failure}, + "decision": "reject_unobservable_palm_phase", + }, + ) + self._pause("palm_orientation_quality_failed") + return + if palm_orientation_rejections: + append_jsonl( + self.raw_path, + { + "kind": "palm_orientation_optional_source_diagnostic", + "rejections": dict(palm_orientation_rejections), + "decision": "use_qualified_redundant_sources", + }, + ) + + motor_by_joint = { + name: int(spec.motor_index) + for name, spec in self.profile.joint_specs.items() + } + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + if self.recalibration_scope == "full": + scoped_endpoint_joints = frozenset( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + ) + else: + want_thumb_endpoints = self.recalibration_scope == "thumb" + scoped_endpoint_joints = frozenset( + name + for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + if name.startswith("thumb_") == want_thumb_endpoints + ) + command_fits = anchor_right_19_mechanical_endpoint_curves( + command_fits, + { + name: ( + [ + record + for record in self.records_by_joint[name] + if int(record["cycle"]) in training_cycle_set + ] + if name == "thumb_cmc_roll" + else self.command_records_by_joint[name] + ) + for name in scoped_endpoint_joints + }, + maximum_direction_difference_rad=( + self.command_maximum_direction_gap_rad + ), + feedback_endpoint_joints=( + frozenset({"thumb_cmc_roll"}) + & scoped_endpoint_joints + ), + endpoint_joints=scoped_endpoint_joints, + ) + measured_endpoint_zero_offsets = ( + derive_right_19_mechanical_endpoint_offsets( + self.source_urdf_path, + command_fits, + maximum_offset_rad=( + self.mechanical_endpoint_maximum_offset_rad + ), + endpoint_joints=scoped_endpoint_joints, + ) + ) + frozen_offsets = dict( + getattr( + self, + "partial_scope_fixed_zero_offsets_rad", + {}, + ) + ) + # Endpoint-derived zeros also move URDF limits or mimic offsets. + # A partial run must preserve those coupled fields together with + # the certified non-target origin, not merely freeze the solver's + # scalar offset after endpoint derivation. + measured_endpoint_zero_offsets.update( + { + name: value + for name, value in frozen_offsets.items() + if name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + } + ) + endpoint_zero_offsets = { + name: value + for name, value in measured_endpoint_zero_offsets.items() + if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + } + post_solve_endpoint_offsets = { + name: value + for name, value in measured_endpoint_zero_offsets.items() + if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS + } + else: + endpoint_zero_offsets = {} + post_solve_endpoint_offsets = {} + zero_solve_arguments = dict( + source_urdf=self.source_urdf_path, + palm_orientation_measurements=palm_orientation_measurements, + curves=training_fits, + motor_by_joint=motor_by_joint, + maximum_offset_rad=self.zero_maximum_offset_rad, + joint_maximum_offset_rad=self.zero_joint_maximum_offsets_rad, + maximum_validation_mae_rad=self.maximum_validation_mae_rad, + maximum_validation_p95_rad=self.maximum_validation_p95_rad, + maximum_validation_error_rad=( + self.maximum_validation_error_rad + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else None + ), + maximum_confidence_half_width_rad=( + self.zero_maximum_confidence_half_width_rad + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else None + ), + maximum_axis_cone_mismatch_rad=( + self.zero_maximum_axis_cone_mismatch_rad + ), + maximum_systematic_axis_cone_bias_rad=( + self.cross_view_roll_maximum_axis_difference_rad + ), + maximum_observability_condition_number=( + self.zero_maximum_observability_condition_number + ), + maximum_pose_axis_line_rms_m=( + self.axis_maximum_pose_line_rms_m + ), + finger_maximum_offset_rad=self.zero_finger_maximum_offset_rad, + maximum_cycle_difference_rad=( + self.zero_maximum_axis_cycle_difference_rad + ), + hand_type=self.hand_type, + tag_layout=self.profile.layout_id, + training_cycles=training_cycles, + validation_cycle=validation_cycle, + ) + frozen_offsets = dict( + getattr(self, "partial_scope_fixed_zero_offsets_rad", {}) + ) + independent_thumb_scope = bool( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + and self.recalibration_scope in {"full", "thumb"} + ) + if independent_thumb_scope: + thumb_profile = get_right_19_thumb_zero_profile() + thumb_names = set(thumb_profile.direct_zero_joints) + thumb_result = solve_urdf_zero_offsets( + **zero_solve_arguments, + measurements=[ + item for item in axes if item.joint in thumb_profile.axis_joints + ], + fixed_direct_zero_offsets_rad={ + name: value + for name, value in endpoint_zero_offsets.items() + if name in thumb_names + }, + static_output_zero_offsets_rad={ + name: value + for name, value in post_solve_endpoint_offsets.items() + if name in thumb_names + }, + zero_profile=thumb_profile, + ) + if self.recalibration_scope == "thumb": + if self.standalone_thumb_calibration: + holdout_zero_result = ( + expand_right_19_thumb_zero_result_with_cad_fingers( + thumb_result + ) + ) + else: + holdout_zero_result = merge_right_19_thumb_zero_result( + thumb_result=thumb_result, + preserved_offsets_rad=frozen_offsets, + ) + else: + # The full-hand solve still determines the 12 finger zeros and + # palm nuisance pose, but all four thumb values are immutable. + # Consequently no finger observation can pull a thumb zero. + companion_result = solve_urdf_zero_offsets( + **zero_solve_arguments, + measurements=axes, + fixed_direct_zero_offsets_rad={ + **self.zero_profile.fixed_direct_zero_offsets_rad, + **endpoint_zero_offsets, + **thumb_result.direct_offsets_rad, + }, + static_output_zero_offsets_rad={ + **self.zero_profile.static_output_zero_offsets_rad, + **post_solve_endpoint_offsets, + }, + ) + holdout_zero_result = merge_right_19_thumb_zero_result( + thumb_result=thumb_result, + companion_result=companion_result, + ) + else: + holdout_zero_result = solve_urdf_zero_offsets( + **zero_solve_arguments, + measurements=axes, + fixed_direct_zero_offsets_rad={ + **self.zero_profile.fixed_direct_zero_offsets_rad, + **endpoint_zero_offsets, + **frozen_offsets, + }, + static_output_zero_offsets_rad={ + **self.zero_profile.static_output_zero_offsets_rad, + **post_solve_endpoint_offsets, + **{ + name: value + for name, value in frozen_offsets.items() + if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS + }, + }, + ) + thumb_yaw_zero_failures = ( + _thumb_yaw_zero_repeatability_failures( + holdout_zero_result, + maximum_cycle_range_rad=( + self.thumb_yaw_maximum_zero_cycle_difference_rad + ), + maximum_confidence_half_width_rad=( + self.thumb_yaw_maximum_confidence_half_width_rad + ), + ) + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else [] + ) + append_jsonl( + self.raw_path, + { + "kind": "zero_holdout_diagnostics", + "hand_type": self.hand_type, + "passed": holdout_zero_result.passed, + "direct_offsets_rad": dict( + holdout_zero_result.direct_offsets_rad + ), + "partial_scope_fixed_zero_offsets_rad": dict( + getattr( + self, + "partial_scope_fixed_zero_offsets_rad", + {}, + ) + ), + "model_base_translation_xyz_m": list( + holdout_zero_result.base_translation_xyz_m + ), + "model_base_quaternion_xyzw": list( + holdout_zero_result.base_quaternion_xyzw + ), + "cycle_offsets_rad": { + name: list(values) + for name, values in ( + holdout_zero_result.cycle_offsets_rad.items() + ) + }, + "offset_uncertainty_rad": dict( + holdout_zero_result.offset_uncertainty_rad + ), + "validation_error_by_joint_rad": dict( + holdout_zero_result.validation_error_by_joint_rad + ), + "validation_original_error_by_joint_rad": dict( + holdout_zero_result.validation_original_error_by_joint_rad + ), + "validation_improvement_by_joint_rad": dict( + holdout_zero_result.validation_improvement_by_joint_rad + ), + "validation_improvement_confidence_lower_rad": dict( + holdout_zero_result + .validation_improvement_confidence_lower_rad + ), + "axis_line_rms_m": holdout_zero_result.axis_line_rms_m, + "validation_line_error_by_joint_m": dict( + holdout_zero_result.validation_line_error_by_joint_m + ), + "offset_confidence_95_half_width_rad": dict( + holdout_zero_result.offset_confidence_half_width_rad + ), + "observability_rank": holdout_zero_result.observability_rank, + "observability_parameter_count": ( + holdout_zero_result.observability_parameter_count + ), + "observability_condition_number": ( + holdout_zero_result.observability_condition_number + ), + "failure_reasons": dict( + holdout_zero_result.failure_reasons + ), + "thumb_yaw_repeatability_failures": ( + thumb_yaw_zero_failures + ), + "axis_cone_mismatch_by_joint_rad": dict( + holdout_zero_result.axis_cone_mismatch_by_joint_rad + ), + "axis_cone_bias_classification_by_joint": dict( + holdout_zero_result + .axis_cone_bias_classification_by_joint + ), + }, + ) + holdout_errors = np.abs( + np.asarray( + [ + value + for name, values in holdout_by_joint.items() + if name in quality_joint_names + for value in values + ], + dtype=float, + ) + ) + trajectory_holdout_passed = bool( + holdout_errors.size + and float(np.mean(holdout_errors)) + <= self.maximum_validation_mae_rad + and float(np.percentile(holdout_errors, 95.0)) + <= self.maximum_validation_p95_rad + and ( + self.profile.layout_id != G20_RIGHT_19_LAYOUT + or float(np.max(holdout_errors)) + <= self.maximum_validation_error_rad + ) + and ( + self.profile.layout_id != G20_RIGHT_19_LAYOUT + or all( + float(np.mean(np.abs(np.asarray(values, dtype=float)))) + <= self.maximum_validation_mae_rad + and float(np.max(np.abs(np.asarray(values, dtype=float)))) + <= self.maximum_validation_error_rad + for name, values in holdout_by_joint.items() + if name in quality_joint_names + ) + ) + ) + self.fit_quality_passed = all( + fit.maximum_monotonic_correction_rad + <= ( + self.maximum_monotonic_correction_rad + if self.profile.joint_specs[name].active + else self.passive_maximum_monotonic_correction_rad + ) + and ( + self.profile.layout_id == G20_RIGHT_19_LAYOUT + or fit.maximum_hysteresis_rad + <= ( + self.maximum_hysteresis_rad + if self.profile.joint_specs[name].active + else self.passive_maximum_hysteresis_rad + ) + ) + for name, fit in training_fits.items() + if name in quality_joint_names + ) and all( + fit.maximum_hysteresis_rad + <= self.command_maximum_direction_gap_rad + for name, fit in command_feedback_fits.items() + if name in quality_joint_names + ) + if not trajectory_holdout_passed: + trajectory_score = { + name: float(np.percentile(np.abs(values), 95.0)) + for name, values in holdout_by_joint.items() + if name in quality_joint_names + } + worst_joint = max(trajectory_score, key=trajectory_score.get) + failed_spec = next( + spec + for spec in self.profile.sweep_specs + if worst_joint in spec.joints + ) + failures: list[dict[str, Any]] = [ + { + "joint": name, + "metric": "third_cycle_trajectory_p95_deg", + "actual": round(math.degrees(error), 6), + "limit": math.degrees( + self.maximum_validation_p95_rad + ), + "comparison": "maximum", + } + for name, error in trajectory_score.items() + if error > self.maximum_validation_p95_rad + ] + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + for name, values in holdout_by_joint.items(): + if name not in quality_joint_names: + continue + absolute = np.abs(np.asarray(values, dtype=float)) + mean_error = float(np.mean(absolute)) + maximum_error = float(np.max(absolute)) + if mean_error > self.maximum_validation_mae_rad: + failures.append( + { + "joint": name, + "metric": "third_cycle_trajectory_mae_deg", + "actual": round( + math.degrees(mean_error), 6 + ), + "limit": math.degrees( + self.maximum_validation_mae_rad + ), + "comparison": "maximum", + } + ) + if maximum_error > self.maximum_validation_error_rad: + failures.append( + { + "joint": name, + "metric": "third_cycle_trajectory_max_deg", + "actual": round( + math.degrees(maximum_error), 6 + ), + "limit": math.degrees( + self.maximum_validation_error_rad + ), + "comparison": "maximum", + } + ) + self._pause_for_provisional_fit_failure(failed_spec, failures) + return + if thumb_yaw_zero_failures: + thumb_yaw_spec = next( + spec + for spec in self.profile.sweep_specs + if "thumb_cmc_yaw" in spec.joints + ) + yaw_pair = self.zero_profile.same_view_axis_pair_by_offset.get( + "thumb_cmc_yaw", () + ) + source_task_key_set = { + observer.task_name + for observer in self.profile.palm_axis_observers + if observer.model_joint in set(yaw_pair) + } + self.retry_source_failure_task_key = thumb_yaw_spec.key + self.retry_source_task_keys = tuple( + candidate.key + for candidate in self.profile.sweep_specs + if candidate.key in source_task_key_set + ) + # A localized scalar outlier is replaced in place while the live + # PnP tracker/reference remains continuous. Non-localized yaw + # failures still select all cycles in the generic retry policy, + # but no dependent retry is allowed to create a new yaw datum. + self.retry_cycle_override.clear() + self._pause_for_provisional_fit_failure( + thumb_yaw_spec, thumb_yaw_zero_failures + ) + return + if not holdout_zero_result.passed: + self._pause_for_zero_model_failure(holdout_zero_result) + return + # Publish the exact frozen training model that was evaluated on the + # isolated final cycle. Re-solving with the holdout would invalidate + # the acceptance result. + zero_result = holdout_zero_result + self.thumb_yaw_cross_session_diagnostic = {} + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + previous_yaw = _previous_passed_joint_zero_offset( + self.session_dir, + self.serial_number, + "thumb_cmc_yaw", + ) + current_yaw = zero_result.all_active_offsets_rad.get( + "thumb_cmc_yaw" + ) + if previous_yaw is not None and current_yaw is not None: + previous_session, previous_offset = previous_yaw + delta = float(current_yaw) - float(previous_offset) + self.thumb_yaw_cross_session_diagnostic = { + "kind": "thumb_yaw_cross_session_diagnostic", + "joint": "thumb_cmc_yaw", + "previous_session": str(previous_session), + "previous_offset_deg": round( + math.degrees(previous_offset), 6 + ), + "current_offset_deg": round( + math.degrees(float(current_yaw)), 6 + ), + "absolute_difference_deg": round( + abs(math.degrees(delta)), 6 + ), + "reference_limit_deg": round( + math.degrees( + self.thumb_yaw_cross_session_diagnostic_rad + ), + 6, + ), + "classification": ( + "review_position_or_tag_installation" + if abs(delta) + > self.thumb_yaw_cross_session_diagnostic_rad + else "within_reference_band" + ), + "decision": "diagnostic_only", + } + append_jsonl( + self.raw_path, + self.thumb_yaw_cross_session_diagnostic, + ) + append_jsonl( + self.raw_path, + { + "kind": "zero_final_diagnostics", + "hand_type": self.hand_type, + "direct_offsets_rad": dict(zero_result.direct_offsets_rad), + "partial_scope_fixed_zero_offsets_rad": dict( + getattr( + self, + "partial_scope_fixed_zero_offsets_rad", + {}, + ) + ), + "model_base_translation_xyz_m": list( + zero_result.base_translation_xyz_m + ), + "model_base_quaternion_xyzw": list( + zero_result.base_quaternion_xyzw + ), + "offset_uncertainty_rad": dict( + zero_result.offset_uncertainty_rad + ), + "offset_confidence_95_half_width_rad": dict( + zero_result.offset_confidence_half_width_rad + ), + "training_cycles": list(zero_result.training_cycles), + "validation_cycle": zero_result.validation_cycle, + "axis_line_rms_m": zero_result.axis_line_rms_m, + "validation_line_error_by_joint_m": dict( + zero_result.validation_line_error_by_joint_m + ), + "observability_rank": zero_result.observability_rank, + "observability_parameter_count": ( + zero_result.observability_parameter_count + ), + "observability_condition_number": ( + zero_result.observability_condition_number + ), + "offset_covariance_rad2": dict( + zero_result.offset_covariance_rad2 + ), + "axis_cone_mismatch_by_joint_rad": dict( + zero_result.axis_cone_mismatch_by_joint_rad + ), + "axis_cone_bias_classification_by_joint": dict( + zero_result.axis_cone_bias_classification_by_joint + ), + }, + ) + # Runtime/MuJoCo consumes requested commands, never feedback bins. + # Static geometry and holdout above intentionally continue to use the + # dense feedback-domain training fit. + if standalone_thumb_calibration: + self.measured_fits = dict(command_fits) + else: + self.measured_fits = clamp_runtime_fits_to_urdf_limits( + self.source_urdf_path, + derive_mimic_passive_fits( + self.source_urdf_path, + command_fits, + profile=self.profile, + ), + endpoint_anchored_offsets_rad=endpoint_zero_offsets, + ) + self.axis_measurements = axes + self.palm_orientation_measurements = list( + palm_orientation_measurements + ) + self.palm_orientation_rejections = dict( + palm_orientation_rejections + ) + self.zero_result = zero_result + self.validated_endpoint_zero_offsets_rad = dict(endpoint_zero_offsets) + self.validation_errors_rad = [ + float(value) + for name, values in holdout_by_joint.items() + if name in quality_joint_names + for value in values + ] + self.validation_errors_rad.extend( + float(value) + for value in holdout_zero_result.validation_errors_rad + ) + if self.combination_validation_enabled: + self._build_combination_validation_items() + self._start_next_combination_validation() + elif self.validation_enabled: + self._build_validation_items() + self._start_next_validation() + else: + self.validation_items.clear() + self.validation_index = 0 + self._begin_return_baseline("finalize") + + def _build_validation_items(self) -> None: + generator = random.Random(self.validation_seed) + self.validation_items = [] + for spec in self.profile.sweep_specs: + commands = generator.sample( + list(range(16, 240)), self.validation_command_count + ) + self.validation_items.extend( + ValidationItem(spec, command) for command in commands + ) + self.validation_index = 0 + + def _build_combination_validation_items(self) -> None: + self.combination_validation_items = list( + _combination_validation_items(self.baseline_command) + ) + self.combination_validation_index = 0 + self.combination_validation_completed = False + + def _start_next_combination_validation(self) -> None: + if self.combination_validation_index >= len( + self.combination_validation_items + ): + self.active_combination_validation = None + self.combination_validation_completed = True + self._begin_return_baseline("finalize") + return + item = self.combination_validation_items[ + self.combination_validation_index + ] + self.active_combination_validation = item + for frames in self.combination_validation_frames_buffer.values(): + frames.clear() + self.position_hold_since = None + self.validation_stage_started_at = time.monotonic() + self.motion_stage_started_at = self.validation_stage_started_at + self._reset_motion_progress( + self.validation_stage_started_at, + self._command_vector_error_u8(item.command_u8), + ) + self.state = STATE_VALIDATION_MOVE + self.reason = f"combination_validation_move_{item.name}" + self._publish_speed_profile(self._normal_speed_profile()) + self._publish_command(list(item.command_u8)) + + def _finish_combination_validation_capture(self) -> None: + item = self.active_combination_validation + if item is None: + raise RuntimeError("combination validation item is missing") + observations: dict[str, Any] = {} + all_frames = [ + frame + for frames in self.combination_validation_frames_buffer.values() + for frame in frames + ] + feedback = np.median( + np.asarray([frame.state_u8 for frame in all_frames], dtype=float), + axis=0, + ) + motor_directions = _combination_motor_directions( + self.combination_validation_items, + self.combination_validation_index, + self.baseline_command, + ) + angles = _combination_joint_angles( + self.profile, + self.measured_fits, + item.command_u8, + motor_directions, + ) + model = UrdfKinematicModel(self.source_urdf_path) + model_base_common = transform_matrix( + self.zero_result.base_translation_xyz_m, + self.zero_result.base_quaternion_xyzw, + ) + pose_position_errors: list[float] = [] + pose_orientation_errors: list[float] = [] + target_errors: list[dict[str, Any]] = [] + observed_target_keys: list[str] = [] + evaluated_target_keys: list[str] = [] + pending_mounts: dict[str, np.ndarray] = {} + for view, frames in self.combination_validation_frames_buffer.items(): + by_joint: dict[str, Any] = {} + joint_names = sorted( + set.intersection( + *(set(frame.joint_quaternions_xyzw) for frame in frames) + ) + ) + for name in joint_names: + by_joint[name] = { + "relative_quaternion_xyzw": [ + float(value) + for value in robust_rotation_summary( + [frame.joint_quaternions_xyzw[name] for frame in frames] + )[0] + ], + "parent_pose_common": _robust_pose_payload( + [frame.parent_poses_common[name] for frame in frames] + ), + "child_pose_common": _robust_pose_payload( + [frame.child_poses_common[name] for frame in frames] + ), + } + observations[view] = by_joint + base_role = COMBINATION_BASE_ROLE_BY_VIEW[view] + base_candidates = [ + name + for name in joint_names + if self.profile.record_specs[name].parent_role == base_role + ] + preferred_base = COMBINATION_BASE_OBSERVER_BY_VIEW[view] + if preferred_base in base_candidates: + base_name = preferred_base + elif base_candidates: + base_name = base_candidates[0] + else: + self._retry_combination_validation_or_pause( + f"combination_{view}_has_no_visible_base_target", + time.monotonic(), + ) + return + base_pose = _robust_pose_payload( + [frame.parent_poses_common[base_name] for frame in frames] + ) + base_matrix = transform_matrix( + base_pose["translation_xyz_m"], base_pose["quaternion_xyzw"] + ) + for observation_name, model_joint in COMBINATION_TAG_TARGETS_BY_VIEW[view]: + if observation_name not in by_joint: + continue + child_pose = by_joint[observation_name]["child_pose_common"] + child_matrix = transform_matrix( + child_pose["translation_xyz_m"], + child_pose["quaternion_xyzw"], + ) + observed = np.linalg.inv(base_matrix) @ child_matrix + # ``observed`` is expressed in this view's fixed palm-Tag + # frame, while UrdfKinematicModel returns hand_base->link. + # The zero solve already estimates hand_base->common; carry + # the model through common and into the observer base frame + # before solving or applying the moving Tag mount. + link = _model_link_in_observer_base( + base_matrix, + model_base_common, + model.link_transform( + model_joint, + zero_offsets=( + self.zero_result.all_active_offsets_rad + ), + joint_angles=angles, + independent_mimic_angles=True, + ), + ) + key = f"{view}:{observation_name}" + quality_gated = key in G20_COMBINATION_REQUIRED_TARGET_KEYS + observed_target_keys.append(key) + if key not in self.combination_tag_mounts: + pending_mounts[key] = np.linalg.inv(link) @ observed + continue + else: + mount = self.combination_tag_mounts[key] + predicted = link @ mount + if quality_gated: + evaluated_target_keys.append(key) + position_error = float( + np.linalg.norm(predicted[:3, 3] - observed[:3, 3]) + ) + orientation_error = float( + ( + Rotation.from_matrix(predicted[:3, :3]).inv() + * Rotation.from_matrix(observed[:3, :3]) + ).magnitude() + ) + if quality_gated: + pose_position_errors.append(position_error) + pose_orientation_errors.append(orientation_error) + target_errors.append( + { + "target": key, + "model_joint": model_joint, + "quality_gated": quality_gated, + "position_error_m": round(position_error, 8), + "orientation_error_rad": round( + orientation_error, 8 + ), + "orientation_error_deg": round( + math.degrees(orientation_error), 6 + ), + "observed_pose_in_base": matrix_payload(observed), + "predicted_pose_in_base": matrix_payload(predicted), + } + ) + if self.combination_validation_index > 0 and not pose_position_errors: + self._retry_combination_validation_or_pause( + "combination_pose_has_no_previously_observed_visible_target", + time.monotonic(), + ) + return + position_p95 = ( + 0.0 + if not pose_position_errors + else float(np.percentile(pose_position_errors, 95.0)) + ) + orientation_p95 = ( + 0.0 + if not pose_orientation_errors + else float(np.percentile(pose_orientation_errors, 95.0)) + ) + if ( + position_p95 > self.combination_maximum_position_p95_m + or orientation_p95 > self.combination_maximum_orientation_p95_rad + ): + retry_key = ("combination", self.combination_validation_index) + append_jsonl( + self.raw_path, + { + "kind": "combination_validation_failure", + "reason": "combination_pose_prediction_failed", + "pose_index": self.combination_validation_index, + "pose_name": item.name, + "label_zh": item.label_zh, + "capture_attempt": ( + self.validation_retry_counts.get(retry_key, 0) + 1 + ), + "requested_command_u8": list(item.command_u8), + "feedback_u8": [ + round(float(value), 6) for value in feedback + ], + "angle_branch_by_motor": list(motor_directions), + "evaluated_targets": sorted(set(evaluated_target_keys)), + "target_errors": sorted( + target_errors, key=lambda value: value["target"] + ), + "valid_frames_by_view": { + view: len(frames) + for view, frames in ( + self.combination_validation_frames_buffer.items() + ) + }, + "position_p95_m": round(position_p95, 8), + "maximum_position_p95_m": round( + self.combination_maximum_position_p95_m, 8 + ), + "orientation_p95_rad": round(orientation_p95, 8), + "orientation_p95_deg": round( + math.degrees(orientation_p95), 6 + ), + "maximum_orientation_p95_rad": round( + self.combination_maximum_orientation_p95_rad, 8 + ), + "maximum_orientation_p95_deg": round( + math.degrees( + self.combination_maximum_orientation_p95_rad + ), + 6, + ), + "passed": False, + }, + ) + self._retry_combination_validation_or_pause( + "combination_pose_prediction_failed", time.monotonic() + ) + return + self.combination_tag_mounts.update(pending_mounts) + for key in set(observed_target_keys): + self.combination_tag_observation_counts[key] = ( + self.combination_tag_observation_counts.get(key, 0) + 1 + ) + for key in set(evaluated_target_keys): + self.combination_tag_validation_counts[key] = ( + self.combination_tag_validation_counts.get(key, 0) + 1 + ) + is_final_pose = self.combination_validation_index + 1 >= len( + self.combination_validation_items + ) + if is_final_pose: + coverage = combination_target_coverage( + self.combination_tag_observation_counts, + self.combination_tag_validation_counts, + ) + if not coverage["coverage_passed"]: + append_jsonl( + self.raw_path, + { + "kind": "combination_target_coverage_failure", + "pose_name": item.name, + **coverage, + }, + ) + self._retry_combination_validation_or_pause( + "combination_target_coverage_incomplete", time.monotonic() + ) + return + self.combination_position_errors_m.extend(pose_position_errors) + self.combination_orientation_errors_rad.extend(pose_orientation_errors) + append_jsonl( + self.raw_path, + { + "kind": "combination_validation_sample", + "pose_index": self.combination_validation_index, + "pose_name": item.name, + "label_zh": item.label_zh, + "requested_command_u8": list(item.command_u8), + "feedback_u8": [round(float(value), 6) for value in feedback], + "image_stamp_ns": int( + np.median([frame.stamp_ns for frame in all_frames]) + ), + "views": observations, + "observed_targets": sorted(set(observed_target_keys)), + "evaluated_targets": sorted(set(evaluated_target_keys)), + "valid_frames_by_view": { + view: len(frames) + for view, frames in self.combination_validation_frames_buffer.items() + }, + "position_p95_m": round(position_p95, 8), + "orientation_p95_rad": round(orientation_p95, 8), + "passed": True, + }, + ) + self.combination_validation_index += 1 + self.active_combination_validation = None + if self.combination_validation_index >= len( + self.combination_validation_items + ): + self.combination_validation_completed = True + self._begin_return_baseline("finalize") + else: + self._begin_return_baseline("combination_next") + + def _retry_combination_validation_or_pause( + self, reason: str, now: float + ) -> None: + item = self.active_combination_validation + if item is None: + self._pause(reason) + return + key = ("combination", self.combination_validation_index) + retries = self.validation_retry_counts.get(key, 0) + if retries >= self.automatic_sweep_retry_limit: + self._pause(reason) + return + retries += 1 + self.validation_retry_counts[key] = retries + for view in self.combination_validation_frames_buffer: + runtime = self.views.get(view) + if runtime is None: + continue + self._reset_view_trackers(runtime) + runtime.pnp_invalid_since = None + runtime.pnp_reset_count += 1 + append_jsonl( + self.raw_path, + { + "kind": "automatic_combination_retry", + "pose_name": item.name, + "reason": reason, + "retry": retries, + "retry_limit": self.automatic_sweep_retry_limit, + }, + ) + for frames in self.combination_validation_frames_buffer.values(): + frames.clear() + self.validation_stage_started_at = now + self.motion_stage_started_at = now + self.position_hold_since = None + self.state = STATE_VALIDATION_MOVE + self.reason = f"automatic_retry_{reason}" + self._reset_motion_progress(now, self._command_vector_error_u8(item.command_u8)) + self._publish_command(list(item.command_u8)) + + def _start_next_validation(self) -> None: + if self.validation_index >= len(self.validation_items): + self.active_validation = None + self._begin_return_baseline("finalize") + return + self.active_validation = self.validation_items[self.validation_index] + validation_motor = self.active_validation.spec.motor_index + validation_target = float(self.active_validation.command_u8) + validation_current = ( + float(self.latest_state_u8[validation_motor]) + if len(self.latest_state_u8) == 20 + else validation_target + ) + self.active_validation_direction = ( + DIRECTION_INCREASING + if validation_target > validation_current + 0.5 + else DIRECTION_DECREASING + if validation_target < validation_current - 0.5 + else canonical_zero_direction( + self.profile, self.active_validation.spec.joints[0] + ) + ) + self.validation_frames_buffer.clear() + self.position_hold_since = None + self.validation_stage_started_at = time.monotonic() + self._reset_motion_progress( + self.validation_stage_started_at, + self._motion_command_error_u8( + self.active_validation.spec, + self.active_validation.command_u8, + ), + ) + self.state = STATE_VALIDATION_MOVE + self.reason = ( + f"validation_{self.active_validation.spec.view}_motor_" + f"{self.active_validation.spec.motor_index}_" + f"command_{self.active_validation.command_u8}" + ) + self._publish_speed_profile( + self._speed_profile_for_spec(self.active_validation.spec) + ) + self._publish_command( + build_calibration_motion_command( + self.active_validation.spec, + self.active_validation.command_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ) + + def _finish_validation_capture(self) -> None: + assert self.active_validation is not None + item = self.active_validation + command = item.command_u8 + for name in item.spec.joints: + selected = _frames_for_joint( + self.validation_frames_buffer, name + ) + if len(selected) < self.validation_frames: + raise RuntimeError( + f"random validation {name} has too few frames" + ) + fit = self.measured_fits.get(name) + if fit is None: + fit = self.validation_only_fits[name] + quaternion = robust_rotation_summary( + [ + frame.joint_quaternions_xyzw[name] + for frame in selected + ] + )[0] + image_relative_xy_px = np.median( + np.asarray( + [ + frame.image_vectors_xy_px[name] + for frame in selected + ], + dtype=float, + ), + axis=0, + ) + observed = measure_joint_curve_observation( + fit, + quaternion_xyzw=quaternion, + image_relative_xy_px=image_relative_xy_px, + ) + expected_curve = ( + fit.decreasing_rad + if self.active_validation_direction == DIRECTION_DECREASING + else fit.increasing_rad + if self.active_validation_direction == DIRECTION_INCREASING + else fit.angle_rad + ) + expected = float(expected_curve[command]) + self.validation_errors_rad.append(observed - expected) + previous_spec = item.spec + self.validation_index += 1 + self.active_validation = None + self.active_validation_direction = None + if self.validation_index >= len(self.validation_items): + self._begin_return_baseline("finalize") + elif self.validation_items[self.validation_index].spec != previous_spec: + self._begin_return_baseline("validation_next") + else: + self._start_next_validation() + + def _finalize(self) -> None: + if self.zero_result is None: + raise RuntimeError("URDF zero solution is missing") + errors = np.abs(np.asarray(self.validation_errors_rad, dtype=float)) + validation_passed = bool( + self.zero_result.passed + and ( + not self.combination_validation_enabled + or self.combination_validation_completed + ) + and ( + not self.validation_enabled + or ( + errors.size > 0 + and float(np.mean(errors)) + <= self.maximum_validation_mae_rad + and float(np.percentile(errors, 95.0)) + <= self.maximum_validation_p95_rad + and ( + self.profile.layout_id != G20_RIGHT_19_LAYOUT + or float(np.max(errors)) + <= self.maximum_validation_error_rad + ) + ) + ) + ) + passed = bool(self.fit_quality_passed and validation_passed) + if not passed: + raise RuntimeError("final_calibration_quality_failed") + endpoint_zero_offsets = ( + G20ThreeCameraCalibrationNode + ._endpoint_zero_offsets_for_publication(self) + ) + stamp = ( + self.session_dir.name + if re.fullmatch(r"\d{8}_\d{6}", self.session_dir.name) + else None + ) + # Zero calibration changes only joint-origin rotations. Measured + # motion curves belong in schema-v4 JSON and must never enlarge the + # original CAD/mechanical safety limits in the generated URDF. + # JSON schema v4 publishes eight decimal places. Generate the URDF + # from those exact same values so the pair is numerically inseparable + # instead of differing by harmless formatter quantisation. + published_zero_offsets = { + name: round(float(value), 8) + for name, value in self.zero_result.all_active_offsets_rad.items() + } + urdf_offsets = published_zero_offsets + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + urdf_offsets = { + name: published_zero_offsets[name] + for name in self.zero_profile.direct_zero_joints + } + correction_plan = None + typed_profile = getattr(self, "calibration_profile", None) + if typed_profile is not None: + frozen_names = typed_profile.scope.frozen_joints[ + self.recalibration_scope + ] + correction_plan = build_correction_plan( + typed_profile, + source_sha256=_file_sha256(self.source_urdf_path), + scope=self.recalibration_scope, + frozen_offsets_rad={ + name: urdf_offsets[name] for name in frozen_names + }, + ) + self.corrected_urdf_path = write_zero_corrected_urdf( + source_urdf=self.source_urdf_path, + output_directory=self.corrected_urdf_output_dir, + serial_number=self.serial_number, + offsets_rad=urdf_offsets, + endpoint_anchored_offsets_rad=endpoint_zero_offsets, + timestamp=stamp, + correction_plan=correction_plan, + ) + try: + if self.standalone_thumb_calibration: + payload = build_standalone_thumb_payload( + profile=self.profile, + serial_number=self.serial_number, + measured_fits=self.measured_fits, + thumb_offsets_rad={ + name: published_zero_offsets[name] + for name in ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + ) + }, + validation_errors_rad=self.validation_errors_rad, + baseline_command_u8=self.baseline_command, + source_urdf_sha256=_file_sha256( + self.source_urdf_path + ), + camera_extrinsics_sha256=_file_sha256( + self.camera_extrinsics_file + ), + corrected_urdf_sha256=_file_sha256( + self.corrected_urdf_path + ), + ) + else: + payload = build_compact_payload( + serial_number=self.serial_number, + measured_fits=self.measured_fits, + urdf_zero_offsets_rad=published_zero_offsets, + validation_errors_rad=self.validation_errors_rad, + passed=passed, + baseline=self.baseline_command, + side=self.hand_type, + layout_id=self.profile.layout_id, + zero_uncertainty_rad=( + self.zero_result.offset_confidence_half_width_rad + ), + zero_cycle_offsets_rad=self.zero_result.cycle_offsets_rad, + zero_observers=self.zero_profile.offset_observer_joint, + artifact_hashes=( + { + "source_urdf_sha256": _file_sha256( + self.source_urdf_path + ), + "camera_extrinsics_sha256": _file_sha256( + self.camera_extrinsics_file + ), + "corrected_urdf_sha256": _file_sha256( + self.corrected_urdf_path + ), + } + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else None + ), + cross_view_roll_metrics=getattr( + self, "cross_view_roll_metrics", {} + ), + joint_dynamic_diagnostics=getattr( + self, "joint_dynamic_diagnostics", {} + ), + zero_geometry_diagnostics=( + { + "training_cycles": self.zero_result.training_cycles, + "validation_cycle": self.zero_result.validation_cycle, + "axis_line_rms_m": self.zero_result.axis_line_rms_m, + "validation_line_error_by_joint_m": ( + self.zero_result.validation_line_error_by_joint_m + ), + "observability_rank": ( + self.zero_result.observability_rank + ), + "observability_parameter_count": ( + self.zero_result.observability_parameter_count + ), + "observability_condition_number": ( + self.zero_result.observability_condition_number + ), + "offset_covariance_rad2": ( + self.zero_result.offset_covariance_rad2 + ), + "palm_orientation_sources": [ + { + "source_joint": item.source_joint, + "model_joint": item.model_joint, + "cycle": item.cycle, + "observed_arc_rad": item.observed_arc_rad, + "rotation_orthogonal_rms_rad": ( + item.rotation_orthogonal_rms_rad + ), + "axis_estimator": item.axis_estimator, + "incremental_pair_count": ( + item.incremental_pair_count + ), + } + for item in getattr( + self, + "palm_orientation_measurements", + (), + ) + ], + "palm_orientation_rejections": dict( + getattr( + self, + "palm_orientation_rejections", + {}, + ) + ), + } + if self.profile.layout_id == G20_RIGHT_19_LAYOUT + else None + ), + ) + # The JSON is the commit marker for the URDF/JSON pair. It is + # atomically renamed only after the corrected URDF and all three + # artifact hashes have been produced and schema-validated. + atomic_write_json(self.final_path, payload) + except Exception: + if self.profile.layout_id == G20_RIGHT_19_LAYOUT: + rejected = self.corrected_urdf_path.with_suffix( + ".urdf.rejected" + ) + self.corrected_urdf_path.replace(rejected) + self.corrected_urdf_path = None + raise + self.completed_payload = payload + self.state = STATE_COMPLETE + self.reason = "calibration_passed" if passed else "quality_failed" + self.get_logger().info( + f"Three-camera calibration complete: passed={passed}; {self.final_path}" + ) + + def _pause(self, reason: str) -> None: + self._publish_hold_current() + self.paused_reason = str(reason) + self.state = STATE_PAUSED + self.reason = str(reason) + self.position_hold_since = None + + def _retry_motion_or_pause(self, reason: str, now: float) -> None: + retries = self.motion_retry_counts.get(reason, 0) + if retries >= self.automatic_motion_retry_limit: + self._pause(reason) + return + retries += 1 + self.motion_retry_counts[reason] = retries + reset_views: list[str] = [] + task_reference_preserved = False + if ( + reason == "sweep_start_tag_timeout" + and self.state == STATE_PREPARE_SWEEP + and self.active_sweep is not None + ): + # Re-publishing the same endpoint command cannot repair a PnP + # tracker stuck on a planar mirror branch. Give every active + # camera a genuinely fresh bounded initialization window while + # retaining the task-level endpoint reference established by the + # visibility precheck/earlier cycles. + profile = _node_profile(self) + task_reference_preserved = ( + _preserve_pnp_task_reference_for_sweep( + self.active_sweep, + is_fit_retry=bool( + getattr(self, "active_sweep_is_fit_retry", False) + ), + has_precheck_anchor=( + profile.layout_id == G20_RIGHT_19_LAYOUT + ), + ) + ) + reset_trackers = getattr(self, "_reset_view_trackers", None) + reset_diagnostics = getattr( + self, "_reset_view_pnp_diagnostics", None + ) + for view in _sweep_views(profile, self.active_sweep.spec): + runtime = getattr(self, "views", {}).get(view) + if runtime is None or not callable(reset_trackers): + continue + reset_trackers( + runtime, + preserve_task_reference=task_reference_preserved, + ) + if callable(reset_diagnostics): + reset_diagnostics(runtime) + runtime.pnp_invalid_since = None + runtime.pnp_reset_count += 1 + reset_views.append(view) + getattr(self, "sweep_start_frames", []).clear() + append_jsonl( + self.raw_path, + { + "kind": "automatic_motion_retry", + "state": self.state, + "reason": reason, + "retry": retries, + "retry_limit": self.automatic_motion_retry_limit, + "pnp_trackers_reset": reset_views, + "task_reference_preserved": task_reference_preserved, + }, + ) + self.motion_stage_started_at = now + self.position_hold_since = None + if self.state == STATE_RETURN_BASELINE: + self._reset_motion_progress(now, self._baseline_error_u8()) + return_command = ( + G20ThreeCameraCalibrationNode._current_return_command(self) + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, return_command + ) + ) + self._publish_command( + list(return_command) + ) + elif self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: + preparation_command = getattr( + self, + "preparation_command_u8", + tuple( + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ), + ) + self._reset_motion_progress( + now, + self._command_vector_error_u8(preparation_command), + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, preparation_command + ) + ) + self._publish_command(list(preparation_command)) + self.reason = f"automatic_retry_{reason}" + + def _retry_validation_or_pause(self, reason: str, now: float) -> None: + if self.active_validation is None: + self._pause(reason) + return + item = self.active_validation + key = (_sweep_storage_key(item.spec), item.command_u8) + retries = self.validation_retry_counts.get(key, 0) + retry_limit = self.automatic_sweep_retry_limit + if retries >= retry_limit: + self._pause(reason) + return + retries += 1 + self.validation_retry_counts[key] = retries + for view in _sweep_views(_node_profile(self), item.spec): + runtime = self.views[view] + self._reset_view_trackers(runtime) + runtime.pnp_reset_count += 1 + self.validation_frames_buffer.clear() + self.validation_stage_started_at = now + self._reset_motion_progress( + now, + self._motion_command_error_u8(item.spec, item.command_u8), + ) + self.position_hold_since = None + self.state = STATE_VALIDATION_MOVE + append_jsonl( + self.raw_path, + { + "kind": "automatic_validation_retry", + "view": item.spec.view, + "motor_index": item.spec.motor_index, + "requested_command_u8": item.command_u8, + "reason": reason, + "retry": retries, + "retry_limit": retry_limit, + }, + ) + self._publish_speed_profile(self._speed_profile_for_spec(item.spec)) + self._publish_command( + build_calibration_motion_command( + item.spec, + item.command_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ) + self.reason = f"automatic_retry_{reason}" + + def _timer_callback(self) -> None: + now = time.monotonic() + try: + self._advance(now) + except Exception as error: + self.get_logger().error( + f"Calibration paused: {error}\n{traceback.format_exc()}" + ) + self._pause(str(error)) + if now - self.last_status_publish >= 0.5: + self.last_status_publish = now + self._publish_status(now) + + def _advance(self, now: float) -> None: + if self.state == STATE_PREFLIGHT: + if not self.started: + if self._all_devices_ready(now): + self.state = STATE_WAIT_START + self.reason = "call_start_for_baseline_recovery" + 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(): + self._start_next_sweep() + else: + self.reason = "locking_fixed_base_references" + return + if self.state == STATE_WAIT_START: + if not self._all_devices_ready(now): + self.state = STATE_PREFLIGHT + self.reason = "device_preflight_lost" + return + if self.state == STATE_RETURN_BASELINE: + baseline_reached = self._baseline_reached() + baseline_details = self._baseline_error_details() + if ( + not baseline_reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(baseline_details["error_u8"]), + context="return_baseline", + details=baseline_details, + ) + ): + return + if now - self.motion_stage_started_at > self.position_timeout_seconds: + self._retry_motion_or_pause("return_baseline_timeout", now) + return + if baseline_reached: + return_waypoints = getattr( + self, "return_waypoints", deque() + ) + if return_waypoints: + # Intermediate avoidance waypoints need confirmed feedback, + # but only the final baseline needs the dedicated 0.5 s + # backlash hold. This removes several seconds of no-op + # waiting from every multi-finger return sequence. + self.return_command_u8 = return_waypoints.popleft() + self.position_hold_since = None + self.motion_stage_started_at = now + self._reset_motion_progress( + now, self._baseline_error_u8() + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, self.return_command_u8 + ) + ) + self._publish_command( + list(self.return_command_u8) + ) + return + if self.position_hold_since is None: + self.position_hold_since = now + elif now - self.position_hold_since >= self.baseline_hold_seconds: + after = self.baseline_after + self.motion_retry_counts.pop("return_baseline_timeout", None) + self.position_hold_since = None + if after in { + "next_sweep", + "next_cycle", + "next_task_same_finger", + "resume_sweep", + "retry_sweep", + }: + self._start_next_sweep() + elif after == "fit": + self._fit_all_curves() + elif after in {"validation_next", "resume_validation"}: + self._start_next_validation() + elif after == "combination_next": + self._start_next_combination_validation() + elif after == "finalize": + self._finalize() + elif after == "startup_tag_preflight": + # Discard observations and PnP branches from the + # interrupted pose. The normal preflight then requires + # a fresh window of all three fixed palm Tags while the + # hand is confirmed at baseline. + self._finish_startup_baseline_recovery() + elif after == "abort": + self.state = STATE_ABORTED + self.reason = ( + "safe_abort_complete:" + f"{self.abort_original_reason or 'operator_abort'}" + ) + else: + self.position_hold_since = None + return + if self.state == STATE_PREPARE_SWEEP: + assert self.active_sweep is not None + preparation_command = getattr( + self, + "preparation_command_u8", + tuple( + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ), + ) + reached = self._command_vector_reached(preparation_command) + if now - self.motion_stage_started_at > self.position_timeout_seconds: + self._retry_motion_or_pause( + ( + "sweep_start_tag_timeout" + if reached + else "sweep_start_position_timeout" + ), + now, + ) + return + prepare_context = ( + f"prepare_motor_{self.active_sweep.spec.motor_index}" + ) + prepare_details = self._command_vector_error_details( + preparation_command, prepare_context + ) + if ( + not reached + and self._pause_if_motion_stalled( + now=now, + error_u8=self._command_vector_error_u8( + preparation_command + ), + context=prepare_context, + details=prepare_details, + ) + ): + return + preparation_waypoints = getattr( + self, "preparation_waypoints", deque() + ) + if reached and preparation_waypoints: + self.preparation_command_u8 = ( + preparation_waypoints.popleft() + ) + self.motion_stage_started_at = now + self.position_hold_since = None + self._reset_motion_progress( + now, + self._command_vector_error_u8( + self.preparation_command_u8 + ), + ) + self._publish_speed_profile( + G20ThreeCameraCalibrationNode._transition_speed_profile( + self, self.preparation_command_u8 + ) + ) + self._publish_command(list(self.preparation_command_u8)) + return + if reached: + # Pose-entry waypoints use conservative joint-class speeds. + # Switch to the accepted task scan speed only after the full + # start pose has been reached, then honour the SDK settle time. + self._publish_speed_profile( + self._speed_profile_for_spec(self.active_sweep.spec) + ) + speed_ready = bool( + now - self.speed_commanded_at + >= self.speed_setting_settle_seconds + ) + # Do not leave the start endpoint until every contributing camera + # has enough timestamp-synchronised observations. The retained + # frames become the strict 0/255 endpoint bin and, in the first + # training round, the first steady command checkpoint. + minimum_start_frames = ( + 3 + if _steady_checkpoint_commands( + _node_profile(self), self.active_sweep + ) + else 1 + ) + start_frames_ready = _frames_cover_sweep_joints( + self.sweep_start_frames, + self.active_sweep.spec, + minimum_start_frames, + ) + if reached and speed_ready and not start_frames_ready: + self.reason = "waiting_for_task_tags_at_sweep_start" + if ( + reached + and speed_ready + and start_frames_ready + ): + if self.position_hold_since is None: + self.position_hold_since = now + elif now - self.position_hold_since >= self._active_endpoint_hold_seconds(): + self.motion_retry_counts.pop( + "sweep_start_position_timeout", None + ) + self.motion_retry_counts.pop( + "sweep_start_tag_timeout", None + ) + self.position_hold_since = None + self._begin_active_sweep(now) + else: + self.position_hold_since = None + return + if self.state == STATE_SWEEP: + assert self.active_sweep is not None + motor = self.active_sweep.spec.motor_index + if now - self.sweep_started_at > self._active_sweep_timeout_seconds(): + self._retry_active_sweep_or_pause("sweep_timeout") + return + stale_views = [ + view + for view in _sweep_views( + self.profile, self.active_sweep.spec + ) + if now + - getattr(self, "sweep_last_valid_at_by_view", {}).get( + view, self.sweep_started_at + ) + > self.invalid_timeout_seconds + ] + if stale_views: + self._retry_active_sweep_or_pause( + "synchronised_tag_state_timeout:" + + ",".join(stale_views) + ) + return + checkpoint_target_value = getattr( + self, "sweep_checkpoint_target_u8", None + ) + checkpoint_active = bool( + getattr(self, "sweep_checkpoint_mode", False) + and checkpoint_target_value is not None + and not getattr(self, "sweep_baseline_pending", False) + ) + motion_target_u8 = ( + int(self.baseline_command[motor]) + if getattr(self, "sweep_baseline_pending", False) + else int( + checkpoint_target_value + if checkpoint_target_value is not None + else self.active_sweep.target_u8 + ) + ) + target_reached = ( + self._steady_checkpoint_reached( + self.active_sweep.spec, motion_target_u8 + ) + if checkpoint_active + else self._motion_command_reached( + self.active_sweep.spec, motion_target_u8 + ) + ) + sweep_context = ( + f"sweep_baseline_motor_{motor}" + if getattr(self, "sweep_baseline_pending", False) + else f"sweep_motor_{motor}" + ) + sweep_details = self._motion_command_error_details( + self.active_sweep.spec, + motion_target_u8, + sweep_context, + ) + if ( + checkpoint_active + and int(sweep_details.get("motor_index", -1)) == motor + ): + sweep_details["tolerance_u8"] = max( + float( + self.steady_checkpoint_command_feedback_tolerance_u8 + ), + float(sweep_details["tolerance_u8"]), + ) + if ( + not target_reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(sweep_details["error_u8"]), + context=sweep_context, + details=sweep_details, + ) + ): + return + if getattr(self, "sweep_baseline_pending", False): + if target_reached: + if self.sweep_baseline_hold_since is None: + # Frames acquired during deceleration into 127 are not + # static observations. Wait baseline_hold_seconds + # after feedback first reaches 127, then collect a + # fresh multi-frame baseline sample. + self.sweep_baseline_frames.clear() + self.sweep_baseline_hold_since = now + elif ( + now - self.sweep_baseline_hold_since + >= self.baseline_hold_seconds + and _frames_cover_sweep_joints( + self.sweep_baseline_frames, + self.active_sweep.spec, + self.minimum_baseline_hold_frames, + ) + ): + self.sweep_baseline_pending = False + self.sweep_baseline_hold_since = None + self.sweep_endpoint_since = None + self.reason = ( + "collecting_timestamp_synchronised_tag_centres" + ) + if getattr(self, "sweep_checkpoint_mode", False): + self._publish_next_checkpoint(now) + else: + self._reset_motion_progress( + now, + self._motion_command_error_u8( + self.active_sweep.spec, + self.active_sweep.target_u8, + ), + ) + self._publish_command( + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.target_u8, + baseline=self.baseline_command, + profile=self.profile, + ) + ) + else: + self.sweep_baseline_hold_since = None + self.sweep_baseline_frames.clear() + return + if getattr(self, "sweep_checkpoint_mode", False): + checkpoint_target = self.sweep_checkpoint_target_u8 + if checkpoint_target is None: + if self.sweep_checkpoint_commands: + # Recover a missing in-flight target by publishing the + # next queued checkpoint instead of aborting a healthy + # hardware session. + self._publish_next_checkpoint(now) + return + self.sweep_checkpoint_mode = False + else: + if target_reached: + if self.sweep_checkpoint_hold_since is None: + self.sweep_checkpoint_frames.clear() + self.sweep_checkpoint_hold_since = now + elif ( + now - self.sweep_checkpoint_hold_since + >= self.endpoint_hold_seconds + and _frames_cover_sweep_joints( + self.sweep_checkpoint_frames, + self.active_sweep.spec, + 3, + ) + ): + if not self._steady_checkpoint_feedback_is_stable( + self.active_sweep.spec, + self.sweep_checkpoint_frames, + ): + # The command/feedback offset is acceptable, + # but feedback is still settling inside that + # band. Restart the hold window without + # issuing another command or weakening the + # mechanical-stall gate. + self.sweep_checkpoint_hold_since = now + self.sweep_checkpoint_frames.clear() + return + self._record_command_checkpoint( + self.active_sweep, + int(checkpoint_target), + list(self.sweep_checkpoint_frames), + ) + self.sweep_checkpoint_hold_since = None + self.sweep_checkpoint_frames.clear() + if self._publish_next_checkpoint(now): + return + self.sweep_endpoint_since = now + else: + self.sweep_checkpoint_hold_since = None + self.sweep_checkpoint_frames.clear() + if self.sweep_checkpoint_target_u8 is not None: + return + if target_reached: + if self.sweep_endpoint_since is None: + self.sweep_endpoint_since = now + else: + self.sweep_endpoint_since = None + if ( + self.sweep_endpoint_since is not None + and now - self.sweep_endpoint_since + >= self._active_endpoint_hold_seconds() + and _frames_cover_sweep_motion( + self.sweep_frames, + self.active_sweep.spec, + motor_index=motor, + minimum_per_joint=self.minimum_sweep_frames, + minimum_span_u8=self.minimum_state_span_u8, + ) + ): + self._finish_active_sweep() + return + if self.state == STATE_VALIDATION_MOVE: + if self.active_combination_validation is not None: + command = self.active_combination_validation.command_u8 + reached = self._command_vector_reached(command) + details = self._command_vector_error_details( + command, + f"combination_{self.active_combination_validation.name}", + ) + if ( + not reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(details["error_u8"]), + context=str(details["stage"]), + details=details, + ) + ): + return + if reached: + if self.position_hold_since is None: + self.position_hold_since = now + elif now - self.position_hold_since >= self.endpoint_hold_seconds: + self.position_hold_since = None + for frames in self.combination_validation_frames_buffer.values(): + frames.clear() + self.validation_stage_started_at = now + self.state = STATE_VALIDATION_CAPTURE + self.reason = ( + "combination_validation_capture_" + f"{self.active_combination_validation.name}" + ) + else: + self.position_hold_since = None + if now - self.validation_stage_started_at > self.validation_timeout_seconds: + self._retry_combination_validation_or_pause( + "combination_validation_move_timeout", now + ) + return + assert self.active_validation is not None + reached = self._motion_command_reached( + self.active_validation.spec, + self.active_validation.command_u8, + ) + validation_context = ( + "validation_motor_" + f"{self.active_validation.spec.motor_index}" + ) + validation_details = self._motion_command_error_details( + self.active_validation.spec, + self.active_validation.command_u8, + validation_context, + ) + if ( + not reached + and self._pause_if_motion_stalled( + now=now, + error_u8=float(validation_details["error_u8"]), + context=validation_context, + details=validation_details, + ) + ): + return + speed_ready = bool( + now - self.speed_commanded_at + >= self.speed_setting_settle_seconds + ) + if reached and speed_ready: + if self.position_hold_since is None: + self.position_hold_since = now + elif now - self.position_hold_since >= self.endpoint_hold_seconds: + self.position_hold_since = None + self.validation_frames_buffer.clear() + self.validation_stage_started_at = now + self.state = STATE_VALIDATION_CAPTURE + self.reason = "capturing_random_validation_pose" + else: + self.position_hold_since = None + if now - self.validation_stage_started_at > self.validation_timeout_seconds: + self._retry_validation_or_pause("validation_move_timeout", now) + return + if self.state == STATE_VALIDATION_CAPTURE: + if self.active_combination_validation is not None: + if all( + len(frames) >= self.combination_validation_frames + for frames in self.combination_validation_frames_buffer.values() + ): + self._finish_combination_validation_capture() + elif now - self.validation_stage_started_at > self.validation_timeout_seconds: + self._retry_combination_validation_or_pause( + "combination_validation_capture_timeout", now + ) + return + if _frames_cover_sweep_joints( + self.validation_frames_buffer, + self.active_validation.spec, + self.validation_frames, + ): + self._finish_validation_capture() + elif now - self.validation_stage_started_at > self.validation_timeout_seconds: + self._retry_validation_or_pause( + "validation_capture_timeout", now + ) + + def _publish_status(self, now: float) -> None: + views: dict[str, dict[str, Any]] = {} + for name, runtime in self.views.items(): + required_roles = set(self._required_roles_for_view(name)) + live_required_roles = set( + self._live_required_roles_for_view( + name, tuple(required_roles) + ) + ) + locked_role = self._locked_base_role_for_active_capture(name) + locked_roles = set(() if locked_role is None else (locked_role,)) + views[name] = { + "ready": self._view_ready(runtime, now), + "stream_alive": bool( + runtime.last_message_at > 0.0 + and now - runtime.last_message_at <= 1.0 + ), + "camera_info_valid": runtime.camera_info_valid, + "camera_extrinsics_valid": runtime.extrinsics_valid, + "detection_hz": round(runtime.detection_hz, 2), + "valid_rate": round(runtime.valid_rate, 4), + "required_tag_ids": sorted( + runtime.view_tags[role] for role in required_roles + ), + "locked_reference_tag_ids": sorted( + runtime.view_tags[role] for role in locked_roles + ), + "locked_base_corner_drift_px": round( + runtime.latest_locked_base_corner_drift_px, 4 + ), + "locked_base_maximum_corner_drift_px": ( + self.fixed_base_maximum_corner_drift_px + ), + "locked_base_corner_drift_count": ( + runtime.locked_base_corner_drift_count + ), + "configured_tag_ids": sorted(runtime.view_tags.values()), + "visible_configured_tag_ids": sorted( + tag_id + for role, tag_id in runtime.view_tags.items() + if role in runtime.latest_tag_quality + and self._quality_valid( + runtime.latest_tag_quality[role], include_pnp=False + ) + ), + "detected_tag_ids": sorted( + tag_id + for tag_id, role in runtime.role_by_id.items() + if role in required_roles + and role in runtime.latest_tag_quality + ), + "missing_tag_ids": sorted( + tag_id + for tag_id, role in runtime.role_by_id.items() + if role in live_required_roles + and role not in runtime.latest_tag_quality + ), + "pnp_rejections": dict(runtime.latest_pnp_rejections), + "group_pnp_reason": runtime.latest_group_pnp_reason, + "group_missing_candidate_roles": list( + runtime.latest_group_missing_candidate_roles + ), + "pnp_candidate_diagnostics": { + role: { + "tag_id": runtime.view_tags[role], + **dict(diagnostics), + } + for role, diagnostics in ( + runtime.tracker.last_candidate_diagnostics_by_role.items() + ) + if role in required_roles and role not in locked_roles + }, + "pnp_pose_valid": runtime.latest_pnp_valid, + "pnp_rejection_counts": dict( + runtime.pnp_rejection_counts + ), + "group_pnp_rejection_counts": dict( + runtime.group_pnp_rejection_counts + ), + "pnp_initialization_progress": ( + None + if runtime.pnp_initialization_progress is None + else { + "accepted": runtime.pnp_initialization_progress[0], + "required": runtime.pnp_initialization_progress[1], + } + ), + "pnp_reset_count": runtime.pnp_reset_count, + "pnp_invalid_seconds": ( + 0.0 + if runtime.pnp_invalid_since is None + else round(max(0.0, now - runtime.pnp_invalid_since), 3) + ), + } + active: dict[str, Any] = {} + if self.motion_stall_details: + active = dict(self.motion_stall_details) + elif self.active_sweep is not None: + motor = self.active_sweep.spec.motor_index + start_tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, + self.active_sweep.spec, + self.active_sweep.start_u8, + ) + ) + target_tolerance = ( + G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( + self, + self.active_sweep.spec, + self.active_sweep.target_u8, + ) + ) + mechanical_start_tolerance = self._endpoint_tolerance_for_spec( + self.active_sweep.spec, self.active_sweep.start_u8 + ) + mechanical_target_tolerance = self._endpoint_tolerance_for_spec( + self.active_sweep.spec, self.active_sweep.target_u8 + ) + values = [ + float(frame.state_u8[motor]) for frame in self.sweep_frames + ] + bins = sorted( + { + ( + self.active_sweep.start_u8 + if abs(value - self.active_sweep.start_u8) + <= start_tolerance + else self.active_sweep.target_u8 + if abs(value - self.active_sweep.target_u8) + <= target_tolerance + else int(np.clip(np.rint(value), 0, 255)) + ) + for value in values + } + ) + adjacent_bin_gaps = [ + (right - left, left, right) + for left, right in zip(bins, bins[1:]) + ] + maximum_bin_gap, gap_start_u8, gap_end_u8 = max( + adjacent_bin_gaps, + default=(0, None, None), + ) + missing_endpoints = [ + endpoint for endpoint in (0, 255) if endpoint not in bins + ] + actual = ( + None + if len(self.latest_state_u8) != 20 + else float(self.latest_state_u8[motor]) + ) + motion_progress = None + if actual is not None: + motion_progress = float( + np.clip( + abs(actual - self.active_sweep.start_u8) / 255.0, + 0.0, + 1.0, + ) + ) + storage_key = _sweep_storage_key(self.active_sweep.spec) + frame_counts = { + name: len(_frames_for_joint(self.sweep_frames, name)) + for name in self.active_sweep.spec.joints + } + baseline_frame_counts = { + name: len( + _frames_for_joint(self.sweep_baseline_frames, name) + ) + for name in self.active_sweep.spec.joints + } + detection_rates_by_view = { + view: ( + 0.0 + if getattr( + self, "sweep_detection_total_by_view", {} + ).get(view, 0) <= 0 + else getattr( + self, "sweep_detection_valid_by_view", {} + ).get(view, 0) + / getattr( + self, "sweep_detection_total_by_view", {} + )[view] + ) + for view in _sweep_views( + self.profile, self.active_sweep.spec + ) + } + active = { + "kind": "sweep", + "view": self.active_sweep.spec.view, + "motor_index": motor, + "joints": list(self.active_sweep.spec.joints), + "cycle": self.active_sweep.cycle + 1, + "repetitions": self.repetitions, + "direction": self.active_sweep.direction, + "task_name": self.active_sweep.spec.key, + "fit_attempt": self.sweep_attempts.get( + storage_key, 1 + ), + "fit_attempt_limit": self.automatic_fit_retry_limit + 1, + "fit_retry_cycles": ( + [ + cycle + 1 + for cycle in sorted( + getattr(self, "retry_cycles", set()) + ) + ] + if getattr(self, "active_sweep_is_fit_retry", False) + else [] + ), + "automatic_retry_count": max( + self.sweep_retry_counts.get( + ( + storage_key, + self.active_sweep.cycle, + self.active_sweep.direction, + ), + 0, + ), + self.motion_retry_counts.get( + "sweep_start_tag_timeout", 0 + ), + self.motion_retry_counts.get( + "sweep_start_position_timeout", 0 + ), + ), + "automatic_retry_limit": self.automatic_sweep_retry_limit, + "retry_speed_scale": ( + 1.0 + if self.sweep_retry_counts.get( + ( + storage_key, + self.active_sweep.cycle, + self.active_sweep.direction, + ), + 0, + ) + == 0 + else self.retry_speed_scales[ + min( + self.sweep_retry_counts[ + ( + storage_key, + self.active_sweep.cycle, + self.active_sweep.direction, + ) + ], + len(self.retry_speed_scales), + ) + - 1 + ] + ), + "endpoint_hold_seconds": self._active_endpoint_hold_seconds(), + "sweep_timeout_seconds": self._active_sweep_timeout_seconds(), + "motor_stall_timeout_seconds": self.motor_stall_timeout_seconds, + "motor_stall_minimum_progress_u8": ( + self.motor_stall_minimum_progress_u8 + ), + "start_u8": self.active_sweep.start_u8, + "target_u8": self.active_sweep.target_u8, + "current_motion_target_u8": ( + int( + getattr( + self, + "preparation_command_u8", + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.start_u8, + baseline=self.baseline_command, + profile=self.profile, + ), + )[motor] + ) + if self.state == STATE_PREPARE_SWEEP + else int(self.baseline_command[motor]) + if getattr(self, "sweep_baseline_pending", False) + else int( + self.sweep_checkpoint_target_u8 + if getattr(self, "sweep_checkpoint_target_u8", None) + is not None + else self.active_sweep.target_u8 + ) + ), + "steady_checkpoint_mode": bool( + getattr(self, "sweep_checkpoint_mode", False) + ), + "remaining_steady_checkpoints": len( + getattr(self, "sweep_checkpoint_commands", ()) + ), + "baseline_hold_pending": bool( + getattr(self, "sweep_baseline_pending", False) + ), + "baseline_hold_valid_frames": min( + baseline_frame_counts.values(), default=0 + ), + "minimum_baseline_hold_frames": int( + getattr(self, "minimum_baseline_hold_frames", 10) + ), + "actual_u8": actual, + "motion_progress": motion_progress, + "valid_frames": min(frame_counts.values(), default=0), + "valid_frames_by_joint": frame_counts, + "detection_frames": int( + getattr(self, "sweep_detection_total_frames", 0) + ), + "detection_valid_frames": int( + getattr(self, "sweep_detection_valid_frames", 0) + ), + "detection_rate": min( + detection_rates_by_view.values(), default=0.0 + ), + "detection_rate_by_view": detection_rates_by_view, + "sample": { + "minimum_u8": None if not values else min(values), + "maximum_u8": None if not values else max(values), + "span_u8": 0.0 if not values else max(values) - min(values), + "bin_count": len(bins), + "minimum_bin_count": self.minimum_sweep_bins, + "maximum_bin_gap": int(maximum_bin_gap), + "maximum_bin_gap_start_u8": gap_start_u8, + "maximum_bin_gap_end_u8": gap_end_u8, + "allowed_maximum_bin_gap": self.maximum_bin_gap, + "missing_endpoint_u8": missing_endpoints, + "endpoint_tolerance_u8": max( + start_tolerance, target_tolerance + ), + "start_endpoint_tolerance_u8": start_tolerance, + "target_endpoint_tolerance_u8": target_tolerance, + "mechanical_endpoint_tolerance_u8": max( + mechanical_start_tolerance, + mechanical_target_tolerance, + ), + "synchronised_endpoint_tolerance_margin_u8": float( + getattr( + self, + "synchronised_endpoint_tolerance_margin_u8", + 1.0, + ) + ), + }, + "auxiliary_motors": [ + { + "motor_index": index, + "command_u8": command, + "actual_u8": ( + None + if len(self.latest_state_u8) != 20 + else float(self.latest_state_u8[index]) + ), + } + for index, command in calibration_auxiliary_commands( + self.active_sweep.spec, profile=self.profile + ).items() + ], + "speed": { + "commanded_finger_speed": self._speed_profile_for_spec( + self.active_sweep.spec + ), + "reported_finger_speed": self.latest_hand_info.get("speed"), + "normal_speed": self.normal_calibration_speed, + "index_roll_speed": self.index_roll_calibration_speed, + "index_flex_speed": self.index_flex_calibration_speed, + "adaptive_formal_speed_scale": float( + self.formal_speed_scales.get( + self.active_sweep.spec.key, 1.0 + ) + ), + }, + } + elif self.active_validation is not None: + motor = self.active_validation.spec.motor_index + active = { + "kind": "validation", + "view": self.active_validation.spec.view, + "motor_index": motor, + "command_u8": self.active_validation.command_u8, + "actual_u8": ( + None + if len(self.latest_state_u8) != 20 + else float(self.latest_state_u8[motor]) + ), + "valid_frames": len(self.validation_frames_buffer), + "auxiliary_motors": [ + { + "motor_index": index, + "command_u8": command, + "actual_u8": ( + None + if len(self.latest_state_u8) != 20 + else float(self.latest_state_u8[index]) + ), + } + for index, command in calibration_auxiliary_commands( + self.active_validation.spec, profile=self.profile + ).items() + ], + "speed": { + "commanded_finger_speed": self._speed_profile_for_spec( + self.active_validation.spec + ), + "reported_finger_speed": self.latest_hand_info.get("speed"), + "normal_speed": self.normal_calibration_speed, + "index_roll_speed": self.index_roll_calibration_speed, + "index_flex_speed": self.index_flex_calibration_speed, + "adaptive_formal_speed_scale": float( + self.formal_speed_scales.get( + self.active_validation.spec.key, 1.0 + ) + ), + }, + } + elif self.fit_failure: + active = dict(self.fit_failure) + group_pnp_reasons = { + name: runtime.latest_group_pnp_reason + for name, runtime in self.views.items() + if runtime.latest_group_pnp_reason + } + if group_pnp_reasons: + active = dict(active) + active["group_pnp_reasons"] = group_pnp_reasons + pnp_rejection_counts = { + name: dict(runtime.pnp_rejection_counts) + for name, runtime in self.views.items() + if runtime.pnp_rejection_counts + } + if pnp_rejection_counts: + active = dict(active) + active["pnp_rejection_counts"] = pnp_rejection_counts + group_pnp_rejection_counts = { + name: dict(runtime.group_pnp_rejection_counts) + for name, runtime in self.views.items() + if runtime.group_pnp_rejection_counts + } + if group_pnp_rejection_counts: + active = dict(active) + active["group_pnp_rejection_counts"] = ( + group_pnp_rejection_counts + ) + pnp_initialization_progress = { + name: { + "accepted": runtime.pnp_initialization_progress[0], + "required": runtime.pnp_initialization_progress[1], + } + for name, runtime in self.views.items() + if runtime.pnp_initialization_progress is not None + } + if pnp_initialization_progress: + active = dict(active) + active["pnp_initialization_progress"] = ( + pnp_initialization_progress + ) + resumed_tasks = set(self.resumed_task_keys) + completed_sweep_count = sum( + 1 + for index, item in enumerate(self.sweep_items) + if index < self.sweep_index or item.spec.key in resumed_tasks + ) + scan_progress = ( + 0.0 + if not self.sweep_items + else float( + np.clip( + completed_sweep_count / len(self.sweep_items), 0.0, 1.0 + ) + ) + ) + status_validation_index = ( + self.combination_validation_index + if self.combination_validation_items + else self.validation_index + ) + status_validation_total = ( + len(self.combination_validation_items) + if self.combination_validation_items + else len(self.validation_items) + ) + payload = { + "serial_number": self.serial_number, + "session_dir": str(self.session_dir), + "hand_type": self.hand_type, + "reference_finger": self.profile.reference_finger, + "state": self.state, + "reason": self.reason, + "preflight_mode": ( + "recovering_baseline" + if self.state == STATE_RETURN_BASELINE + and self.baseline_after == "startup_tag_preflight" + else ( + "device_only_before_baseline" + if self.state in {STATE_PREFLIGHT, STATE_WAIT_START} + and not self.started + else ( + "baseline_tags_after_recovery" + if self.state == STATE_PREFLIGHT + and self.started + and self.startup_baseline_recovered + else "" + ) + ) + ), + "progress": round( + _overall_progress( + self.state, + scan_progress=scan_progress, + validation_index=status_validation_index, + validation_total=status_validation_total, + ), + 4, + ), + "scan_progress": round(scan_progress, 4), + "completed_sweeps": self.sweep_index, + "total_sweeps": len(self.sweep_items), + "baseline_command_u8": list(self.baseline_command), + "return_command_u8": list( + G20ThreeCameraCalibrationNode._current_return_command(self) + ), + "hand_state_fresh": bool( + len(self.latest_state_u8) == 20 + and now - self.last_state_at <= 1.0 + ), + "feedback_hz": round( + ( + 0.0 + if len(self.state_receive_times) < 2 + or self.state_receive_times[-1] + <= self.state_receive_times[0] + else (len(self.state_receive_times) - 1) + / ( + self.state_receive_times[-1] + - self.state_receive_times[0] + ) + ), + 2, + ), + "views": views, + "active": active, + "result_path": ( + str(self.final_path) if self.completed_payload is not None else "" + ), + "corrected_urdf_path": ( + "" + if self.corrected_urdf_path is None + else str(self.corrected_urdf_path) + ), + "camera_extrinsics_error": self.extrinsics_error, + "resume": { + "used": bool(self.resumed_task_keys), + "source_session": self.resume_source_session, + "recalibration_scope": self.recalibration_scope, + "recalibration_task_keys": list( + self.recalibration_task_keys + ), + "completed_task_count": len(self.resumed_task_keys), + "total_task_count": len(self.profile.sweep_specs), + "completed_task_keys": list(self.resumed_task_keys), + }, + "base_import": dict( + getattr(self, "base_import_progress", {}) + ), + "quality": ( + {} if self.completed_payload is None else self.completed_payload["quality"] + ), + "thumb_yaw_cross_session": dict( + getattr(self, "thumb_yaw_cross_session_diagnostic", {}) + ), + "combination_validation": { + "enabled": bool(self.combination_validation_enabled), + "completed": bool(self.combination_validation_completed), + "completed_poses": int(self.combination_validation_index), + "total_poses": len(self.combination_validation_items), + "position_p95_m": ( + None + if not self.combination_position_errors_m + else round( + float( + np.percentile( + self.combination_position_errors_m, 95.0 + ) + ), + 8, + ) + ), + "orientation_p95_rad": ( + None + if not self.combination_orientation_errors_rad + else round( + float( + np.percentile( + self.combination_orientation_errors_rad, 95.0 + ) + ), + 8, + ) + ), + **combination_target_coverage( + self.combination_tag_observation_counts, + self.combination_tag_validation_counts, + ), + }, + } + status = String() + status.data = json.dumps(payload, ensure_ascii=False) + self.status_publisher.publish(status) + text = String() + text.data = render_three_camera_status_text_zh(payload) + self.status_text_publisher.publish(text) + + +def main(args: list[str] | None = None) -> None: + configure_fastdds_large_image_transport() + rclpy.init(args=args) + node: G20ThreeCameraCalibrationNode | None = None + try: + node = G20ThreeCameraCalibrationNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py new file mode 100644 index 0000000..7f7a9ef --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/offline_replay.py @@ -0,0 +1,2032 @@ +"""Safely replay a complete three-camera session without moving the hand.""" + +from __future__ import annotations + +import argparse +from collections import defaultdict +from dataclasses import replace +import hashlib +import json +import math +import os +from pathlib import Path +import re +import tempfile +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation +import yaml + +from ...compat.legacy import uses_coupled_full_hand_zero_solver +from ...extrinsics import load_three_camera_extrinsics +from .profile import ( + G20_REFERENCE_THUMB_CMC_JOINTS, + G20_RIGHT_19_LAYOUT, + RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, + HandCalibrationProfile, + JointCurveFit, + build_calibration_motion_command, + build_compact_payload, + canonical_zero_direction, + clamp_runtime_fits_to_urdf_limits, + cross_view_roll_diagnostic_metrics, + derive_mimic_passive_fits, + fit_joint_image_curve, + get_hand_calibration_profile, + validate_compact_payload, +) +from ...product import get_product_calibration_contract +from ...sample_schema import fitting_sample_record, fitting_sample_records +from ...storage import atomic_write_json +from .publication import clamp_compact_payload_to_urdf_limits +from .zero_solver import ( + JointAxisMeasurement, + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, + UrdfKinematicModel, + _angles_from_state, + anchor_right_19_mechanical_endpoint_curves, + axis_line_uses_depth_free_interpretation_plane, + axis_line_cycle_rms_m, + baseline_hysteresis_by_cycle_rad, + circle_direction_is_constrained, + cross_view_side_line_source, + fit_joint_axis_measurement, + fit_partial_palm_orientation_measurements, + fit_rotation_joint_curve, + get_zero_calibration_profile, + get_right_19_thumb_zero_profile, + derive_right_19_mechanical_endpoint_offsets, + joint_curve_holdout_errors, + refit_axis_line_group_with_shared_radius, + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, + RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, + merge_right_19_thumb_zero_result, + select_cross_view_roll_direction_source, + solve_urdf_zero_offsets, + with_depth_free_axis_projection, + write_zero_corrected_urdf, +) + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _payload_sha256(payload: Mapping[str, Any]) -> str: + encoded = ( + json.dumps(payload, ensure_ascii=False, indent=2) + "\n" + ).encode("utf-8") + return hashlib.sha256(encoded).hexdigest() + + +def _legacy_published_quality( + session: Path, *, side: str, serial_number: str +) -> dict[str, Any]: + """Recover v1 random-validation aggregates that raw JSONL omitted.""" + path = session / f"g20_{side}_{serial_number}_calibration.json" + if not path.is_file(): + raise ValueError( + "legacy replay requires its published JSON because v1 raw " + "samples omitted random-validation observations" + ) + payload = json.loads(path.read_text(encoding="utf-8")) + quality = payload.get("quality") + if not isinstance(quality, Mapping) or set(quality) != { + "passed", + "validation_mae_rad", + "validation_p95_rad", + }: + raise ValueError("legacy published JSON has an invalid quality block") + if not bool(quality["passed"]): + raise ValueError("legacy published JSON did not pass validation") + values = ( + float(quality["validation_mae_rad"]), + float(quality["validation_p95_rad"]), + ) + if any(not math.isfinite(value) or value < 0.0 for value in values): + raise ValueError("legacy published quality contains an invalid error") + return dict(quality) + + +def _output_suffix(output_tag: str | None) -> str: + """Return a filename-safe suffix for a non-destructive replay variant.""" + if output_tag is None: + return "" + tag = str(output_tag) + if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag): + raise ValueError( + "output tag must be 1-64 filename-safe characters, beginning " + "with a letter or digit" + ) + return f"_{tag}" + + +def _load_parameters(path: Path) -> dict[str, Any]: + payload = yaml.safe_load(path.read_text(encoding="utf-8")) + return dict(payload["g20_calibration"]["ros__parameters"]) + + +def _steady_records_in_feedback_domain( + records: Sequence[Mapping[str, Any]], +) -> list[dict[str, Any]]: + """Mirror the live mechanical-backlash domain for offline replay.""" + return fitting_sample_records( + records, + domain="feedback", + allow_legacy_command=True, + snap_requested_endpoints=True, + ) + + +def _latest_attempt_records( + rows: Sequence[Mapping[str, Any]], + *, + kind: str = "sample", +) -> dict[str, list[dict[str, Any]]]: + """Reproduce the online retry buffer from append-only raw samples. + + Online retry clears only the failed joint/cycle/direction from memory, + while JSONL deliberately retains every attempt for audit. Offline replay + must therefore select the greatest attempt independently for each logical + trajectory rather than mixing rejected attempts into the final fit. + """ + samples = [dict(row) for row in rows if row.get("kind") == kind] + latest_attempt: dict[tuple[str, int, str], int] = {} + for row in samples: + key = ( + str(row["joint"]), + int(row["cycle"]), + str(row["direction"]), + ) + latest_attempt[key] = max( + latest_attempt.get(key, 0), int(row.get("attempt", 1)) + ) + records: dict[str, list[dict[str, Any]]] = defaultdict(list) + for row in samples: + key = ( + str(row["joint"]), + int(row["cycle"]), + str(row["direction"]), + ) + if int(row.get("attempt", 1)) == latest_attempt[key]: + records[key[0]].append(row) + return dict(records) + + +def _latest_palm_axis_records( + rows: Sequence[Mapping[str, Any]], +) -> dict[str, list[dict[str, Any]]]: + """Select the latest accepted append-only palm side-channel attempts.""" + samples = [ + dict(row) for row in rows if row.get("kind") == "palm_axis_sample" + ] + latest_attempt: dict[tuple[str, int, str], int] = {} + for row in samples: + key = ( + str(row["source_joint"]), + int(row["cycle"]), + str(row["direction"]), + ) + latest_attempt[key] = max( + latest_attempt.get(key, 0), int(row.get("attempt", 1)) + ) + records: dict[str, list[dict[str, Any]]] = defaultdict(list) + for row in samples: + key = ( + str(row["source_joint"]), + int(row["cycle"]), + str(row["direction"]), + ) + if int(row.get("attempt", 1)) == latest_attempt[key]: + records[key[0]].append(row) + return dict(records) + + +def _load_raw_session( + session_dir: Path, +) -> tuple[ + dict[str, Any], + dict[str, list[dict[str, Any]]], + dict[str, list[dict[str, Any]]], + dict[str, list[dict[str, Any]]], + dict[str, list[dict[str, Any]]], + Path, +]: + raw_path = session_dir / "raw_samples.jsonl" + if not raw_path.is_file(): + raise ValueError(f"raw session does not exist: {raw_path}") + rows = [ + json.loads(line) + for line in raw_path.read_text(encoding="utf-8").splitlines() + if line.strip() + ] + # Both online fitting and replay use the same canonical-to-fit projection. + # raw_samples.jsonl itself never contains the ambiguous compatibility key. + for row in rows: + if row.get("kind") in { + "sample", + "baseline_hold_sample", + "steady_command_sample", + "palm_axis_sample", + }: + projected = fitting_sample_record( + row, allow_legacy_command=True + ) + row.clear() + row.update(projected) + starts = [row for row in rows if row.get("kind") == "session_start"] + if len(starts) != 1: + raise ValueError("raw session must contain exactly one session_start") + return ( + starts[0], + _latest_attempt_records(rows), + _latest_attempt_records(rows, kind="baseline_hold_sample"), + _latest_attempt_records(rows, kind="steady_command_sample"), + _latest_palm_axis_records(rows), + raw_path, + ) + + +def _fit_curve( + name: str, + records: Sequence[Mapping[str, Any]], + *, + profile: HandCalibrationProfile, + baseline: Sequence[int], +) -> JointCurveFit: + motor = profile.record_specs[name].motor_index + if ( + profile.layout_id == G20_RIGHT_19_LAYOUT + and ( + name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS + or ( + name.endswith("_mcp_roll") + and not name.startswith("thumb_") + ) + ) + ): + return fit_joint_image_curve(records) + return fit_rotation_joint_curve( + records, + zero_command_u8=int(baseline[motor]), + canonical_zero_direction=canonical_zero_direction(profile, name), + ) + + +def _physical_rotation_axis_for_curve( + name: str, + records: Sequence[Mapping[str, Any]], + fit: JointCurveFit, + *, + profile: HandCalibrationProfile, + baseline: Sequence[int], +) -> Sequence[float]: + """Keep baseline pose hysteresis independent of the dynamic curve space.""" + axis = fit.circle.get("axis_xyz") + if axis is not None: + return axis + motor = profile.record_specs[name].motor_index + diagnostic_fit = fit_rotation_joint_curve( + records, + zero_command_u8=int(baseline[motor]), + canonical_zero_direction=canonical_zero_direction(profile, name), + ) + return diagnostic_fit.circle["axis_xyz"] + + +def _fit_axes( + *, + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + profile: HandCalibrationProfile, + baseline: Sequence[int], + extrinsics_file: Path, + source_urdf: Path, + repetitions: int, +) -> list[JointAxisMeasurement]: + zero_profile = get_zero_calibration_profile( + profile.side, profile.layout_id + ) + extrinsics = load_three_camera_extrinsics(extrinsics_file) + cache: dict[tuple[str, int], JointAxisMeasurement] = {} + cross_view_direction_source_cache: dict[str, str] = {} + upstream_by_joint = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + **{ + f"{finger}_pip": f"{finger}_mcp_pitch" + for finger in ("index", "middle", "ring", "pinky") + }, + **{ + f"{finger}_dip": f"{finger}_pip" + for finger in ("index", "middle", "ring", "pinky") + }, + } + if profile.layout_id == G20_RIGHT_19_LAYOUT: + reference_roll = f"{profile.reference_finger}_mcp_roll" + upstream_by_joint.update( + { + f"{finger}_mcp_roll": reference_roll + for finger in ("index", "middle", "ring", "pinky") + if f"{finger}_mcp_roll" != reference_roll + } + ) + + def fit_raw(name: str, cycle: int) -> JointAxisMeasurement: + key = (name, cycle) + if key in cache: + return cache[key] + upstream = upstream_by_joint.get(name) + constraint = None if upstream is None else fit_raw(upstream, cycle).axis_common_xyz + spec = profile.record_specs[name] + view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray( + [0.0, 0.0, 1.0], dtype=float + ) + result = fit_joint_axis_measurement( + name, + records_by_joint[name], + cycle=cycle, + zero_command_u8=int(baseline[spec.motor_index]), + axis_common_constraint=constraint, + constrained_circle_joints=( + zero_profile.constrained_circle_joints + | ({name} if name.endswith("_side") else set()) + ), + view_normal_common_xyz=view_normal, + canonical_zero_direction=canonical_zero_direction(profile, name), + ) + sweep_spec = next( + sweep for sweep in profile.sweep_specs if name in sweep.joints + ) + condition = build_calibration_motion_command( + sweep_spec, + int(baseline[spec.motor_index]), + baseline=baseline, + profile=profile, + ) + result = replace( + result, + condition_command_u8=( + None + if profile.layout_id == G20_RIGHT_19_LAYOUT + else tuple(float(value) for value in condition) + ), + view_normal_common_xyz=tuple(float(value) for value in view_normal), + ) + result = with_depth_free_axis_projection( + result, extrinsics.transform(spec.view)[:3, 3] + ) + cache[key] = result + return result + + def fit_one(name: str, cycle: int) -> JointAxisMeasurement: + primary = fit_raw(name, cycle) + validation_name = (profile.axis_validation_sources or {}).get(name) + if validation_name is None: + return primary + secondary = fit_raw(validation_name, cycle) + primary_axis = np.asarray(primary.axis_common_xyz, dtype=float) + secondary_axis = np.asarray(secondary.axis_common_xyz, dtype=float) + if float(primary_axis @ secondary_axis) < 0.0: + secondary_axis = -secondary_axis + difference = math.acos( + float(np.clip(primary_axis @ secondary_axis, -1.0, 1.0)) + ) + primary_point = np.asarray(primary.point_common_xyz_m, dtype=float) + secondary_point = np.asarray(secondary.point_common_xyz_m, dtype=float) + line_distance = float( + np.linalg.norm( + np.cross(secondary_point - primary_point, primary_axis) + ) + ) + if ( + profile.record_specs[validation_name].zero_kind + == "axis_cross_view_validation" + ): + point_view = profile.record_specs[name].view + if point_view is None: + raise ValueError( + f"{name}: primary point source has no view" + ) + point_camera_center = extrinsics.transform(point_view)[:3, 3] + point_ray = primary_point - point_camera_center + interpretation_plane_normal = np.cross( + point_ray, primary_axis + ) + interpretation_plane_normal /= np.linalg.norm( + interpretation_plane_normal + ) + return replace( + primary, + point_common_xyz_m=tuple( + float(value) + for value in primary.point_common_xyz_m + ), + pose_axis_line_rms_m=primary.pose_axis_line_rms_m, + pose_axis_line_source_joints=(name,), + axis_point_source=( + "front_interpretation_plane_cross_view_validated" + ), + axis_point_camera_center_common_xyz_m=tuple( + float(value) + for value in point_camera_center + ), + axis_point_interpretation_plane_normal_common_xyz=tuple( + float(value) + for value in interpretation_plane_normal + ), + ) + # Mirror the live node's tiered policy: the side-view planar-tag + # IPPE bias makes sub-degree cross-view agreement unreachable, so + # disagreement above the fusion gate keeps the front-only axis while + # only a gross direction error (wrong link / loose tag) still fails. + # The front screw-driven roll trajectory yields a displaced + # pseudo-line, so its distance to the side physical line is retained + # only as a diagnostic. The live node likewise keeps only the gross + # direction gate here and validates side-line quality independently. + if difference > math.radians(15.0): + raise ValueError(f"{name}: cross-view axis gross disagreement") + if difference > math.radians(0.75) or line_distance > 0.001: + selected_direction = primary_axis + selected_source = primary.axis_direction_source + observer_name = next( + ( + observer + for observer, parent in zero_profile.axis_parent_joint.items() + if parent == name + ), + None, + ) + if observer_name is not None: + model = UrdfKinematicModel(source_urdf) + parent_axis, _ = model.axis_line( + name, zero_offsets={}, joint_angles={} + ) + observer_axis, _ = model.axis_line( + observer_name, zero_offsets={}, joint_angles={} + ) + expected_cone = math.acos( + abs(float(np.clip(parent_axis @ observer_axis, -1.0, 1.0))) + ) + def cone_residual( + candidate: np.ndarray, measured_observer: np.ndarray + ) -> float: + measured_cone = math.acos( + abs( + float( + np.clip( + candidate @ measured_observer, -1.0, 1.0 + ) + ) + ) + ) + return abs(measured_cone - expected_cone) + + if name not in cross_view_direction_source_cache: + primary_residuals: list[float] = [] + secondary_residuals: list[float] = [] + for group_cycle in range(repetitions): + group_observer = np.asarray( + fit_raw( + observer_name, group_cycle + ).axis_common_xyz, + dtype=float, + ) + primary_residuals.append( + cone_residual( + np.asarray( + fit_raw( + name, group_cycle + ).axis_common_xyz, + dtype=float, + ), + group_observer, + ) + ) + secondary_residuals.append( + cone_residual( + np.asarray( + fit_raw( + validation_name, group_cycle + ).axis_common_xyz, + dtype=float, + ), + group_observer, + ) + ) + cross_view_direction_source_cache[name] = ( + select_cross_view_roll_direction_source( + primary_residuals, + secondary_residuals, + math.radians(5.0), + ) + ) + if cross_view_direction_source_cache[name] == "secondary": + selected_direction = secondary_axis + selected_source = "cross_view_cone_selected_secondary" + # Mirror the live node: use the view whose direction satisfies + # the zero-invariant parent/child cone, and take the physical line + # position from the side PIP-link circle. + return replace( + primary, + axis_common_xyz=tuple( + float(value) for value in selected_direction + ), + point_common_xyz_m=tuple( + float(value) + for value in secondary.point_common_xyz_m + ), + pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, + pose_axis_line_source_joints=(validation_name,), + axis_direction_source=selected_source, + axis_point_source="side_circle_cross_view", + ) + primary_variance = max( + primary.radial_rms_m ** 2 + primary.pose_axis_line_rms_m ** 2, + 1.0e-12, + ) + secondary_variance = max( + secondary.radial_rms_m ** 2 + secondary.pose_axis_line_rms_m ** 2, + 1.0e-12, + ) + primary_weight = 1.0 / primary_variance + secondary_weight = 1.0 / secondary_variance + fused_axis = ( + primary_weight * primary_axis + secondary_weight * secondary_axis + ) + fused_axis /= np.linalg.norm(fused_axis) + delta = secondary_point - primary_point + perpendicular = delta - fused_axis * float(delta @ fused_axis) + fused_point = primary_point + ( + secondary_weight / (primary_weight + secondary_weight) + ) * perpendicular + return replace( + primary, + axis_common_xyz=tuple(float(value) for value in fused_axis), + point_common_xyz_m=tuple(float(value) for value in fused_point), + pose_axis_line_rms_m=max( + primary.pose_axis_line_rms_m, + secondary.pose_axis_line_rms_m, + line_distance, + ), + pose_axis_line_source_joints=(name, validation_name), + axis_direction_source="cross_view_weighted_fusion", + ) + + result: list[JointAxisMeasurement] = [] + for name in zero_profile.axis_joints: + group = [fit_one(name, cycle) for cycle in range(repetitions)] + side_sources = { + source + for measurement in group + if (source := cross_view_side_line_source(measurement)) is not None + } + if ( + len(side_sources) == 1 + and all( + cross_view_side_line_source(measurement) is not None + for measurement in group + ) + and not any( + axis_line_uses_depth_free_interpretation_plane(measurement) + for measurement in group + ) + ): + source = next(iter(side_sources)) + source_spec = profile.record_specs[source] + group = list( + refit_axis_line_group_with_shared_radius( + group, + records_by_joint[source], + zero_command_u8=int( + baseline[source_spec.motor_index] + ), + canonical_zero_direction=canonical_zero_direction( + profile, source + ), + ) + ) + result.extend(group) + return result + + +def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float: + maximum = 0.0 + for left in axes: + for right in axes: + maximum = max( + maximum, + math.acos( + abs( + float( + np.clip( + np.asarray(left, dtype=float) + @ np.asarray(right, dtype=float), + -1.0, + 1.0, + ) + ) + ) + ), + ) + return maximum + + +def _quality_failures( + *, + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + baseline_records_by_joint: Mapping[ + str, Sequence[Mapping[str, Any]] + ] | None = None, + profile: HandCalibrationProfile, + baseline: Sequence[int], + fits: Mapping[str, JointCurveFit], + axes: Sequence[JointAxisMeasurement], + parameters: Mapping[str, Any], +) -> list[str]: + failures: list[str] = [] + repetitions = int(parameters["repetitions"]) + zero_profile = get_zero_calibration_profile( + profile.side, profile.layout_id + ) + axis_by_key = {(item.joint, item.cycle): item for item in axes} + expected_directions = {"decreasing", "increasing"} + for name in profile.measured_joints: + records = list(records_by_joint.get(name, ())) + if not records: + failures.append(f"{name}: no samples") + continue + attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set) + for record in records: + attempts_by_direction[ + (int(record["cycle"]), str(record["direction"])) + ].add(int(record.get("attempt", 1))) + for cycle in range(repetitions): + for direction in expected_directions: + selected = [ + record + for record in records + if int(record["cycle"]) == cycle + and str(record["direction"]) == direction + ] + attempts = attempts_by_direction.get((cycle, direction), set()) + if len(attempts) != 1: + failures.append( + f"{name} cycle {cycle + 1} {direction}: ambiguous attempts {sorted(attempts)}" + ) + continue + commands = sorted({int(record["command_u8"]) for record in selected}) + if len(selected) < int(parameters["minimum_sweep_frames"]): + failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames") + if not commands or max(commands) - min(commands) < float( + parameters["minimum_state_span_u8"] + ): + failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span") + if len(commands) < int(parameters["minimum_sweep_bins"]): + failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins") + if 0 not in commands or 255 not in commands: + failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing") + if commands and max(np.diff(commands), default=0) > int( + parameters["maximum_bin_gap"] + ): + failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap") + + sync_p95 = float( + np.percentile( + [float(record.get("state_image_sync_error_ms", 0.0)) for record in records], + 95.0, + ) + ) + if sync_p95 > float(parameters["maximum_state_image_skew_ms"]): + failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms") + spec = profile.joint_specs[name] + fit = fits[name] + if fit.circle.get("space") == "image_2d": + if float(fit.quality["radial_rms_px"]) > float( + parameters["image_trajectory_maximum_radial_rms_px"] + ): + failures.append(f"{name}: image radial RMS") + if float(fit.quality["radial_p95_px"]) > float( + parameters["image_trajectory_maximum_radial_p95_px"] + ): + failures.append(f"{name}: image radial P95") + if float(fit.quality["radius_px"]) < float( + parameters["image_trajectory_minimum_radius_px"] + ): + failures.append(f"{name}: image radius") + else: + orthogonal_limit = math.radians( + float( + parameters[ + "active_maximum_rotation_orthogonal_rms_deg" + if spec.active + else "passive_maximum_rotation_orthogonal_rms_deg" + ] + ) + ) + if ( + float(fit.quality["rotation_orthogonal_rms_rad"]) + > orthogonal_limit + ): + failures.append(f"{name}: rotation orthogonal RMS") + if float(fit.quality["arc_rad"]) < math.radians( + float(parameters["trajectory_minimum_arc_deg"]) + ): + failures.append(f"{name}: trajectory arc") + monotonic_limit = math.radians( + float( + parameters[ + "maximum_monotonic_correction_deg" + if spec.active + else "passive_maximum_monotonic_correction_deg" + ] + ) + ) + hysteresis_limit = math.radians( + float( + parameters[ + "maximum_hysteresis_deg" + if spec.active + else "passive_maximum_hysteresis_deg" + ] + ) + ) + if fit.maximum_monotonic_correction_rad > monotonic_limit: + failures.append(f"{name}: monotonic correction") + if ( + profile.layout_id != G20_RIGHT_19_LAYOUT + and fit.maximum_hysteresis_rad > hysteresis_limit + ): + failures.append(f"{name}: hysteresis") + if profile.layout_id == G20_RIGHT_19_LAYOUT: + hysteresis_records = records + dedicated = (baseline_records_by_joint or {}).get(name, ()) + if int(baseline[spec.motor_index]) not in {0, 255} and dedicated: + hysteresis_records = list(dedicated) + baseline_hysteresis = baseline_hysteresis_by_cycle_rad( + hysteresis_records, + zero_command_u8=int(baseline[spec.motor_index]), + axis_xyz=_physical_rotation_axis_for_curve( + name, + records, + fit, + profile=profile, + baseline=baseline, + ), + ) + if canonical_zero_direction(profile, name) is not None: + gap_limit = math.radians( + float( + parameters.get( + "directional_zero_maximum_branch_gap_deg", 1.5 + ) + ) + ) + gap_range_limit = math.radians( + float( + parameters.get( + "directional_zero_maximum_branch_gap_range_deg", + 0.3, + ) + ) + ) + if max(baseline_hysteresis) > gap_limit: + failures.append(f"{name}: baseline directional gap") + if ( + max(baseline_hysteresis) - min(baseline_hysteresis) + > gap_range_limit + and spec.zero_kind != "axis_cross_view_validation" + ): + failures.append( + f"{name}: baseline directional gap repeatability" + ) + elif max(baseline_hysteresis) > math.radians( + float(parameters.get("baseline_maximum_hysteresis_deg", 0.5)) + ): + failures.append(f"{name}: baseline hysteresis") + + cycle_travels: list[float] = [] + cycle_axes: list[Sequence[float]] = [] + cycle_axis_sources: list[str] = [] + cycle_axis_measurements: list[JointAxisMeasurement] = [] + for cycle in range(repetitions): + cycle_fit = _fit_curve( + name, + [record for record in records if int(record["cycle"]) == cycle], + profile=profile, + baseline=baseline, + ) + cycle_travels.append( + abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255])) + ) + axis = axis_by_key[(name, cycle)] + cycle_axes.append(axis.axis_common_xyz) + cycle_axis_sources.append(axis.axis_direction_source) + cycle_axis_measurements.append(axis) + if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]): + failures.append(f"{name} cycle {cycle + 1}: radial RMS") + if ( + profile.record_specs[name].zero_kind + != "axis_cross_view_validation" + and cross_view_side_line_source(axis) is None + and axis.pose_axis_line_rms_m + > float(parameters["axis_maximum_pose_line_rms_m"]) + ): + failures.append( + f"{name} cycle {cycle + 1}: pose axis-line RMS" + ) + if not circle_direction_is_constrained( + name, zero_profile.constrained_circle_joints + ): + plane_limit = float( + parameters[ + "axis_maximum_plane_rms_m" + if spec.active + else "passive_axis_maximum_plane_rms_m" + ] + ) + if axis.plane_rms_m > plane_limit: + failures.append(f"{name} cycle {cycle + 1}: plane RMS") + if ( + axis.rotation_circle_axis_difference_rad > math.radians( + float(parameters["axis_maximum_rotation_circle_difference_deg"]) + ) + ): + failures.append(f"{name} cycle {cycle + 1}: axis disagreement") + side_line_sources = { + source + for measurement in cycle_axis_measurements + if ( + source := cross_view_side_line_source(measurement) + ) is not None + } + if ( + len(cycle_axis_measurements) == repetitions + and len(side_line_sources) == 1 + and all( + cross_view_side_line_source(measurement) is not None + for measurement in cycle_axis_measurements + ) + and not any( + axis_line_uses_depth_free_interpretation_plane(measurement) + for measurement in cycle_axis_measurements + ) + and axis_line_cycle_rms_m(cycle_axis_measurements) + > float(parameters["axis_maximum_pose_line_rms_m"]) + ): + source = next(iter(side_line_sources)) + failures.append(f"{source}: axis-line cycle RMS") + travel_limit = math.radians( + float( + parameters[ + "trajectory_maximum_cycle_travel_difference_deg" + if spec.active + else "passive_maximum_cycle_travel_difference_deg" + ] + ) + ) + if max(cycle_travels) - min(cycle_travels) > travel_limit: + failures.append(f"{name}: cycle travel difference") + if ( + not all( + source == "upstream_constraint" + for source in cycle_axis_sources + ) + and spec.zero_kind != "axis_cross_view_validation" + and _maximum_undirected_axis_difference(cycle_axes) > math.radians( + float(parameters["zero_maximum_axis_cycle_difference_deg"]) + ) + ): + failures.append(f"{name}: cycle axis difference") + return failures + + +def _joint_xml(path: Path) -> dict[str, ET.Element]: + return { + str(joint.get("name")): joint + for joint in ET.parse(path).getroot().findall("joint") + } + + +def _triplet(value: str) -> np.ndarray: + return np.asarray([float(item) for item in value.split()], dtype=float) + + +def _validate_corrected_urdf( + *, + source: Path, + corrected: Path, + offsets: Mapping[str, float], + endpoint_anchored_offsets: Mapping[str, float], + axes: Sequence[JointAxisMeasurement], + curves: Mapping[str, JointCurveFit], + motor_by_joint: Mapping[str, int], + inherited_zero_joints: Mapping[str, str], +) -> dict[str, float]: + source_joints = _joint_xml(source) + corrected_joints = _joint_xml(corrected) + if set(source_joints) != set(corrected_joints): + raise ValueError("corrected URDF changed the joint set") + maximum_origin_rotation_error = 0.0 + maximum_origin_translation_error = 0.0 + for name, original_joint in source_joints.items(): + corrected_joint = corrected_joints[name] + original_origin = original_joint.find("origin") + corrected_origin = corrected_joint.find("origin") + if original_origin is None or corrected_origin is None: + continue + original_xyz = _triplet(original_origin.get("xyz", "0 0 0")) + corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0")) + maximum_origin_translation_error = max( + maximum_origin_translation_error, + float(np.linalg.norm(corrected_xyz - original_xyz)), + ) + original_rotation = Rotation.from_euler( + "xyz", _triplet(original_origin.get("rpy", "0 0 0")) + ) + corrected_rotation = Rotation.from_euler( + "xyz", _triplet(corrected_origin.get("rpy", "0 0 0")) + ) + expected = original_rotation + if name in offsets: + axis_node = original_joint.find("axis") + axis = _triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + axis /= np.linalg.norm(axis) + expected = original_rotation * Rotation.from_rotvec( + axis * float(offsets[name]) + ) + error = float((expected.inv() * corrected_rotation).magnitude()) + maximum_origin_rotation_error = max(maximum_origin_rotation_error, error) + original_limit = original_joint.find("limit") + corrected_limit = corrected_joint.find("limit") + if original_limit is not None and corrected_limit is not None: + expected_lower = float(original_limit.get("lower")) + expected_upper = float(original_limit.get("upper")) + if name in endpoint_anchored_offsets: + expected_upper -= float(endpoint_anchored_offsets[name]) + if ( + abs(float(corrected_limit.get("lower")) - expected_lower) > 1.0e-12 + or abs(float(corrected_limit.get("upper")) - expected_upper) > 1.0e-12 + ): + raise ValueError(f"corrected URDF unexpectedly changed {name} limits") + original_mimic = original_joint.find("mimic") + corrected_mimic = corrected_joint.find("mimic") + if original_mimic is not None and corrected_mimic is not None: + source_joint = str(original_mimic.get("joint")) + multiplier = float(original_mimic.get("multiplier", "1")) + expected_mimic_offset = float(original_mimic.get("offset", "0")) + expected_mimic_offset += multiplier * float( + endpoint_anchored_offsets.get(source_joint, 0.0) + ) + if abs( + float(corrected_mimic.get("offset", "0")) + - expected_mimic_offset + ) > 1.0e-12: + raise ValueError(f"corrected URDF has invalid {name} mimic offset") + if maximum_origin_translation_error > 1.0e-12: + raise ValueError("corrected URDF changed a joint origin translation") + if maximum_origin_rotation_error > 1.0e-10: + raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)") + original_model = UrdfKinematicModel(source) + corrected_model = UrdfKinematicModel(corrected) + maximum_axis_error = 0.0 + maximum_point_error = 0.0 + for measurement in axes: + state = ( + measurement.condition_state_u8 + if measurement.condition_command_u8 is None + else measurement.condition_command_u8 + ) + angles = _angles_from_state( + state, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=inherited_zero_joints, + ) + expected_axis, expected_point = original_model.axis_line( + measurement.joint, + zero_offsets=offsets, + joint_angles=angles, + ) + actual_axis, actual_point = corrected_model.axis_line( + measurement.joint, + zero_offsets={}, + joint_angles=angles, + ) + maximum_axis_error = max( + maximum_axis_error, + math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))), + ) + maximum_point_error = max( + maximum_point_error, + float(np.linalg.norm(expected_point - actual_point)), + ) + if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10: + raise ValueError("written URDF kinematics differ from the solved correction") + return { + "maximum_origin_rotation_error_rad": maximum_origin_rotation_error, + "maximum_origin_translation_error_m": maximum_origin_translation_error, + "maximum_axis_equivalence_error_rad": maximum_axis_error, + "maximum_axis_point_equivalence_error_m": maximum_point_error, + } + + +def replay_session( + session_dir: str | Path, + *, + serial_number: str | None = None, + config_file: str | Path | None = None, + write_outputs: bool = False, + 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" + if config_file is None + else Path(config_file).expanduser().resolve() + ) + parameters = _load_parameters(config) + ( + start, + records_by_joint, + baseline_records_by_joint, + command_records_by_joint, + palm_axis_records_by_source, + raw_path, + ) = _load_raw_session(session) + legacy_coupled_zero_solver = uses_coupled_full_hand_zero_solver(start) + model = str(start.get("model", "G20")).strip().upper() + side = str(start["hand_type"]).lower() + requested_layout_id = str( + start.get("tag_layout", "legacy_11") + ).lower() + contract = get_product_calibration_contract( + model, side, requested_layout_id + ) + profile = contract.profile + # Normalise the former g20_right_15 compatibility alias immediately so + # every downstream product guard uses the actual physical 19-Tag profile + # instead of silently falling through legacy-11 logic. + layout_id = profile.layout_id + zero_profile = contract.zero_profile + baseline = tuple(int(value) for value in start["baseline_command_u8"]) + if len(baseline) != profile.command_count: + raise ValueError( + "session baseline length differs from the registered product schema" + ) + if ( + layout_id == G20_RIGHT_19_LAYOUT + and write_outputs + and bool(parameters.get("combination_validation_enabled", False)) + ): + replay_rows = [ + json.loads(line) + for line in raw_path.read_text(encoding="utf-8").splitlines() + if line.strip() + ] + combinations = [ + row + for row in replay_rows + if row.get("kind") == "combination_validation_sample" + and bool(row.get("passed")) + ] + expected_poses = { + "all_open", "thumb_middle", "index_middle", "middle_middle", + "ring_middle", "pinky_middle", "half_grip", "light_pinch", + } + if {str(row.get("pose_name")) for row in combinations} != expected_poses: + raise ValueError( + "incomplete session cannot publish: eight combination poses are required" + ) + if any( + float(row.get("position_p95_m", float("inf"))) > 0.003 + or float(row.get("orientation_p95_rad", float("inf"))) + > math.radians(2.0) + for row in combinations + ): + raise ValueError("combination pose validation exceeds product limits") + required_record_joints = set(profile.record_joints) + if ( + not required_record_joints.issubset(records_by_joint) + or not set(records_by_joint).issubset(set(profile.record_joints)) + ): + raise ValueError( + "raw session does not contain the required task record set" + ) + + def hysteresis_records( + name: str, + ) -> Sequence[Mapping[str, Any]]: + spec = profile.record_specs[name] + zero = int(baseline[spec.motor_index]) + if ( + layout_id == G20_RIGHT_19_LAYOUT + and zero not in {0, 255} + and start.get("baseline_hysteresis_source") + == "dedicated_mid_sweep_hold" + ): + records = baseline_records_by_joint.get(name, []) + if not records: + raise ValueError( + f"{name} is missing dedicated baseline hold samples" + ) + return records + return records_by_joint[name] + + def model_records( + name: str, *, require_full_pose: bool = False + ) -> list[dict[str, Any]]: + """Merge settled baseline holds into directional model records.""" + records = [ + dict(record) for record in records_by_joint.get(name, ()) + ] + if canonical_zero_direction(profile, name) is None: + return records + required = { + "relative_translation_xyz_m", + "parent_pose_common", + } + records.extend( + dict(record) + for record in baseline_records_by_joint.get(name, ()) + if not require_full_pose or required.issubset(record) + ) + return records + + curve_records_by_joint = { + name: model_records(name) for name in profile.record_joints + } + axis_records_by_joint = { + name: model_records(name, require_full_pose=True) + for name in profile.record_joints + } + source_urdf = Path(start["source_urdf_path"]).expanduser().resolve() + extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve() + if not source_urdf.is_file() or not extrinsics_file.is_file(): + raise ValueError("session source URDF or camera extrinsics is missing") + if "zero_calibrated" in source_urdf.stem.lower(): + raise ValueError("offline replay requires the original CAD URDF") + hand_serial = str(serial_number or session.parent.name) + output_suffix = _output_suffix(output_tag) + repetitions = int( + parameters.get("g20_right_19_repetitions", 4) + if layout_id == G20_RIGHT_19_LAYOUT + else parameters["repetitions"] + ) + + measured_fits = { + name: _fit_curve( + name, + curve_records_by_joint[name], + profile=profile, + baseline=baseline, + ) + for name in profile.measured_joints + } + validation_cycle = repetitions - 1 + training_cycles = tuple(range(validation_cycle)) + training_cycle_set = set(training_cycles) + if layout_id == G20_RIGHT_19_LAYOUT and len(training_cycles) < 3: + raise ValueError( + "g20_right_19 replay requires at least 3 training cycles and " + "one isolated holdout" + ) + training_fits = { + name: _fit_curve( + name, + [ + record + for record in curve_records_by_joint[name] + if int(record["cycle"]) in training_cycle_set + ], + profile=profile, + baseline=baseline, + ) + for name in profile.measured_joints + } + command_fits = dict(training_fits) + if layout_id == G20_RIGHT_19_LAYOUT: + if set(command_records_by_joint) != required_record_joints: + raise ValueError( + "g20_right_19 session is incomplete: missing steady command checkpoints" + ) + command_fits = {} + for name in profile.measured_joints: + if name in G20_REFERENCE_THUMB_CMC_JOINTS: + command_fits[name] = measured_fits[name] + continue + records = [ + dict(record) + for record in command_records_by_joint[name] + if int(record["cycle"]) == 0 + ] + if canonical_zero_direction(profile, name) is not None: + records.extend( + dict(record) + for record in baseline_records_by_joint.get(name, ()) + if int(record["cycle"]) == 0 + ) + if len(records) < 18: + raise ValueError( + f"{name} has {len(records)}/18 steady command checkpoints" + ) + zero_command = int( + baseline[profile.joint_specs[name].motor_index] + ) + if name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS: + command_fits[name] = fit_joint_image_curve( + records, + maximum_radial_rms_px=float( + parameters["image_trajectory_maximum_radial_rms_px"] + ), + maximum_radial_p95_px=float( + parameters["image_trajectory_maximum_radial_p95_px"] + ), + minimum_radius_px=float( + parameters["image_trajectory_minimum_radius_px"] + ), + minimum_arc_rad=math.radians( + float(parameters["trajectory_minimum_arc_deg"]) + ), + ) + else: + command_fits[name] = fit_rotation_joint_curve( + records, + zero_command_u8=zero_command, + canonical_zero_direction=canonical_zero_direction( + profile, name + ), + ) + command_gap_limit = math.radians( + float(parameters.get("command_maximum_direction_gap_deg", 2.0)) + ) + command_feedback_fits = { + name: _fit_curve( + name, + _steady_records_in_feedback_domain( + [ + dict(record) + for record in command_records_by_joint[name] + if int(record["cycle"]) == 0 + ] + ), + profile=profile, + baseline=baseline, + ) + for name in profile.measured_joints + if name not in G20_REFERENCE_THUMB_CMC_JOINTS + } + failed_command_gaps = { + name: fit.maximum_hysteresis_rad + for name, fit in command_feedback_fits.items() + if fit.maximum_hysteresis_rad > command_gap_limit + } + if failed_command_gaps: + raise ValueError( + "command-direction gap exceeds 2deg: " + + ", ".join( + f"{name}={math.degrees(value):.3f}deg" + for name, value in failed_command_gaps.items() + ) + ) + command_fits = derive_mimic_passive_fits( + source_urdf, + command_fits, + profile=profile, + ) + command_fits = anchor_right_19_mechanical_endpoint_curves( + command_fits, + { + name: ( + [ + record + for record in curve_records_by_joint[name] + if int(record["cycle"]) in training_cycle_set + ] + if name == "thumb_cmc_roll" + else command_records_by_joint[name] + ) + for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + }, + maximum_direction_difference_rad=command_gap_limit, + feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}), + ) + cross_view_roll_metrics: dict[str, dict[str, float]] = {} + for name, validation_name in ( + profile.axis_validation_sources or {} + ).items(): + validation_training = _fit_curve( + validation_name, + [ + record + for record in curve_records_by_joint[validation_name] + if int(record["cycle"]) in training_cycle_set + ], + profile=profile, + baseline=baseline, + ) + validation_all = _fit_curve( + validation_name, + curve_records_by_joint[validation_name], + profile=profile, + baseline=baseline, + ) + training_metrics = cross_view_roll_diagnostic_metrics( + training_fits[name], validation_training + ) + final_metrics = cross_view_roll_diagnostic_metrics( + measured_fits[name], validation_all + ) + cross_view_roll_metrics[name] = { + **{ + f"training_{key}": value + for key, value in training_metrics.items() + }, + **{ + f"final_{key}": value + for key, value in final_metrics.items() + }, + } + axes = _fit_axes( + records_by_joint=axis_records_by_joint, + profile=profile, + baseline=baseline, + extrinsics_file=extrinsics_file, + source_urdf=source_urdf, + repetitions=repetitions, + ) + palm_records_for_fit = palm_axis_records_by_source + if legacy_coupled_zero_solver: + palm_records_for_fit = { + name: [ + { + key: value + for key, value in record.items() + if key != "child_pose_common" + } + for record in records + ] + for name, records in palm_axis_records_by_source.items() + } + palm_orientation_measurements, palm_orientation_rejections = ( + fit_partial_palm_orientation_measurements( + sources=profile.palm_orientation_sources, + records_by_joint=palm_records_for_fit, + motor_by_source=profile.palm_axis_motor_by_source, + baseline_command_u8=baseline, + cycles=range(repetitions), + minimum_sources=int( + profile.minimum_palm_orientation_sources + ), + minimum_arc_rad=( + profile.palm_orientation_minimum_arc_rad + ), + maximum_rotation_orthogonal_rms_rad=math.radians( + float( + parameters[ + "active_maximum_rotation_orthogonal_rms_deg" + ] + ) + ), + maximum_command_distance_u8=( + profile.palm_orientation_maximum_command_distance_u8 + ), + ) + ) + failures = _quality_failures( + records_by_joint=records_by_joint, + baseline_records_by_joint=( + baseline_records_by_joint + if start.get("baseline_hysteresis_source") + == "dedicated_mid_sweep_hold" + else None + ), + profile=profile, + baseline=baseline, + fits=measured_fits, + axes=axes, + parameters=parameters, + ) + if failures: + raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures)) + + holdout_by_joint = { + name: joint_curve_holdout_errors( + training_fits[name], + [ + record + for record in curve_records_by_joint[name] + if int(record["cycle"]) == validation_cycle + ], + zero_command_u8=int( + baseline[profile.joint_specs[name].motor_index] + ), + ) + for name in profile.measured_joints + } + joint_dynamic_diagnostics: dict[str, dict[str, Any]] = {} + for name in profile.measured_joints: + cycle_travel: list[float] = [] + for cycle in range(repetitions): + cycle_fit = _fit_curve( + name, + [ + record + for record in records_by_joint[name] + if int(record["cycle"]) == cycle + ], + profile=profile, + baseline=baseline, + ) + cycle_travel.append( + abs( + float(cycle_fit.angle_rad[0]) + - float(cycle_fit.angle_rad[255]) + ) + ) + holdout = np.abs(np.asarray(holdout_by_joint[name], dtype=float)) + baseline_hysteresis = baseline_hysteresis_by_cycle_rad( + hysteresis_records(name), + zero_command_u8=int( + baseline[profile.joint_specs[name].motor_index] + ), + axis_xyz=_physical_rotation_axis_for_curve( + name, + records_by_joint[name], + measured_fits[name], + profile=profile, + baseline=baseline, + ), + ) + joint_dynamic_diagnostics[name] = { + "cycle_travel_rad": [float(value) for value in cycle_travel], + "cycle_travel_range_rad": max(cycle_travel) - min(cycle_travel), + "baseline_hysteresis_by_cycle_rad": [ + float(value) for value in baseline_hysteresis + ], + "holdout_cycle": validation_cycle, + "holdout_cycle_mae_rad": float(np.mean(holdout)), + "holdout_cycle_max_rad": float(np.max(holdout)), + } + trajectory_errors = np.abs( + np.asarray( + [value for values in holdout_by_joint.values() for value in values], + dtype=float, + ) + ) + maximum_validation_mae = math.radians( + float(parameters["maximum_validation_mae_deg"]) + ) + maximum_validation_p95 = math.radians( + float(parameters["maximum_validation_p95_deg"]) + ) + trajectory_mae = float(np.mean(trajectory_errors)) + trajectory_p95 = float(np.percentile(trajectory_errors, 95.0)) + if ( + trajectory_mae > maximum_validation_mae + or trajectory_p95 > maximum_validation_p95 + or ( + layout_id == G20_RIGHT_19_LAYOUT + and float(np.max(trajectory_errors)) + > math.radians( + float(parameters.get("maximum_validation_error_deg", 1.5)) + ) + ) + or ( + layout_id == G20_RIGHT_19_LAYOUT + and any( + float(np.mean(np.abs(np.asarray(values, dtype=float)))) + > maximum_validation_mae + or float(np.max(np.abs(np.asarray(values, dtype=float)))) + > math.radians( + float( + parameters.get("maximum_validation_error_deg", 1.5) + ) + ) + for values in holdout_by_joint.values() + ) + ) + ): + raise ValueError("isolated trajectory holdout failed") + + motor_by_joint = { + name: int(spec.motor_index) for name, spec in profile.joint_specs.items() + } + measured_endpoint_zero_offsets = ( + derive_right_19_mechanical_endpoint_offsets( + source_urdf, + command_fits, + maximum_offset_rad=math.radians( + float( + parameters.get( + "mechanical_endpoint_maximum_offset_deg", 5.0 + ) + ) + ), + ) + if layout_id == G20_RIGHT_19_LAYOUT + else {} + ) + endpoint_zero_offsets = { + name: value + for name, value in measured_endpoint_zero_offsets.items() + if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + } + post_solve_endpoint_offsets = { + name: value + for name, value in measured_endpoint_zero_offsets.items() + if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS + } + command_fits = clamp_runtime_fits_to_urdf_limits( + source_urdf, + command_fits, + endpoint_anchored_offsets_rad=endpoint_zero_offsets, + ) + joint_limits: dict[str, float] = {} + solve_arguments = { + "source_urdf": source_urdf, + "measurements": axes, + "palm_orientation_measurements": palm_orientation_measurements, + "motor_by_joint": motor_by_joint, + "maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])), + "finger_maximum_offset_rad": math.radians( + float(parameters.get("zero_finger_maximum_offset_deg", 3.0)) + ), + "joint_maximum_offset_rad": joint_limits, + "maximum_cycle_difference_rad": math.radians( + float(parameters["zero_maximum_axis_cycle_difference_deg"]) + ), + "maximum_axis_cone_mismatch_rad": math.radians( + float(parameters["zero_maximum_axis_cone_mismatch_deg"]) + ), + "maximum_systematic_axis_cone_bias_rad": math.radians( + float( + parameters.get( + "cross_view_roll_maximum_axis_difference_deg", 15.0 + ) + ) + ), + "maximum_observability_condition_number": float( + parameters.get( + "zero_maximum_observability_condition_number", 1.0e10 + ) + ), + "maximum_pose_axis_line_rms_m": float( + parameters["axis_maximum_pose_line_rms_m"] + ), + "maximum_validation_mae_rad": maximum_validation_mae, + "maximum_validation_p95_rad": maximum_validation_p95, + "maximum_validation_error_rad": ( + math.radians( + float(parameters.get("maximum_validation_error_deg", 1.5)) + ) + if layout_id == G20_RIGHT_19_LAYOUT + else None + ), + "maximum_confidence_half_width_rad": ( + math.radians( + float( + parameters.get( + "zero_maximum_confidence_half_width_deg", 0.5 + ) + ) + ) + if layout_id == G20_RIGHT_19_LAYOUT + else None + ), + "hand_type": side, + "tag_layout": layout_id, + "training_cycles": training_cycles, + "validation_cycle": validation_cycle, + "fixed_direct_zero_offsets_rad": { + **zero_profile.fixed_direct_zero_offsets_rad, + **endpoint_zero_offsets, + }, + "static_output_zero_offsets_rad": { + **zero_profile.static_output_zero_offsets_rad, + **post_solve_endpoint_offsets, + }, + } + def solve_with_product_partition( + fits: Mapping[str, JointCurveFit], + arguments: Mapping[str, Any], + ): + if layout_id != G20_RIGHT_19_LAYOUT or legacy_coupled_zero_solver: + return solve_urdf_zero_offsets(curves=fits, **arguments) + thumb_profile = get_right_19_thumb_zero_profile() + thumb_names = set(thumb_profile.direct_zero_joints) + thumb = solve_urdf_zero_offsets( + curves=fits, + **{ + **arguments, + "measurements": [ + item + for item in arguments["measurements"] + if item.joint in thumb_profile.axis_joints + ], + "joint_maximum_offset_rad": { + name: value + for name, value in arguments[ + "joint_maximum_offset_rad" + ].items() + if name in thumb_names + }, + "fixed_direct_zero_offsets_rad": { + name: value + for name, value in arguments[ + "fixed_direct_zero_offsets_rad" + ].items() + if name in thumb_names + }, + "static_output_zero_offsets_rad": { + name: value + for name, value in arguments[ + "static_output_zero_offsets_rad" + ].items() + if name in thumb_names + }, + "zero_profile": thumb_profile, + }, + ) + companion = solve_urdf_zero_offsets( + curves=fits, + **{ + **arguments, + "fixed_direct_zero_offsets_rad": { + **arguments["fixed_direct_zero_offsets_rad"], + **thumb.direct_offsets_rad, + }, + }, + ) + return merge_right_19_thumb_zero_result( + thumb_result=thumb, + companion_result=companion, + ) + + holdout_zero = solve_with_product_partition(training_fits, solve_arguments) + if not holdout_zero.passed: + failure = { + "reasons": dict(holdout_zero.failure_reasons), + "fitted_offsets_deg": { + name: math.degrees(value) + for name, value in holdout_zero.direct_offsets_rad.items() + }, + "cycle_offsets_deg": { + name: [math.degrees(value) for value in values] + for name, values in holdout_zero.cycle_offsets_rad.items() + }, + } + raise ValueError( + "isolated zero/URDF holdout failed: " + + json.dumps(failure, ensure_ascii=False, sort_keys=True) + ) + # Keep the model frozen after the holdout decision. Re-fitting with the + # held-out cycle would make the published artifact differ from the model + # that was independently evaluated above. + final_zero = holdout_zero + measured_fits = command_fits + for target, source_name in zero_profile.inherited_static_zero_joints.items(): + if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]: + raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}") + for target in ( + set(zero_profile.inherited_zero_joints) + - set(zero_profile.inherited_static_zero_joints) + ): + if final_zero.all_active_offsets_rad[target] != 0.0: + raise ValueError(f"unobserved static zero must retain source CAD: {target}") + + validation_errors = [ + float(value) for values in holdout_by_joint.values() for value in values + ] + validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad) + stamp = session.name + final_json = session / ( + f"g20_{side}_{hand_serial}_calibration{output_suffix}.json" + ) + expected_urdf_name = ( + f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}" + f"{output_suffix}.urdf" + ) + final_urdf = session / expected_urdf_name + report_path = session / ( + f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json" + ) + if write_outputs: + existing = [path for path in (final_json, final_urdf, report_path) if path.exists()] + if existing: + raise ValueError( + "refusing to overwrite replay outputs: " + + ", ".join(str(path) for path in existing) + ) + + source_hash_before = _sha256(source_urdf) + published_zero_offsets = { + name: round(float(value), 8) + for name, value in final_zero.all_active_offsets_rad.items() + } + urdf_offsets = ( + { + name: published_zero_offsets[name] + for name in zero_profile.direct_zero_joints + } + if layout_id == G20_RIGHT_19_LAYOUT + else published_zero_offsets + ) + with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary: + candidate = write_zero_corrected_urdf( + source_urdf=source_urdf, + output_directory=temporary, + serial_number=hand_serial, + offsets_rad=urdf_offsets, + endpoint_anchored_offsets_rad=endpoint_zero_offsets, + timestamp=stamp, + ) + urdf_checks = _validate_corrected_urdf( + source=source_urdf, + corrected=candidate, + offsets=urdf_offsets, + endpoint_anchored_offsets=endpoint_zero_offsets, + axes=axes, + curves=training_fits, + motor_by_joint=motor_by_joint, + inherited_zero_joints=zero_profile.inherited_zero_joints, + ) + residual_zero = solve_with_product_partition( + # Re-check the corrected URDF in the exact frozen trajectory + # domain that produced the accepted zero offsets. Runtime + # requested-command curves are a separate transfer function and + # differ slightly at settled checkpoints; using them here creates + # a fictitious residual zero after a structurally correct write. + training_fits, + { + **solve_arguments, + "static_output_zero_offsets_rad": { + name: 0.0 + for name in ( + set(zero_profile.static_output_zero_offsets_rad) + | set(post_solve_endpoint_offsets) + ) + }, + "source_urdf": candidate, + "fixed_direct_zero_offsets_rad": { + name: 0.0 + for name in ( + set(zero_profile.fixed_direct_zero_offsets_rad) + | set(endpoint_zero_offsets) + ) + }, + }, + ) + maximum_residual_offset = max( + abs(float(value)) for value in residual_zero.direct_offsets_rad.values() + ) + if not residual_zero.passed or maximum_residual_offset > math.radians(0.3): + raise ValueError( + "written URDF retains a significant zero correction: " + + json.dumps( + { + "passed": residual_zero.passed, + "residual_offsets_deg": { + name: math.degrees(value) + for name, value in ( + residual_zero.direct_offsets_rad.items() + ) + }, + "failure_reasons": dict( + residual_zero.failure_reasons + ), + }, + ensure_ascii=False, + sort_keys=True, + ) + ) + candidate_hash = _sha256(candidate) + payload = build_compact_payload( + serial_number=hand_serial, + measured_fits=command_fits, + urdf_zero_offsets_rad=published_zero_offsets, + validation_errors_rad=validation_errors, + passed=True, + baseline=baseline, + side=side, + layout_id=layout_id, + zero_uncertainty_rad=( + final_zero.offset_confidence_half_width_rad + ), + zero_cycle_offsets_rad=final_zero.cycle_offsets_rad, + zero_observers=zero_profile.offset_observer_joint, + artifact_hashes=( + { + "source_urdf_sha256": source_hash_before, + "camera_extrinsics_sha256": _sha256(extrinsics_file), + "corrected_urdf_sha256": candidate_hash, + } + if layout_id == G20_RIGHT_19_LAYOUT + else None + ), + cross_view_roll_metrics=cross_view_roll_metrics, + joint_dynamic_diagnostics=joint_dynamic_diagnostics, + zero_geometry_diagnostics=( + { + "training_cycles": final_zero.training_cycles, + "validation_cycle": final_zero.validation_cycle, + "axis_line_rms_m": final_zero.axis_line_rms_m, + "validation_line_error_by_joint_m": ( + final_zero.validation_line_error_by_joint_m + ), + "observability_rank": final_zero.observability_rank, + "observability_parameter_count": ( + final_zero.observability_parameter_count + ), + "observability_condition_number": ( + final_zero.observability_condition_number + ), + "offset_covariance_rad2": ( + final_zero.offset_covariance_rad2 + ), + "palm_orientation_sources": [ + { + "source_joint": item.source_joint, + "model_joint": item.model_joint, + "cycle": item.cycle, + "observed_arc_rad": item.observed_arc_rad, + "rotation_orthogonal_rms_rad": ( + item.rotation_orthogonal_rms_rad + ), + "axis_estimator": item.axis_estimator, + "incremental_pair_count": ( + item.incremental_pair_count + ), + } + for item in palm_orientation_measurements + ], + "palm_orientation_rejections": dict( + palm_orientation_rejections + ), + } + if layout_id == G20_RIGHT_19_LAYOUT + else None + ), + ) + payload, _clipped_runtime_joints = ( + clamp_compact_payload_to_urdf_limits(payload, candidate) + ) + legacy_quality_source = None + if legacy_coupled_zero_solver: + payload["quality"] = _legacy_published_quality( + session, + side=side, + serial_number=hand_serial, + ) + legacy_quality_source = ( + "published_v1_artifact_random_validation_omitted_from_raw" + ) + validate_compact_payload(payload) + if write_outputs: + if _sha256(source_urdf) != source_hash_before: + raise ValueError("source URDF changed during offline replay") + os.replace(candidate, final_urdf) + try: + # As in the live path, the atomically written JSON is the + # commit marker for the already validated URDF/JSON pair. + atomic_write_json(final_json, payload) + except Exception: + if layout_id == G20_RIGHT_19_LAYOUT and final_urdf.exists(): + rejected = final_urdf.with_suffix(".urdf.rejected") + final_urdf.replace(rejected) + raise + + if _sha256(source_urdf) != source_hash_before: + raise ValueError("source URDF changed during offline replay") + + report: dict[str, Any] = { + "passed": True, + "session_dir": str(session), + "side": side, + "serial_number": hand_serial, + "output_tag": output_tag, + "raw_samples_sha256": _sha256(raw_path), + "source_urdf": str(source_urdf), + "source_urdf_sha256": source_hash_before, + "joint_limits_deg": { + "finger_default": float( + parameters.get("zero_finger_maximum_offset_deg", 3.0) + ), + "thumb_default": float(parameters["zero_maximum_offset_deg"]), + }, + "static_zero_policy": ( + "visual_axes_plus_measured_mechanical_endpoint" + if layout_id == G20_RIGHT_19_LAYOUT + else "direct_measurements_only" + ), + "fixed_zero_offsets_deg": { + name: math.degrees(value) + for name, value in zero_profile.fixed_direct_zero_offsets_rad.items() + }, + "endpoint_anchored_zero_offsets_deg": { + name: math.degrees(value) + for name, value in endpoint_zero_offsets.items() + }, + "static_output_zero_offsets_deg": { + name: math.degrees(value) + for name, value in { + **zero_profile.static_output_zero_offsets_rad, + **post_solve_endpoint_offsets, + }.items() + }, + "direct_offsets_deg": { + name: math.degrees(value) + for name, value in final_zero.direct_offsets_rad.items() + }, + "model_base_translation_xyz_m": list( + final_zero.base_translation_xyz_m + ), + "model_base_quaternion_xyzw": list( + final_zero.base_quaternion_xyzw + ), + "cycle_offsets_deg": { + name: [math.degrees(value) for value in values] + for name, values in holdout_zero.cycle_offsets_rad.items() + }, + "axis_cone_mismatch_by_joint_deg": { + name: math.degrees(value) + for name, value in ( + holdout_zero.axis_cone_mismatch_by_joint_rad.items() + ) + }, + "axis_cone_bias_classification_by_joint": dict( + holdout_zero.axis_cone_bias_classification_by_joint + ), + "offset_uncertainty_deg": { + name: math.degrees(value) + for name, value in holdout_zero.offset_uncertainty_rad.items() + }, + "trajectory_holdout_mae_deg": math.degrees(trajectory_mae), + "trajectory_holdout_p95_deg": math.degrees(trajectory_p95), + "zero_holdout_error_deg": { + name: math.degrees(value) + for name, value in holdout_zero.validation_error_by_joint_rad.items() + }, + "zero_original_error_deg": { + name: math.degrees(value) + for name, value in holdout_zero.validation_original_error_by_joint_rad.items() + }, + "zero_improvement_95pct_lower_deg": { + name: math.degrees(value) + for name, value in ( + holdout_zero.validation_improvement_confidence_lower_rad.items() + ) + }, + "corrected_urdf_checks": { + **urdf_checks, + "maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset), + }, + "corrected_urdf_sha256": candidate_hash, + "computed_final_json_sha256": _payload_sha256(payload), + "legacy_quality_source": legacy_quality_source, + "final_json": str(final_json) if write_outputs else None, + "corrected_urdf": str(final_urdf) if write_outputs else None, + } + if write_outputs: + report["final_json_sha256"] = _sha256(final_json) + if _sha256(final_urdf) != candidate_hash: + raise ValueError("formal corrected URDF differs from validated candidate") + atomic_write_json(report_path, report) + report["validation_report"] = str(report_path) + return report + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Replay and validate a registered hand-calibration session." + ) + parser.add_argument("session_dir") + parser.add_argument("--serial-number", default=None) + parser.add_argument("--config-file", default=None) + parser.add_argument("--write", action="store_true") + parser.add_argument( + "--output-tag", + default=None, + help="safe suffix for a replay variant; existing outputs are never overwritten", + ) + arguments = parser.parse_args() + result = replay_session( + arguments.session_dir, + serial_number=arguments.serial_number, + config_file=arguments.config_file, + write_outputs=arguments.write, + output_tag=arguments.output_tag, + ) + print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True)) + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py new file mode 100644 index 0000000..90ded41 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/profile.py @@ -0,0 +1,2241 @@ +"""Pure three-camera G20 calibration model and compact runtime schema. + +The hardware node records one parent/child AprilTag trajectory for each +directly observable joint. This module deliberately contains no ROS imports: +curve fitting, baseline centring, inheritance, and schema validation +remain deterministic and unit-testable without connected cameras or a hand. +""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import math +from pathlib import Path +import re +from typing import Any, Iterable, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np + +from .command_layout import G20_COMMAND_NAMES as COMMAND_NAMES +from ...trajectory import ( + _angle_for_circle, + _fit_circle_with_axis, + _fit_joint_curve, + _fit_plane_axis, + _orient_circle_positive, +) +from ...zero_calibration import ( + _fit_circle, + _trajectory_arc_rad, + circular_median_rad, + maximum_pairwise_angle_difference_rad, + wrap_angle_rad, +) + + +THREE_CAMERA_BASELINE_COMMAND: tuple[int, ...] = ( + 255, 255, 255, 255, 255, 255, + 127, 127, 127, 127, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, +) + + +@dataclass(frozen=True) +class JointSpec: + name: str + motor_index: int + active: bool + view: str | None + parent_role: str | None + child_role: str | None + source_joint: str | None = None + zero_kind: str | None = None + + @property + def measured(self) -> bool: + return self.source_joint is None + + +@dataclass(frozen=True) +class SweepSpec: + view: str + motor_index: int + joints: tuple[str, ...] + task_name: str = "" + auxiliary_commands: tuple[tuple[int, int], ...] = () + validation_only: bool = False + + @property + def key(self) -> str: + if self.task_name: + return self.task_name + return f"{self.view}:motor{self.motor_index}:{','.join(self.joints)}" + + +@dataclass(frozen=True) +class PalmAxisObserver: + """Non-blocking direction observation attached to an existing task. + + This is deliberately not a ``JointSpec``: it has no curve, endpoint or + retry semantics. The camera callback records it only while both Tags are + visible during the named sweep. + """ + + source_name: str + task_name: str + view: str + parent_role: str + child_role: str + model_joint: str + motor_index: int + + +@dataclass(frozen=True) +class JointCurveFit: + angle_rad: tuple[float, ...] + decreasing_rad: tuple[float, ...] + increasing_rad: tuple[float, ...] + circle: Mapping[str, Any] + maximum_monotonic_correction_rad: float + maximum_hysteresis_rad: float + quality: Mapping[str, float] + zero_offset_rad: float = 0.0 + + +@dataclass(frozen=True) +class HandCalibrationProfile: + side: str + reference_finger: str + view_tags: Mapping[str, Mapping[str, int]] + preflight_view_roles: Mapping[str, tuple[str, ...]] + joint_specs: Mapping[str, JointSpec] + sweep_specs: tuple[SweepSpec, ...] + image_trajectory_joints: frozenset[str] + roll_clearance_commands: Mapping[int, int] + thumb_pitch_clearance_commands: Mapping[int, int] + layout_id: str = "legacy_11" + measurement_specs: Mapping[str, JointSpec] | None = None + axis_validation_sources: Mapping[str, str] | None = None + palm_axis_observers: tuple[PalmAxisObserver, ...] = () + minimum_palm_orientation_sources: int = 0 + palm_orientation_minimum_arc_rad: float = math.radians(15.0) + palm_orientation_maximum_command_distance_u8: int = 255 + # Hardware and algorithm differences belong to the product profile, not + # to the acquisition state machine. Keeping these fields here lets a new + # model (or the mirrored hand) reuse the same recorder/fitter by supplying + # a different declarative profile. + model: str = "G20" + command_names: tuple[str, ...] = COMMAND_NAMES + baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND + capabilities: frozenset[str] = frozenset() + precheck_sweeps: bool = False + steady_command_checkpoints: bool = False + directional_zero: bool = False + isolated_holdout: bool = False + cross_view_roll_curve: bool = False + stable_cross_view_cone_bias: bool = False + + @property + def command_count(self) -> int: + return len(self.command_names) + + def supports(self, capability: str) -> bool: + return str(capability) in self.capabilities + + @property + def measured_joints(self) -> tuple[str, ...]: + return tuple( + name for name, spec in self.joint_specs.items() if spec.measured + ) + + @property + def record_specs(self) -> Mapping[str, JointSpec]: + return self.joint_specs if self.measurement_specs is None else self.measurement_specs + + @property + def record_joints(self) -> tuple[str, ...]: + return tuple( + name for name, spec in self.record_specs.items() if spec.measured + ) + + @property + def palm_orientation_sources(self) -> Mapping[str, str]: + return { + observer.source_name: observer.model_joint + for observer in self.palm_axis_observers + } + + @property + def palm_axis_motor_by_source(self) -> Mapping[str, int]: + return { + observer.source_name: int(observer.motor_index) + for observer in self.palm_axis_observers + } + + @property + def active_joints(self) -> tuple[str, ...]: + return tuple( + name for name, spec in self.joint_specs.items() if spec.active + ) + + @property + def passive_joints(self) -> tuple[str, ...]: + return tuple( + name for name, spec in self.joint_specs.items() if not spec.active + ) + + @property + def reference_roll_motor(self) -> int: + return int(self.joint_specs[f"{self.reference_finger}_mcp_roll"].motor_index) + + @property + def reference_pitch_motor(self) -> int: + return int(self.joint_specs[f"{self.reference_finger}_mcp_pitch"].motor_index) + + @property + def reference_pip_motor(self) -> int: + return int(self.joint_specs[f"{self.reference_finger}_pip"].motor_index) + + @property + def reference_speed_slot(self) -> int: + return {"index": 1, "middle": 2, "ring": 3, "pinky": 4}[ + self.reference_finger + ] + + +_FINGERS: tuple[str, ...] = ("index", "middle", "ring", "pinky") + +# The side camera looks almost along these physical flexion axes. The child +# Tag centre therefore traces a high-resolution image-plane circle against the +# fixed palm Tag, while monocular planar-PnP tilt is not accurate enough to be +# the dynamic angle source. Passive DIP is intentionally excluded because +# its parent Tag moves with PIP; a raw image-centre difference would mix both +# joint angles. +RIGHT_19_END_ON_IMAGE_CURVE_JOINTS = frozenset( + { + *(f"{finger}_mcp_pitch" for finger in _FINGERS), + *(f"{finger}_pip" for finger in _FINGERS), + } +) +# These joints have no independent actuator. Their distal Tags directly +# measure the passive dynamic curve, while their fitted axes serve only as +# downstream phase observers for the corresponding active PIP. +RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS = frozenset( + f"{finger}_dip" for finger in _FINGERS +) +_MOTOR_BY_JOINT: dict[str, int] = { + **{f"{finger}_mcp_pitch": index + 1 for index, finger in enumerate(_FINGERS)}, + **{f"{finger}_mcp_roll": index + 6 for index, finger in enumerate(_FINGERS)}, + **{f"{finger}_pip": index + 16 for index, finger in enumerate(_FINGERS)}, +} + + +def _build_hand_profile(side: str) -> HandCalibrationProfile: + hand_side = str(side).lower() + if hand_side not in {"left", "right"}: + raise ValueError("hand side must be left or right") + reference = "index" if hand_side == "left" else "pinky" + roll_role = f"{reference}_roll" + mcp_role = f"{reference}_mcp" + pip_role = f"{reference}_pip" + dip_role = f"{reference}_dip" + view_tags: dict[str, dict[str, int]] = { + "front": { + "front_base": 0, + "thumb_cmc": 1, + "thumb_mcp": 2, + "thumb_ip": 3, + roll_role: 10, + }, + "side": { + "side_base": 4, + mcp_role: 5, + pip_role: 6, + dip_role: 7, + }, + "top": {"top_base": 8, "thumb_yaw": 9}, + } + preflight_view_roles = { + view: tuple(tags) for view, tags in view_tags.items() + } + joint_specs: dict[str, JointSpec] = {} + # Both hands use the front base/Tag 1 pair for motor 0. Keeping the + # complete thumb root chain in one calibrated view avoids introducing a + # cross-camera axis-line phase into the URDF zero solve. + thumb_pitch_view = "front" + thumb_pitch_parent = "front_base" + joint_specs["thumb_cmc_pitch"] = JointSpec( + "thumb_cmc_pitch", + 0, + True, + thumb_pitch_view, + thumb_pitch_parent, + "thumb_cmc", + zero_kind="urdf_axis_chain", + ) + for finger in _FINGERS: + name = f"{finger}_mcp_pitch" + source = None if finger == reference else f"{reference}_mcp_pitch" + joint_specs[name] = JointSpec( + name, + _MOTOR_BY_JOINT[name], + True, + "side" if source is None else None, + "side_base" if source is None else None, + mcp_role if source is None else None, + source_joint=source, + zero_kind="urdf_axis_chain" if source is None else "inherited", + ) + joint_specs["thumb_cmc_roll"] = JointSpec( + "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc", + zero_kind="urdf_axis_chain", + ) + for finger in _FINGERS: + name = f"{finger}_mcp_roll" + source = None if finger == reference else f"{reference}_mcp_roll" + joint_specs[name] = JointSpec( + name, + _MOTOR_BY_JOINT[name], + True, + "front" if source is None else None, + "front_base" if source is None else None, + roll_role if source is None else None, + source_joint=source, + zero_kind="urdf_axis_chain" if source is None else "inherited", + ) + joint_specs["thumb_cmc_yaw"] = JointSpec( + "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw", + zero_kind="urdf_axis_chain", + ) + joint_specs["thumb_mcp"] = JointSpec( + "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp", + zero_kind="urdf_axis_chain", + ) + for finger in _FINGERS: + name = f"{finger}_pip" + source = None if finger == reference else f"{reference}_pip" + joint_specs[name] = JointSpec( + name, + _MOTOR_BY_JOINT[name], + True, + "side" if source is None else None, + mcp_role if source is None else None, + pip_role if source is None else None, + source_joint=source, + zero_kind="urdf_axis_chain" if source is None else "inherited", + ) + joint_specs["thumb_ip"] = JointSpec( + "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip", + ) + for finger in _FINGERS: + name = f"{finger}_dip" + source = None if finger == reference else f"{reference}_dip" + joint_specs[name] = JointSpec( + name, + _MOTOR_BY_JOINT[f"{finger}_pip"], + False, + "side" if source is None else None, + pip_role if source is None else None, + dip_role if source is None else None, + source_joint=source, + ) + sweep_specs = ( + SweepSpec(thumb_pitch_view, 0, ("thumb_cmc_pitch",)), + SweepSpec("front", 5, ("thumb_cmc_roll",)), + SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")), + SweepSpec("front", _MOTOR_BY_JOINT[f"{reference}_mcp_roll"], ( + f"{reference}_mcp_roll", + )), + SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_mcp_pitch"], ( + f"{reference}_mcp_pitch", + )), + SweepSpec("side", _MOTOR_BY_JOINT[f"{reference}_pip"], ( + f"{reference}_pip", f"{reference}_dip", + )), + SweepSpec("top", 10, ("thumb_cmc_yaw",)), + ) + image_joints = frozenset( + { + "thumb_cmc_pitch", + "thumb_cmc_roll", + "thumb_mcp", + "thumb_ip", + f"{reference}_mcp_roll", + f"{reference}_mcp_pitch", + f"{reference}_pip", + } + ) + measured_roll = _MOTOR_BY_JOINT[f"{reference}_mcp_roll"] + # The roll channels are mirrored mechanically between hands. Command 0 + # moves the three non-reference fingers to the camera-right clearance pose + # on the left hand, while the same physical pose is command 255 on the + # right hand. Using 0 for both sides made the right index/middle/ring + # fingers lean left and could obstruct or load the pinky roll sweep. + roll_clearance_command = 0 if hand_side == "left" else 255 + clearance = { + _MOTOR_BY_JOINT[f"{finger}_mcp_roll"]: roll_clearance_command + for finger in _FINGERS + if finger != reference + } + assert measured_roll not in clearance + # Motor 0 is observed from the front with both other thumb-root channels + # fully open. Store the commands explicitly for the right hand so a + # caller-provided baseline cannot silently restore the obsolete 50/120 + # pose and hide Tag 1 from the front camera. + thumb_pitch_clearance = {10: 255, 5: 255} if hand_side == "right" else {} + return HandCalibrationProfile( + side=hand_side, + reference_finger=reference, + view_tags=view_tags, + preflight_view_roles=preflight_view_roles, + joint_specs=joint_specs, + sweep_specs=sweep_specs, + image_trajectory_joints=image_joints, + roll_clearance_commands=clearance, + thumb_pitch_clearance_commands=thumb_pitch_clearance, + ) + + +LEFT_HAND_PROFILE = _build_hand_profile("left") +RIGHT_HAND_PROFILE = _build_hand_profile("right") + + +# ``g20_right_15`` remains accepted as a read/CLI compatibility alias, but a +# newly recorded product session always persists the physical 19-Tag layout +# identifier. Together with the exact ``view_tags`` resume check this keeps a +# former 15-Tag checkpoint from being imported after the four distal Tags are +# installed again. +G20_RIGHT_15_LAYOUT = "g20_right_15" +G20_RIGHT_19_LAYOUT = "g20_right_19" + +# The three CMC axes keep the calibration domain that was proven on this hand +# in a609d521: dense relative-rotation sweeps with an isolated-cycle holdout. +# Monocular Tag translation is useful as a diagnostic, but is not an +# independent quality measurement for these short, strongly coupled links. +G20_REFERENCE_THUMB_CMC_JOINTS: frozenset[str] = frozenset( + {"thumb_cmc_pitch", "thumb_cmc_roll", "thumb_cmc_yaw"} +) + +G20_COMBINATION_REQUIRED_TARGET_KEYS: tuple[str, ...] = tuple( + sorted( + { + "front:index_mcp_roll", + "front:middle_mcp_roll", + "front:ring_mcp_roll", + "front:pinky_mcp_roll", + "side:index_pip", + "side:middle_pip", + "side:ring_pip", + "side:pinky_pip", + } + ) +) + +MIMIC_DERIVED_FINGER_DIPS: Mapping[str, str] = { + f"{finger}_dip": f"{finger}_pip" for finger in _FINGERS +} + + +def canonical_zero_direction( + profile: HandCalibrationProfile, joint_name: str +) -> str | None: + """Return the repeatable approach direction used as physical zero. + + A mid-range encoder value does not uniquely determine a linkage pose when + the transmission has backlash. The right-hand product four-finger roll profile + therefore defines command 127 after a 255->127 approach as the sole + physical zero. The opposite branch remains offset in the published + direction-aware curve instead of being silently re-zeroed. + """ + name = str(joint_name) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return None + canonical = { + *(f"{finger}_mcp_roll" for finger in _FINGERS), + *(f"{finger}_mcp_roll_side" for finger in _FINGERS), + } + return "decreasing" if name in canonical else None + + +def _right_19_auxiliary_commands( + finger: str, *, side_view: bool, target_kind: str +) -> tuple[tuple[int, int], ...]: + """Return the reviewed right-hand fan/occlusion parking pose.""" + order = ("pinky", "ring", "middle", "index") + target_index = order.index(finger) + commands: dict[int, int] = {} + for other_index, other in enumerate(order): + if other == finger: + continue + roll_motor = _MOTOR_BY_JOINT[f"{other}_mcp_roll"] + if other_index < target_index: + commands[roll_motor] = 127 if side_view else 0 + else: + commands[roll_motor] = 255 + if side_view and other_index < target_index: + commands[_MOTOR_BY_JOINT[f"{other}_mcp_pitch"]] = 0 + commands[_MOTOR_BY_JOINT[f"{other}_pip"]] = 0 + if target_kind in {"pitch", "pip"}: + commands[_MOTOR_BY_JOINT[f"{finger}_mcp_roll"]] = 127 + if target_kind == "roll": + commands[_MOTOR_BY_JOINT[f"{finger}_mcp_pitch"]] = 255 + commands[_MOTOR_BY_JOINT[f"{finger}_pip"]] = 255 + elif target_kind == "pitch": + commands[_MOTOR_BY_JOINT[f"{finger}_pip"]] = 255 + elif target_kind == "pip": + commands[_MOTOR_BY_JOINT[f"{finger}_mcp_pitch"]] = 255 + return tuple(sorted((int(index), int(value)) for index, value in commands.items())) + + +def _build_right_19_profile() -> HandCalibrationProfile: + """Build the 19-Tag/16-motion-task right-hand profile.""" + view_tags: dict[str, dict[str, int]] = { + "front": { + "front_base": 0, + "thumb_cmc": 1, + "thumb_mcp": 2, + "thumb_ip": 3, + "pinky_roll": 10, + "ring_roll": 11, + "middle_roll": 12, + "index_roll": 13, + }, + "side": { + "side_base": 4, + "ring_pip": 5, + "pinky_pip": 6, + "pinky_dip": 7, + "ring_dip": 14, + "middle_pip": 15, + "middle_dip": 16, + "index_pip": 17, + "index_dip": 18, + }, + "top": {"top_base": 8, "thumb_yaw": 9}, + } + # Four adjacent fingers inevitably occlude some Tags in the + # common open-hand pose. Startup therefore proves only the three fixed + # palm references. Every moving Tag is still mandatory, but is checked + # by its task-local visibility sweep after the other fingers have moved to + # that task's clearance pose. Requiring all 15 at one instant would make + # a physically correct 19-Tag installation impossible to start. + preflight_view_roles = { + "front": ("front_base",), + "side": ("side_base",), + "top": ("top_base",), + } + joint_specs: dict[str, JointSpec] = { + "thumb_cmc_pitch": JointSpec( + "thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc", + zero_kind="urdf_axis_chain", + ), + "thumb_cmc_roll": JointSpec( + "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc", + zero_kind="urdf_axis_chain", + ), + "thumb_cmc_yaw": JointSpec( + "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw", + zero_kind="urdf_axis_chain", + ), + "thumb_mcp": JointSpec( + "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp", + zero_kind="urdf_axis_chain", + ), + "thumb_ip": JointSpec( + "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip", + ), + } + for finger in ("index", "middle", "ring", "pinky"): + roll = f"{finger}_mcp_roll" + pitch = f"{finger}_mcp_pitch" + pip = f"{finger}_pip" + dip = f"{finger}_dip" + joint_specs[roll] = JointSpec( + roll, _MOTOR_BY_JOINT[roll], True, "front", "front_base", + f"{finger}_roll", zero_kind="urdf_axis_chain", + ) + # The side PIP Tag is on the middle phalanx after PIP. With PIP held + # at 255 it observes MCP pitch; with MCP pitch held at 255 it observes + # PIP itself. + joint_specs[pitch] = JointSpec( + pitch, _MOTOR_BY_JOINT[pitch], True, "side", "side_base", + f"{finger}_pip", zero_kind="urdf_axis_chain", + ) + joint_specs[pip] = JointSpec( + pip, _MOTOR_BY_JOINT[pip], True, "side", "side_base", + f"{finger}_pip", zero_kind="urdf_axis_chain", + ) + # DIP is passive, but no longer unobserved. The PIP sweep moves the + # complete linkage once while the middle/distal Tag pair directly + # measures DIP. Its dynamic curve is therefore visual truth; its + # absolute static URDF zero deliberately remains source CAD because an + # arbitrary distal-Tag mounting angle is indistinguishable from a DIP + # zero offset without a mechanical zero fixture. + joint_specs[dip] = JointSpec( + dip, + _MOTOR_BY_JOINT[pip], + False, + "side", + f"{finger}_pip", + f"{finger}_dip", + ) + + measurement_specs = dict(joint_specs) + axis_validation_sources: dict[str, str] = {} + for finger in ("pinky", "ring", "middle", "index"): + canonical = f"{finger}_mcp_roll" + alias = f"{canonical}_side" + measurement_specs[alias] = JointSpec( + alias, + _MOTOR_BY_JOINT[canonical], + True, + "side", + "side_base", + f"{finger}_pip", + zero_kind="axis_cross_view_validation", + ) + axis_validation_sources[canonical] = alias + + sweeps: list[SweepSpec] = [ + SweepSpec( + "front", 0, ("thumb_cmc_pitch",), "thumb_cmc_pitch_front", + ((5, 255), (10, 255)), + ), + SweepSpec("front", 5, ("thumb_cmc_roll",), "thumb_cmc_roll_front"), + SweepSpec( + "front", 15, ("thumb_mcp", "thumb_ip"), "thumb_mcp_ip_front" + ), + SweepSpec( + "top", 10, ("thumb_cmc_yaw",), "thumb_cmc_yaw_top", ((5, 145),) + ), + ] + for finger in ("pinky", "ring", "middle", "index"): + roll_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_roll"] + pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] + pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] + sweeps.extend( + ( + SweepSpec( + "front", + roll_motor, + ( + f"{finger}_mcp_roll", + f"{finger}_mcp_roll_side", + ), + f"{finger}_roll_multiview", + _right_19_auxiliary_commands( + finger, side_view=True, target_kind="roll" + ), + ), + SweepSpec( + "side", + pitch_motor, + (f"{finger}_mcp_pitch",), + f"{finger}_pitch_side", + _right_19_auxiliary_commands( + finger, side_view=True, target_kind="pitch" + ), + ), + SweepSpec( + "side", + pip_motor, + (f"{finger}_pip", f"{finger}_dip"), + f"{finger}_pip_side", + _right_19_auxiliary_commands( + finger, side_view=True, target_kind="pip" + ), + ), + ) + ) + image_joints = frozenset( + { + "thumb_cmc_pitch", + "thumb_cmc_roll", + "thumb_mcp", + "thumb_ip", + *(f"{finger}_mcp_roll" for finger in ("index", "middle", "ring", "pinky")), + } + ) + return HandCalibrationProfile( + side="right", + reference_finger="pinky", + view_tags=view_tags, + preflight_view_roles=preflight_view_roles, + joint_specs=joint_specs, + sweep_specs=tuple(sweeps), + image_trajectory_joints=image_joints, + roll_clearance_commands={}, + thumb_pitch_clearance_commands={10: 255, 5: 255}, + layout_id=G20_RIGHT_19_LAYOUT, + measurement_specs=measurement_specs, + axis_validation_sources=axis_validation_sources, + # Observe the zero-adjacent roll and pitch axes from Tag 9 while the + # existing front sweeps run. Their same-view angle identifies the + # intervening yaw zero without adding a motion task or reference URDF. + palm_axis_observers=( + PalmAxisObserver( + source_name="thumb_cmc_pitch_top_axis", + task_name="thumb_cmc_pitch_front", + view="top", + parent_role="top_base", + child_role="thumb_yaw", + model_joint="thumb_cmc_pitch", + motor_index=joint_specs["thumb_cmc_pitch"].motor_index, + ), + PalmAxisObserver( + source_name="thumb_cmc_roll_top_axis", + task_name="thumb_cmc_roll_front", + view="top", + parent_role="top_base", + child_role="thumb_yaw", + model_joint="thumb_cmc_roll", + motor_index=joint_specs["thumb_cmc_roll"].motor_index, + ), + ), + minimum_palm_orientation_sources=2, + palm_orientation_minimum_arc_rad=math.radians(10.0), + palm_orientation_maximum_command_distance_u8=64, + capabilities=frozenset( + { + "precheck_sweeps", + "steady_command_checkpoints", + "directional_zero", + "isolated_holdout", + "cross_view_roll_curve", + "stable_cross_view_cone_bias", + "measured_passive_dips", + "urdf_zero_publication", + "palm_axis_relative_motion_v3", + } + ), + precheck_sweeps=True, + steady_command_checkpoints=True, + directional_zero=True, + isolated_holdout=True, + cross_view_roll_curve=True, + stable_cross_view_cone_bias=True, + ) + + +RIGHT_19_HAND_PROFILE = _build_right_19_profile() + + +def get_hand_calibration_profile( + side: str, layout_id: str = "legacy_11" +) -> HandCalibrationProfile: + value = str(side).lower() + layout = str(layout_id).lower() + if layout in {G20_RIGHT_19_LAYOUT, G20_RIGHT_15_LAYOUT}: + if value != "right": + raise ValueError("G20 right product layout requires hand side right") + return RIGHT_19_HAND_PROFILE + if layout != "legacy_11": + raise ValueError(f"unknown G20 tag layout: {layout_id}") + if value == "left": + return LEFT_HAND_PROFILE + if value == "right": + return RIGHT_HAND_PROFILE + raise ValueError("hand side must be left or right") + + +def derive_mimic_passive_fits( + source_urdf: str | Path, + fits: Mapping[str, JointCurveFit], + *, + profile: HandCalibrationProfile, +) -> dict[str, JointCurveFit]: + """Complete legacy non-visual DIP curves using the URDF mimic contract. + + The runtime curves are relative to the baseline command, so a URDF mimic + offset cancels while the multiplier scales the measured PIP motion. The + URDF itself remains byte-for-byte protected by publication checks. + """ + result = dict(fits) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return result + visually_measured = all( + profile.joint_specs[name].source_joint is None + for name in MIMIC_DERIVED_FINGER_DIPS + ) + if visually_measured: + missing = sorted(set(profile.joint_specs) - set(result)) + extra = sorted(set(result) - set(profile.joint_specs)) + if missing or extra: + raise ValueError( + f"runtime fit set is incomplete: missing={missing} extra={extra}" + ) + return result + path = Path(source_urdf).expanduser().resolve() + if not path.is_file(): + raise ValueError(f"source URDF does not exist: {path}") + joints = { + str(node.get("name")): node + for node in ET.parse(path).getroot().findall("joint") + } + for passive_name, source_name in MIMIC_DERIVED_FINGER_DIPS.items(): + spec = profile.joint_specs[passive_name] + if spec.source_joint != source_name: + raise ValueError(f"{passive_name} has the wrong curve source") + if source_name not in result: + raise ValueError(f"missing measured source curve {source_name}") + joint = joints.get(passive_name) + mimic = None if joint is None else joint.find("mimic") + if mimic is None or mimic.get("joint") != source_name: + raise ValueError( + f"source URDF {passive_name} must mimic {source_name}" + ) + multiplier = float(mimic.get("multiplier", "1")) + offset = float(mimic.get("offset", "0")) + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError(f"source URDF {passive_name} mimic multiplier is invalid") + if not math.isfinite(offset): + raise ValueError(f"source URDF {passive_name} mimic offset is invalid") + source = result[source_name] + + def scaled(values: Sequence[float]) -> tuple[float, ...]: + return tuple(multiplier * float(value) for value in values) + + result[passive_name] = JointCurveFit( + angle_rad=scaled(source.angle_rad), + decreasing_rad=scaled(source.decreasing_rad), + increasing_rad=scaled(source.increasing_rad), + circle={ + "curve_source": source_name, + "source_urdf_mimic_multiplier": multiplier, + "source_urdf_mimic_offset_rad": offset, + "visual_measurement": False, + }, + maximum_monotonic_correction_rad=( + abs(multiplier) * source.maximum_monotonic_correction_rad + ), + maximum_hysteresis_rad=( + abs(multiplier) * source.maximum_hysteresis_rad + ), + quality={ + "mimic_multiplier": multiplier, + "mimic_offset_rad": offset, + "visual_measurement": 0.0, + }, + zero_offset_rad=multiplier * source.zero_offset_rad, + ) + if set(result) != set(profile.joint_specs): + missing = sorted(set(profile.joint_specs) - set(result)) + extra = sorted(set(result) - set(profile.joint_specs)) + raise ValueError( + f"runtime fit set is incomplete: missing={missing} extra={extra}" + ) + return result + + +def clamp_runtime_fits_to_urdf_limits( + source_urdf: str | Path, + fits: Mapping[str, JointCurveFit], + *, + endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, +) -> dict[str, JointCurveFit]: + """Saturate published curves at protected physical limits. + + Raw trajectory fits and all fit/zero diagnostics remain untouched. The + returned transfer functions are the commands a URDF/MuJoCo runtime can + actually represent safely. An endpoint-anchored joint uses the corrected + coordinate upper limit ``CAD_upper - origin_offset``; its physical maximum + remains exactly the original CAD maximum after the origin correction. + """ + path = Path(source_urdf).expanduser().resolve() + joints = { + str(node.get("name")): node + for node in ET.parse(path).getroot().findall("joint") + } + result: dict[str, JointCurveFit] = {} + endpoint_offsets = { + str(name): float(value) + for name, value in dict(endpoint_anchored_offsets_rad or {}).items() + } + for name, fit in fits.items(): + joint = joints.get(name) + limit = None if joint is None else joint.find("limit") + if ( + limit is None + or limit.get("lower") is None + or limit.get("upper") is None + ): + raise ValueError(f"source URDF joint {name} has no finite position limit") + lower = float(limit.get("lower")) + upper = float(limit.get("upper")) + if name in endpoint_offsets: + upper -= endpoint_offsets[name] + if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: + raise ValueError(f"source URDF joint {name} has invalid position limits") + + clipped_counts: dict[str, int] = {} + + def clipped(values: Sequence[float], branch: str) -> tuple[float, ...]: + source = np.asarray(values, dtype=float) + bounded = np.clip(source, lower, upper) + clipped_counts[branch] = int(np.count_nonzero(bounded != source)) + return tuple(float(value) for value in bounded) + + circle = dict(fit.circle) + bounded_angle = clipped(fit.angle_rad, "angle_rad") + bounded_decreasing = clipped(fit.decreasing_rad, "decreasing_rad") + bounded_increasing = clipped(fit.increasing_rad, "increasing_rad") + if any(clipped_counts.values()): + circle["runtime_urdf_limit_clipped_bins"] = clipped_counts + circle["runtime_urdf_limits_rad"] = [lower, upper] + result[name] = replace( + fit, + angle_rad=bounded_angle, + decreasing_rad=bounded_decreasing, + increasing_rad=bounded_increasing, + circle=circle, + ) + return result + + +# Backwards-compatible aliases keep the existing left-hand API stable. +VIEW_TAGS = LEFT_HAND_PROFILE.view_tags +JOINT_SPECS = LEFT_HAND_PROFILE.joint_specs +SWEEP_SPECS = LEFT_HAND_PROFILE.sweep_specs +MEASURED_JOINTS = LEFT_HAND_PROFILE.measured_joints +ACTIVE_JOINTS = LEFT_HAND_PROFILE.active_joints +PASSIVE_JOINTS = LEFT_HAND_PROFILE.passive_joints +SPLAY_JOINTS: tuple[str, ...] = ( + "index_mcp_roll", + "middle_mcp_roll", + "ring_mcp_roll", + "pinky_mcp_roll", +) + +# These directly actuated motions have a fixed parent link during their sweep +# and a motion plane that is close to the active camera's image plane. Their +# projected circles are substantially more repeatable than the difference of +# two independently estimated planar-Tag PnP depths. thumb_ip remains projected +# because its front-view PnP depth is not repeatable enough for a 3-D fit. The +# side-view index_dip and oblique top-view yaw remain parent-relative full +# SE(3) measurements; their circle directions are constrained by the more +# repeatable relative-orientation screw axis in urdf_zero.py. +IMAGE_TRAJECTORY_JOINTS = LEFT_HAND_PROFILE.image_trajectory_joints + +# Keep the three unmeasured finger-roll motors away from the front camera's +# line of sight while index_mcp_roll is measured. +INDEX_ROLL_CLEARANCE_COMMANDS = LEFT_HAND_PROFILE.roll_clearance_commands + +# Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is +# measured. This keeps the moving top-view tag sufficiently front-facing. +THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = { + 5: 145, +} + + +def calibration_auxiliary_commands( + spec: SweepSpec, + *, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, +) -> dict[int, int]: + """Return motors that must remain fixed throughout one calibration task.""" + if spec.auxiliary_commands: + return {int(index): int(value) for index, value in spec.auxiliary_commands} + if spec.motor_index == 0: + return dict(profile.thumb_pitch_clearance_commands) + if spec.motor_index == profile.reference_roll_motor: + return dict(profile.roll_clearance_commands) + if spec.motor_index == 10: + return dict(THUMB_YAW_CLEARANCE_COMMANDS) + return {} + + +def build_full_hand_command( + motor_index: int, + command_u8: int, + baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, +) -> list[int]: + if len(baseline) != 20: + raise ValueError("baseline must contain exactly 20 values") + motor = int(motor_index) + if motor not in {spec.motor_index for spec in profile.joint_specs.values()}: + raise ValueError("motor_index is not a controlled G20 calibration channel") + command = int(command_u8) + if not 0 <= command <= 255: + raise ValueError("command_u8 must be in [0, 255]") + result = [int(value) for value in baseline] + if any(not 0 <= value <= 255 for value in result): + raise ValueError("baseline values must be in [0, 255]") + result[motor] = command + return result + + +def build_calibration_motion_command( + spec: SweepSpec, + command_u8: int, + baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, +) -> list[int]: + """Build a sweep command, including any required clearance pose.""" + result = build_full_hand_command( + spec.motor_index, + command_u8, + baseline=baseline, + profile=profile, + ) + for motor_index, auxiliary_command in calibration_auxiliary_commands( + spec, profile=profile + ).items(): + result[motor_index] = auxiliary_command + return result + + +def _append_single_motor_waypoint( + waypoints: list[tuple[int, ...]], current: list[int], motor: int, value: int +) -> None: + target = int(value) + if current[int(motor)] == target: + return + current[int(motor)] = target + waypoints.append(tuple(current)) + + +def build_calibration_preparation_waypoints( + spec: SweepSpec, + command_u8: int, + *, + current_command: Sequence[int], + baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, + parallel: bool = True, +) -> tuple[tuple[int, ...], ...]: + """Enter a task pose in the reviewed safe order. + + ``parallel`` merges the sequential entry into joint-class groups: all + neighbouring roll axes open together, then the clearance fingers flex + their PIPs together, then their MCP pitches, then any remaining auxiliary + motors, and the measured roll channel moves last. Every roll still opens + before any finger flexes, each finger's PIP still flexes before its + pitch, and the roll targets are unchanged, so no axis is forced across + its coupled mechanical boundary. + """ + if len(current_command) != 20: + raise ValueError("current_command must contain exactly 20 values") + final = build_calibration_motion_command( + spec, command_u8, baseline=baseline, profile=profile + ) + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return (tuple(final),) + current = [int(np.clip(round(float(value)), 0, 255)) for value in current_command] + controlled_motors = { + int(joint.motor_index) for joint in profile.joint_specs.values() + } + # G20 feedback channels 11..14 are reserved and remain zero in the SDK. + # Preserve their reported values in transition vectors; waiting for the + # nominal 255 baseline on a non-existent actuator would deadlock before the + # first real motor is allowed to move. + for motor in range(20): + if motor not in controlled_motors: + final[motor] = current[motor] + auxiliary = calibration_auxiliary_commands(spec, profile=profile) + finger_order = ("pinky", "ring", "middle", "index") + handled: set[int] = set() + capture_views = { + profile.record_specs[joint_name].view for joint_name in spec.joints + } + target_roll_motor: int | None = None + lower_roll_motors: list[int] = [] + if "side" in capture_views: + target_finger = str(spec.joints[0]).split("_", 1)[0] + target_roll_motor = _MOTOR_BY_JOINT[f"{target_finger}_mcp_roll"] + lower_roll_motors = list(range(9, target_roll_motor, -1)) + if parallel: + roll_group: list[int] = [] + pip_group: list[int] = [] + pitch_group: list[int] = [] + if target_roll_motor is not None: + for motor in range(6, target_roll_motor): + if auxiliary.get(motor) == 255: + roll_group.append(motor) + handled.add(motor) + for roll_motor in lower_roll_motors: + roll_group.append(roll_motor) + handled.add(roll_motor) + finger = finger_order[9 - roll_motor] + pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] + pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] + if auxiliary.get(pip_motor) == 0: + pip_group.append(pip_motor) + handled.add(pip_motor) + if auxiliary.get(pitch_motor) == 0: + pitch_group.append(pitch_motor) + handled.add(pitch_motor) + other_group = [ + motor + for motor in sorted(controlled_motors | set(auxiliary)) + if motor not in handled + and motor != spec.motor_index + and current[motor] != final[motor] + ] + waypoints: list[tuple[int, ...]] = [] + _append_class_waypoint(waypoints, current, final, roll_group) + _append_class_waypoint(waypoints, current, final, pip_group) + _append_class_waypoint(waypoints, current, final, pitch_group) + _append_class_waypoint(waypoints, current, final, other_group) + # The measured channel moves last, after the swept volume is clear. + _append_single_motor_waypoint( + waypoints, current, spec.motor_index, final[spec.motor_index] + ) + return tuple(waypoints or [tuple(final)]) + waypoints = [] + if "side" in capture_views: + # Side-view clearance never forces a neighbouring roll axis across its + # coupled mechanical boundary. Real sessions showed neighbouring + # roll axes saturating at feedback 64 (motor 9) and 58 (motor 8) when + # commanded to 0 in a coupled avoidance pose. Keep those roll axes at + # neutral 127 and remove occlusion only by flexing the neighbour. + for motor in range(6, target_roll_motor): + if auxiliary.get(motor) == 255: + _append_single_motor_waypoint( + waypoints, current, motor, final[motor] + ) + handled.add(motor) + for roll_motor in lower_roll_motors: + if roll_motor in auxiliary: + _append_single_motor_waypoint( + waypoints, current, roll_motor, final[roll_motor] + ) + handled.add(roll_motor) + finger = finger_order[9 - roll_motor] + pip_motor = _MOTOR_BY_JOINT[f"{finger}_pip"] + pitch_motor = _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"] + # Once the finger has rolled away from its neighbour, flex it out + # of the camera line of sight using the reviewed PIP->pitch order. + if auxiliary.get(pip_motor) == 0: + _append_single_motor_waypoint( + waypoints, current, pip_motor, final[pip_motor] + ) + handled.add(pip_motor) + if auxiliary.get(pitch_motor) == 0: + _append_single_motor_waypoint( + waypoints, current, pitch_motor, final[pitch_motor] + ) + handled.add(pitch_motor) + # Move the measured roll only after its neighbours have flexed out of + # its swept volume. Pitch/PIP tasks leave this roll neutral throughout. + if target_roll_motor == spec.motor_index: + _append_single_motor_waypoint( + waypoints, + current, + target_roll_motor, + final[target_roll_motor], + ) + handled.add(target_roll_motor) + for motor in sorted(auxiliary): + if motor not in handled and motor != spec.motor_index: + _append_single_motor_waypoint( + waypoints, current, motor, final[motor] + ) + if spec.motor_index not in handled: + _append_single_motor_waypoint( + waypoints, current, spec.motor_index, final[spec.motor_index] + ) + if not waypoints or waypoints[-1] != tuple(final): + # Normally every difference has already been visited. Keep this + # assertion-style fallback explicit so a future profile field cannot + # silently bypass the one-motor transition policy. + differing = [ + index for index, (left, right) in enumerate(zip(current, final)) + if index in controlled_motors and left != right + ] + for motor in differing: + _append_single_motor_waypoint( + waypoints, current, motor, final[motor] + ) + return tuple(waypoints or [tuple(final)]) + + +def _append_class_waypoint( + waypoints: list[tuple[int, ...]], + current: list[int], + target: Sequence[int], + motors: Iterable[int], +) -> None: + """Move one joint-class group of motors in a single parallel waypoint.""" + moved = False + for motor in motors: + if current[int(motor)] != target[int(motor)]: + current[int(motor)] = target[int(motor)] + moved = True + if moved: + waypoints.append(tuple(current)) + + +def build_calibration_return_waypoints( + target_command: Sequence[int], + *, + current_command: Sequence[int], + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, + anchor_roll_motor: int | None = None, + parallel: bool = True, +) -> tuple[tuple[int, ...], ...]: + """Leave a product-layout avoidance pose using the inverse safe sequence. + + ``parallel`` merges the reviewed sequential order into joint-class groups: + all differing rolls return to neutral together, then all differing MCP + pitches, then all differing PIPs, then the remaining controlled motors. + Every finger still unfolds only after every roll is back at neutral, each + finger's pitch still moves before its PIP, and simultaneous same-direction + roll motion removes the "one finger sweeps across parked neighbours" + stall scenario that motivated the anchored one-motor sequence. + """ + if len(current_command) != 20 or len(target_command) != 20: + raise ValueError("return commands must contain exactly 20 values") + target = [int(value) for value in target_command] + if profile.layout_id != G20_RIGHT_19_LAYOUT: + return (tuple(target),) + current = [int(np.clip(round(float(value)), 0, 255)) for value in current_command] + controlled_motors = { + int(joint.motor_index) for joint in profile.joint_specs.values() + } + for motor in range(20): + if motor not in controlled_motors: + target[motor] = current[motor] + if parallel: + waypoints: list[tuple[int, ...]] = [] + _append_class_waypoint(waypoints, current, target, range(6, 10)) + _append_class_waypoint(waypoints, current, target, range(1, 5)) + _append_class_waypoint(waypoints, current, target, range(16, 20)) + _append_class_waypoint( + waypoints, + current, + target, + sorted( + motor + for motor in controlled_motors + if motor not in set(range(6, 10)) + | set(range(1, 5)) + | set(range(16, 20)) + ), + ) + return tuple(waypoints or [tuple(target)]) + waypoints = [] + handled: set[int] = set() + # All four roll channels return to neutral before any bent clearance + # finger is unfolded. In the reviewed product sequence the current/next + # target finger must move first, then neighbouring fingers move outward. + # Returning index first from an all-255 pinky task makes it converge into + # three still-parked fingers and can mechanically stall around command175. + roll_order: Sequence[int] = tuple(range(6, 10)) + anchor = None if anchor_roll_motor is None else int(anchor_roll_motor) + if profile.layout_id == G20_RIGHT_19_LAYOUT: + if anchor in range(6, 10): + roll_order = tuple( + sorted( + range(6, 10), + key=lambda motor: (abs(motor - anchor), -motor), + ) + ) + else: + roll_order = tuple(reversed(range(6, 10))) + for motor in roll_order: + if current[motor] != target[motor]: + _append_single_motor_waypoint( + waypoints, current, motor, target[motor] + ) + handled.add(motor) + # Reverse the entry order, one finger at a time: roll, pitch, then PIP. + for finger in ("index", "middle", "ring", "pinky"): + for motor in ( + _MOTOR_BY_JOINT[f"{finger}_mcp_pitch"], + _MOTOR_BY_JOINT[f"{finger}_pip"], + ): + if current[motor] != target[motor]: + _append_single_motor_waypoint( + waypoints, current, motor, target[motor] + ) + handled.add(motor) + for motor in sorted(controlled_motors): + if motor not in handled and current[motor] != target[motor]: + _append_single_motor_waypoint( + waypoints, current, motor, target[motor] + ) + return tuple(waypoints or [tuple(target)]) + + +def build_calibration_speed_profile( + spec: SweepSpec, + *, + normal_speed: int, + index_roll_speed: int, + index_flex_speed: int, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, +) -> list[int]: + """Return G20's five per-finger speeds for one calibration task.""" + normal = int(normal_speed) + index_roll = int(index_roll_speed) + index_flex = int(index_flex_speed) + if not all( + 0 <= speed <= 255 for speed in (normal, index_roll, index_flex) + ): + raise ValueError("calibration speeds must be in [0, 255]") + speeds = [normal] * 5 + if spec.motor_index in range(6, 10): + speeds[spec.motor_index - 5] = index_roll + elif spec.motor_index in range(1, 5): + speeds[spec.motor_index] = index_flex + elif spec.motor_index in range(16, 20): + speeds[spec.motor_index - 15] = index_flex + return speeds + + +def _record_vector(record: Mapping[str, Any]) -> np.ndarray: + value = np.asarray(record.get("relative_translation_xyz_m"), dtype=float) + if value.shape != (3,) or not np.all(np.isfinite(value)): + raise ValueError("record relative_translation_xyz_m must contain 3 values") + return value + + +def _record_image_vector(record: Mapping[str, Any]) -> np.ndarray: + value = np.asarray(record.get("image_relative_xy_px"), dtype=float) + if value.shape != (2,) or not np.all(np.isfinite(value)): + raise ValueError("record image_relative_xy_px must contain 2 values") + return value + + +def fit_joint_center_curve( + records: Sequence[Mapping[str, Any]], + *, + maximum_plane_rms_m: float = 0.004, + maximum_radial_rms_m: float = 0.004, + minimum_radius_m: float = 0.003, + minimum_arc_rad: float = math.radians(15.0), +) -> JointCurveFit: + """Fit one command-indexed curve from a parent-frame centre trajectory.""" + samples = [dict(record) for record in records] + if len(samples) < 12: + raise ValueError("joint trajectory requires at least 12 samples") + points = np.asarray([_record_vector(record) for record in samples]) + axis, common_plane_rms = _fit_plane_axis([points]) + circle = _fit_circle_with_axis(points, axis) + circle = _orient_circle_positive(circle, samples, list(points)) + values = [_angle_for_circle(point, circle) for point in points] + curves, correction, hysteresis = _fit_joint_curve(samples, values) + quality = { + "plane_rms_m": max( + float(common_plane_rms), float(circle["plane_rms_m"]) + ), + "radial_rms_m": float(circle["radial_rms_m"]), + "radius_m": float(circle["radius_m"]), + "arc_rad": float(circle["observed_arc_rad"]), + } + failures: list[str] = [] + if quality["plane_rms_m"] > float(maximum_plane_rms_m): + failures.append("plane_rms") + if quality["radial_rms_m"] > float(maximum_radial_rms_m): + failures.append("radial_rms") + if quality["radius_m"] < float(minimum_radius_m): + failures.append("radius") + if quality["arc_rad"] < float(minimum_arc_rad): + failures.append("arc") + if failures: + raise ValueError("joint_trajectory_quality_failed:" + ",".join(failures)) + return JointCurveFit( + angle_rad=tuple(float(value) for value in curves["angle_rad"]), + decreasing_rad=tuple( + float(value) for value in curves["decreasing_rad"] + ), + increasing_rad=tuple( + float(value) for value in curves["increasing_rad"] + ), + circle=dict(circle), + maximum_monotonic_correction_rad=float(correction), + maximum_hysteresis_rad=float(hysteresis), + quality=quality, + ) + + +def _signed_image_circle_angle( + point_xy_px: Sequence[float], circle: Mapping[str, Any] +) -> float: + point = np.asarray(point_xy_px, dtype=float) + centre = np.asarray(circle["center_xy_px"], dtype=float) + reference = np.asarray(circle["reference_xy_px"], dtype=float) + vector = point - centre + if point.shape != (2,) or not np.all(np.isfinite(point)): + raise ValueError("image point must contain 2 finite values") + if float(np.linalg.norm(vector)) < 1.0e-9: + raise ValueError("image point lies at the fitted circle centre") + return float(circle["orientation_sign"]) * math.atan2( + float(reference[0] * vector[1] - reference[1] * vector[0]), + float(np.dot(reference, vector)), + ) + + +def fit_joint_image_curve( + records: Sequence[Mapping[str, Any]], + *, + maximum_radial_rms_px: float = 2.0, + maximum_radial_p95_px: float = 3.5, + minimum_radius_px: float = 20.0, + minimum_arc_rad: float = math.radians(15.0), +) -> JointCurveFit: + """Fit angular phase from a stable projected parent/child circle.""" + samples = [dict(record) for record in records] + if len(samples) < 12: + raise ValueError("joint image trajectory requires at least 12 samples") + points = np.asarray([_record_image_vector(record) for record in samples]) + centre, radius = _fit_circle(points) + radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius) + quality = { + "radial_rms_px": float(np.sqrt(np.mean(np.square(radial_error)))), + "radial_p95_px": float(np.percentile(radial_error, 95.0)), + "radius_px": float(radius), + "arc_rad": float(_trajectory_arc_rad(points, centre)), + } + failures: list[str] = [] + if quality["radial_rms_px"] > float(maximum_radial_rms_px): + failures.append("radial_rms") + if quality["radial_p95_px"] > float(maximum_radial_p95_px): + failures.append("radial_p95") + if quality["radius_px"] < float(minimum_radius_px): + failures.append("radius") + if quality["arc_rad"] < float(minimum_arc_rad): + failures.append("arc") + if failures: + raise ValueError( + "joint_image_trajectory_quality_failed:" + ",".join(failures) + ) + + endpoint_points = np.asarray( + [ + point + for record, point in zip(samples, points) + if int(record["command_u8"]) == 255 + ] + ) + if endpoint_points.size == 0: + raise ValueError("joint image trajectory is missing command 255") + reference = np.median(endpoint_points, axis=0) - centre + reference_norm = float(np.linalg.norm(reference)) + if reference_norm < 1.0e-9: + raise ValueError("joint image trajectory endpoint is degenerate") + reference /= reference_norm + circle: dict[str, Any] = { + "space": "image_2d", + "center_xy_px": [float(value) for value in centre], + "reference_xy_px": [float(value) for value in reference], + "orientation_sign": 1.0, + "radius_px": float(radius), + } + values = [_signed_image_circle_angle(point, circle) for point in points] + command_zero_values = [ + value + for record, value in zip(samples, values) + if int(record["command_u8"]) == 0 + ] + if not command_zero_values: + raise ValueError("joint image trajectory is missing command 0") + if float(np.median(command_zero_values)) < 0.0: + circle["orientation_sign"] = -1.0 + values = [-value for value in values] + curves, correction, hysteresis = _fit_joint_curve(samples, values) + return JointCurveFit( + angle_rad=tuple(float(value) for value in curves["angle_rad"]), + decreasing_rad=tuple( + float(value) for value in curves["decreasing_rad"] + ), + increasing_rad=tuple( + float(value) for value in curves["increasing_rad"] + ), + circle=circle, + maximum_monotonic_correction_rad=float(correction), + maximum_hysteresis_rad=float(hysteresis), + quality=quality, + ) + + +def fit_measured_joint_curve( + joint_name: str, + records: Sequence[Mapping[str, Any]], + *, + maximum_plane_rms_m: float = 0.004, + maximum_radial_rms_m: float = 0.004, + minimum_radius_m: float = 0.003, + minimum_arc_rad: float = math.radians(15.0), + image_maximum_radial_rms_px: float = 2.0, + image_maximum_radial_p95_px: float = 3.5, + image_minimum_radius_px: float = 20.0, + profile: HandCalibrationProfile = LEFT_HAND_PROFILE, +) -> JointCurveFit: + """Select the stable trajectory representation for one measured joint.""" + if str(joint_name) in profile.image_trajectory_joints: + return fit_joint_image_curve( + records, + maximum_radial_rms_px=image_maximum_radial_rms_px, + maximum_radial_p95_px=image_maximum_radial_p95_px, + minimum_radius_px=image_minimum_radius_px, + minimum_arc_rad=minimum_arc_rad, + ) + return fit_joint_center_curve( + records, + maximum_plane_rms_m=maximum_plane_rms_m, + maximum_radial_rms_m=maximum_radial_rms_m, + minimum_radius_m=minimum_radius_m, + minimum_arc_rad=minimum_arc_rad, + ) + + +def center_splay_curve(fit: JointCurveFit) -> tuple[JointCurveFit, int, float]: + """Centre a measured 255->0 splay travel on its angular midpoint.""" + raw = np.asarray(fit.angle_rad, dtype=float) + if raw.shape != (256,) or not np.all(np.isfinite(raw)): + raise ValueError("splay curve must contain 256 finite values") + if np.any(np.diff(raw) > 1.0e-7): + raise ValueError("raw splay curve must be non-increasing") + travel_midpoint = 0.5 * float(raw[0] + raw[255]) + if travel_midpoint <= 0.0: + raise ValueError("splay travel must be positive") + centred = raw - travel_midpoint + # Make the public endpoint symmetry exact after decimal rounding. + endpoint = round(0.5 * float(raw[0] - raw[255]), 8) + centred[0] = endpoint + centred[255] = -endpoint + centred = np.asarray([round(float(value), 8) for value in centred]) + zero_command = int(np.argmin(np.abs(centred))) + return ( + replace( + fit, + angle_rad=tuple(float(value) for value in centred), + zero_offset_rad=round(float(travel_midpoint), 8), + ), + zero_command, + round(float(travel_midpoint), 8), + ) + + +def measure_joint_vector(fit: JointCurveFit, vector_xyz_m: Sequence[float]) -> float: + """Measure one vector with a fitted model, including splay centring.""" + return float( + _angle_for_circle(vector_xyz_m, fit.circle) - fit.zero_offset_rad + ) + + +def measure_joint_observation( + fit: JointCurveFit, + *, + vector_xyz_m: Sequence[float], + image_vector_xy_px: Sequence[float], +) -> float: + """Measure an observation in the same space used to fit its curve.""" + if fit.circle.get("space") == "image_2d": + return float( + _signed_image_circle_angle(image_vector_xy_px, fit.circle) + - fit.zero_offset_rad + ) + return measure_joint_vector(fit, vector_xyz_m) + + +def compare_cross_view_roll_curves( + primary: JointCurveFit, + secondary: JointCurveFit, + *, + maximum_rms_difference_rad: float = math.radians(1.0), + maximum_branch_gap_difference_rad: float = math.radians(0.3), + allow_projection_scale: bool = False, + maximum_projection_scale_ratio: float = 1.5, +) -> dict[str, float]: + """Validate two independently fitted roll curves before axis fusion. + + The G20 right roll primary is an image-plane centre trajectory while its + side validation is a 3-D rotation trajectory. A fixed Tag mounting tilt + changes the image-plane angular scale without changing the mechanism. In + projection-scale mode, remove that one constant scale before comparing + curve shape. Direction, scale bounds, direction-dependent branch gap and + every single-view quality/holdout gate remain independent hard checks. + """ + if maximum_rms_difference_rad <= 0.0: + raise ValueError("cross-view RMS limit must be positive") + if maximum_branch_gap_difference_rad <= 0.0: + raise ValueError("cross-view branch-gap limit must be positive") + if maximum_projection_scale_ratio <= 1.0: + raise ValueError("cross-view projection-scale ratio must exceed one") + + fields: dict[str, tuple[np.ndarray, np.ndarray]] = {} + for field_name in ("angle_rad", "decreasing_rad", "increasing_rad"): + left = np.asarray(getattr(primary, field_name), dtype=float) + right = np.asarray(getattr(secondary, field_name), dtype=float) + if left.shape != (256,) or right.shape != (256,): + raise ValueError("cross-view curves must contain 256 values") + if not np.all(np.isfinite(left)) or not np.all(np.isfinite(right)): + raise ValueError("cross-view curves must be finite") + fields[field_name] = (left, right) + + primary_travel = float( + fields["angle_rad"][0][0] - fields["angle_rad"][0][255] + ) + secondary_travel = float( + fields["angle_rad"][1][0] - fields["angle_rad"][1][255] + ) + if primary_travel * secondary_travel <= 0.0: + raise ValueError("cross_view_roll_curve_direction_disagrees") + + primary_zero = int(primary.circle.get("zero_command_u8", 127)) + secondary_zero = int(secondary.circle.get("zero_command_u8", 127)) + if not 0 <= primary_zero <= 255 or not 0 <= secondary_zero <= 255: + raise ValueError("cross-view zero command must be in [0, 255]") + primary_anchor = float(primary.decreasing_rad[primary_zero]) + secondary_anchor = float(secondary.decreasing_rad[secondary_zero]) + projection_scale = 1.0 + if allow_projection_scale: + primary_values = np.concatenate( + [left - primary_anchor for left, _ in fields.values()] + ) + secondary_values = np.concatenate( + [right - secondary_anchor for _, right in fields.values()] + ) + denominator = float(secondary_values @ secondary_values) + if denominator <= np.finfo(float).eps: + raise ValueError("cross_view_roll_projection_scale_unobservable") + projection_scale = float( + (primary_values @ secondary_values) / denominator + ) + if projection_scale <= 0.0 or not math.isfinite(projection_scale): + raise ValueError("cross_view_roll_curve_direction_disagrees") + scale_ratio = max(projection_scale, 1.0 / projection_scale) + if scale_ratio > float(maximum_projection_scale_ratio): + raise ValueError( + "cross_view_roll_projection_scale_ratio_too_large:" + f"{scale_ratio:.6f}" + ) + + metrics: dict[str, float] = { + "projection_scale": projection_scale, + "projection_scale_ratio": max( + projection_scale, 1.0 / projection_scale + ), + } + for field_name, (left, right) in fields.items(): + metrics[f"raw_{field_name}_rms_difference_rad"] = float( + np.sqrt(np.mean(np.square(left - right))) + ) + if allow_projection_scale: + left = left - primary_anchor + right = projection_scale * (right - secondary_anchor) + rms = float(np.sqrt(np.mean(np.square(left - right)))) + metrics[f"{field_name}_rms_difference_rad"] = rms + if rms > maximum_rms_difference_rad: + raise ValueError( + "cross_view_roll_curve_difference_too_large:" + f"{field_name}:{math.degrees(rms):.6f}deg" + ) + metrics["travel_difference_rad"] = abs(primary_travel - secondary_travel) + primary_gap = float( + primary.increasing_rad[primary_zero] + - primary.decreasing_rad[primary_zero] + ) + secondary_gap = float( + secondary.increasing_rad[secondary_zero] + - secondary.decreasing_rad[secondary_zero] + ) + metrics["raw_baseline_branch_gap_difference_rad"] = abs( + primary_gap - secondary_gap + ) + branch_gap_difference = abs( + primary_gap - projection_scale * secondary_gap + ) + metrics["baseline_branch_gap_difference_rad"] = branch_gap_difference + # In projection-scale mode the primary is an image-circle phase while the + # secondary is a 3-D rotation angle. A fixed Tag tilt/offset changes the + # image projection of the two motion branches independently, so their + # absolute gap is not a cross-view invariant. Both views retain their + # own hard absolute-gap and four-cycle gap-range checks in the acquisition + # quality gate. Keep this value as a diagnostic, but apply the cross-view + # absolute-gap gate only when both curves live in comparable coordinates. + metrics["baseline_branch_gap_gate_applied"] = float( + not allow_projection_scale + ) + if ( + not allow_projection_scale + and branch_gap_difference > maximum_branch_gap_difference_rad + ): + raise ValueError( + "cross_view_roll_branch_gap_difference_too_large:" + f"{math.degrees(branch_gap_difference):.6f}deg" + ) + return metrics + + +def cross_view_roll_diagnostic_metrics( + primary: JointCurveFit, + secondary: JointCurveFit, +) -> dict[str, float]: + """Compare a validation-only roll view without creating a retry gate. + + G20-right publishes the front image-plane roll curve. The side PIP-link + observation supplies an independently quality-gated physical axis-line + position, but its angle includes projection and linkage effects and is + therefore diagnostic rather than a second measurement of the published + curve. Live fitting and offline replay must use the same non-blocking + comparison; otherwise a stable view bias can recall an already complete + task even though reacquiring the same motion cannot change the result. + """ + try: + return compare_cross_view_roll_curves( + primary, + secondary, + maximum_rms_difference_rad=float("inf"), + maximum_branch_gap_difference_rad=float("inf"), + allow_projection_scale=True, + maximum_projection_scale_ratio=float("inf"), + ) + except ValueError as error: + if str(error) == "cross_view_roll_curve_direction_disagrees": + return {"direction_disagrees": 1.0} + raise + + +def fit_projected_zero( + records: Sequence[Mapping[str, Any]], + *, + minimum_radius_px: float = 20.0, + minimum_arc_rad: float = math.radians(15.0), + maximum_radial_rms_px: float = 2.0, + maximum_radial_p95_px: float = 3.5, + maximum_round_difference_rad: float = math.radians(1.0), +) -> float: + """Return the image/table-projected zero from three sweep rounds.""" + samples = [dict(record) for record in records] + points = np.asarray( + [record.get("image_relative_xy_px") for record in samples], dtype=float + ) + if points.ndim != 2 or points.shape[1] != 2 or len(points) < 12: + raise ValueError("projected zero requires 2-D trajectory samples") + if not np.all(np.isfinite(points)): + raise ValueError("projected zero samples must be finite") + centre, radius = _fit_circle(points) + radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius) + radial_rms = float(np.sqrt(np.mean(np.square(radial_error)))) + radial_p95 = float(np.percentile(radial_error, 95.0)) + arc = _trajectory_arc_rad(points, centre) + if radius < minimum_radius_px: + raise ValueError("projected_zero_radius_too_small") + if arc < minimum_arc_rad: + raise ValueError("projected_zero_arc_too_small") + if radial_rms > maximum_radial_rms_px: + raise ValueError("projected_zero_radial_rms_too_large") + if radial_p95 > maximum_radial_p95_px: + raise ValueError("projected_zero_radial_p95_too_large") + + cycles = sorted({int(record["cycle"]) for record in samples}) + if len(cycles) < 3: + raise ValueError("projected zero requires three sweep rounds") + angles: list[float] = [] + for cycle in cycles: + zero_points = np.asarray( + [ + record["image_relative_xy_px"] + for record in samples + if int(record["cycle"]) == cycle + and int(record["command_u8"]) == 255 + ], + dtype=float, + ) + if zero_points.size == 0: + raise ValueError("projected zero is missing command 255") + point = np.median(zero_points, axis=0) + inward = centre - point + if float(np.linalg.norm(inward)) < 1.0e-9: + raise ValueError("projected zero point is degenerate") + angles.append( + wrap_angle_rad(math.atan2(-float(inward[1]), float(inward[0]))) + ) + if maximum_pairwise_angle_difference_rad(angles) > maximum_round_difference_rad: + raise ValueError("projected_zero_round_difference_too_large") + return round(float(circular_median_rad(angles)), 8) + + +def build_compact_payload( + *, + serial_number: str, + measured_fits: Mapping[str, JointCurveFit], + urdf_zero_offsets_rad: Mapping[str, float], + validation_errors_rad: Sequence[float], + passed: bool, + baseline: Sequence[int] = THREE_CAMERA_BASELINE_COMMAND, + side: str = "left", + layout_id: str = "legacy_11", + zero_uncertainty_rad: Mapping[str, float] | None = None, + zero_cycle_offsets_rad: Mapping[str, Sequence[float]] | None = None, + zero_observers: Mapping[str, str] | None = None, + artifact_hashes: Mapping[str, str] | None = None, + cross_view_roll_metrics: Mapping[str, Mapping[str, float]] | None = None, + joint_dynamic_diagnostics: Mapping[str, Mapping[str, Any]] | None = None, + zero_geometry_diagnostics: Mapping[str, Any] | None = None, +) -> dict[str, Any]: + profile = get_hand_calibration_profile(side, layout_id) + expected_fits = ( + set(profile.joint_specs) + if profile.layout_id == G20_RIGHT_19_LAYOUT + else set(profile.measured_joints) + ) + if set(measured_fits) != expected_fits: + raise ValueError("measured_fits has the wrong runtime joint set") + expected_active = { + name for name, spec in profile.joint_specs.items() if spec.active + } + if set(urdf_zero_offsets_rad) != expected_active: + raise ValueError("urdf_zero_offsets_rad has the wrong active-joint set") + if len(baseline) != 20: + raise ValueError("baseline must contain exactly 20 commands") + + if profile.layout_id == G20_RIGHT_19_LAYOUT: + return _build_right_19_compact_payload( + serial_number=serial_number, + measured_fits=measured_fits, + urdf_zero_offsets_rad=urdf_zero_offsets_rad, + validation_errors_rad=validation_errors_rad, + passed=passed, + baseline=baseline, + profile=profile, + zero_uncertainty_rad=zero_uncertainty_rad or {}, + zero_cycle_offsets_rad=zero_cycle_offsets_rad or {}, + zero_observers=zero_observers or {}, + artifact_hashes=artifact_hashes or {}, + cross_view_roll_metrics=cross_view_roll_metrics or {}, + joint_dynamic_diagnostics=joint_dynamic_diagnostics or {}, + zero_geometry_diagnostics=zero_geometry_diagnostics or {}, + ) + + joints: dict[str, dict[str, Any]] = {} + for name, spec in profile.joint_specs.items(): + source_name = spec.source_joint or name + fit = measured_fits[source_name] + joint: dict[str, Any] = { + "motor_index": int(spec.motor_index), + "angle_rad": [round(float(value), 8) for value in fit.angle_rad], + } + if spec.active: + joint["zero_command_u8"] = int(baseline[spec.motor_index]) + joint["zero_angles"] = { + "urdf_zero_offset_rad": round( + float(urdf_zero_offsets_rad[name]), 8 + ) + } + else: + joint["passive"] = True + if spec.source_joint is not None: + joint["source_joint"] = spec.source_joint + joints[name] = joint + + errors = np.abs(np.asarray(validation_errors_rad, dtype=float)) + mae = float(np.mean(errors)) if errors.size else float("nan") + p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan") + payload = { + "schema_version": 4, + "model": "G20", + "side": profile.side, + "serial_number": str(serial_number), + "angle_unit": "rad", + "command_range": [0, 255], + "baseline_command_u8": [int(value) for value in baseline], + "joints": joints, + "quality": { + "passed": bool(passed), + "validation_mae_rad": ( + None if not math.isfinite(mae) else round(mae, 8) + ), + "validation_p95_rad": ( + None if not math.isfinite(p95) else round(p95, 8) + ), + }, + } + validate_compact_payload(payload) + return payload + + +def _build_right_19_compact_payload( + *, + serial_number: str, + measured_fits: Mapping[str, JointCurveFit], + urdf_zero_offsets_rad: Mapping[str, float], + validation_errors_rad: Sequence[float], + passed: bool, + baseline: Sequence[int], + profile: HandCalibrationProfile, + zero_uncertainty_rad: Mapping[str, float], + zero_cycle_offsets_rad: Mapping[str, Sequence[float]], + zero_observers: Mapping[str, str], + artifact_hashes: Mapping[str, str], + cross_view_roll_metrics: Mapping[str, Mapping[str, float]], + joint_dynamic_diagnostics: Mapping[str, Mapping[str, Any]], + zero_geometry_diagnostics: Mapping[str, Any], +) -> dict[str, Any]: + # The public runtime artifact intentionally keeps the established schema-v4 + # contract. Directional curves, uncertainty, hashes and geometry metrics + # are calibration diagnostics and belong in calibration_summary_zh.json; + # exposing them here made it too easy for a runtime to apply a feedback + # curve or a zero offset twice. + del ( + zero_uncertainty_rad, + zero_cycle_offsets_rad, + zero_observers, + artifact_hashes, + cross_view_roll_metrics, + joint_dynamic_diagnostics, + zero_geometry_diagnostics, + ) + joints: dict[str, dict[str, Any]] = {} + for name, spec in profile.joint_specs.items(): + fit = measured_fits[name] + zero_command = int(baseline[spec.motor_index]) + decreasing = np.asarray(fit.decreasing_rad, dtype=float) + increasing = np.asarray(fit.increasing_rad, dtype=float) + if decreasing.shape != (256,) or increasing.shape != (256,): + raise ValueError(f"{name} directional curves must contain 256 values") + # A single schema-v4 curve is the steady-state midpoint of both + # approach directions. Publication separately rejects a branch gap + # above 2 degrees, bounding the midpoint's direction error to 1 degree. + curve = 0.5 * (decreasing + increasing) + curve -= float(curve[zero_command]) + joint: dict[str, Any] = { + "motor_index": int(spec.motor_index), + "angle_rad": [round(float(value), 8) for value in curve], + } + if spec.active: + joint["zero_command_u8"] = zero_command + joint["zero_angles"] = { + "urdf_zero_offset_rad": round( + float(urdf_zero_offsets_rad[name]), 8 + ) + } + else: + joint["passive"] = True + joints[name] = joint + + errors = np.abs(np.asarray(validation_errors_rad, dtype=float)) + mae = float(np.mean(errors)) if errors.size else float("nan") + p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan") + payload = { + "schema_version": 4, + "model": "G20", + "side": "right", + "serial_number": str(serial_number), + "angle_unit": "rad", + "command_range": [0, 255], + "baseline_command_u8": [int(value) for value in baseline], + "joints": joints, + "quality": { + "passed": bool(passed), + "validation_mae_rad": ( + None if not math.isfinite(mae) else round(mae, 8) + ), + "validation_p95_rad": ( + None if not math.isfinite(p95) else round(p95, 8) + ), + }, + } + validate_compact_payload(payload) + return payload + + +def infer_compact_payload_layout(payload: Mapping[str, Any]) -> str: + """Infer the layout for schema-v4 while honoring explicit newer schemas.""" + explicit = str(payload.get("layout_id", "")).strip().lower() + if explicit: + return explicit + raw_joints = payload.get("joints", {}) + independent_right = bool( + str(payload.get("side", "")).lower() == "right" + and isinstance(raw_joints, Mapping) + and raw_joints + and all( + not isinstance(value, Mapping) or "source_joint" not in value + for value in raw_joints.values() + ) + ) + return G20_RIGHT_19_LAYOUT if independent_right else "legacy_11" + + +def validate_compact_payload(payload: Mapping[str, Any]) -> None: + if payload.get("schema_version") == 5: + _validate_right_19_compact_payload(payload) + return + expected_top = { + "schema_version", "model", "side", "serial_number", "angle_unit", + "command_range", "baseline_command_u8", "joints", "quality", + } + if set(payload) != expected_top: + raise ValueError("compact calibration has unexpected top-level fields") + if payload["schema_version"] != 4: + raise ValueError("schema_version must be 4") + if payload["model"] != "G20" or payload["side"] not in {"left", "right"}: + raise ValueError("payload must describe a left or right G20") + # schema-v4 has no layout field. A right-hand payload without inherited + # source_joint fields is the independent right-hand product layout; legacy + # payloads retain their source_joint markers and remain readable. + layout_id = infer_compact_payload_layout(payload) + independent_right = layout_id == G20_RIGHT_19_LAYOUT + profile = get_hand_calibration_profile( + str(payload["side"]), + layout_id, + ) + if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: + raise ValueError("payload angle or command units are invalid") + baseline = payload["baseline_command_u8"] + if not isinstance(baseline, list) or len(baseline) != 20: + raise ValueError("baseline_command_u8 must contain 20 values") + joints = payload["joints"] + if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs): + raise ValueError("payload must contain exactly 21 G20 joints") + + for name, spec in profile.joint_specs.items(): + joint = joints[name] + allowed = {"motor_index", "angle_rad"} + allowed.add("zero_command_u8" if spec.active else "passive") + if spec.active: + allowed.add("zero_angles") + if spec.source_joint is not None and not independent_right: + allowed.add("source_joint") + if set(joint) != allowed: + raise ValueError(f"{name} has unexpected fields") + if int(joint["motor_index"]) != spec.motor_index: + raise ValueError(f"{name} has the wrong motor_index") + curve = np.asarray(joint["angle_rad"], dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name}.angle_rad must contain 256 finite values") + if np.any(np.diff(curve) > 1.0e-7): + raise ValueError(f"{name}.angle_rad must be non-increasing") + if spec.active: + zero = joint["zero_command_u8"] + if not isinstance(zero, int) or not 0 <= zero <= 255: + raise ValueError(f"{name}.zero_command_u8 is invalid") + zero_angles = joint.get("zero_angles") + if not isinstance(zero_angles, Mapping) or set(zero_angles) != { + "urdf_zero_offset_rad" + }: + raise ValueError( + f"{name}.zero_angles must contain urdf_zero_offset_rad" + ) + offset = float(zero_angles["urdf_zero_offset_rad"]) + if not math.isfinite(offset): + raise ValueError(f"{name}.urdf_zero_offset_rad is invalid") + if abs(float(curve[zero])) > 1.0e-6: + raise ValueError( + f"{name}.angle_rad must be zero at zero_command_u8" + ) + elif joint.get("passive") is not True: + raise ValueError(f"{name} must be marked passive") + if spec.source_joint is not None and not independent_right: + if joint.get("source_joint") != spec.source_joint: + raise ValueError(f"{name} has the wrong source_joint") + source = joints[spec.source_joint] + if joint["angle_rad"] != source["angle_rad"]: + raise ValueError(f"{name} must copy its source curve exactly") + if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]: + raise ValueError(f"{name} must copy its source zero command") + # source_joint denotes a dynamic command-angle curve source. The + # absolute URDF zero belongs to an independent motor/assembly and + # must remain per-joint unless it was independently observed. + + quality = payload["quality"] + if set(quality) != { + "passed", "validation_mae_rad", "validation_p95_rad" + }: + raise ValueError("quality must contain only compact summary fields") + + +def _validate_right_19_compact_payload(payload: Mapping[str, Any]) -> None: + expected_top = { + "schema_version", "layout_id", "model", "side", "serial_number", + "angle_unit", "command_range", "curve_input_domain", + "runtime_curve_policy", "baseline_command_u8", + "static_zero_targets", "frozen_static_zero_joints", "joints", + "cross_view_roll", "static_zero_validation", "artifacts", "quality", + } + if set(payload) != expected_top: + raise ValueError("schema v5 calibration has unexpected top-level fields") + if ( + payload["layout_id"] not in {"g20_right_19", G20_RIGHT_15_LAYOUT} + or payload["model"] != "G20" + or payload["side"] != "right" + ): + raise ValueError("schema v5 must describe a G20 right product layout") + profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) + baseline = payload["baseline_command_u8"] + if not isinstance(baseline, list) or len(baseline) != 20: + raise ValueError("baseline_command_u8 must contain 20 values") + if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: + raise ValueError("payload angle or command units are invalid") + if payload["curve_input_domain"] != "feedback_u8": + raise ValueError("schema v5 curves must be feedback-indexed") + runtime_curve_policy = payload["runtime_curve_policy"] + if runtime_curve_policy not in { + "direction_aware", + "direction_aware_canonical_zero", + }: + raise ValueError("schema v5 runtime curve policy must be direction-aware") + # v5 is a read-only legacy contract from the earlier 15-zero product. + # Keep its historical scope stable even though new v4 publications now + # calibrate thumb_mcp as the 16th active static zero. + static_targets = { + name + for name, spec in profile.joint_specs.items() + if spec.zero_kind == "urdf_axis_chain" + } - {"thumb_mcp"} + frozen = {"thumb_mcp", *profile.passive_joints} + if set(payload["static_zero_targets"]) != static_targets: + raise ValueError("schema v5 must contain exactly 15 static-zero targets") + if set(payload["frozen_static_zero_joints"]) != frozen: + raise ValueError("schema v5 frozen static-zero joint set is invalid") + static_validation = payload["static_zero_validation"] + training_cycles = static_validation.get("training_cycles", []) + holdout_cycle = static_validation.get("holdout_cycle") + expected_cycle_count = len(training_cycles) + 1 + joints = payload["joints"] + if not isinstance(joints, Mapping) or set(joints) != set(profile.joint_specs): + raise ValueError("schema v5 must contain exactly 21 independent joints") + for name, spec in profile.joint_specs.items(): + joint = joints[name] + expected_fields = { + "motor_index", "zero_command_u8", "angle_rad", "decreasing_rad", + "increasing_rad", "dynamic_quality", "static_zero_status", + "zero_angles" if spec.active else "passive", + } + if set(joint) != expected_fields: + raise ValueError(f"{name} has unexpected schema v5 fields") + if int(joint["motor_index"]) != spec.motor_index: + raise ValueError(f"{name} has the wrong motor_index") + zero_command = joint["zero_command_u8"] + if zero_command != int(baseline[spec.motor_index]): + raise ValueError(f"{name} has the wrong zero command") + for field_name in ("angle_rad", "decreasing_rad", "increasing_rad"): + curve = np.asarray(joint[field_name], dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name}.{field_name} must contain 256 values") + if np.any(np.diff(curve) > 1.0e-7): + raise ValueError(f"{name}.{field_name} must be non-increasing") + allow_directional_offset = ( + runtime_curve_policy == "direction_aware_canonical_zero" + and canonical_zero_direction(profile, name) is not None + and field_name == "increasing_rad" + ) + if ( + not allow_directional_offset + and abs(float(curve[zero_command])) > 1.0e-6 + ): + raise ValueError(f"{name}.{field_name} must be zero at baseline") + expected_status = ( + "static_zero_calibrated" + if name in static_targets + else "cad_static_zero_retained_front_only" + if name == "thumb_mcp" + else "dynamic_measured_static_unobservable" + ) + if joint["static_zero_status"] != expected_status: + raise ValueError(f"{name} has the wrong static-zero status") + dynamic_quality = joint["dynamic_quality"] + if set(dynamic_quality) != { + "maximum_monotonic_correction_rad", + "maximum_hysteresis_rad", + "fit", + "cycle_travel_rad", + "cycle_travel_range_rad", + "baseline_hysteresis_by_cycle_rad", + "holdout_cycle", + "holdout_cycle_mae_rad", + "holdout_cycle_max_rad", + }: + raise ValueError(f"{name}.dynamic_quality fields are invalid") + if ( + dynamic_quality["holdout_cycle"] != holdout_cycle + or len(dynamic_quality["cycle_travel_rad"]) != expected_cycle_count + or len( + dynamic_quality["baseline_hysteresis_by_cycle_rad"] + ) != expected_cycle_count + ): + raise ValueError(f"{name} must record all dynamic cycles") + if spec.active: + zero = joint["zero_angles"] + if set(zero) != { + "urdf_zero_offset_rad", + "confidence_interval_95_half_width_rad", + "cycle_offsets_rad", + "observer_joint", + }: + raise ValueError(f"{name}.zero_angles fields are invalid") + if not math.isfinite(float(zero["urdf_zero_offset_rad"])): + raise ValueError(f"{name} zero offset is invalid") + if name in static_targets: + confidence = zero["confidence_interval_95_half_width_rad"] + if ( + confidence is None + or not math.isfinite(float(confidence)) + or not 0.0 <= float(confidence) <= math.radians(0.5) + ): + raise ValueError(f"{name} confidence interval is invalid") + if len(zero["cycle_offsets_rad"]) != len(training_cycles): + raise ValueError( + f"{name} must record training-only zero estimates" + ) + if not isinstance(zero["observer_joint"], str): + raise ValueError(f"{name} must record its zero observer") + if name == "thumb_mcp" and float(zero["urdf_zero_offset_rad"]) != 0.0: + raise ValueError("thumb_mcp static zero must remain source CAD") + elif joint.get("passive") is not True: + raise ValueError(f"{name} must be marked passive") + artifacts = payload["artifacts"] + expected_hashes = { + "source_urdf_sha256", "camera_extrinsics_sha256", + "corrected_urdf_sha256", + } + if set(artifacts) != expected_hashes or any( + re.fullmatch(r"[0-9a-f]{64}", str(value)) is None + for value in artifacts.values() + ): + raise ValueError("schema v5 artifact SHA-256 values are invalid") + if set(payload["cross_view_roll"]) != { + f"{finger}_mcp_roll" for finger in ("index", "middle", "ring", "pinky") + }: + raise ValueError("schema v5 requires four cross-view roll diagnostics") + if set(static_validation) != { + "training_cycles", "holdout_cycle", "axis_line_rms_m", + "axis_line_rms_by_joint_m", "observability_rank", + "observability_parameter_count", "observability_condition_number", + "offset_covariance_rad2", + }: + raise ValueError("schema v5 static-zero validation fields are invalid") + training_cycles = static_validation["training_cycles"] + holdout_cycle = static_validation["holdout_cycle"] + if ( + not isinstance(training_cycles, list) + or len(training_cycles) < 3 + or any(not isinstance(value, int) for value in training_cycles) + or not isinstance(holdout_cycle, int) + or holdout_cycle in training_cycles + ): + raise ValueError("schema v5 training/holdout cycle split is invalid") + axis_line_rms = float(static_validation["axis_line_rms_m"]) + if not math.isfinite(axis_line_rms) or axis_line_rms < 0.0: + raise ValueError("schema v5 axis-line RMS is invalid") + line_by_joint = static_validation["axis_line_rms_by_joint_m"] + if not isinstance(line_by_joint, Mapping) or any( + not math.isfinite(float(value)) or float(value) < 0.0 + for value in line_by_joint.values() + ): + raise ValueError("schema v5 per-joint axis-line RMS is invalid") + parameter_count = int(static_validation["observability_parameter_count"]) + rank = int(static_validation["observability_rank"]) + condition = float(static_validation["observability_condition_number"]) + if parameter_count != 21 or rank != parameter_count: + raise ValueError("schema v5 zero-observation Jacobian is rank deficient") + if not math.isfinite(condition) or condition < 1.0: + raise ValueError("schema v5 observability condition number is invalid") + offset_covariance = static_validation["offset_covariance_rad2"] + if set(offset_covariance) != static_targets or any( + not math.isfinite(float(value)) or float(value) < 0.0 + for value in offset_covariance.values() + ): + raise ValueError("schema v5 offset covariance is invalid") + quality = payload["quality"] + if set(quality) != { + "passed", "all_15_static_targets_required", "validation_mae_rad", + "validation_p95_rad", + } or quality["all_15_static_targets_required"] is not True: + raise ValueError("schema v5 quality summary is invalid") diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/publication.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/publication.py new file mode 100644 index 0000000..0650610 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/publication.py @@ -0,0 +1,914 @@ +"""Validate and atomically publish one inseparable G20 JSON/URDF session.""" + +from __future__ import annotations + +from datetime import datetime, timezone +import copy +import json +import math +import os +from pathlib import Path +import re +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from .profile import ( + G20_COMBINATION_REQUIRED_TARGET_KEYS, + G20_RIGHT_19_LAYOUT, + get_hand_calibration_profile, + validate_compact_payload, +) +from ...product import ProductConfig, sha256_file +from ...storage import atomic_write_json +from ...core.urdf import UrdfCorrectionPlan, build_correction_plan +from .zero_solver import ( + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, + get_zero_calibration_profile, +) + + +PASSIVE_JOINTS = frozenset( + {"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"} +) +VISUALLY_MEASURED_FINGER_DIPS = frozenset( + {"index_dip", "middle_dip", "ring_dip", "pinky_dip"} +) +ACTIVE_ZERO_JOINTS = frozenset( + get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints +) +RETAINED_ACTIVE_ZERO_JOINTS = frozenset( + get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints +) - ACTIVE_ZERO_JOINTS +SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9 + + +def _load_json(path: Path) -> dict[str, Any]: + with path.open("r", encoding="utf-8") as stream: + payload = json.load(stream) + if not isinstance(payload, dict): + raise ValueError(f"{path} must contain a JSON object") + return payload + + +def _joint_blocks(text: str) -> dict[str, str]: + pattern = re.compile( + r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", + re.DOTALL, + ) + return {match.group("name"): match.group(0) for match in pattern.finditer(text)} + + +def _mask_origin_rpy(block: str) -> str: + return re.sub( + r"(]*\brpy\s*=\s*)([\"'])[^\"']*\2", + r"\1\2__CALIBRATED_RPY__\2", + block, + count=1, + ) + + +def _mask_active_origin_rpy_fields(text: str) -> str: + pattern = re.compile( + r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", + re.DOTALL, + ) + + def replace(match: re.Match[str]) -> str: + block = match.group(0) + if match.group("name") not in ACTIVE_ZERO_JOINTS: + return block + masked = _mask_origin_rpy(block) + if masked == block: + raise ValueError( + f"active joint {match.group('name')} has no origin.rpy" + ) + return masked + + return pattern.sub(replace, text) + + +def _mask_endpoint_coordinate_fields(text: str) -> str: + """Mask only limit/mimic fields induced by endpoint zero coordinates.""" + pattern = re.compile( + r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", + re.DOTALL, + ) + + def replace(match: re.Match[str]) -> str: + block = match.group(0) + name = match.group("name") + if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS: + return re.sub( + r"(]*\bupper\s*=\s*)([\"'])[^\"']*\2", + r"\1\2__CALIBRATED_UPPER__\2", + block, + count=1, + ) + mimic = re.search( + r"]*\bjoint\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>", + block, + re.DOTALL, + ) + if ( + mimic is not None + and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + ): + return re.sub( + r"(]*\boffset\s*=\s*)([\"'])[^\"']*\2", + r"\1\2__CALIBRATED_MIMIC_OFFSET__\2", + block, + count=1, + ) + return block + + return pattern.sub(replace, text) + + +def _triplet(value: str) -> np.ndarray: + result = np.asarray([float(item) for item in value.split()], dtype=float) + if result.shape != (3,) or not np.all(np.isfinite(result)): + raise ValueError(f"invalid URDF triplet: {value}") + return result + + +def _joint_elements(path: str | Path) -> dict[str, ET.Element]: + return { + str(joint.get("name")): joint + for joint in ET.parse(Path(path)).getroot().findall("joint") + } + + +def _verify_expected_origin_offsets( + source: str | Path, + corrected: str | Path, + expected_offsets_rad: Mapping[str, float], +) -> None: + offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()} + if set(offsets) != ACTIVE_ZERO_JOINTS or any( + not math.isfinite(value) for value in offsets.values() + ): + raise ValueError("expected offsets must contain all finite active static-zero values") + before = _joint_elements(source) + after = _joint_elements(corrected) + maximum_rotation_error = 0.0 + for name, original_joint in before.items(): + corrected_joint = after[name] + original_origin = original_joint.find("origin") + corrected_origin = corrected_joint.find("origin") + if original_origin is None or corrected_origin is None: + if original_origin is not corrected_origin: + raise ValueError(f"corrected URDF changed origin presence for {name}") + continue + original_xyz = _triplet(original_origin.get("xyz", "0 0 0")) + corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0")) + if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0): + raise ValueError(f"corrected URDF changed origin.xyz for {name}") + original_rotation = Rotation.from_euler( + "xyz", _triplet(original_origin.get("rpy", "0 0 0")) + ) + corrected_rotation = Rotation.from_euler( + "xyz", _triplet(corrected_origin.get("rpy", "0 0 0")) + ) + expected_rotation = original_rotation + if name in offsets: + axis_node = original_joint.find("axis") + axis = _triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + norm = float(np.linalg.norm(axis)) + if norm <= 1.0e-12: + raise ValueError(f"source URDF joint {name} has a degenerate axis") + expected_rotation = expected_rotation * Rotation.from_rotvec( + axis / norm * offsets[name] + ) + error = float((expected_rotation.inv() * corrected_rotation).magnitude()) + maximum_rotation_error = max(maximum_rotation_error, error) + # Schema v4 intentionally stores zero offsets at eight decimal places. + # Older sessions wrote the corrected URDF from the full-precision solve, + # so comparing that file with the published JSON necessarily permits half + # of one last-place unit. This is about 2.9e-7 degrees and is far below + # any calibration or URDF numerical significance. + if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD: + raise ValueError( + "corrected URDF origin.rpy does not match the published zero offsets" + ) + + +def verify_corrected_urdf( + source: str | Path, + corrected: str | Path, + *, + expected_offsets_rad: Mapping[str, float] | None = None, + endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, + correction_plan: UrdfCorrectionPlan | None = None, +) -> tuple[str, ...]: + """Prove that only active-joint origin.rpy attributes changed. + + Passive joint blocks, including their mimic elements, are compared as raw + UTF-8 text so formatting and numeric spelling are protected as well. + """ + source_text = Path(source).read_text(encoding="utf-8") + corrected_text = Path(corrected).read_text(encoding="utf-8") + if correction_plan is not None: + correction_plan.verify_source(source) + if expected_offsets_rad is None: + raise ValueError( + "correction-plan validation requires expected offsets" + ) + correction_plan.authorize_offsets(expected_offsets_rad) + before = _joint_blocks(source_text) + after = _joint_blocks(corrected_text) + if set(before) != set(after): + raise ValueError("corrected URDF changed the joint set") + # Text patchers may normalize only the final newline. It has no URDF/XML + # semantics; every joint block and every non-rpy byte remains guarded + # below. + masked_source = _mask_endpoint_coordinate_fields( + _mask_active_origin_rpy_fields(source_text) + ).rstrip("\r\n") + masked_corrected = _mask_endpoint_coordinate_fields( + _mask_active_origin_rpy_fields(corrected_text) + ).rstrip("\r\n") + if masked_source != masked_corrected: + raise ValueError( + "corrected URDF changed content outside active origin.rpy or " + "endpoint-calibrated fields" + ) + changed: list[str] = [] + for name in before: + if before[name] == after[name]: + continue + if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_JOINTS: + raise ValueError(f"corrected URDF changed protected joint {name}") + if name in ACTIVE_ZERO_JOINTS: + changed.append(name) + if expected_offsets_rad is not None: + _verify_expected_origin_offsets(source, corrected, expected_offsets_rad) + endpoint_offsets = { + str(name): float(value) + for name, value in dict(endpoint_anchored_offsets_rad or {}).items() + } + if endpoint_offsets: + if correction_plan is not None and not set(endpoint_offsets).issubset( + correction_plan.endpoint_limit_joints + ): + raise ValueError( + "endpoint offsets are not authorized by the correction plan" + ) + source_joints = _joint_elements(source) + corrected_joints = _joint_elements(corrected) + for name, offset in endpoint_offsets.items(): + original_limit = source_joints[name].find("limit") + corrected_limit = corrected_joints[name].find("limit") + expected_upper = float(original_limit.get("upper")) - offset + # Schema v4 stores the corresponding zero at eight decimal + # places, while the URDF is written from the full-precision solve. + # Match the half-last-place tolerance used for origin rotations. + if ( + abs(float(corrected_limit.get("upper")) - expected_upper) + > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD + ): + raise ValueError(f"corrected URDF has invalid {name} upper limit") + for name in PASSIVE_JOINTS: + original_mimic = source_joints[name].find("mimic") + corrected_mimic = corrected_joints[name].find("mimic") + if original_mimic is None or corrected_mimic is None: + continue + source_name = str(original_mimic.get("joint")) + multiplier = float(original_mimic.get("multiplier", "1")) + expected = float(original_mimic.get("offset", "0")) + expected += multiplier * endpoint_offsets.get(source_name, 0.0) + # The active endpoint offset comes from schema-v4 JSON rounded to + # eight decimal places, while the URDF mimic was written from the + # full-precision solve. Propagate exactly the same accepted + # quantization through the mimic multiplier; retain a much + # smaller allowance for XML decimal formatting itself. + tolerance = ( + abs(multiplier) + * SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD + + 1.0e-12 + ) + if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance: + raise ValueError(f"corrected URDF has invalid {name} mimic offset") + return tuple(sorted(changed)) + + +def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]: + """Return every session-local mesh after proving it resolves safely.""" + path = Path(urdf).expanduser().resolve() + root = ET.parse(path).getroot() + resources: dict[str, Path] = {} + for mesh in root.findall(".//mesh"): + filename = str(mesh.get("filename", "")).strip() + if not filename or "://" in filename or filename.startswith("package:"): + continue + relative = Path(filename) + if relative.is_absolute() or ".." in relative.parts: + raise ValueError(f"URDF has an unsafe local mesh path: {filename}") + resolved = (path.parent / relative).resolve() + try: + resolved.relative_to(path.parent) + except ValueError as error: + raise ValueError(f"URDF mesh escapes the session: {filename}") from error + if not resolved.is_file(): + raise ValueError(f"URDF mesh resource is missing: {filename}") + resources[relative.as_posix()] = resolved + return dict(sorted(resources.items())) + + +def validate_runtime_curves_against_urdf_limits( + payload: Mapping[str, Any], runtime_urdf: str | Path +) -> None: + """Reject a curve whose q leaves its runtime URDF coordinate limits. + + Curves in schema v4 are expressed in the corrected URDF joint coordinate, + not in the source-CAD coordinate. Endpoint zero calibration can therefore + move a corrected coordinate limit while preserving the same physical CAD + endpoint; callers publishing a calibrated pair must pass that corrected + URDF here. + """ + validate_compact_payload(payload) + runtime_joints = _joint_elements(runtime_urdf) + for name, calibration in payload["joints"].items(): + joint = runtime_joints.get(str(name)) + if joint is None: + raise ValueError(f"runtime URDF is missing joint {name}") + limit = joint.find("limit") + if limit is None or limit.get("lower") is None or limit.get("upper") is None: + raise ValueError(f"runtime URDF joint {name} has no finite position limit") + lower = float(limit.get("lower")) + upper = float(limit.get("upper")) + if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: + raise ValueError(f"runtime URDF joint {name} has invalid position limits") + curve = np.asarray(calibration["angle_rad"], dtype=float) + minimum = float(np.min(curve)) + maximum = float(np.max(curve)) + tolerance = 1.0e-7 + if minimum < lower - tolerance or maximum > upper + tolerance: + raise ValueError( + f"runtime curve exceeds runtime URDF limit for {name}: " + f"[{minimum:.9g}, {maximum:.9g}] not within " + f"[{lower:.9g}, {upper:.9g}]" + ) + + +def clamp_compact_payload_to_urdf_limits( + payload: Mapping[str, Any], runtime_urdf: str | Path +) -> tuple[dict[str, Any], dict[str, int]]: + """Return a schema-preserving payload bounded in its runtime coordinates.""" + result = copy.deepcopy(dict(payload)) + validate_compact_payload(result) + runtime_joints = _joint_elements(runtime_urdf) + clipped_by_joint: dict[str, int] = {} + for name, calibration in result["joints"].items(): + joint = runtime_joints.get(str(name)) + limit = None if joint is None else joint.find("limit") + if ( + limit is None + or limit.get("lower") is None + or limit.get("upper") is None + ): + raise ValueError(f"runtime URDF joint {name} has no finite position limit") + lower = float(limit.get("lower")) + upper = float(limit.get("upper")) + if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: + raise ValueError(f"runtime URDF joint {name} has invalid position limits") + source = np.asarray(calibration["angle_rad"], dtype=float) + bounded = np.clip(source, lower, upper) + count = int(np.count_nonzero(bounded != source)) + if count: + clipped_by_joint[str(name)] = count + calibration["angle_rad"] = [ + round(float(value), 8) for value in bounded + ] + validate_compact_payload(result) + return result, clipped_by_joint + + +def build_mujoco_validation_commands( + baseline: Sequence[int], +) -> dict[str, Any]: + if len(baseline) != 20: + raise ValueError("baseline must contain exactly 20 commands") + base = [int(value) for value in baseline] + if any(not 0 <= value <= 255 for value in base): + raise ValueError("baseline commands must be in [0, 255]") + + def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]: + values = list(base) + for index, value in changes.items(): + values[int(index)] = int(value) + return {"name": name, "label_zh": label, "command_u8": values} + + return { + "schema_version": 1, + "topic": "/g20/cb_right_hand_control_cmd", + "serial_number": "G20_RIGHT_001", + "poses": [ + pose("all_open", "全开", {}), + pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}), + pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}), + pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}), + pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}), + pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}), + pose( + "half_grip", + "四指半握", + {1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160}, + ), + pose( + "light_pinch", + "轻捏", + {0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176}, + ), + ], + } + + +def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path: + parent = Path(root).resolve() + target = Path(session).resolve() + if target.parent != parent: + raise ValueError("session pointer target must be a direct child of the serial root") + if not target.is_dir(): + raise ValueError(f"session directory does not exist: {target}") + if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}: + raise ValueError("unsupported session pointer name") + parent.mkdir(parents=True, exist_ok=True) + destination = parent / name + temporary = parent / f".{name}.{os.getpid()}.tmp" + if temporary.is_symlink(): + temporary.unlink() + elif temporary.exists(): + raise ValueError(f"temporary pointer path is occupied: {temporary}") + os.symlink(target.name, temporary, target_is_directory=True) + os.replace(temporary, destination) + return destination + + +def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]: + validate_compact_payload(payload) + result = { + name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"]) + for name in ACTIVE_ZERO_JOINTS + } + if set(result) != ACTIVE_ZERO_JOINTS or any( + not math.isfinite(value) for value in result.values() + ): + raise ValueError( + "payload does not contain all finite observable active zero offsets" + ) + return result + + +def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]: + """Validate a standalone thumb artifact and expand CAD finger zeros.""" + if ( + payload.get("schema_version") != 1 + or payload.get("artifact_type") + != "g20_right_standalone_thumb_calibration" + or payload.get("side") != "right" + or payload.get("non_thumb_zero_policy") != "source_cad_unchanged" + or not bool(payload.get("quality", {}).get("passed")) + ): + raise ValueError("standalone thumb JSON contract is invalid") + joints = payload.get("joints") + expected_joint_names = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + } + if not isinstance(joints, Mapping) or set(joints) != expected_joint_names: + raise ValueError("standalone thumb JSON has the wrong joint set") + thumb_names = expected_joint_names - {"thumb_ip"} + result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS} + for name in thumb_names: + try: + value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"]) + except (KeyError, TypeError, ValueError) as error: + raise ValueError( + f"standalone thumb JSON is missing zero for {name}" + ) from error + curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"standalone thumb JSON curve is invalid: {name}") + result[name] = value + passive_curve = np.asarray( + joints["thumb_ip"].get("angle_rad", ()), dtype=float + ) + if ( + passive_curve.shape != (256,) + or not np.all(np.isfinite(passive_curve)) + or not bool(joints["thumb_ip"].get("passive")) + or any(not math.isfinite(value) for value in result.values()) + ): + raise ValueError("standalone thumb JSON passive curve is invalid") + return result + + +def verify_partial_scope_preserves_certified_zeros( + *, + scope: str, + source_session: str | Path, + serial_root: str | Path | None = None, + serial_number: str, + current_offsets: Mapping[str, float], +) -> None: + """Reject a partial artifact that changes any certified non-target zero.""" + selected = str(scope).strip().lower() + if selected not in {"thumb", "fingers"}: + return + source = Path(source_session).expanduser() + if not source.is_absolute(): + if serial_root is None: + raise ValueError( + "relative partial base session requires its serial root" + ) + source = Path(serial_root).expanduser() / source + source = source.resolve() + base_payload = _load_json( + source / f"g20_right_{serial_number}_calibration.json" + ) + base_offsets = ( + standalone_thumb_offsets(base_payload) + if base_payload.get("artifact_type") + == "g20_right_standalone_thumb_calibration" + else active_offsets(base_payload) + ) + preserve_thumb = selected == "fingers" + changed = { + name: abs(float(current_offsets[name]) - float(base_offsets[name])) + for name in ACTIVE_ZERO_JOINTS + if name.startswith("thumb_") == preserve_thumb + and abs(float(current_offsets[name]) - float(base_offsets[name])) + > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD + } + if changed: + raise ValueError( + f"partial {selected} calibration changed certified non-target " + "zeros: " + + ",".join(sorted(changed)) + ) + + +def compare_session_offsets( + first: Mapping[str, Any], + second: Mapping[str, Any], + *, + maximum_difference_rad: float, +) -> dict[str, float]: + first_standalone = ( + first.get("artifact_type") + == "g20_right_standalone_thumb_calibration" + ) + second_standalone = ( + second.get("artifact_type") + == "g20_right_standalone_thumb_calibration" + ) + if first_standalone != second_standalone: + raise ValueError("cannot compare standalone thumb and full-hand artifacts") + left = ( + standalone_thumb_offsets(first) + if first_standalone + else active_offsets(first) + ) + right = ( + standalone_thumb_offsets(second) + if second_standalone + else active_offsets(second) + ) + differences = { + name: abs(left[name] - right[name]) + for name in sorted(left) + } + failed = {name: value for name, value in differences.items() if value > maximum_difference_rad} + if failed: + details = ", ".join( + f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items() + ) + raise ValueError(f"independent-session static-zero mismatch: {details}") + return differences + + +def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]: + directory = Path(session).resolve() + urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf")) + if len(urdfs) != 1: + raise ValueError("session must contain exactly one corrected URDF") + return { + "json": directory / f"g20_right_{serial_number}_calibration.json", + "urdf": urdfs[0], + "summary": directory / "calibration_summary_zh.json", + "commands": directory / "mujoco_validation_commands.json", + "raw": directory / "raw_samples.jsonl", + "log": directory / "calibration.log", + } + + +def find_compatible_prior_session( + config: ProductConfig, + current: str | Path, + current_payload: Mapping[str, Any], +) -> tuple[Path | None, dict[str, float]]: + current_path = Path(current).resolve() + for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True): + if candidate.resolve() == current_path: + continue + json_path = candidate / f"g20_right_{config.serial_number}_calibration.json" + summary_path = candidate / "calibration_summary_zh.json" + if not json_path.is_file() or not summary_path.is_file(): + continue + try: + summary = _load_json(summary_path) + if not bool(summary.get("quality", {}).get("passed")): + continue + payload = _load_json(json_path) + differences = compare_session_offsets( + payload, + current_payload, + maximum_difference_rad=config.static_repeatability_rad, + ) + except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError): + continue + return candidate, differences + return None, {} + + +def _verify_combination_validation(combination: Mapping[str, Any]) -> None: + # The formal product uses the independently held-out fourth sweep cycle. + # The optional eight-pose check remains a developer diagnostic because the + # axis-line zero solve does not establish an absolute Cartesian hand-base + # transform. When explicitly enabled, retain its strict coverage and + # error checks. + if not bool(combination.get("enabled")): + return + position_p95 = float(combination.get("position_p95_m", float("inf"))) + orientation_p95 = float( + combination.get("orientation_p95_rad", float("inf")) + ) + required = {str(value) for value in combination.get("required_targets", ())} + expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS) + observations = combination.get("observation_counts") + validations = combination.get("validation_counts") + if required != expected: + raise ValueError("combination validation has the wrong required-target set") + if not isinstance(observations, Mapping) or not isinstance(validations, Mapping): + raise ValueError("combination validation is missing per-target coverage") + missing_observations = sorted( + key for key in expected if int(observations.get(key, 0)) < 2 + ) + missing_validations = sorted( + key for key in expected if int(validations.get(key, 0)) < 1 + ) + if missing_observations or missing_validations: + raise ValueError( + "combination validation target coverage is incomplete: " + f"observations={','.join(missing_observations) or '-'}; " + f"validations={','.join(missing_validations) or '-'}" + ) + if ( + not bool(combination.get("completed")) + or int(combination.get("completed_poses", 0)) != 8 + or position_p95 > 0.003 + or orientation_p95 > math.radians(2.0) + ): + raise ValueError( + "eight-pose four-finger combination validation is incomplete or failed" + ) + + +def finalize_session_artifacts( + config: ProductConfig, + session: str | Path, + *, + node_status: Mapping[str, Any], +) -> tuple[dict[str, Any], bool]: + """Create diagnostics and update latest_passed only after every guard.""" + directory = Path(session).resolve() + paths = session_artifact_paths(directory, config.serial_number) + for name in ("json", "urdf", "raw", "log"): + if not paths[name].is_file(): + raise ValueError(f"session is missing {name}: {paths[name]}") + payload = _load_json(paths["json"]) + standalone_thumb = bool( + payload.get("artifact_type") + == "g20_right_standalone_thumb_calibration" + ) + if standalone_thumb: + standalone_thumb_offsets(payload) + else: + validate_compact_payload(payload) + if payload.get("schema_version") != 4 or payload.get("side") != "right": + raise ValueError("runtime JSON is not the compact right-hand schema v4") + if not bool(payload.get("quality", {}).get("passed")): + raise ValueError("runtime JSON quality is not passed") + combination = node_status.get("combination_validation") + if not isinstance(combination, Mapping): + raise ValueError("node status is missing combination validation") + _verify_combination_validation(combination) + resume = node_status.get("resume", {}) + if not isinstance(resume, Mapping): + raise ValueError("node status has invalid resume provenance") + calibration_scope = str( + resume.get("recalibration_scope", "full") + ).strip().lower() + if calibration_scope not in {"full", "thumb", "fingers"}: + raise ValueError("node status has an unsupported recalibration scope") + recalibration_tasks = tuple( + str(value) for value in resume.get("recalibration_task_keys", ()) + ) + if calibration_scope == "thumb" and not standalone_thumb and ( + not bool(resume.get("used")) + or not str(resume.get("source_session", "")) + or len(recalibration_tasks) != 4 + or any("thumb_" not in name for name in recalibration_tasks) + ): + raise ValueError( + "thumb recalibration is missing its passed base-session provenance" + ) + if calibration_scope == "fingers" and ( + not bool(resume.get("used")) + or not str(resume.get("source_session", "")) + or len(recalibration_tasks) != 12 + or any("thumb_" in name for name in recalibration_tasks) + ): + raise ValueError( + "finger recalibration is missing its certified thumb-session " + "provenance" + ) + if standalone_thumb and calibration_scope != "thumb": + raise ValueError("standalone thumb artifact has the wrong node scope") + offsets = ( + standalone_thumb_offsets(payload) + if standalone_thumb + else active_offsets(payload) + ) + if calibration_scope != "full" and not standalone_thumb: + verify_partial_scope_preserves_certified_zeros( + scope=calibration_scope, + source_session=str(resume.get("source_session", "")), + serial_root=directory.parent, + serial_number=config.serial_number, + current_offsets=offsets, + ) + endpoint_offsets = { + name: offsets[name] + for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + } + typed_profile = config.calibration_contract.typed_profile + frozen_names = typed_profile.scope.frozen_joints[calibration_scope] + correction_plan = build_correction_plan( + typed_profile, + source_sha256=config.source_urdf_sha256, + scope=calibration_scope, + frozen_offsets_rad={name: offsets[name] for name in frozen_names}, + ) + changed_joints = verify_corrected_urdf( + config.source_urdf, + paths["urdf"], + expected_offsets_rad=offsets, + endpoint_anchored_offsets_rad=endpoint_offsets, + correction_plan=correction_plan, + ) + if standalone_thumb: + clipped_runtime_joints = {} + else: + payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits( + payload, paths["urdf"] + ) + if clipped_runtime_joints: + atomic_write_json(paths["json"], payload) + validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) + mesh_resources = verify_urdf_mesh_resources(paths["urdf"]) + mesh_hashes = { + name: sha256_file(path) for name, path in mesh_resources.items() + } + commands = ( + { + "artifact_type": "standalone_thumb_no_full_hand_validation", + "poses": [], + } + if standalone_thumb + else build_mujoco_validation_commands(payload["baseline_command_u8"]) + ) + atomic_write_json(paths["commands"], commands) + + prior, differences = find_compatible_prior_session(config, directory, payload) + release_ready = config.required_independent_passes == 1 or prior is not None + quality = dict(payload["quality"]) + preserved_certified_zeros = sorted( + name + for name in ACTIVE_ZERO_JOINTS + if ( + calibration_scope == "thumb" + and not name.startswith("thumb_") + ) + or ( + calibration_scope == "fingers" + and name.startswith("thumb_") + ) + ) + if standalone_thumb: + preserved_certified_zeros = [] + calibrated_zero_joints = ( + sorted( + name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_") + ) + if standalone_thumb + else sorted(ACTIVE_ZERO_JOINTS) + ) + summary: dict[str, Any] = { + "schema_version": 1, + "serial_number": config.serial_number, + "session_id": f"{config.serial_number}_{directory.name}", + "calibration_scope": calibration_scope, + "inherited_base_session": ( + None + if calibration_scope == "full" or standalone_thumb + else str(resume.get("source_session")) + ), + "freshly_calibrated_task_keys": list(recalibration_tasks), + "preserved_certified_zero_joints": preserved_certified_zeros, + "result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION", + "quality": quality, + "runtime_limit_clipped_bins": clipped_runtime_joints, + "runtime_curve_domain": ( + "thumb_diagnostic_only" + if standalone_thumb + else "requested_command_u8" + ), + "static_zero_calibrated_joints": calibrated_zero_joints, + "non_thumb_zero_policy": ( + "source_cad_unchanged" if standalone_thumb else None + ), + "retained_active_urdf_zero_joints": sorted( + RETAINED_ACTIVE_ZERO_JOINTS + ), + "retained_passive_urdf_joints": sorted(PASSIVE_JOINTS), + "mimic_derived_dynamic_joints": [], + "visually_measured_passive_joints": sorted( + VISUALLY_MEASURED_FINGER_DIPS + ), + "changed_urdf_joint_origins": list(changed_joints), + "hashes": { + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + "corrected_urdf_sha256": sha256_file(paths["urdf"]), + "mesh_resources_sha256": mesh_hashes, + "calibration_json_sha256": sha256_file(paths["json"]), + "raw_samples_sha256": sha256_file(paths["raw"]), + }, + "holdout": { + "training_cycles": [0, 1, 2], + "validation_cycle": 3, + "isolated": True, + }, + "combination_validation": dict(combination), + "formal_release": { + "passed": release_ready, + "required_independent_passes": config.required_independent_passes, + "comparison_session": None if prior is None else prior.name, + "maximum_static_difference_deg": ( + None if not differences else math.degrees(max(differences.values())) + ), + }, + "node_status": dict(node_status), + "created_at_utc": datetime.now(timezone.utc).isoformat(), + "artifacts": {name: path.name for name, path in paths.items()}, + } + atomic_write_json(paths["summary"], summary) + # Recompute hashes after all files exist and validate the pair once more + # immediately before the one atomic publication operation. + if sha256_file(config.source_urdf) != config.source_urdf_sha256: + raise ValueError("source URDF changed during calibration") + verify_corrected_urdf( + config.source_urdf, + paths["urdf"], + expected_offsets_rad=offsets, + endpoint_anchored_offsets_rad=endpoint_offsets, + ) + # Revalidate the same coordinate contract immediately before publication: + # verify_corrected_urdf proves that corrected endpoint limits map back to + # the original physical CAD endpoints, while the runtime curves must stay + # inside those corrected-coordinate limits. + if not standalone_thumb: + validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) + final_mesh_hashes = { + name: sha256_file(path) + for name, path in verify_urdf_mesh_resources(paths["urdf"]).items() + } + if final_mesh_hashes != mesh_hashes: + raise ValueError("URDF mesh resources changed during publication") + if release_ready: + atomic_session_pointer( + config.session_root, + "latest_thumb_passed" if standalone_thumb else "latest_passed", + directory, + ) + return summary, release_ready diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py index 074653f..4948065 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py @@ -1,7 +1,7 @@ """Independent reviewed profile for the right 19-Tag product layout.""" from ...core import ProfileKey -from ...full_hand import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile +from .profile import G20_RIGHT_19_LAYOUT, get_hand_calibration_profile from .zero_policy import ( RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py index ebcf339..e9169d7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py @@ -27,7 +27,7 @@ from ...operator_report import ( render_progress_zh, ) from ...product import ProductConfig, load_product_config, sha256_file -from ...publication import atomic_session_pointer, finalize_session_artifacts +from .publication import atomic_session_pointer, finalize_session_artifacts from ...storage import atomic_write_json diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py index f2e6650..c2a4bbf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py @@ -1,7 +1,7 @@ """Reviewed static-zero, endpoint, and mimic topology exports.""" -from ...full_hand import MIMIC_DERIVED_FINGER_DIPS -from ...urdf_zero import ( +from .profile import MIMIC_DERIVED_FINGER_DIPS +from .zero_solver import ( RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py new file mode 100644 index 0000000..be258b8 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py @@ -0,0 +1,4635 @@ +"""Three-dimensional joint-axis fitting and URDF zero correction.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +from datetime import datetime +import math +import os +from pathlib import Path +import re +import shutil +from typing import Any, Mapping, Sequence +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation +from scipy.stats import t as student_t + +from ...core import fit_rotation_axis, robust_rotation_summary +from ...core.urdf import UrdfCorrectionPlan +from .profile import ( + G20_RIGHT_19_LAYOUT, + IMAGE_TRAJECTORY_JOINTS, + LEFT_HAND_PROFILE, + RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, + RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, + HandCalibrationProfile, + JointCurveFit, + get_hand_calibration_profile, +) +from ...sample_schema import explicit_domain_value +from ...trajectory import ( + _fit_circle_with_axis, + _fit_joint_curve, + _fit_plane_axis, + _plane_basis, +) + + +@dataclass(frozen=True) +class ZeroCalibrationProfile: + hand: HandCalibrationProfile + direct_zero_joints: tuple[str, ...] + axis_joints: tuple[str, ...] + # Dynamic command-angle curves may be shared by unobserved fingers. This + # mapping is intentionally *not* an authorization to copy an absolute + # encoder/URDF zero between independent motors. + inherited_zero_joints: Mapping[str, str] + # Absolute static-zero inheritance requires an independent mechanical + # guarantee. With the current 11-Tag layout only the reference finger is + # observed, so the conservative mapping is empty and unobserved active + # finger origins retain their source-CAD zero. + inherited_static_zero_joints: Mapping[str, str] + constrained_circle_joints: frozenset[str] + root_anchor_joints: frozenset[str] + axis_parent_joint: Mapping[str, str] + phase_parent_joint: Mapping[str, str] + offset_observer_joint: Mapping[str, str] + # Some serial offsets can be observed from the angle between two axes + # captured by the same camera. The mapping is geometric topology only; + # it never contains a model- or serial-specific zero value. + same_view_axis_pair_by_offset: Mapping[str, tuple[str, str]] + fixed_direct_zero_offsets_rad: Mapping[str, float] + static_output_zero_offsets_rad: Mapping[str, float] + # Product full-hand fitting uses the five root-axis line pattern. The + # thumb kernel instead uses only the serial thumb chain, so its nuisance + # palm pose cannot be influenced by finger observations. + base_pose_strategy: str = "full_hand" + + @property + def reference_finger(self) -> str: + return self.hand.reference_finger + + +def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: + if hand.layout_id == G20_RIGHT_19_LAYOUT: + return _build_right_19_zero_profile(hand) + reference = hand.reference_finger + reference_roll = f"{reference}_mcp_roll" + reference_pitch = f"{reference}_mcp_pitch" + reference_pip = f"{reference}_pip" + reference_dip = f"{reference}_dip" + direct = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + reference_roll, + reference_pitch, + reference_pip, + ) + inherited = { + f"{finger}_{suffix}": f"{reference}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + if finger != reference + for suffix in ("mcp_roll", "mcp_pitch", "pip") + } + axis_parent = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", + reference_pitch: reference_roll, + } + phase_parent = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + reference_pip: reference_pitch, + reference_dip: reference_pip, + } + observer = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_cmc_pitch", + "thumb_cmc_pitch": "thumb_mcp", + "thumb_mcp": "thumb_ip", + reference_roll: reference_pitch, + reference_pitch: reference_pip, + reference_pip: reference_dip, + } + fixed_direct_zero_offsets = { + "thumb_mcp": 0.0, + reference_roll: 0.0, + reference_pitch: 0.0, + reference_pip: 0.0, + } + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=direct, + axis_joints=( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + reference_roll, + reference_pitch, + reference_pip, + reference_dip, + ), + inherited_zero_joints=inherited, + inherited_static_zero_joints={}, + constrained_circle_joints=( + hand.image_trajectory_joints | {reference_dip, "thumb_cmc_yaw"} + ), + root_anchor_joints=frozenset({"thumb_cmc_roll", reference_roll}), + axis_parent_joint=axis_parent, + phase_parent_joint=phase_parent, + offset_observer_joint=observer, + same_view_axis_pair_by_offset={}, + fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets, + static_output_zero_offsets_rad={}, + ) + + +def _build_right_19_zero_profile( + hand: HandCalibrationProfile, +) -> ZeroCalibrationProfile: + """Return the 16-active-zero graph for the 19-Tag layout. + + Distal Tags recover all four PIP/DIP dynamic curves and axis quality, but + repeated motion with one unchanged camera/Tag installation cannot by + itself distinguish a fixed parallel-axis phase bias from an encoder zero. + The three coupled thumb-CMC offsets must therefore remain a visual + multi-axis solve; their electrical endpoints are not assumed to coincide + with source-CAD limits. Independently repeatable contact endpoints still + anchor thumb MCP, finger MCP-pitch and PIP from each session. DIP stays + passive and has no independently identifiable static zero. + """ + fingers = ("index", "middle", "ring", "pinky") + direct = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in fingers + for suffix in ("mcp_roll", "mcp_pitch", "pip") + ), + ) + axis_joints = ( + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + "thumb_ip", + *( + f"{finger}_{suffix}" + for finger in fingers + for suffix in ("mcp_roll", "mcp_pitch", "pip", "dip") + ), + ) + axis_parent = { + "thumb_cmc_yaw": "thumb_cmc_roll", + "thumb_cmc_pitch": "thumb_cmc_yaw", + **{ + f"{finger}_mcp_pitch": f"{finger}_mcp_roll" + for finger in fingers + }, + } + phase_parent = { + "thumb_mcp": "thumb_cmc_pitch", + "thumb_ip": "thumb_mcp", + **{ + f"{finger}_{child}": f"{finger}_{parent}" + for finger in fingers + for child, parent in ( + ("pip", "mcp_pitch"), + ("dip", "pip"), + ) + }, + } + observer = { + "thumb_cmc_roll": "thumb_cmc_yaw", + "thumb_cmc_yaw": "thumb_cmc_pitch", + "thumb_cmc_pitch": "thumb_mcp", + "thumb_mcp": "thumb_ip", + **{ + f"{finger}_{target}": f"{finger}_{observed}" + for finger in fingers + for target, observed in ( + ("mcp_roll", "mcp_pitch"), + ("mcp_pitch", "pip"), + ("pip", "dip"), + ) + }, + } + root_anchors = frozenset( + {"thumb_cmc_roll", *(f"{finger}_mcp_roll" for finger in fingers)} + ) + constrained = frozenset( + set(hand.image_trajectory_joints) + | {"thumb_cmc_yaw"} + # The side camera observes all four flexion axes nearly end-on. A + # 16 mm monocular planar Tag gives a very repeatable orientation axis, + # while its command-correlated depth bias can tilt the otherwise clean + # centre-trajectory plane by several degrees. Use the orientation + # axis to constrain that circle; PIP retains its independently fitted + # axis line and all final SE(3)/holdout quality gates. + | set(RIGHT_19_END_ON_IMAGE_CURVE_JOINTS) + # _fit_axis_measurement_raw constrains every passive DIP direction to + # its source-URDF-parallel upstream PIP. Declare the same fact here + # so validation does not compare that trusted direction with the + # depth-biased free plane of a distal Tag trajectory. + | set(RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS) + ) + return ZeroCalibrationProfile( + hand=hand, + direct_zero_joints=tuple(direct), + axis_joints=tuple(axis_joints), + inherited_zero_joints={}, + inherited_static_zero_joints={}, + constrained_circle_joints=constrained, + root_anchor_joints=root_anchors, + axis_parent_joint=axis_parent, + phase_parent_joint=phase_parent, + offset_observer_joint=observer, + same_view_axis_pair_by_offset={ + "thumb_cmc_yaw": ("thumb_cmc_roll", "thumb_cmc_pitch") + }, + # Endpoint-observable offsets are supplied by the caller. CMC remains + # visually solved and no serial-specific zero is hidden in the shared + # profile. + fixed_direct_zero_offsets_rad={}, + static_output_zero_offsets_rad={}, + ) + + +def get_right_19_thumb_zero_profile() -> ZeroCalibrationProfile: + """Return the independent four-zero thumb observer graph. + + Roll and MCP are supplied as mechanical endpoint datums by the caller; + yaw is observed by the same-view roll/pitch axis pair and pitch by the + pitch/MCP serial phase. No finger axis is part of this profile. + """ + full = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + thumb_direct = tuple( + name for name in full.direct_zero_joints if name.startswith("thumb_") + ) + thumb_axes = tuple( + name for name in full.axis_joints if name.startswith("thumb_") + ) + return replace( + full, + direct_zero_joints=thumb_direct, + axis_joints=thumb_axes, + constrained_circle_joints=frozenset( + name + for name in full.constrained_circle_joints + if name.startswith("thumb_") + ), + root_anchor_joints=frozenset({"thumb_cmc_roll"}), + axis_parent_joint={ + child: parent + for child, parent in full.axis_parent_joint.items() + if child.startswith("thumb_") and parent.startswith("thumb_") + }, + phase_parent_joint={ + child: parent + for child, parent in full.phase_parent_joint.items() + if child.startswith("thumb_") and parent.startswith("thumb_") + }, + offset_observer_joint={ + target: observer + for target, observer in full.offset_observer_joint.items() + if target.startswith("thumb_") and observer.startswith("thumb_") + }, + same_view_axis_pair_by_offset={ + target: pair + for target, pair in full.same_view_axis_pair_by_offset.items() + if target.startswith("thumb_") + }, + fixed_direct_zero_offsets_rad={}, + static_output_zero_offsets_rad={}, + base_pose_strategy="thumb_serial", + ) + + +RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset( + { + "thumb_mcp", + *( + f"{finger}_{suffix}" + for finger in ("index", "middle", "ring", "pinky") + for suffix in ("mcp_pitch", "pip") + ), + } +) + +# Thumb CMC roll has a directly observed actuator-to-actuator travel, but its +# electrical stop is not used as a runtime URDF limit. Keep it out of +# RIGHT_19_MECHANICAL_ENDPOINT_JOINTS (whose offsets also move upper limits), +# and use its endpoint estimate only as the final static roll origin. This +# makes roll independent of the fitted palm-frame phase and of all yaw logic. +RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS = frozenset({"thumb_cmc_roll"}) +RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS = ( + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS + | RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS +) +RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS = RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS + + +def anchor_right_19_mechanical_endpoint_curves( + curves: Mapping[str, JointCurveFit], + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + *, + maximum_direction_difference_rad: float = math.radians(1.0), + maximum_curve_correction_rad: float = math.radians(3.0), + feedback_endpoint_joints: frozenset[str] = frozenset(), + endpoint_joints: frozenset[str] | None = None, +) -> dict[str, JointCurveFit]: + """Set mechanical-endpoint curve scale from direct SO(3) travel. + + A fitted-axis projection is useful for a dense, signed command curve, but + a small view-dependent orthogonal PnP component can change that + projection's full-scale value. The rotation magnitude between the two + settled endpoint poses is the revolute-joint travel itself: conjugating + both poses by a rigid hand/camera transform or either fixed Tag mounting + rotation cannot change it. + + Use every supplied cycle and both sweep directions as independent endpoint + measurements, reject disagreement, and apply their robust median as one + scale correction to the already validated curve. Dense CMC-roll records + are selected in feedback coordinates; settled mechanical-contact records + remain selected in requested-command coordinates. + """ + if ( + not math.isfinite(maximum_direction_difference_rad) + or maximum_direction_difference_rad <= 0.0 + ): + raise ValueError("endpoint direction difference limit must be positive") + if ( + not math.isfinite(maximum_curve_correction_rad) + or maximum_curve_correction_rad <= 0.0 + ): + raise ValueError("endpoint curve correction limit must be positive") + + result = dict(curves) + selected_endpoint_joints = ( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + if endpoint_joints is None + else frozenset(str(name) for name in endpoint_joints) + ) + unknown_endpoint_joints = sorted( + set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) + ) + if unknown_endpoint_joints: + raise ValueError( + "endpoint joints are not endpoint measurements: " + + ",".join(unknown_endpoint_joints) + ) + unknown_feedback_joints = sorted( + set(feedback_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) + ) + if unknown_feedback_joints: + raise ValueError( + "feedback endpoint joints are not endpoint measurements: " + + ",".join(unknown_feedback_joints) + ) + + for name in sorted(selected_endpoint_joints): + fit = curves.get(name) + records = list(records_by_joint.get(name, ())) + if fit is None: + raise ValueError(f"missing measured endpoint curve for {name}") + + direction_travel: dict[str, float] = {} + all_travel: list[float] = [] + for direction in ("decreasing", "increasing"): + cycles = sorted( + { + int(record.get("cycle", 0)) + for record in records + if str(record.get("direction")) == direction + } + ) + branch_travel: list[float] = [] + for cycle in cycles: + endpoint_rotations: dict[int, Rotation] = {} + for command in (0, 255): + quaternions = [] + for record in records: + if ( + str(record.get("direction")) != direction + or int(record.get("cycle", 0)) != cycle + ): + continue + endpoint_value = ( + explicit_domain_value(record, "feedback") + if name in feedback_endpoint_joints + else record.get( + "requested_command_u8", + record.get("command_u8", -1), + ) + ) + if endpoint_value is None: + continue + if int(round(float(endpoint_value))) == command: + quaternions.append(_relative_rotation(record)) + if not quaternions: + raise ValueError( + f"{name} {direction} cycle {cycle} is missing " + f"settled endpoint {command}" + ) + endpoint_rotations[command] = Rotation.from_quat( + robust_rotation_summary(quaternions)[0] + ) + branch_travel.append( + float( + ( + endpoint_rotations[255].inv() + * endpoint_rotations[0] + ).magnitude() + ) + ) + if not branch_travel: + raise ValueError(f"{name} is missing {direction} endpoint travel") + direction_travel[direction] = float(np.median(branch_travel)) + all_travel.extend(branch_travel) + + travel_values = np.asarray(list(direction_travel.values()), dtype=float) + direction_difference = float(np.ptp(travel_values)) + if direction_difference > maximum_direction_difference_rad: + raise ValueError( + f"{name} settled endpoint directions disagree: " + f"{math.degrees(direction_difference):.3f}deg" + ) + cycle_range = float(np.ptp(np.asarray(all_travel, dtype=float))) + if cycle_range > maximum_direction_difference_rad: + raise ValueError( + f"{name} settled endpoint cycles disagree: " + f"{math.degrees(cycle_range):.3f}deg" + ) + direct_travel = float(np.median(np.asarray(all_travel, dtype=float))) + curve = np.asarray(fit.angle_rad, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name} endpoint curve must contain 256 finite bins") + projected_travel = float(curve[0] - curve[255]) + if direct_travel <= 0.0 or projected_travel <= 0.0: + raise ValueError(f"{name} endpoint travel must be positive") + correction = abs(projected_travel - direct_travel) + if correction > maximum_curve_correction_rad: + raise ValueError( + f"{name} fitted/direct endpoint travel differs by " + f"{math.degrees(correction):.3f}deg" + ) + scale = direct_travel / projected_travel + + def scaled(values: Sequence[float]) -> tuple[float, ...]: + return tuple(float(value) * scale for value in values) + + circle = dict(fit.circle) + circle.update( + { + "mechanical_endpoint_direct_travel_rad": direct_travel, + "mechanical_endpoint_direction_difference_rad": ( + direction_difference + ), + "mechanical_endpoint_cycle_range_rad": cycle_range, + "mechanical_endpoint_sample_count": len(all_travel), + "mechanical_endpoint_raw_curve_travel_rad": projected_travel, + "mechanical_endpoint_curve_scale": scale, + } + ) + quality = dict(fit.quality) + if "arc_rad" in quality: + quality["arc_rad"] = float(quality["arc_rad"]) * scale + result[name] = replace( + fit, + angle_rad=scaled(fit.angle_rad), + decreasing_rad=scaled(fit.decreasing_rad), + increasing_rad=scaled(fit.increasing_rad), + circle=circle, + maximum_monotonic_correction_rad=( + float(fit.maximum_monotonic_correction_rad) * scale + ), + maximum_hysteresis_rad=float(fit.maximum_hysteresis_rad) * scale, + quality=quality, + zero_offset_rad=float(fit.zero_offset_rad) * scale, + ) + return result + + +def derive_right_19_mechanical_endpoint_offsets( + source_urdf: str | Path, + curves: Mapping[str, JointCurveFit], + *, + maximum_offset_rad: float = math.radians(5.0), + endpoint_joints: frozenset[str] | None = None, +) -> dict[str, float]: + """Estimate encoder origins from repeatable measured mechanical endpoints. + + For the four fingers feedback 0 is the independently verified palm-contact + endpoint; thumb MCP uses its independently verified actuator endpoint. + The source URDF upper limit describes those same physical endpoints. + Therefore ``origin_offset + measured_travel == CAD_upper``. + + Thumb CMC pitch/yaw remain excluded because their endpoint-to-CAD contract + is not independently established. CMC roll is included as a post-solve + origin only: its measured travel determines roll without allowing the + palm common-direction fit to write that joint. + + The returned corrections are measured, generally non-zero encoder zeros; + they do not retain the CAD origin. Using full relative rotation travel + makes them invariant to rigid hand movement and arbitrary fixed Tag + mounting rotation, unlike a cross-camera absolute planar-PnP phase. + """ + if not math.isfinite(maximum_offset_rad) or maximum_offset_rad <= 0.0: + raise ValueError("mechanical endpoint maximum offset must be positive") + root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot() + joints = {str(node.get("name")): node for node in root.findall("joint")} + result: dict[str, float] = {} + selected_endpoint_joints = ( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS + if endpoint_joints is None + else frozenset(str(name) for name in endpoint_joints) + ) + unknown_endpoint_joints = sorted( + set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) + ) + if unknown_endpoint_joints: + raise ValueError( + "endpoint joints are not endpoint measurements: " + + ",".join(unknown_endpoint_joints) + ) + for name in sorted(selected_endpoint_joints): + fit = curves.get(name) + joint = joints.get(name) + limit = None if joint is None else joint.find("limit") + if fit is None: + raise ValueError(f"missing measured endpoint curve for {name}") + if limit is None or limit.get("upper") is None: + raise ValueError(f"source URDF joint {name} has no upper limit") + upper = float(limit.get("upper")) + curve = np.asarray(fit.angle_rad, dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name} endpoint curve must contain 256 finite bins") + measured_endpoint = float(curve[0]) + if measured_endpoint <= 0.0 or abs(float(curve[255])) > math.radians(0.05): + raise ValueError( + f"{name} endpoint curve does not use command 255 as its zero reference" + ) + if abs(measured_endpoint - float(np.max(curve))) > math.radians(0.05): + raise ValueError( + f"{name} command 0 is not the measured upper mechanical endpoint" + ) + offset = upper - measured_endpoint + if not math.isfinite(offset) or abs(offset) > maximum_offset_rad: + raise ValueError( + f"{name} endpoint-derived zero offset is outside the safe range: " + f"{math.degrees(offset):.3f}deg" + ) + result[name] = offset + return result + + +def get_zero_calibration_profile( + side: str, layout_id: str = "legacy_11" +) -> ZeroCalibrationProfile: + return _build_zero_profile(get_hand_calibration_profile(side, layout_id)) + + +LEFT_ZERO_PROFILE = _build_zero_profile(LEFT_HAND_PROFILE) + +# Backwards-compatible left-hand aliases. +CONSTRAINED_CIRCLE_JOINTS = LEFT_ZERO_PROFILE.constrained_circle_joints +DIRECT_ZERO_JOINTS = LEFT_ZERO_PROFILE.direct_zero_joints +FIXED_DIRECT_ZERO_OFFSETS_RAD = dict( + LEFT_ZERO_PROFILE.fixed_direct_zero_offsets_rad +) +OPTIMIZED_ZERO_JOINTS = tuple( + name for name in DIRECT_ZERO_JOINTS + if name not in FIXED_DIRECT_ZERO_OFFSETS_RAD +) +AXIS_JOINTS = LEFT_ZERO_PROFILE.axis_joints +INHERITED_ZERO_JOINTS = dict(LEFT_ZERO_PROFILE.inherited_zero_joints) + + +def circle_direction_is_constrained( + joint_name: str, constrained_circle_joints: frozenset[str] +) -> bool: + """Match the constraint used by both canonical and side aliases.""" + name = str(joint_name) + return name in constrained_circle_joints or name.endswith("_side") + + +def select_cross_view_roll_direction_source( + primary_cone_residuals_rad: Sequence[float], + secondary_cone_residuals_rad: Sequence[float], + maximum_cone_mismatch_rad: float, +) -> str: + """Choose one roll-axis view for the complete repeated-sweep group. + + The choice must not be made independently for every cycle. A residual + sitting just either side of the cone gate can otherwise alternate the + selected camera and turn a fixed cross-view bias into a false cycle-axis + spread. The secondary view wins only with a three-quarter consensus and + a median residual inside the unchanged geometry gate; ambiguous groups + retain the primary view. + """ + primary = np.asarray(primary_cone_residuals_rad, dtype=float) + secondary = np.asarray(secondary_cone_residuals_rad, dtype=float) + if ( + primary.ndim != 1 + or secondary.ndim != 1 + or len(primary) != len(secondary) + or len(primary) < 3 + or not np.all(np.isfinite(primary)) + or not np.all(np.isfinite(secondary)) + or float(maximum_cone_mismatch_rad) <= 0.0 + ): + return "primary" + limit = float(maximum_cone_mismatch_rad) + required_consensus = int(math.ceil(0.75 * len(primary))) + primary_passes = int(np.count_nonzero(primary <= limit)) + secondary_passes = int(np.count_nonzero(secondary <= limit)) + if ( + secondary_passes >= required_consensus + and primary_passes < required_consensus + and float(np.median(secondary)) <= limit + and float(np.median(primary)) > limit + ): + return "secondary" + return "primary" + + +ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 = 16 + +# Fit an axis-line point from the complete relative SE(3) trajectory instead +# of using only the moving Tag centre's free 3-D circle. For an approximately +# end-on view, planar-PnP optical depth is deliberately projected out: image +# x/y still observes the radial motion that determines an axis line, while +# the independent Tag depths are the dominant source of false zero phase. +AXIS_POINT_MINIMUM_ROTATION_RAD = math.radians(3.0) +AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD = math.radians(45.0) +AXIS_POINT_RESIDUAL_SCALE_M = 0.001 + +# A joint's own screw axis is invariant to its own encoder-zero offset. Only +# the downstream axis selected by the kinematic chain can observe that zero. +# Feeding every axis-direction and axis-line residual into the optimizer lets +# fixed CAD/PnP geometry residuals push unrelated zero offsets to their bounds. +# Keep the two root lines as the palm-pose anchor, then use only the component +# that actually observes the preceding zero in each serial chain. +ZERO_AXIS_PARENT_JOINT = dict(LEFT_ZERO_PROFILE.axis_parent_joint) +ZERO_AXIS_OBSERVATION_JOINTS = frozenset( + set(LEFT_ZERO_PROFILE.root_anchor_joints) | set(ZERO_AXIS_PARENT_JOINT) +) +ZERO_LINE_OBSERVATION_JOINTS = frozenset( + set(LEFT_ZERO_PROFILE.root_anchor_joints) + | set(LEFT_ZERO_PROFILE.phase_parent_joint) +) +ZERO_ROOT_ANCHOR_JOINTS = LEFT_ZERO_PROFILE.root_anchor_joints +ZERO_MINIMUM_AXIS_CONE_RAD = math.radians(15.0) + + +def _zero_sensitive_axis_error_rad( + predicted_axis: Sequence[float], + observed_axis: Sequence[float], + parent_axis: Sequence[float], +) -> float: + """Return only the axis error that a parent-joint zero can change. + + Rotating a downstream axis about its parent preserves their mutual cone + angle. The component normal to their plane is therefore the observable + encoder-zero error; cone-angle mismatch belongs to fixed geometry/PnP and + must not push a zero offset or fail its holdout validation. + """ + predicted = _vector(predicted_axis, 3, name="predicted axis") + predicted /= np.linalg.norm(predicted) + observed = _vector(observed_axis, 3, name="observed axis") + observed /= np.linalg.norm(observed) + parent = _vector(parent_axis, 3, name="parent axis") + parent /= np.linalg.norm(parent) + if float(predicted @ observed) < 0.0: + observed = -observed + predicted_projected = predicted - parent * float(predicted @ parent) + observed_projected = observed - parent * float(observed @ parent) + predicted_norm = float(np.linalg.norm(predicted_projected)) + observed_norm = float(np.linalg.norm(observed_projected)) + minimum_projection = math.sin(ZERO_MINIMUM_AXIS_CONE_RAD) + if min(predicted_norm, observed_norm) < minimum_projection: + raise ValueError( + "parent and downstream axes have an unobservable cone angle" + ) + predicted_projected /= predicted_norm + observed_projected /= observed_norm + return math.atan2( + float(parent @ np.cross(predicted_projected, observed_projected)), + float( + np.clip(predicted_projected @ observed_projected, -1.0, 1.0) + ), + ) + + +def _axis_cone_mismatch_rad( + predicted_axis: Sequence[float], + observed_axis: Sequence[float], + parent_axis: Sequence[float], +) -> float: + """Return zero-invariant parent/downstream cone-angle disagreement.""" + predicted = _vector(predicted_axis, 3, name="predicted axis") + predicted /= np.linalg.norm(predicted) + observed = _vector(observed_axis, 3, name="observed axis") + observed /= np.linalg.norm(observed) + parent = _vector(parent_axis, 3, name="parent axis") + parent /= np.linalg.norm(parent) + predicted_cone = math.acos( + abs(float(np.clip(parent @ predicted, -1.0, 1.0))) + ) + observed_cone = math.acos( + abs(float(np.clip(parent @ observed, -1.0, 1.0))) + ) + return abs(predicted_cone - observed_cone) + + +def _vector(value: Sequence[float], size: int, *, name: str) -> np.ndarray: + result = np.asarray(value, dtype=float) + if result.shape != (size,) or not np.all(np.isfinite(result)): + raise ValueError(f"{name} must contain {size} finite values") + return result + + +def _pose_matrix(payload: Mapping[str, Any]) -> np.ndarray: + translation = _vector(payload["translation_xyz_m"], 3, name="translation") + quaternion = _vector(payload["quaternion_xyzw"], 4, name="quaternion") + quaternion /= np.linalg.norm(quaternion) + result = np.eye(4) + result[:3, :3] = Rotation.from_quat(quaternion).as_matrix() + result[:3, 3] = translation + return result + + +def _relative_rotation(record: Mapping[str, Any]) -> np.ndarray: + quaternion = _vector( + record["relative_quaternion_xyzw"], 4, name="relative quaternion" + ) + return quaternion / np.linalg.norm(quaternion) + + +def _reference_group_key(record: Mapping[str, Any]) -> tuple[Any, Any]: + return record.get("cycle"), record.get("direction") + + +def _canonical_reference_records( + records: Sequence[Mapping[str, Any]], + canonical_zero_direction: str | None, +) -> list[Mapping[str, Any]]: + """Select the sole physical-zero branch when one is configured.""" + if canonical_zero_direction is None: + return list(records) + if canonical_zero_direction not in {"decreasing", "increasing"}: + raise ValueError("canonical zero direction is invalid") + selected = [ + record + for record in records + if str(record.get("direction")) == canonical_zero_direction + ] + if not selected: + raise ValueError( + f"canonical {canonical_zero_direction} zero branch has no samples" + ) + return selected + + +def _interpolate_reference_rotation( + records: Sequence[Mapping[str, Any]], zero_command_u8: int +) -> Rotation | None: + by_command: dict[int, list[np.ndarray]] = {} + for record in records: + command = int(record["command_u8"]) + by_command.setdefault(command, []).append(_relative_rotation(record)) + if not by_command: + return None + + rotations = { + command: Rotation.from_quat(robust_rotation_summary(values)[0]) + for command, values in by_command.items() + } + zero = int(zero_command_u8) + if zero in rotations: + return rotations[zero] + + lower = [command for command in rotations if command < zero] + upper = [command for command in rotations if command > zero] + lower_command = max(lower) if lower else None + upper_command = min(upper) if upper else None + if lower_command is not None and upper_command is not None: + lower_distance = zero - lower_command + upper_distance = upper_command - zero + if max(lower_distance, upper_distance) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + lower_rotation = rotations[lower_command] + upper_rotation = rotations[upper_command] + fraction = lower_distance / (upper_command - lower_command) + delta = (lower_rotation.inv() * upper_rotation).as_rotvec() + return lower_rotation * Rotation.from_rotvec(delta * fraction) + + nearest_command = min(rotations, key=lambda command: abs(command - zero)) + if abs(nearest_command - zero) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + return rotations[nearest_command] + return None + + +def _near_zero_records( + records: Sequence[Mapping[str, Any]], zero_command_u8: int +) -> list[Mapping[str, Any]]: + groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} + for record in records: + groups.setdefault(_reference_group_key(record), []).append(record) + selected: list[Mapping[str, Any]] = [] + zero = int(zero_command_u8) + for group in groups.values(): + distance = min(abs(int(record["command_u8"]) - zero) for record in group) + if distance > ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: + continue + selected.extend( + record + for record in group + if abs(int(record["command_u8"]) - zero) == distance + ) + return selected + + +def _baseline_reference( + records: Sequence[Mapping[str, Any]], zero_command_u8: int +) -> tuple[float, float, float, float]: + groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} + for record in records: + groups.setdefault(_reference_group_key(record), []).append(record) + values = [ + rotation.as_quat() + for group in groups.values() + if ( + rotation := _interpolate_reference_rotation( + group, zero_command_u8 + ) + ) + is not None + ] + if not values: + raise ValueError( + "joint records have no samples within " + f"{ZERO_REFERENCE_MAXIMUM_DISTANCE_U8} commands of zero " + f"{zero_command_u8}" + ) + return robust_rotation_summary(values)[0] + + +def baseline_hysteresis_by_cycle_rad( + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + axis_xyz: Sequence[float] | None = None, +) -> tuple[float, ...]: + """Return decreasing/increasing joint-angle disagreement per cycle. + + Full SO(3) disagreement includes planar-PnP tilt noise that is orthogonal + to the fitted revolute axis. When an axis is supplied, report only the + physically meaningful component about that axis; the orthogonal component + remains covered by the independent rotation-model residual gate. + """ + samples = [dict(record) for record in records] + axis: np.ndarray | None = None + if axis_xyz is not None: + axis = _vector(axis_xyz, 3, name="baseline hysteresis axis") + norm = float(np.linalg.norm(axis)) + if norm <= 0.0: + raise ValueError("baseline hysteresis axis must be non-zero") + axis /= norm + result: list[float] = [] + for cycle in sorted({int(record["cycle"]) for record in samples}): + rotations: dict[str, Rotation] = {} + for direction in ("decreasing", "increasing"): + selected = [ + record + for record in samples + if int(record["cycle"]) == cycle + and str(record["direction"]) == direction + ] + reference = _interpolate_reference_rotation( + selected, zero_command_u8 + ) + if reference is None: + raise ValueError( + f"cycle {cycle} {direction} is missing baseline samples" + ) + rotations[direction] = reference + delta = ( + rotations["decreasing"].inv() + * rotations["increasing"] + ) + result.append( + float( + delta.magnitude() + if axis is None + else abs(float(delta.as_rotvec() @ axis)) + ) + ) + if not result: + raise ValueError("baseline hysteresis requires at least one cycle") + return tuple(result) + + +def fit_rotation_joint_curve( + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + canonical_zero_direction: str | None = None, +) -> JointCurveFit: + """Fit a direction-aware curve from parent-to-child Tag orientations. + + With ``canonical_zero_direction`` both branches share one physical + reference. Only the canonical branch is zero at ``zero_command_u8``; + the other branch retains its measured backlash/compliance offset. + """ + samples = [dict(record) for record in records] + if len(samples) < 12: + raise ValueError("rotation trajectory requires at least 12 samples") + vectors: list[np.ndarray] = [] + commands: list[int] = [] + values_by_record: list[float] = [] + references: dict[int, Rotation] = {} + for cycle in sorted({int(record["cycle"]) for record in samples}): + cycle_records = [ + record for record in samples if int(record["cycle"]) == cycle + ] + references[cycle] = Rotation.from_quat( + _baseline_reference( + _canonical_reference_records( + cycle_records, canonical_zero_direction + ), + zero_command_u8, + ) + ) + for record in samples: + observed = Rotation.from_quat(_relative_rotation(record)) + vector = ( + references[int(record["cycle"])].inv() * observed + ).as_rotvec() + vectors.append(vector) + commands.append(int(record["command_u8"])) + axis = fit_rotation_axis(vectors, commands) + values_by_record = [float(vector @ axis) for vector in vectors] + curves, correction, hysteresis = _fit_joint_curve( + samples, + values_by_record, + preserve_direction_offset=canonical_zero_direction is not None, + ) + if canonical_zero_direction is None: + for key in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(curves[key], dtype=float) + values -= float(values[int(zero_command_u8)]) + curves[key] = [round(float(value), 8) for value in values] + else: + canonical_key = f"{canonical_zero_direction}_rad" + shared_zero = float(curves[canonical_key][int(zero_command_u8)]) + for key in ("decreasing_rad", "increasing_rad"): + values = np.asarray(curves[key], dtype=float) - shared_zero + curves[key] = [round(float(value), 8) for value in values] + # Before the bridge has observed motion direction it must use the + # same branch that defines the URDF physical zero, never an average + # pose that the mechanism may not be able to occupy. + curves["angle_rad"] = list(curves[canonical_key]) + orthogonal = [ + float(np.linalg.norm(vector - float(vector @ axis) * axis)) + for vector in vectors + ] + return JointCurveFit( + angle_rad=tuple(float(value) for value in curves["angle_rad"]), + decreasing_rad=tuple(float(value) for value in curves["decreasing_rad"]), + increasing_rad=tuple(float(value) for value in curves["increasing_rad"]), + circle={ + "space": "relative_rotation_3d", + "axis_xyz": [float(value) for value in axis], + "zero_command_u8": int(zero_command_u8), + "reference_quaternion_xyzw": [ + float(value) + for value in _baseline_reference( + _canonical_reference_records( + samples, canonical_zero_direction + ), + zero_command_u8, + ) + ], + "canonical_zero_direction": canonical_zero_direction, + }, + maximum_monotonic_correction_rad=float(correction), + maximum_hysteresis_rad=float(hysteresis), + quality={ + "rotation_orthogonal_rms_rad": float( + np.sqrt(np.mean(np.square(orthogonal))) + ), + "arc_rad": float( + max(curves["angle_rad"]) - min(curves["angle_rad"]) + ), + }, + ) + + +def measure_rotation_joint_observation( + fit: JointCurveFit, quaternion_xyzw: Sequence[float] +) -> float: + """Measure one parent-to-child orientation with a fitted 3-D curve.""" + if fit.circle.get("space") != "relative_rotation_3d": + raise ValueError("joint fit is not a relative-rotation curve") + reference = Rotation.from_quat( + _vector( + fit.circle["reference_quaternion_xyzw"], + 4, + name="reference quaternion", + ) + ) + observed = Rotation.from_quat( + _vector(quaternion_xyzw, 4, name="observed quaternion") + ) + axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") + axis /= np.linalg.norm(axis) + return float((reference.inv() * observed).as_rotvec() @ axis) + + +def measure_joint_curve_observation( + fit: JointCurveFit, + *, + quaternion_xyzw: Sequence[float] | None = None, + image_relative_xy_px: Sequence[float] | None = None, +) -> float: + """Measure one observation in the same space as its fitted curve.""" + if fit.circle.get("space") == "relative_rotation_3d": + if quaternion_xyzw is None: + raise ValueError("rotation observation quaternion is missing") + return measure_rotation_joint_observation(fit, quaternion_xyzw) + if fit.circle.get("space") != "image_2d": + raise ValueError("unsupported joint curve observation representation") + if image_relative_xy_px is None: + raise ValueError("image curve observation point is missing") + point = np.asarray(image_relative_xy_px, dtype=float) + centre = np.asarray(fit.circle["center_xy_px"], dtype=float) + reference = np.asarray(fit.circle["reference_xy_px"], dtype=float) + vector = point - centre + if ( + point.shape != (2,) + or not np.all(np.isfinite(point)) + or float(np.linalg.norm(vector)) < 1.0e-9 + ): + raise ValueError("image curve observation point is invalid") + return float(fit.circle["orientation_sign"]) * math.atan2( + float(reference[0] * vector[1] - reference[1] * vector[0]), + float(reference @ vector), + ) + + +def rotation_curve_holdout_errors( + fit: JointCurveFit, + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, +) -> tuple[float, ...]: + """Validate a fitted curve on an untouched scan cycle.""" + samples = [dict(record) for record in records] + if not samples: + raise ValueError("holdout records are empty") + canonical_zero_direction = fit.circle.get("canonical_zero_direction") + reference = Rotation.from_quat( + _baseline_reference( + _canonical_reference_records(samples, canonical_zero_direction), + zero_command_u8, + ) + ) + axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") + axis /= np.linalg.norm(axis) + errors: list[float] = [] + for record in samples: + observed = Rotation.from_quat(_relative_rotation(record)) + angle = float((reference.inv() * observed).as_rotvec() @ axis) + command = int(record["command_u8"]) + direction = str(record["direction"]) + expected_curve = ( + fit.decreasing_rad + if direction == "decreasing" + else fit.increasing_rad + ) + errors.append(angle - float(expected_curve[command])) + return tuple(errors) + + +def joint_curve_holdout_errors( + fit: JointCurveFit, + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, +) -> tuple[float, ...]: + """Validate either a rotation or fixed-parent image-circle curve.""" + if fit.circle.get("space") == "relative_rotation_3d": + return rotation_curve_holdout_errors( + fit, records, zero_command_u8=zero_command_u8 + ) + if fit.circle.get("space") != "image_2d": + raise ValueError("unsupported joint curve holdout representation") + if not records: + raise ValueError("holdout records are empty") + errors: list[float] = [] + for record in records: + observed = measure_joint_curve_observation( + fit, + image_relative_xy_px=record["image_relative_xy_px"], + ) + command = int(record["command_u8"]) + direction = str(record["direction"]) + expected_curve = ( + fit.decreasing_rad + if direction == "decreasing" + else fit.increasing_rad + ) + errors.append(observed - float(expected_curve[command])) + return tuple(errors) + + +@dataclass(frozen=True) +class JointAxisMeasurement: + joint: str + cycle: int + axis_common_xyz: tuple[float, float, float] + point_common_xyz_m: tuple[float, float, float] + condition_state_u8: tuple[float, ...] + plane_rms_m: float + radial_rms_m: float + rotation_circle_axis_difference_rad: float + # Optical-axis direction expressed in the shared calibration frame. It + # lets the phase solver discard the least reliable monocular-PnP depth + # component when the joint axis is viewed approximately end-on. Older + # recordings and synthetic callers may omit it and retain the 3-D path. + view_normal_common_xyz: tuple[float, float, float] | None = None + # Logical commands that produced the condition. Firmware feedback may + # saturate a few u8 short of an endpoint (for example right motor 10 reads + # 250 for command 255); kinematics must use the command-indexed curve while + # retaining condition_state_u8 for diagnostics and safety. + condition_command_u8: tuple[float, ...] | None = None + axis_direction_source: str = "unspecified" + circle_axis_observability: float = 0.0 + axis_point_source: str = "circle_center" + pose_axis_line_rms_m: float = 0.0 + # Measurement record(s) that supplied pose_axis_line_rms_m. A combined + # cross-view axis may keep the front direction but take its physical line + # point and line-quality residual from the side alias. Retry logic must + # clear the actual quality source instead of blindly rescanning ``joint``. + pose_axis_line_source_joints: tuple[str, ...] = () + # Camera centre that observed ``point_common_xyz_m``. When populated, + # the zero solver can use only the ray from this centre to the fitted axis + # point. That ray is the depth-free interpretation-plane observation of + # the physical axis; translating a monocular planar-PnP solution along + # its optical ray therefore cannot rotate the recovered palm frame. + axis_point_camera_center_common_xyz_m: ( + tuple[float, float, float] | None + ) = None + # Normal of the source camera's interpretation plane for this axis line. + # Unlike a 3-D PnP line point, the plane is unchanged by optical-depth + # error. Several named parallel root lines jointly recover their common + # physical direction as the null direction of these plane normals. + axis_point_interpretation_plane_normal_common_xyz: ( + tuple[float, float, float] | None + ) = None + + +@dataclass(frozen=True) +class PalmOrientationMeasurement: + """Direction-only joint observation from a partially visible sweep. + + The moving Tag may disappear before the motor reaches its far endpoint. + Only the relative SO(3) trajectory is retained, so fixed Tag translation + and mounting rotation cannot define the palm phase. + """ + + source_joint: str + model_joint: str + cycle: int + axis_common_xyz: tuple[float, float, float] + condition_state_u8: tuple[float, ...] + observed_arc_rad: float + rotation_orthogonal_rms_rad: float + axis_estimator: str = "baseline_relative_so3" + incremental_pair_count: int = 0 + + +PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 = 24 +PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD = math.radians(0.3) +PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT = 24 +PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE = 75.0 + + +def _incremental_common_rotation_axis( + samples_by_bin: Mapping[ + tuple[str, int], Sequence[Mapping[str, Any]] + ], + *, + command_separation_u8: int = ( + PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 + ), + minimum_rotation_rad: float = ( + PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD + ), + minimum_pair_count: int = PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT, + consensus_percentile: float = ( + PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE + ), +) -> tuple[np.ndarray, int]: + """Fit one physical axis from robust local Tag rotations. + + A single baseline-to-endpoint logarithm is sensitive to smooth planar-PnP + curvature: the fitted axis then changes when the same Tag trajectory is + translated to another part of the image. Local finite rotations are + expressed directly in the common camera frame, and their undirected + weighted consensus estimates the physical revolute axis. The worst + quartile is discarded once more after the initial consensus, which is + enough to reject endpoint branch curvature without learning a + serial-specific yaw value or an image-position correction table. + """ + separation = int(command_separation_u8) + if separation < 4 or separation > 64: + raise ValueError("palm axis command separation must be in [4, 64]") + if not 0.0 < float(minimum_rotation_rad) < math.pi: + raise ValueError("palm axis minimum increment must be in (0, pi)") + if int(minimum_pair_count) < 6: + raise ValueError("palm axis minimum pair count must be at least six") + percentile = float(consensus_percentile) + if not 50.0 <= percentile <= 90.0: + raise ValueError("palm axis consensus percentile must be in [50, 90]") + + binned_common_rotations: dict[tuple[str, int], Rotation] = {} + for key, group in samples_by_bin.items(): + child_quaternions = [] + for record in group: + pose = record.get("child_pose_common") + if not isinstance(pose, Mapping): + continue + quaternion = pose.get("quaternion_xyzw") + if quaternion is not None: + child_quaternions.append(quaternion) + if not child_quaternions: + continue + binned_common_rotations[(str(key[0]), int(key[1]))] = ( + Rotation.from_quat( + robust_rotation_summary(child_quaternions)[0] + ) + ) + + directions: list[np.ndarray] = [] + weights: list[float] = [] + for direction in ("decreasing", "increasing"): + commands = sorted( + command + for candidate_direction, command in binned_common_rotations + if candidate_direction == direction + ) + for start_command in commands: + candidates = [ + command + for command in commands + if separation <= command - start_command <= separation + 2 + ] + if not candidates: + continue + end_command = candidates[0] + start = binned_common_rotations[(direction, start_command)] + end = binned_common_rotations[(direction, end_command)] + local_rotvec = (start.inv() * end).as_rotvec() + common_rotvec = start.apply(local_rotvec) + increment = float(np.linalg.norm(common_rotvec)) + if increment < float(minimum_rotation_rad): + continue + directions.append(common_rotvec / increment) + weights.append(increment) + + if len(directions) < int(minimum_pair_count): + raise ValueError( + "palm axis has too few full-stroke local rotation pairs: " + f"{len(directions)}/{int(minimum_pair_count)}" + ) + vectors = np.asarray(directions, dtype=float) + increments = np.asarray(weights, dtype=float) + + def consensus(selected: np.ndarray) -> np.ndarray: + scatter = np.einsum( + "n,ni,nj->ij", + increments[selected], + vectors[selected], + vectors[selected], + ) + eigenvalues, eigenvectors = np.linalg.eigh(scatter) + if not np.all(np.isfinite(eigenvalues)): + raise ValueError("palm axis local-rotation consensus is invalid") + axis = eigenvectors[:, -1] + return axis / np.linalg.norm(axis) + + selected = np.ones(len(vectors), dtype=bool) + axis = consensus(selected) + # Two deterministic refinement rounds prevent the initial scatter from + # being pulled toward a dense endpoint-bias cluster. + for _ in range(2): + deviations = np.arccos( + np.clip(np.abs(vectors @ axis), -1.0, 1.0) + ) + limit = float(np.percentile(deviations, percentile)) + selected = deviations <= limit + if int(np.count_nonzero(selected)) < int(minimum_pair_count): + raise ValueError("palm axis robust consensus retained too few pairs") + axis = consensus(selected) + return axis, int(np.count_nonzero(selected)) + + +def fit_partial_palm_orientation_measurement( + source_joint: str, + model_joint: str, + records: Sequence[Mapping[str, Any]], + *, + cycle: int, + zero_command_u8: int, + minimum_arc_rad: float = math.radians(15.0), + maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), + maximum_command_distance_u8: int = 255, +) -> PalmOrientationMeasurement: + """Fit a physical axis within a configured window around encoder zero.""" + cycle_samples = [ + dict(record) + for record in records + if int(record.get("cycle", -1)) == int(cycle) + ] + if len(cycle_samples) < 12: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has too few visible samples" + ) + if not 0.0 < float(minimum_arc_rad) < math.pi: + raise ValueError("palm orientation minimum arc must be in (0, pi)") + if float(maximum_rotation_orthogonal_rms_rad) <= 0.0: + raise ValueError( + "palm orientation rotation residual limit must be positive" + ) + command_distance = int(maximum_command_distance_u8) + if command_distance < 1 or command_distance > 255: + raise ValueError( + "palm orientation command distance must be in [1, 255]" + ) + zero = int(zero_command_u8) + samples = [ + record + for record in cycle_samples + if abs(int(record["command_u8"]) - zero) <= command_distance + ] + if len(samples) < 12: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has too few zero-adjacent " + "visible samples" + ) + reference = Rotation.from_quat( + _baseline_reference(samples, int(zero_command_u8)) + ) + samples_by_bin: dict[tuple[str, int], list[Mapping[str, Any]]] = {} + for record in samples: + key = ( + str(record.get("direction", "")), + int(record["command_u8"]), + ) + samples_by_bin.setdefault(key, []).append(record) + if len(samples_by_bin) < 6: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has too few visible command bins" + ) + binned_rotations = [ + Rotation.from_quat( + robust_rotation_summary( + [_relative_rotation(record) for record in group] + )[0] + ) + for group in samples_by_bin.values() + ] + vectors = [ + (reference.inv() * rotation).as_rotvec() + for rotation in binned_rotations + ] + axis_child = fit_rotation_axis( + vectors, [command for _direction, command in samples_by_bin] + ) + angles = np.asarray( + [float(vector @ axis_child) for vector in vectors], dtype=float + ) + observed_arc = float(np.ptp(angles)) + orthogonal_rms = float( + np.sqrt( + np.mean( + [ + np.linalg.norm( + vector - float(vector @ axis_child) * axis_child + ) + ** 2 + for vector in vectors + ] + ) + ) + ) + if observed_arc < float(minimum_arc_rad): + raise ValueError( + f"{source_joint} cycle {cycle + 1} visible rotation arc " + f"{math.degrees(observed_arc):.3f}deg is below " + f"{math.degrees(minimum_arc_rad):.3f}deg" + ) + if orthogonal_rms > float(maximum_rotation_orthogonal_rms_rad): + raise ValueError( + f"{source_joint} cycle {cycle + 1} zero-adjacent rotation " + f"residual {math.degrees(orthogonal_rms):.3f}deg exceeds " + f"{math.degrees(maximum_rotation_orthogonal_rms_rad):.3f}deg" + ) + zero_records = _near_zero_records(samples, int(zero_command_u8)) + if not zero_records: + raise ValueError( + f"{source_joint} cycle {cycle + 1} has no visible zero pose" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + axis_parent = reference.apply(axis_child) + axis_common = Rotation.from_quat(parent_quaternion).apply(axis_parent) + axis_common /= np.linalg.norm(axis_common) + axis_estimator = "baseline_relative_so3" + incremental_pair_count = 0 + # Product recordings persist the selected moving-Tag pose in the common + # camera frame. Use the complete visible stroke only for a local-motion + # axis consensus; retain the zero-adjacent fit above as the unchanged arc + # and residual quality gate. Legacy/synthetic records without the common + # child pose keep their previous estimator exactly. + if any( + isinstance(record.get("child_pose_common"), Mapping) + for record in cycle_samples + ): + full_stroke_bins: dict[ + tuple[str, int], list[Mapping[str, Any]] + ] = {} + for record in cycle_samples: + key = ( + str(record.get("direction", "")), + int(record["command_u8"]), + ) + full_stroke_bins.setdefault(key, []).append(record) + incremental_axis, incremental_pair_count = ( + _incremental_common_rotation_axis(full_stroke_bins) + ) + if float(incremental_axis @ axis_common) < 0.0: + incremental_axis = -incremental_axis + axis_common = incremental_axis + axis_estimator = "robust_full_stroke_local_so3_v1" + state = np.median( + np.asarray( + [record["state_u8"] for record in zero_records], dtype=float + ), + axis=0, + ) + return PalmOrientationMeasurement( + source_joint=str(source_joint), + model_joint=str(model_joint), + cycle=int(cycle), + axis_common_xyz=tuple(float(value) for value in axis_common), + condition_state_u8=tuple(float(value) for value in state), + observed_arc_rad=observed_arc, + rotation_orthogonal_rms_rad=orthogonal_rms, + axis_estimator=axis_estimator, + incremental_pair_count=incremental_pair_count, + ) + + +def fit_partial_palm_orientation_measurements( + *, + sources: Mapping[str, str], + records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], + motor_by_source: Mapping[str, int], + baseline_command_u8: Sequence[int], + cycles: Sequence[int], + minimum_sources: int, + minimum_arc_rad: float = math.radians(15.0), + maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), + maximum_command_distance_u8: int = 255, +) -> tuple[tuple[PalmOrientationMeasurement, ...], Mapping[str, str]]: + """Fit every usable optional source and require configured coverage.""" + source_map = {str(name): str(model) for name, model in sources.items()} + minimum = int(minimum_sources) + if not source_map: + if minimum != 0: + raise ValueError( + "palm orientation minimum is non-zero without sources" + ) + return (), {} + if minimum < 2 or minimum > len(source_map): + raise ValueError( + "palm orientation minimum source count must be between two " + "and the configured source count" + ) + missing_motors = sorted(set(source_map) - set(motor_by_source)) + if missing_motors: + raise ValueError( + "palm orientation sources are missing motor mappings: " + + ",".join(missing_motors) + ) + fitted: list[PalmOrientationMeasurement] = [] + rejected: dict[str, str] = {} + for cycle in (int(value) for value in cycles): + cycle_fitted: list[PalmOrientationMeasurement] = [] + for source_joint, model_joint in source_map.items(): + motor = int(motor_by_source[source_joint]) + try: + measurement = fit_partial_palm_orientation_measurement( + source_joint, + model_joint, + records_by_joint.get(source_joint, ()), + cycle=cycle, + zero_command_u8=int(baseline_command_u8[motor]), + minimum_arc_rad=minimum_arc_rad, + maximum_rotation_orthogonal_rms_rad=( + maximum_rotation_orthogonal_rms_rad + ), + maximum_command_distance_u8=( + maximum_command_distance_u8 + ), + ) + except Exception as error: + rejected[f"{source_joint}:cycle{cycle + 1}"] = str(error) + continue + cycle_fitted.append(measurement) + if len(cycle_fitted) < minimum: + cycle_reasons = { + key: value + for key, value in rejected.items() + if key.endswith(f":cycle{cycle + 1}") + } + raise ValueError( + f"palm orientation cycle {cycle + 1} has " + f"{len(cycle_fitted)}/{minimum} usable sources: " + + "; ".join( + f"{key}={value}" for key, value in cycle_reasons.items() + ) + ) + fitted.extend(cycle_fitted) + return tuple(fitted), rejected + + +def with_depth_free_axis_projection( + measurement: JointAxisMeasurement, + camera_center_common_xyz_m: Sequence[float], +) -> JointAxisMeasurement: + """Attach the source-camera interpretation plane of an axis line. + + The plane is defined by the camera centre and the fitted 3-D line, but is + only a projective observation: moving either fitted line point along its + optical ray leaves the plane unchanged. A fixed Tag mount changes the + moving point trajectory, not its recovered physical screw axis. + """ + camera_center = _vector( + camera_center_common_xyz_m, 3, name="axis camera centre" + ) + point = np.asarray(measurement.point_common_xyz_m, dtype=float) + axis = np.asarray(measurement.axis_common_xyz, dtype=float) + axis /= np.linalg.norm(axis) + ray = point - camera_center + ray_norm = float(np.linalg.norm(ray)) + if ray_norm <= 1.0e-6: + raise ValueError("axis point coincides with its source camera") + plane_normal = np.cross(ray / ray_norm, axis) + plane_norm = float(np.linalg.norm(plane_normal)) + if plane_norm <= 1.0e-6: + raise ValueError("axis projection is degenerate in its source camera") + plane_normal /= plane_norm + return replace( + measurement, + axis_point_camera_center_common_xyz_m=tuple( + float(value) for value in camera_center + ), + axis_point_interpretation_plane_normal_common_xyz=tuple( + float(value) for value in plane_normal + ), + ) + + +def cross_view_side_line_source( + measurement: JointAxisMeasurement, +) -> str | None: + """Return the side alias that supplied a fallback axis-line point.""" + sources = tuple( + str(source) + for source in getattr( + measurement, "pose_axis_line_source_joints", () + ) + ) + if ( + getattr(measurement, "axis_point_source", "") + in { + "side_circle_cross_view", + "side_circle_shared_radius_cross_view", + "side_interpretation_plane_cross_view", + } + and len(sources) == 1 + and sources[0].endswith("_side") + ): + return sources[0] + return None + + +def axis_line_uses_depth_free_interpretation_plane( + measurement: JointAxisMeasurement, +) -> bool: + """Return whether only the source-camera bearing is geometrically used.""" + return bool( + measurement.axis_point_source + in { + "side_interpretation_plane_cross_view", + "front_interpretation_plane_cross_view_validated", + } + and measurement.axis_point_camera_center_common_xyz_m is not None + ) + + +def refit_axis_line_group_with_shared_radius( + measurements: Sequence[JointAxisMeasurement], + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + canonical_zero_direction: str | None, +) -> tuple[JointAxisMeasurement, ...]: + """Refit repeated cross-view axis lines with one physical radius. + + A side-view roll alias observes the same child Tag and the same physical + lever arm in every cycle. Fitting an independent radius to each short, + near-edge-on arc leaves radius and circle centre strongly correlated; + sub-pixel PnP noise can then move the reported axis line by several + millimetres even though every trajectory has a low radial residual. + + Keep a separate centre (and therefore an independent line-repeatability + check) for every cycle, but solve one shared radius from all cycles. This + is a physical constraint rather than a relaxed quality gate: the returned + lines are still checked against the unchanged cycle RMS limit, and each + cycle's radial residual remains an independent hard check. + """ + group = tuple(measurements) + if len(group) < 2: + return group + cycles = [int(measurement.cycle) for measurement in group] + if len(set(cycles)) != len(cycles): + raise ValueError("shared-radius axis group contains duplicate cycles") + + entries: list[dict[str, Any]] = [] + initial_parameters: list[float] = [] + initial_radii: list[float] = [] + for measurement in group: + cycle_records = [ + dict(record) + for record in records + if int(record.get("cycle", -1)) == int(measurement.cycle) + ] + motion_records = [ + record + for record in cycle_records + if str(record.get("kind", "sample")) != "baseline_hold_sample" + ] + if len(motion_records) < 6: + motion_records = cycle_records + if len(motion_records) < 6: + raise ValueError( + f"cycle {measurement.cycle + 1} has too few shared-radius samples" + ) + + reference_records = _canonical_reference_records( + cycle_records, canonical_zero_direction + ) + zero_records = _near_zero_records( + reference_records, int(zero_command_u8) + ) + if not zero_records: + raise ValueError( + f"cycle {measurement.cycle + 1} has no shared-radius zero pose" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_translation = np.median(parent_poses[:, :3, 3], axis=0) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + parent_rotation = Rotation.from_quat(parent_quaternion) + common_axis = _vector( + measurement.axis_common_xyz, 3, name="shared-radius common axis" + ) + common_axis /= np.linalg.norm(common_axis) + parent_axis = parent_rotation.inv().apply(common_axis) + parent_axis /= np.linalg.norm(parent_axis) + + points = np.asarray( + [record["relative_translation_xyz_m"] for record in motion_records], + dtype=float, + ) + basis_x, basis_y = _plane_basis(parent_axis) + origin = np.mean(points, axis=0) + local = points - origin + points_xy = np.column_stack((local @ basis_x, local @ basis_y)) + initial = _fit_circle_with_axis(points, parent_axis) + initial_center = _vector( + initial["center_xyz_m"], 3, name="initial shared-radius centre" + ) + initial_parameters.extend( + [ + float((initial_center - origin) @ basis_x), + float((initial_center - origin) @ basis_y), + ] + ) + initial_radii.append(float(initial["radius_m"])) + entries.append( + { + "measurement": measurement, + "points": points, + "points_xy": points_xy, + "origin": origin, + "basis_x": basis_x, + "basis_y": basis_y, + "parent_axis": parent_axis, + "parent_rotation": parent_rotation, + "parent_translation": parent_translation, + } + ) + + median_radius = float(np.median(initial_radii)) + if median_radius <= 1.0e-6 or not math.isfinite(median_radius): + raise ValueError("shared-radius axis group has an invalid initial radius") + initial_value = np.asarray( + [*initial_parameters, median_radius], dtype=float + ) + + def residual(parameters: np.ndarray) -> np.ndarray: + radius = float(parameters[-1]) + return np.concatenate( + [ + np.linalg.norm( + entry["points_xy"] + - parameters[2 * index : 2 * index + 2], + axis=1, + ) + - radius + for index, entry in enumerate(entries) + ] + ) / 0.0005 + + lower = np.full(initial_value.shape, -np.inf, dtype=float) + upper = np.full(initial_value.shape, np.inf, dtype=float) + lower[-1] = max(1.0e-6, 0.5 * median_radius) + upper[-1] = 2.0 * median_radius + solution = least_squares( + residual, + initial_value, + bounds=(lower, upper), + loss="soft_l1", + f_scale=1.0, + max_nfev=3000, + ) + if not solution.success: + raise ValueError( + "shared-radius axis optimization failed: " + solution.message + ) + + shared_radius = float(solution.x[-1]) + if not math.isfinite(shared_radius): + raise ValueError("shared-radius axis optimization is non-finite") + result: list[JointAxisMeasurement] = [] + for index, entry in enumerate(entries): + center_xy = solution.x[2 * index : 2 * index + 2] + center_parent = ( + entry["origin"] + + float(center_xy[0]) * entry["basis_x"] + + float(center_xy[1]) * entry["basis_y"] + ) + # The coordinate along an infinite axis is a gauge. Retain the + # robust centre of this cycle's observed axial coordinates. + axial_offsets = ( + entry["points"] - center_parent + ) @ entry["parent_axis"] + center_parent += float(np.median(axial_offsets)) * entry["parent_axis"] + center_common = ( + entry["parent_rotation"].apply(center_parent) + + entry["parent_translation"] + ) + radial_residual = ( + np.linalg.norm( + entry["points_xy"] - center_xy, + axis=1, + ) + - shared_radius + ) + radial_rms = float(np.sqrt(np.mean(np.square(radial_residual)))) + measurement = entry["measurement"] + result.append( + replace( + measurement, + point_common_xyz_m=tuple( + float(value) for value in center_common + ), + radial_rms_m=max(float(measurement.radial_rms_m), radial_rms), + axis_point_source="side_circle_shared_radius_cross_view", + ) + ) + return tuple(result) + + +def maximum_axis_line_cycle_spread_m( + measurements: Sequence[JointAxisMeasurement], +) -> float: + """Measure repeatability of independently fitted near-parallel lines. + + Axis-line points have an arbitrary coordinate along their own direction. + Compare only the perpendicular displacement, symmetrically against both + fitted directions, so that the result remains meaningful with the small + allowed cycle-to-cycle direction variation. + """ + maximum = 0.0 + for left_index, left in enumerate(measurements): + left_axis = np.asarray(left.axis_common_xyz, dtype=float) + left_axis /= np.linalg.norm(left_axis) + left_point = np.asarray(left.point_common_xyz_m, dtype=float) + for right in measurements[left_index + 1 :]: + right_axis = np.asarray(right.axis_common_xyz, dtype=float) + right_axis /= np.linalg.norm(right_axis) + delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point + maximum = max( + maximum, + float(np.linalg.norm(np.cross(delta, left_axis))), + float(np.linalg.norm(np.cross(delta, right_axis))), + ) + return maximum + + +def axis_line_cycle_rms_m( + measurements: Sequence[JointAxisMeasurement], +) -> float: + """Return RMS line-position scatter about the four-cycle consensus. + + For parallel lines, the sum of squared pairwise distances divided by + ``n**2`` equals the mean squared distance from their centroid. Averaging + each pair's distance against both near-parallel directions preserves that + identity while avoiding an arbitrary choice of one cycle's direction. + """ + count = len(measurements) + if count < 2: + return 0.0 + squared_pairwise_sum = 0.0 + for left_index, left in enumerate(measurements): + left_axis = np.asarray(left.axis_common_xyz, dtype=float) + left_axis /= np.linalg.norm(left_axis) + left_point = np.asarray(left.point_common_xyz_m, dtype=float) + for right in measurements[left_index + 1 :]: + right_axis = np.asarray(right.axis_common_xyz, dtype=float) + right_axis /= np.linalg.norm(right_axis) + delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point + left_distance = float(np.linalg.norm(np.cross(delta, left_axis))) + right_distance = float(np.linalg.norm(np.cross(delta, right_axis))) + squared_pairwise_sum += 0.5 * ( + left_distance**2 + right_distance**2 + ) + return float(math.sqrt(squared_pairwise_sum / (count**2))) + + +def _fit_axis_point_from_pose_trajectory( + records: Sequence[Mapping[str, Any]], + *, + zero_command_u8: int, + axis_parent_xyz: Sequence[float], + angle_axis_parent_xyz: Sequence[float], + phase_reference_point_parent_xyz: Sequence[float], + view_normal_common_xyz: Sequence[float] | None, + canonical_zero_direction: str | None = None, + allow_axial_translation: bool = False, +) -> tuple[np.ndarray, float, str]: + """Fit the closest point on a revolute axis from full relative poses. + + If ``T(q)`` maps the moving Tag into its parent Tag frame, then + ``T(q) @ inv(T(0))`` is a rotation about the physical joint axis and its + translation obeys ``(I - R(q)) p = t(q)``. Solving this equation over the + complete trajectory uses both pose orientation and translation and avoids + treating a monocular Tag-centre depth arc as ground-truth geometry. + """ + samples = [dict(record) for record in records] + reference_samples = _canonical_reference_records( + samples, canonical_zero_direction + ) + zero_records = _near_zero_records(reference_samples, zero_command_u8) + if not zero_records: + raise ValueError( + f"axis-point fit has no record near baseline {zero_command_u8}" + ) + + reference_rotation = Rotation.from_quat( + _baseline_reference(reference_samples, zero_command_u8) + ).as_matrix() + reference_translation = np.median( + np.asarray( + [record["relative_translation_xyz_m"] for record in zero_records], + dtype=float, + ), + axis=0, + ) + axis = _vector(axis_parent_xyz, 3, name="axis-point direction") + axis /= np.linalg.norm(axis) + angle_axis = _vector( + angle_axis_parent_xyz, 3, name="axis-point angle direction" + ) + angle_axis /= np.linalg.norm(angle_axis) + phase_reference_point = _vector( + phase_reference_point_parent_xyz, + 3, + name="axis-point phase reference", + ) + helper = ( + np.asarray([1.0, 0.0, 0.0]) + if abs(float(axis[0])) < 0.8 + else np.asarray([0.0, 1.0, 0.0]) + ) + basis_first = np.cross(axis, helper) + basis_first /= np.linalg.norm(basis_first) + basis = np.column_stack( + (basis_first, np.cross(axis, basis_first)) + ) + + view_normal_common = None + use_image_plane_projection = False + if view_normal_common_xyz is not None: + view_normal_common = _vector( + view_normal_common_xyz, 3, name="view normal" + ) + view_normal_common /= np.linalg.norm(view_normal_common) + reference_parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix( + _pose_matrix(record["parent_pose_common"])[:3, :3] + ).as_quat() + for record in zero_records + ] + )[0] + reference_parent_rotation = Rotation.from_quat( + reference_parent_quaternion + ) + reference_view_normal_parent = reference_parent_rotation.inv().apply( + view_normal_common + ) + reference_view_normal_parent /= np.linalg.norm( + reference_view_normal_parent + ) + use_image_plane_projection = abs( + float(axis @ reference_view_normal_parent) + ) >= math.cos(AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD) + + matrices: list[np.ndarray] = [] + translations: list[np.ndarray] = [] + used_image_plane_projection = False + raw_angles: list[float] = [] + translation_angles: list[float] = [] + reference_radial = reference_translation - phase_reference_point + reference_radial -= axis * float(reference_radial @ axis) + reference_radial_norm = float(np.linalg.norm(reference_radial)) + if reference_radial_norm < 1.0e-6: + raise ValueError("axis-point phase reference radius is degenerate") + reference_radial /= reference_radial_norm + for record in samples: + observed_rotation = Rotation.from_quat( + _relative_rotation(record) + ).as_matrix() + observed_delta_rotation = observed_rotation @ reference_rotation.T + raw_angle = float( + Rotation.from_matrix(observed_delta_rotation).as_rotvec() + @ angle_axis + ) + if abs(raw_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: + continue + observed_translation = _vector( + record["relative_translation_xyz_m"], + 3, + name="relative translation", + ) + observed_radial = observed_translation - phase_reference_point + observed_radial -= axis * float(observed_radial @ axis) + observed_radial_norm = float(np.linalg.norm(observed_radial)) + if observed_radial_norm <= 1.0e-6: + continue + observed_radial /= observed_radial_norm + raw_angles.append(raw_angle) + translation_angles.append( + math.atan2( + float(axis @ np.cross(reference_radial, observed_radial)), + float( + np.clip(reference_radial @ observed_radial, -1.0, 1.0) + ), + ) + ) + orientation_sign = 1.0 + if raw_angles and float( + np.sum(np.asarray(raw_angles) * np.asarray(translation_angles)) + ) < 0.0: + orientation_sign = -1.0 + + for record in samples: + observed_rotation = Rotation.from_quat( + _relative_rotation(record) + ).as_matrix() + observed_translation = _vector( + record["relative_translation_xyz_m"], + 3, + name="relative translation", + ) + observed_delta_rotation = observed_rotation @ reference_rotation.T + signed_angle = float( + Rotation.from_matrix(observed_delta_rotation).as_rotvec() + @ angle_axis + ) * orientation_sign + if abs(signed_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: + continue + # Direction and angle have deliberately separate sources. A trusted + # upstream/circle direction may replace a biased planar-PnP axis, while + # the relative-orientation trajectory still provides a repeatable + # scalar travel. Reconstruct the admissible revolute motion before + # solving its axis line instead of feeding a contradictory 3-D + # rotation into the translation equations. + # The rotation-trajectory axis is undirected. Align its scalar sign + # with the selected physical axis using the Tag-centre trajectory; + # this remains robust even when a trusted upstream direction replaces + # a badly biased planar-PnP orientation axis. + delta_rotation = Rotation.from_rotvec( + axis * signed_angle + ).as_matrix() + delta_translation = ( + observed_translation + - delta_rotation @ reference_translation + ) + # A screw-driven revolute joint may carry real translation along its + # axis. That component is in the null space of (I - R), contains no + # information about the perpendicular axis-line point, and must not + # inflate or bias the line fit as if the mechanism were pure rotary. + axis_projection = ( + np.eye(3) - np.outer(axis, axis) + if allow_axial_translation + else np.eye(3) + ) + projection = axis_projection + axis_point_matrix = (np.eye(3) - delta_rotation) @ basis + if use_image_plane_projection: + assert view_normal_common is not None + parent_rotation = Rotation.from_matrix( + _pose_matrix(record["parent_pose_common"])[:3, :3] + ) + view_normal_parent = parent_rotation.inv().apply( + view_normal_common + ) + view_normal_parent /= np.linalg.norm(view_normal_parent) + image_projection = np.eye(3) - np.outer( + view_normal_parent, view_normal_parent + ) + projection = image_projection @ axis_projection + # A single end-on camera cannot distinguish absolute depth from a + # shift along the revolute axis. Keep the observable image-plane + # equations only; adding a free baseline-depth variable makes the + # axis point rank-deficient and pretends that this gauge is + # observable. Parallel phase chains are later compared in the + # same view, where their common depth gauge cancels. + used_image_plane_projection = True + matrices.append(projection @ axis_point_matrix) + translations.append(projection @ delta_translation) + + if len(matrices) < 6: + raise ValueError("axis-point fit has too few observable pose samples") + matrix = np.vstack(matrices) + translation = np.concatenate(translations) + singular_values = np.linalg.svd(matrix, compute_uv=False) + if ( + singular_values.size < 2 + or singular_values[-1] <= 1.0e-9 + or singular_values[-1] / singular_values[0] < 1.0e-3 + ): + raise ValueError("axis-point pose trajectory is ill-conditioned") + + unknown_count = matrix.shape[1] + solution = least_squares( + lambda value: ( + matrix @ value - translation + ) / AXIS_POINT_RESIDUAL_SCALE_M, + np.zeros(unknown_count, dtype=float), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + if not solution.success: + raise ValueError( + "axis-point pose optimization failed: " + solution.message + ) + residual = matrix @ solution.x - translation + rms = float(np.sqrt(np.mean(np.square(residual)))) + source = ( + "pose_trajectory_image_plane" + if used_image_plane_projection + else "pose_trajectory_3d" + ) + return basis @ solution.x[:2], rms, source + + +def fit_joint_axis_measurement( + joint: str, + records: Sequence[Mapping[str, Any]], + *, + cycle: int, + zero_command_u8: int, + axis_common_constraint: Sequence[float] | None = None, + constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS, + view_normal_common_xyz: Sequence[float] | None = None, + canonical_zero_direction: str | None = None, +) -> JointAxisMeasurement: + """Fit one physical screw axis from one complete scan cycle.""" + samples = [ + dict(record) for record in records if int(record["cycle"]) == int(cycle) + ] + if len(samples) < 12: + raise ValueError(f"{joint} cycle {cycle + 1} has too few samples") + points = np.asarray( + [record["relative_translation_xyz_m"] for record in samples], dtype=float + ) + free_circle_axis, free_plane_rms = _fit_plane_axis([points]) + singular_values = np.linalg.svd( + points - np.mean(points, axis=0), compute_uv=False + ) + circle_axis_observability = ( + 0.0 + if singular_values.size < 2 or singular_values[0] <= 1.0e-12 + else float(singular_values[1] / singular_values[0]) + ) + + reference_samples = _canonical_reference_records( + samples, canonical_zero_direction + ) + reference = Rotation.from_quat( + _baseline_reference(reference_samples, zero_command_u8) + ) + rotation_vectors = [ + (reference.inv() * Rotation.from_quat(_relative_rotation(record))).as_rotvec() + for record in samples + ] + commands = [int(record["command_u8"]) for record in samples] + rotation_axis_child = fit_rotation_axis(rotation_vectors, commands) + # reference maps the child Tag frame at baseline into the parent Tag + # frame. The quaternion delta axis is expressed in that child frame, + # while the fitted centre circle is expressed in the parent frame. This + # conversion is what makes arbitrary Tag mounting rotations harmless. + rotation_axis = reference.apply(rotation_axis_child) + if float(rotation_axis @ free_circle_axis) < 0.0: + free_circle_axis = -free_circle_axis + disagreement = math.acos( + float(np.clip(rotation_axis @ free_circle_axis, -1.0, 1.0)) + ) + + zero_records = _near_zero_records(reference_samples, zero_command_u8) + if not zero_records: + raise ValueError( + f"{joint} cycle has no record near baseline {zero_command_u8}" + ) + parent_poses = np.asarray( + [_pose_matrix(record["parent_pose_common"]) for record in zero_records] + ) + parent_translation = np.median(parent_poses[:, :3, 3], axis=0) + parent_quaternion = robust_rotation_summary( + [ + Rotation.from_matrix(matrix[:3, :3]).as_quat() + for matrix in parent_poses + ] + )[0] + parent_rotation = Rotation.from_quat(parent_quaternion) + axis_direction_source = "rotation" + + if axis_common_constraint is not None: + common_axis = _vector( + axis_common_constraint, 3, name="common axis constraint" + ) + common_axis /= np.linalg.norm(common_axis) + fitted_axis = parent_rotation.inv().apply(common_axis) + if float(fitted_axis @ rotation_axis) < 0.0: + fitted_axis = -fitted_axis + circle = _fit_circle_with_axis(points, fitted_axis) + plane_rms = float(circle["plane_rms_m"]) + axis_direction_source = "upstream_constraint" + elif str(joint) in constrained_circle_joints: + # The side/front views are close to end-on for these axes. Monocular + # planar-PnP depth bias can therefore tilt even an apparently smooth, + # low-residual Tag-centre circle. The relative orientation trajectory + # observes the screw-axis direction directly and is invariant to an + # arbitrary fixed Tag mounting rotation; use it to constrain the 3-D + # circle and estimate only the axis line. Never switch the direction + # source based on a rotation-vs-circle threshold: that discontinuity + # makes otherwise identical repeat cycles choose different models. + fitted_axis = rotation_axis + circle = _fit_circle_with_axis(points, fitted_axis) + plane_rms = float(circle["plane_rms_m"]) + else: + # Oblique trajectories with an observable 3-D motion plane retain the + # independent rotation/centre cross-check and fuse both estimates. + free_circle = _fit_circle_with_axis(points, free_circle_axis) + fitted_axis = rotation_axis + free_circle_axis + if float(np.linalg.norm(fitted_axis)) < 1.0e-9: + raise ValueError(f"{joint} rotation and centre axes are opposed") + fitted_axis /= np.linalg.norm(fitted_axis) + circle = free_circle + plane_rms = max( + float(free_plane_rms), float(circle["plane_rms_m"]) + ) + axis_direction_source = "rotation_circle_fusion" + axis_common = parent_rotation.apply(fitted_axis) + point_parent, pose_axis_line_rms_m, axis_point_source = ( + _fit_axis_point_from_pose_trajectory( + samples, + zero_command_u8=zero_command_u8, + axis_parent_xyz=fitted_axis, + angle_axis_parent_xyz=rotation_axis, + phase_reference_point_parent_xyz=circle["center_xyz_m"], + view_normal_common_xyz=view_normal_common_xyz, + canonical_zero_direction=canonical_zero_direction, + allow_axial_translation=str(joint).endswith("_side"), + ) + ) + point_common = parent_rotation.apply(point_parent) + parent_translation + state = np.median( + np.asarray([record["state_u8"] for record in zero_records], dtype=float), + axis=0, + ) + return JointAxisMeasurement( + joint=str(joint), + cycle=int(cycle), + axis_common_xyz=tuple(float(value) for value in axis_common), + point_common_xyz_m=tuple(float(value) for value in point_common), + condition_state_u8=tuple(float(value) for value in state), + plane_rms_m=float(plane_rms), + radial_rms_m=float(circle["radial_rms_m"]), + rotation_circle_axis_difference_rad=float(disagreement), + view_normal_common_xyz=( + None + if view_normal_common_xyz is None + else tuple( + float(value) + for value in _vector( + view_normal_common_xyz, 3, name="view normal" + ) + ) + ), + axis_direction_source=axis_direction_source, + circle_axis_observability=float(circle_axis_observability), + axis_point_source=axis_point_source, + pose_axis_line_rms_m=pose_axis_line_rms_m, + pose_axis_line_source_joints=(str(joint),), + ) + + +@dataclass(frozen=True) +class _UrdfJoint: + name: str + parent: str + child: str + origin: np.ndarray + axis: np.ndarray + mimic_joint: str | None + mimic_multiplier: float + mimic_offset: float + + +class UrdfKinematicModel: + def __init__(self, source: str | Path) -> None: + self.source = Path(source).expanduser().resolve() + if not self.source.is_file(): + raise ValueError(f"source URDF does not exist: {self.source}") + root = ET.parse(self.source).getroot() + self.joints: dict[str, _UrdfJoint] = {} + self.parent_joint_by_child: dict[str, str] = {} + for element in root.findall("joint"): + if element.get("type") == "fixed": + pass + name = str(element.get("name")) + parent = element.find("parent") + child = element.find("child") + if parent is None or child is None: + continue + origin_node = element.find("origin") + xyz = _parse_triplet( + "0 0 0" if origin_node is None else origin_node.get("xyz", "0 0 0") + ) + rpy = _parse_triplet( + "0 0 0" if origin_node is None else origin_node.get("rpy", "0 0 0") + ) + origin = np.eye(4) + origin[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() + origin[:3, 3] = xyz + axis_node = element.find("axis") + axis = _parse_triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + axis /= np.linalg.norm(axis) + mimic = element.find("mimic") + model = _UrdfJoint( + name=name, + parent=str(parent.get("link")), + child=str(child.get("link")), + origin=origin, + axis=axis, + mimic_joint=None if mimic is None else str(mimic.get("joint")), + mimic_multiplier=( + 1.0 if mimic is None else float(mimic.get("multiplier", "1")) + ), + mimic_offset=( + 0.0 if mimic is None else float(mimic.get("offset", "0")) + ), + ) + self.joints[name] = model + self.parent_joint_by_child[model.child] = name + + def _chain(self, target_joint: str) -> list[_UrdfJoint]: + if target_joint not in self.joints: + raise ValueError(f"URDF is missing joint {target_joint}") + result: list[_UrdfJoint] = [] + current = self.joints[target_joint] + while True: + result.append(current) + parent_joint = self.parent_joint_by_child.get(current.parent) + if parent_joint is None: + break + current = self.joints[parent_joint] + result.reverse() + return result + + def link_transform( + self, + target_joint: str, + *, + zero_offsets: Mapping[str, float], + joint_angles: Mapping[str, float], + independent_mimic_angles: bool = False, + ) -> np.ndarray: + """Return base-to-child-link pose for calibration validation. + + ``independent_mimic_angles`` lets the product validator use the five + independently measured passive JSON curves while the emitted URDF's + original mimic XML remains untouched. + """ + transform = np.eye(4) + resolved = {str(name): float(value) for name, value in joint_angles.items()} + for joint in self._chain(target_joint): + transform = transform @ joint.origin @ _axis_rotation( + joint.axis, float(zero_offsets.get(joint.name, 0.0)) + ) + if independent_mimic_angles and joint.name in resolved: + angle = resolved[joint.name] + elif joint.mimic_joint is None: + angle = float(resolved.get(joint.name, 0.0)) + else: + angle = ( + joint.mimic_multiplier + * float(resolved.get(joint.mimic_joint, 0.0)) + + joint.mimic_offset + ) + resolved[joint.name] = angle + transform = transform @ _axis_rotation(joint.axis, angle) + return transform + + def axis_line( + self, + target_joint: str, + *, + zero_offsets: Mapping[str, float], + joint_angles: Mapping[str, float], + ) -> tuple[np.ndarray, np.ndarray]: + transform = np.eye(4) + resolved_angles = dict(joint_angles) + for joint in self._chain(target_joint): + corrected_origin = joint.origin @ _axis_rotation( + joint.axis, float(zero_offsets.get(joint.name, 0.0)) + ) + joint_frame = transform @ corrected_origin + if joint.name == target_joint: + return ( + joint_frame[:3, :3] @ joint.axis, + joint_frame[:3, 3].copy(), + ) + if joint.mimic_joint is None: + angle = float(resolved_angles.get(joint.name, 0.0)) + else: + angle = ( + joint.mimic_multiplier + * float(resolved_angles.get(joint.mimic_joint, 0.0)) + + joint.mimic_offset + ) + resolved_angles[joint.name] = angle + transform = joint_frame @ _axis_rotation(joint.axis, angle) + raise RuntimeError(f"could not resolve axis line for {target_joint}") + + +def _parse_triplet(value: str) -> np.ndarray: + result = np.asarray([float(item) for item in str(value).split()], dtype=float) + if result.shape != (3,) or not np.all(np.isfinite(result)): + raise ValueError(f"invalid URDF vector: {value}") + return result + + +def _axis_rotation(axis: Sequence[float], angle: float) -> np.ndarray: + result = np.eye(4) + result[:3, :3] = Rotation.from_rotvec( + _vector(axis, 3, name="joint axis") * float(angle) + ).as_matrix() + return result + + +@dataclass(frozen=True) +class ZeroSolveResult: + direct_offsets_rad: Mapping[str, float] + all_active_offsets_rad: Mapping[str, float] + base_translation_xyz_m: tuple[float, float, float] + base_quaternion_xyzw: tuple[float, float, float, float] + validation_errors_rad: tuple[float, ...] + validation_error_by_joint_rad: Mapping[str, float] + validation_line_error_by_joint_m: Mapping[str, float] + axis_line_rms_m: float + passed: bool + cycle_offsets_rad: Mapping[str, tuple[float, ...]] + offset_uncertainty_rad: Mapping[str, float] + offset_confidence_half_width_rad: Mapping[str, float] + training_cycles: tuple[int, ...] + validation_cycle: int + validation_original_error_by_joint_rad: Mapping[str, float] + validation_improvement_by_joint_rad: Mapping[str, float] + validation_improvement_confidence_lower_rad: Mapping[str, float] + observability_rank: int + observability_parameter_count: int + observability_condition_number: float + offset_covariance_rad2: Mapping[str, float] + axis_cone_mismatch_by_joint_rad: Mapping[str, float] + axis_cone_bias_classification_by_joint: Mapping[str, str] + failure_reasons: Mapping[str, str] + + +def merge_right_19_thumb_zero_result( + *, + thumb_result: ZeroSolveResult, + preserved_offsets_rad: Mapping[str, float] | None = None, + companion_result: ZeroSolveResult | None = None, +) -> ZeroSolveResult: + """Merge an independent thumb solve without allowing reverse coupling. + + ``companion_result`` is the finger/full solver result whose thumb values + were fixed to ``thumb_result``. For a thumb-only run, the 12 certified + finger values are supplied through ``preserved_offsets_rad`` instead. + """ + thumb_names = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + if set(thumb_result.direct_offsets_rad) != thumb_names: + raise ValueError("independent thumb result must contain exactly four zeros") + full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + direct_names = set(full_profile.direct_zero_joints) + preserved = { + str(name): float(value) + for name, value in (preserved_offsets_rad or {}).items() + } + if set(preserved) - (direct_names - thumb_names): + raise ValueError("preserved offsets contain a thumb or unknown joint") + if any(not math.isfinite(value) for value in preserved.values()): + raise ValueError("preserved offsets must be finite") + + if companion_result is None: + if set(preserved) != direct_names - thumb_names: + raise ValueError("thumb-only merge requires all 12 certified finger zeros") + direct = dict(preserved) + all_active = {name: 0.0 for name in full_profile.hand.active_joints} + all_active.update(preserved) + base_result = thumb_result + else: + if set(companion_result.direct_offsets_rad) != direct_names: + raise ValueError("full companion result must contain all 16 zeros") + direct = dict(companion_result.direct_offsets_rad) + all_active = dict(companion_result.all_active_offsets_rad) + base_result = companion_result + direct.update(thumb_result.direct_offsets_rad) + all_active.update(thumb_result.direct_offsets_rad) + + def merged_map(name: str) -> dict[str, Any]: + values = ( + {} + if companion_result is None + else dict(getattr(companion_result, name)) + ) + values.update(dict(getattr(thumb_result, name))) + return values + + failure_reasons = merged_map("failure_reasons") + passed = bool( + thumb_result.passed + and (companion_result is None or companion_result.passed) + ) + return replace( + base_result, + direct_offsets_rad=direct, + all_active_offsets_rad=all_active, + validation_errors_rad=( + tuple(thumb_result.validation_errors_rad) + if companion_result is None + else ( + *companion_result.validation_errors_rad, + *thumb_result.validation_errors_rad, + ) + ), + validation_error_by_joint_rad=merged_map( + "validation_error_by_joint_rad" + ), + validation_line_error_by_joint_m=merged_map( + "validation_line_error_by_joint_m" + ), + axis_line_rms_m=( + thumb_result.axis_line_rms_m + if companion_result is None + else max( + companion_result.axis_line_rms_m, + thumb_result.axis_line_rms_m, + ) + ), + passed=passed, + cycle_offsets_rad=merged_map("cycle_offsets_rad"), + offset_uncertainty_rad=merged_map("offset_uncertainty_rad"), + offset_confidence_half_width_rad=merged_map( + "offset_confidence_half_width_rad" + ), + validation_original_error_by_joint_rad=merged_map( + "validation_original_error_by_joint_rad" + ), + validation_improvement_by_joint_rad=merged_map( + "validation_improvement_by_joint_rad" + ), + validation_improvement_confidence_lower_rad=merged_map( + "validation_improvement_confidence_lower_rad" + ), + offset_covariance_rad2=merged_map("offset_covariance_rad2"), + axis_cone_mismatch_by_joint_rad=merged_map( + "axis_cone_mismatch_by_joint_rad" + ), + axis_cone_bias_classification_by_joint=merged_map( + "axis_cone_bias_classification_by_joint" + ), + failure_reasons=failure_reasons, + ) + + +def expand_right_19_thumb_zero_result_with_cad_fingers( + thumb_result: ZeroSolveResult, +) -> ZeroSolveResult: + """Build a standalone thumb result with every finger left at source CAD. + + This is intentionally different from inheriting a prior finger + calibration: the twelve non-thumb values are explicit zeros and no finger + observation or artifact is required. + """ + full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) + thumb_names = { + "thumb_cmc_roll", + "thumb_cmc_yaw", + "thumb_cmc_pitch", + "thumb_mcp", + } + if set(thumb_result.direct_offsets_rad) != thumb_names: + raise ValueError("independent thumb result must contain exactly four zeros") + direct = {name: 0.0 for name in full_profile.direct_zero_joints} + direct.update(thumb_result.direct_offsets_rad) + all_active = {name: 0.0 for name in full_profile.hand.active_joints} + all_active.update(thumb_result.direct_offsets_rad) + return replace( + thumb_result, + direct_offsets_rad=direct, + all_active_offsets_rad=all_active, + ) + + +def _angles_from_state( + state_u8: Sequence[float], + *, + curves: Mapping[str, JointCurveFit], + motor_by_joint: Mapping[str, int], + inherited_zero_joints: Mapping[str, str] = INHERITED_ZERO_JOINTS, +) -> dict[str, float]: + state = np.asarray(state_u8, dtype=float) + result: dict[str, float] = {} + for joint, motor in motor_by_joint.items(): + source = inherited_zero_joints.get(joint, joint) + if source not in curves: + continue + command = int(np.clip(np.rint(state[int(motor)]), 0, 255)) + result[joint] = float(curves[source].angle_rad[command]) + return result + + +def solve_urdf_zero_offsets( + *, + source_urdf: str | Path, + measurements: Sequence[JointAxisMeasurement], + palm_orientation_measurements: Sequence[ + PalmOrientationMeasurement + ] = (), + curves: Mapping[str, JointCurveFit], + motor_by_joint: Mapping[str, int], + training_cycles: Sequence[int] = (0, 1), + validation_cycle: int = 2, + maximum_offset_rad: float = math.radians(20.0), + finger_maximum_offset_rad: float = math.radians(3.0), + joint_maximum_offset_rad: Mapping[str, float] | None = None, + maximum_cycle_difference_rad: float = math.radians(0.75), + minimum_applied_offset_rad: float = math.radians(0.3), + significance_sigma: float = 3.0, + maximum_validation_mae_rad: float = math.radians(1.0), + maximum_validation_p95_rad: float = math.radians(2.0), + maximum_validation_error_rad: float | None = None, + maximum_confidence_half_width_rad: float | None = None, + maximum_axis_cone_mismatch_rad: float = math.radians(5.0), + maximum_systematic_axis_cone_bias_rad: float | None = None, + maximum_pose_axis_line_rms_m: float = 0.001, + maximum_observability_condition_number: float = 1.0e10, + hand_type: str = "left", + tag_layout: str = "legacy_11", + fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, + static_output_zero_offsets_rad: Mapping[str, float] | None = None, + zero_profile: ZeroCalibrationProfile | None = None, +) -> ZeroSolveResult: + """Solve all directly observable zero offsets without fitting CAD errors. + + The product palm pose uses the robust common root-axis direction together + with independent partial-sweep MCP-pitch directions; root-line points + determine translation only. Legacy layouts retain their single observed + reference chain. Encoder zeros are then fitted from one angular residual + per joint. This prevents link length, along-axis Tag placement and + monocular depth bias from becoming encoder-zero corrections. + """ + profile = ( + get_zero_calibration_profile(hand_type, tag_layout) + if zero_profile is None + else zero_profile + ) + if profile.hand.side != str(hand_type).lower(): + raise ValueError("zero profile hand side does not match hand_type") + if profile.hand.layout_id != str(tag_layout): + raise ValueError("zero profile layout does not match tag_layout") + model = UrdfKinematicModel(source_urdf) + training = [m for m in measurements if m.cycle in set(training_cycles)] + validation = [m for m in measurements if m.cycle == int(validation_cycle)] + expected = set(profile.axis_joints) + if ( + {m.joint for m in training} != expected + or {m.joint for m in validation} != expected + ): + raise ValueError("axis measurements do not contain all required joints/cycles") + configured_palm_sources = { + str(name): str(model_joint) + for name, model_joint in ( + profile.hand.palm_orientation_sources or {} + ).items() + } + orientation_measurements = tuple(palm_orientation_measurements) + if configured_palm_sources: + minimum_orientation_sources = int( + profile.hand.minimum_palm_orientation_sources + ) + unknown_orientation_sources = sorted( + { + item.source_joint + for item in orientation_measurements + if item.source_joint not in configured_palm_sources + or configured_palm_sources[item.source_joint] + != item.model_joint + } + ) + if unknown_orientation_sources: + raise ValueError( + "palm orientation measurements do not match the product " + "profile: " + + ",".join(unknown_orientation_sources) + ) + required_orientation_cycles = { + *(int(value) for value in training_cycles), + int(validation_cycle), + } + for cycle in required_orientation_cycles: + cycle_sources = { + item.source_joint + for item in orientation_measurements + if int(item.cycle) == cycle + } + if len(cycle_sources) < minimum_orientation_sources: + raise ValueError( + f"palm orientation cycle {cycle + 1} contains " + f"{len(cycle_sources)}/{minimum_orientation_sources} " + "qualified sources" + ) + elif orientation_measurements: + raise ValueError( + "palm orientation measurements were supplied without a profile" + ) + orientation_training = tuple( + item + for item in orientation_measurements + if int(item.cycle) in set(int(value) for value in training_cycles) + ) + orientation_validation = tuple( + item + for item in orientation_measurements + if int(item.cycle) == int(validation_cycle) + ) + orientation_by_model_cycle: dict[ + tuple[str, int], PalmOrientationMeasurement + ] = {} + for item in orientation_measurements: + key = (str(item.model_joint), int(item.cycle)) + if key in orientation_by_model_cycle: + raise ValueError( + "palm orientation has multiple sources for " + f"{key[0]} cycle {key[1] + 1}" + ) + orientation_by_model_cycle[key] = item + if not 0.0 < finger_maximum_offset_rad <= maximum_offset_rad: + raise ValueError("finger maximum offset must be positive and no larger than thumb") + joint_limits = { + str(name): float(value) + for name, value in dict(joint_maximum_offset_rad or {}).items() + } + unknown_joint_limits = sorted(set(joint_limits) - set(profile.direct_zero_joints)) + if unknown_joint_limits: + raise ValueError( + "joint maximum offsets contain unknown direct joints: " + + ",".join(unknown_joint_limits) + ) + if any( + not math.isfinite(value) or not 0.0 < value <= math.radians(90.0) + for value in joint_limits.values() + ): + raise ValueError("joint maximum offsets must be finite and in (0, 90deg]") + if maximum_cycle_difference_rad <= 0.0: + raise ValueError("maximum cycle difference must be positive") + if minimum_applied_offset_rad < 0.0 or significance_sigma < 0.0: + raise ValueError("zero significance thresholds must be non-negative") + if maximum_axis_cone_mismatch_rad <= 0.0: + raise ValueError("maximum axis cone mismatch must be positive") + if ( + maximum_systematic_axis_cone_bias_rad is not None + and ( + maximum_systematic_axis_cone_bias_rad + <= maximum_axis_cone_mismatch_rad + or maximum_systematic_axis_cone_bias_rad > math.radians(90.0) + ) + ): + raise ValueError( + "maximum systematic axis cone bias must exceed the precision " + "limit and be at most 90deg" + ) + if maximum_pose_axis_line_rms_m <= 0.0: + raise ValueError("maximum pose axis-line RMS must be positive") + if maximum_observability_condition_number <= 1.0: + raise ValueError( + "maximum observability condition number must be greater than one" + ) + if ( + maximum_validation_error_rad is not None + and maximum_validation_error_rad <= 0.0 + ): + raise ValueError("maximum validation error must be positive") + if ( + maximum_confidence_half_width_rad is not None + and maximum_confidence_half_width_rad <= 0.0 + ): + raise ValueError("maximum confidence half width must be positive") + + def predicted_local( + measurement: JointAxisMeasurement, + offsets: Mapping[str, float], + ) -> tuple[np.ndarray, np.ndarray]: + angles = _angles_from_state( + ( + measurement.condition_state_u8 + if measurement.condition_command_u8 is None + else measurement.condition_command_u8 + ), + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + return model.axis_line( + measurement.joint, + zero_offsets=offsets, + joint_angles=angles, + ) + + def predicted_palm_orientation_local( + measurement: PalmOrientationMeasurement, + offsets: Mapping[str, float], + ) -> np.ndarray: + angles = _angles_from_state( + measurement.condition_state_u8, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + axis, _ = model.axis_line( + measurement.model_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + return axis / np.linalg.norm(axis) + + fixed_offsets = { + str(name): float(value) + for name, value in ( + profile.fixed_direct_zero_offsets_rad + if fixed_direct_zero_offsets_rad is None + else fixed_direct_zero_offsets_rad + ).items() + } + unknown_fixed_offsets = sorted( + set(fixed_offsets) - set(profile.direct_zero_joints) + ) + if unknown_fixed_offsets: + raise ValueError( + "fixed direct zero offsets contain unknown joints: " + + ",".join(unknown_fixed_offsets) + ) + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in fixed_offsets.values() + ): + raise ValueError( + "fixed direct zero offsets must be finite and within +/-90deg" + ) + output_offsets = { + str(name): float(value) + for name, value in ( + profile.static_output_zero_offsets_rad + if static_output_zero_offsets_rad is None + else static_output_zero_offsets_rad + ).items() + } + unknown_output_offsets = sorted( + set(output_offsets) - set(profile.direct_zero_joints) + ) + if unknown_output_offsets: + raise ValueError( + "static output zero offsets contain unknown joints: " + + ",".join(unknown_output_offsets) + ) + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in output_offsets.values() + ): + raise ValueError( + "static output zero offsets must be finite and within +/-90deg" + ) + zero_offsets = {name: 0.0 for name in profile.direct_zero_joints} + zero_offsets.update(fixed_offsets) + def fit_base_pose( + selected: Sequence[JointAxisMeasurement], + pose_offsets: Mapping[str, float] | None = None, + ) -> tuple[Rotation, np.ndarray]: + pose_zero_offsets = zero_offsets if pose_offsets is None else pose_offsets + anchors_by_joint = { + name: [item for item in selected if item.joint == name] + for name in profile.root_anchor_joints + } + if any(not items for items in anchors_by_joint.values()): + raise ValueError("zero solve is missing root axis anchors") + + def undirected_axis_average( + values: Sequence[Sequence[float]], + ) -> np.ndarray: + vectors = np.asarray(values, dtype=float) + reference = vectors[0] / np.linalg.norm(vectors[0]) + signs = np.where(vectors @ reference < 0.0, -1.0, 1.0) + result = np.sum(vectors * signs[:, None], axis=0) + norm = float(np.linalg.norm(result)) + if norm < 1.0e-9: + raise ValueError("root axis average is degenerate") + return result / norm + + root_names = tuple(sorted(profile.root_anchor_joints)) + predicted_axes: dict[str, np.ndarray] = {} + predicted_points: dict[str, np.ndarray] = {} + observed_axes: dict[str, np.ndarray] = {} + observed_points: dict[str, np.ndarray] = {} + for name in root_names: + items = anchors_by_joint[name] + predicted = [predicted_local(item, pose_zero_offsets) for item in items] + predicted_axes[name] = undirected_axis_average( + [axis for axis, _ in predicted] + ) + predicted_points[name] = np.median( + np.asarray([point for _, point in predicted]), axis=0 + ) + observed_axes[name] = undirected_axis_average( + [item.axis_common_xyz for item in items] + ) + observed_points[name] = np.median( + np.asarray( + [item.point_common_xyz_m for item in items], dtype=float + ), + axis=0, + ) + + if ( + profile.hand.layout_id == G20_RIGHT_19_LAYOUT + and profile.base_pose_strategy == "full_hand" + ): + # In the product layout none of the four finger-roll zeros is a + # mechanical prior. Using one finger's pitch axis to orient the + # palm would therefore absorb that finger's roll zero into the + # palm pose. The named positions of five parallel root lines do + # observe rotation about their common direction, independent of + # all five root-joint zeros. Fit that transverse line pattern and + # leave the unobservable translation along the common direction + # at zero; all zero-sensitive phase residuals use line-to-line + # differences and are invariant to that gauge. + predicted_common = undirected_axis_average( + [predicted_axes[name] for name in root_names] + ) + observed_common = undirected_axis_average( + [observed_axes[name] for name in root_names] + ) + predicted_center = np.mean( + np.asarray([predicted_points[name] for name in root_names]), + axis=0, + ) + observed_center = np.mean( + np.asarray([observed_points[name] for name in root_names]), + axis=0, + ) + predicted_pattern = { + name: ( + (delta := predicted_points[name] - predicted_center) + - predicted_common * float(delta @ predicted_common) + ) + for name in root_names + } + observed_pattern = { + name: ( + (delta := observed_points[name] - observed_center) + - observed_common * float(delta @ observed_common) + ) + for name in root_names + } + if max( + np.linalg.norm(value) for value in predicted_pattern.values() + ) < 0.003: + raise ValueError("root axis-line pattern is degenerate") + if max( + np.linalg.norm(value) for value in observed_pattern.values() + ) < 0.003: + raise ValueError("observed root axis-line pattern is degenerate") + + def align_axis(source: np.ndarray, target: np.ndarray) -> Rotation: + source = source / np.linalg.norm(source) + target = target / np.linalg.norm(target) + cross = np.cross(source, target) + cross_norm = float(np.linalg.norm(cross)) + dot = float(np.clip(source @ target, -1.0, 1.0)) + if cross_norm > 1.0e-10: + return Rotation.from_rotvec( + cross / cross_norm * math.atan2(cross_norm, dot) + ) + if dot > 0.0: + return Rotation.identity() + basis = np.eye(3)[int(np.argmin(np.abs(source)))] + axis = np.cross(source, basis) + axis /= np.linalg.norm(axis) + return Rotation.from_rotvec(axis * math.pi) + + candidates: list[tuple[float, Rotation, np.ndarray]] = [] + for sign in (1.0, -1.0): + signed_observed_axis = sign * observed_common + axis_rotation = align_axis( + predicted_common, signed_observed_axis + ) + mapped_pattern = { + name: axis_rotation.apply(predicted_pattern[name]) + for name in root_names + } + cosine = sum( + float(mapped_pattern[name] @ observed_pattern[name]) + for name in root_names + ) + sine = sum( + float( + signed_observed_axis + @ np.cross( + mapped_pattern[name], observed_pattern[name] + ) + ) + for name in root_names + ) + phase_rotation = Rotation.from_rotvec( + signed_observed_axis * math.atan2(sine, cosine) + ) + rotation = phase_rotation * axis_rotation + translation_samples = [] + for name in root_names: + delta = observed_points[name] - rotation.apply( + predicted_points[name] + ) + translation_samples.append( + delta + - signed_observed_axis + * float(delta @ signed_observed_axis) + ) + translation = np.median( + np.asarray(translation_samples), axis=0 + ) + + # Root-axis points recovered from planar Tags occasionally + # contain a large but repeatable depth/line-position outlier. + # A plain Procrustes sum lets one such point rotate the palm + # frame and then makes the same bias look like a common zero + # offset on all four finger-roll joints. Refine only the + # rigid palm pose with a millimetre-scale robust loss. At + # least three mutually consistent named root lines therefore + # determine the transverse pattern while an outlier remains + # visible in the line diagnostics below. + transverse_helper = np.eye(3)[ + int(np.argmin(np.abs(signed_observed_axis))) + ] + transverse_first = np.cross( + signed_observed_axis, transverse_helper + ) + transverse_first /= np.linalg.norm(transverse_first) + transverse_second = np.cross( + signed_observed_axis, transverse_first + ) + + def robust_root_pattern_residual( + value: np.ndarray, + ) -> np.ndarray: + candidate_rotation = ( + Rotation.from_rotvec( + signed_observed_axis * float(value[0]) + ) + * rotation + ) + candidate_translation = ( + translation + + transverse_first * float(value[1]) + + transverse_second * float(value[2]) + ) + residuals: list[float] = [] + for name in root_names: + delta = observed_points[name] - ( + candidate_rotation.apply(predicted_points[name]) + + candidate_translation + ) + residuals.extend( + ( + float(delta @ transverse_first) / 0.001, + float(delta @ transverse_second) / 0.001, + ) + ) + return np.asarray(residuals, dtype=float) + + root_pattern_solution = least_squares( + robust_root_pattern_residual, + np.zeros(3, dtype=float), + bounds=( + np.asarray([-math.pi, -0.25, -0.25]), + np.asarray([math.pi, 0.25, 0.25]), + ), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + if not root_pattern_solution.success: + raise ValueError("robust root axis-line pattern fit failed") + rotation = ( + Rotation.from_rotvec( + signed_observed_axis + * float(root_pattern_solution.x[0]) + ) + * rotation + ) + translation = ( + translation + + transverse_first * float(root_pattern_solution.x[1]) + + transverse_second * float(root_pattern_solution.x[2]) + ) + axis_errors: list[float] = [] + for item in selected: + predicted_axis, _ = predicted_local(item, pose_zero_offsets) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + axis_errors.append( + math.acos( + abs( + float( + np.clip( + rotation.apply(predicted_axis) + @ observed_axis, + -1.0, + 1.0, + ) + ) + ) + ) + ) + line_errors = [] + for name in root_names: + predicted_point = ( + rotation.apply(predicted_points[name]) + translation + ) + delta = observed_points[name] - predicted_point + line_errors.append( + float( + np.linalg.norm( + delta + - signed_observed_axis + * float(delta @ signed_observed_axis) + ) + ) + ) + score = float(np.mean(np.square(axis_errors))) + float( + np.mean(np.square(np.asarray(line_errors) / 0.01)) + ) + candidates.append((score, rotation, translation)) + _, rotation, translation = min( + candidates, key=lambda item: item[0] + ) + non_root_items = [ + item + for item in selected + if item.joint not in profile.root_anchor_joints + ] + if not non_root_items: + raise ValueError( + "non-parallel axes are required for palm axial translation" + ) + + def axial_translation_residual(value: np.ndarray) -> np.ndarray: + candidate_translation = ( + translation + observed_common * float(value[0]) + ) + residuals: list[float] = [] + for item in non_root_items: + predicted_axis, predicted_point = predicted_local( + item, pose_zero_offsets + ) + predicted_axis = rotation.apply(predicted_axis) + predicted_point = ( + rotation.apply(predicted_point) + + candidate_translation + ) + observed_axis = np.asarray( + item.axis_common_xyz, dtype=float + ) + if float(predicted_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + delta = np.asarray( + item.point_common_xyz_m, dtype=float + ) - predicted_point + perpendicular = delta - observed_axis * float( + delta @ observed_axis + ) + residuals.extend( + float(component) / 0.001 + for component in perpendicular + ) + return np.asarray(residuals, dtype=float) + + axial_solution = least_squares( + axial_translation_residual, + np.asarray([0.0]), + bounds=(np.asarray([-1.0]), np.asarray([1.0])), + loss="soft_l1", + f_scale=1.0, + ) + if not axial_solution.success: + raise ValueError("palm axial translation fit failed") + translation = ( + translation + + observed_common * float(axial_solution.x[0]) + ) + return rotation, translation + + # Legacy layouts have only one observed finger chain. Their + # non-parallel reference-finger pitch axis remains the orientation + # anchor; the product profile above deliberately does not use it. + primary = max( + root_names, + key=lambda name: float( + np.ptp(np.asarray(curves[name].angle_rad)) + ), + ) + orientation_anchor = ( + "thumb_cmc_pitch" + if profile.base_pose_strategy == "thumb_serial" + else f"{profile.reference_finger}_mcp_pitch" + ) + orientation_items = [ + item for item in selected if item.joint == orientation_anchor + ] + if not orientation_items: + raise ValueError( + f"zero solve is missing palm orientation anchor " + f"{orientation_anchor}" + ) + predicted_orientation_axis = undirected_axis_average( + [ + predicted_local(item, pose_zero_offsets)[0] + for item in orientation_items + ] + ) + observed_orientation_axis = undirected_axis_average( + [item.axis_common_xyz for item in orientation_items] + ) + + def frame_from_two_axes( + primary_axis: np.ndarray, orientation_axis: np.ndarray + ) -> np.ndarray: + first = primary_axis / np.linalg.norm(primary_axis) + second = orientation_axis - first * float(orientation_axis @ first) + second_norm = float(np.linalg.norm(second)) + if second_norm < math.sin(ZERO_MINIMUM_AXIS_CONE_RAD): + raise ValueError("palm orientation axes are nearly parallel") + second /= second_norm + return np.column_stack((first, second, np.cross(first, second))) + + predicted_frame = frame_from_two_axes( + predicted_axes[primary], + predicted_orientation_axis, + ) + candidates: list[tuple[float, Rotation, np.ndarray]] = [] + for primary_sign in (1.0, -1.0): + for orientation_sign in (1.0, -1.0): + observed_frame = frame_from_two_axes( + primary_sign * observed_axes[primary], + orientation_sign * observed_orientation_axis, + ) + rotation = Rotation.from_matrix( + observed_frame @ predicted_frame.T + ) + translation = np.median( + np.asarray( + [ + observed_points[name] + - rotation.apply(predicted_points[name]) + for name in root_names + ] + ), + axis=0, + ) + + # Resolve both undirected-axis sign branches using every + # distinct measured joint. This selects a palm-frame branch; + # it does not fit link geometry or encoder offsets. + errors_by_joint: dict[str, list[float]] = {} + for item in selected: + predicted_axis, _ = predicted_local(item, pose_zero_offsets) + predicted_axis = rotation.apply(predicted_axis) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + error = math.acos( + abs( + float( + np.clip( + predicted_axis @ observed_axis, -1.0, 1.0 + ) + ) + ) + ) + errors_by_joint.setdefault(item.joint, []).append(error) + branch_score = sum( + min(float(np.median(values)), math.radians(30.0)) ** 2 + for values in errors_by_joint.values() + ) + candidates.append((branch_score, rotation, translation)) + _, rotation, translation = min(candidates, key=lambda item: item[0]) + return rotation, translation + + def phase_error( + predicted_parent_axis: np.ndarray, + predicted_parent_point: np.ndarray, + predicted_child_point: np.ndarray, + observed_parent_axis: np.ndarray, + observed_parent_point: np.ndarray, + observed_child_point: np.ndarray, + view_normal_common: Sequence[float] | None = None, + ) -> float: + parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis) + observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis) + if float(parent_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + predicted_delta = predicted_child_point - predicted_parent_point + observed_delta = observed_child_point - observed_parent_point + predicted_radial = predicted_delta - parent_axis * float( + predicted_delta @ parent_axis + ) + observed_radial = observed_delta - observed_axis * float( + observed_delta @ observed_axis + ) + angle_axis = parent_axis + if view_normal_common is not None: + view_normal = np.asarray(view_normal_common, dtype=float) + normal_norm = float(np.linalg.norm(view_normal)) + if normal_norm > 1.0e-9: + view_normal /= normal_norm + # When looking approximately along the rotation axis, image + # x/y contains the complete radial phase while optical depth + # is both unnecessary and much noisier for a 16 mm planar + # Tag. For an edge-on axis the depth component is genuinely + # needed for observability, so retain the full 3-D residual. + if abs(float(parent_axis @ view_normal)) >= math.cos( + math.radians(45.0) + ): + # Orthographic image-plane displacement is invariant to + # independent optical-depth bias on the two planar Tags. + # With an end-on axis, along-axis placement also projects + # to zero (or a small component for a mildly oblique view). + predicted_radial = predicted_delta - view_normal * float( + predicted_delta @ view_normal + ) + observed_radial = observed_delta - view_normal * float( + observed_delta @ view_normal + ) + angle_axis = view_normal + if float(angle_axis @ parent_axis) < 0.0: + angle_axis = -angle_axis + predicted_radius = float(np.linalg.norm(predicted_radial)) + observed_radius = float(np.linalg.norm(observed_radial)) + if min(predicted_radius, observed_radius) < 0.003: + raise ValueError("parallel-axis radial phase is not observable") + predicted_radial /= predicted_radius + observed_radial /= observed_radius + return math.atan2( + float(angle_axis @ np.cross(predicted_radial, observed_radial)), + float(np.clip(predicted_radial @ observed_radial, -1.0, 1.0)), + ) + + def robust_circular_location(values: Sequence[float]) -> float: + array = np.asarray(values, dtype=float) + if array.size == 0: + raise ValueError("circular residual set is empty") + centre = math.atan2( + float(np.mean(np.sin(array))), + float(np.mean(np.cos(array))), + ) + centred = np.arctan2(np.sin(array - centre), np.cos(array - centre)) + return float( + math.atan2( + math.sin(centre + float(np.median(centred))), + math.cos(centre + float(np.median(centred))), + ) + ) + + def same_view_axis_pair_errors( + offset_joint: str, + selected: Sequence[JointAxisMeasurement], + offsets: Mapping[str, float], + ) -> tuple[float, ...]: + """Return the camera-frame-invariant residual for a serial-axis pair. + + This observation intentionally has two mirror roots. The optimiser + disambiguates them with the original cross-view estimate, but the + pair residual alone determines the final numerical zero. + """ + pair = profile.same_view_axis_pair_by_offset.get(offset_joint) + if pair is None: + return () + anchor_joint, observer_joint = pair + + def angle(left: np.ndarray, right: np.ndarray) -> float: + cosine = float(left @ right) / float( + np.linalg.norm(left) * np.linalg.norm(right) + ) + return math.acos(abs(float(np.clip(cosine, -1.0, 1.0)))) + + errors: list[float] = [] + for cycle in sorted({int(item.cycle) for item in selected}): + anchor = orientation_by_model_cycle.get((anchor_joint, cycle)) + observer = orientation_by_model_cycle.get( + (observer_joint, cycle) + ) + if anchor is None or observer is None: + return () + angles = _angles_from_state( + observer.condition_state_u8, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + predicted_anchor, _ = model.axis_line( + anchor_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + predicted_observer, _ = model.axis_line( + observer_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + observed_anchor = np.asarray( + anchor.axis_common_xyz, dtype=float + ) + observed_observer = np.asarray( + observer.axis_common_xyz, dtype=float + ) + errors.append( + angle(predicted_anchor, predicted_observer) + - angle(observed_anchor, observed_observer) + ) + return tuple(errors) + + def angular_error_samples( + offsets: Mapping[str, float], + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + ) -> dict[str, tuple[float, ...]]: + by_key = {(item.joint, item.cycle): item for item in selected} + errors: dict[str, list[float]] = { + name: [] for name in profile.direct_zero_joints + } + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + pair_errors = same_view_axis_pair_errors( + offset_joint, selected, offsets + ) + if pair_errors: + errors[offset_joint].extend(pair_errors) + continue + observer_items = [ + item for item in selected if item.joint == observer_joint + ] + if not observer_items: + raise ValueError(f"zero observer is missing: {observer_joint}") + for item in observer_items: + observer_axis, observer_point = predicted_local(item, offsets) + observer_axis = base_rotation.apply(observer_axis) + observer_point = ( + base_rotation.apply(observer_point) + base_translation + ) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + if observer_joint in profile.axis_parent_joint: + parent_joint = profile.axis_parent_joint[observer_joint] + angles = _angles_from_state( + ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_u8 + ), + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + parent_axis, _ = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis = base_rotation.apply(parent_axis) + error = _zero_sensitive_axis_error_rad( + observer_axis, observed_axis, parent_axis + ) + else: + parent_joint = profile.phase_parent_joint[observer_joint] + observed_parent = by_key.get((parent_joint, item.cycle)) + if observed_parent is None: + raise ValueError( + f"phase parent is missing: {parent_joint} cycle {item.cycle}" + ) + angles = _angles_from_state( + ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_u8 + ), + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=profile.inherited_zero_joints, + ) + parent_axis, parent_point = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis = base_rotation.apply(parent_axis) + parent_point = ( + base_rotation.apply(parent_point) + base_translation + ) + error = phase_error( + parent_axis, + parent_point, + observer_point, + np.asarray(observed_parent.axis_common_xyz, dtype=float), + np.asarray(observed_parent.point_common_xyz_m, dtype=float), + np.asarray(item.point_common_xyz_m, dtype=float), + item.view_normal_common_xyz, + ) + errors[offset_joint].append(float(error)) + return { + name: tuple(values) + for name, values in errors.items() + if values + } + + def angular_errors( + offsets: Mapping[str, float], + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + ) -> dict[str, float]: + return { + name: robust_circular_location(values) + for name, values in angular_error_samples( + offsets, selected, base_rotation, base_translation + ).items() + } + + product_finger_rolls = tuple( + name + for name in profile.direct_zero_joints + if name.endswith("_mcp_roll") and not name.startswith("thumb_") + ) + offset_limits = np.asarray( + [ + joint_limits.get( + name, + ( + finger_maximum_offset_rad + if name.startswith(("index_", "middle_", "ring_", "pinky_")) + else maximum_offset_rad + ), + ) + for name in profile.direct_zero_joints + ], + dtype=float, + ) + # Do not make a configured safety limit the numerical optimizer's bound. + # Otherwise a genuine out-of-range estimate and a modelling failure both + # collapse to exactly +/-20 or +/-3 degrees, which hides the magnitude and + # encourages pointless rescans. Search farther for diagnostics, then keep + # the original configured limits as unchanged pass/fail gates below. + diagnostic_offset_limits = np.minimum( + math.radians(90.0), + np.maximum(3.0 * offset_limits, offset_limits + math.radians(5.0)), + ) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: + # The absolute rotation of the fitted palm frame about the four + # parallel roll axes is a shared gauge. Each physical finger zero is + # only its deviation from the four-finger median, but the raw scalar + # solves include that common mode. Give those four diagnostic solves + # enough range for the unchanged global common-mode limit plus the + # unchanged per-finger deviation limit; applying the per-finger bound + # before removing the gauge clips every solve to the same value and + # destroys the observable deviations. + for index, name in enumerate(profile.direct_zero_joints): + if name in product_finger_rolls: + diagnostic_offset_limits[index] = min( + math.radians(90.0), + maximum_offset_rad + offset_limits[index], + ) + + def optimise_offsets( + selected: Sequence[JointAxisMeasurement], + base_rotation: Rotation, + base_translation: np.ndarray, + initial: Mapping[str, float] | None = None, + ) -> dict[str, float]: + # The observer graph is a pair of serial chains. Solve it in that + # dependency order so a bad distal phase cannot pull an already + # observable proximal zero toward a bound. Each scalar residual keeps + # the individual cycle values; robust loss then downweights one noisy + # cycle instead of hiding it in a seven-variable compromise. + result = {name: 0.0 for name in profile.direct_zero_joints} + if initial is not None: + result.update( + { + name: float(initial[name]) + for name in profile.direct_zero_joints + } + ) + result.update(fixed_offsets) + for name, diagnostic_limit, configured_limit in zip( + profile.direct_zero_joints, + diagnostic_offset_limits, + offset_limits, + ): + if name in fixed_offsets: + continue + has_axis_pair = name in profile.same_view_axis_pair_by_offset + limit = float( + configured_limit if has_axis_pair else diagnostic_limit + ) + + def residual(value: np.ndarray) -> np.ndarray: + candidate = dict(result) + candidate[name] = float(value[0]) + samples = angular_error_samples( + candidate, selected, base_rotation, base_translation + )[name] + return np.asarray( + [ + math.atan2(math.sin(item), math.cos(item)) + / math.radians(1.0) + for item in samples + ], + dtype=float, + ) + + starts = [0.0, -0.5 * limit, 0.5 * limit] + if initial is not None: + starts.append(float(initial[name])) + solutions = [ + least_squares( + residual, + np.asarray( + [np.clip(start, -limit + 1.0e-9, limit - 1.0e-9)] + ), + bounds=(np.asarray([-limit]), np.asarray([limit])), + loss="soft_l1", + f_scale=1.0, + max_nfev=1000, + ) + for start in starts + ] + solution = min( + solutions, key=lambda item: float(np.sum(np.square(item.fun))) + ) + if not solution.success: + raise ValueError( + f"URDF zero optimization failed for {name}: " + f"{solution.message}" + ) + result[name] = float(solution.x[0]) + return result + + def solve_selected( + selected: Sequence[JointAxisMeasurement], + initial: Mapping[str, float] | None = None, + ) -> tuple[Rotation, np.ndarray, dict[str, float]]: + rotation, translation = fit_base_pose(selected) + offsets = optimise_offsets( + selected, rotation, translation, initial=initial + ) + if profile.base_pose_strategy == "thumb_serial": + # The first pose estimate only supplies a branch for the invariant + # same-view yaw solve. Refit after yaw is known so that the + # downstream pitch phase is evaluated in the corrected serial + # thumb frame, then freeze that pose for holdout validation. + rotation, translation = fit_base_pose(selected, offsets) + offsets = optimise_offsets( + selected, rotation, translation, initial=offsets + ) + return rotation, translation, offsets + + base_rotation, base_translation, training_offsets = solve_selected(training) + + def observability_residual(parameters: np.ndarray) -> np.ndarray: + rotation = Rotation.from_rotvec(parameters[:3]) + translation = parameters[3:6] * 0.05 + offsets = { + name: float(value) + for name, value in zip( + profile.direct_zero_joints, parameters[6:] + ) + } + residuals: list[float] = [] + for item in training: + predicted_axis, predicted_point = predicted_local(item, offsets) + predicted_axis = rotation.apply(predicted_axis) + predicted_axis /= np.linalg.norm(predicted_axis) + predicted_point = rotation.apply(predicted_point) + translation + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + observed_axis /= np.linalg.norm(observed_axis) + if float(predicted_axis @ observed_axis) < 0.0: + observed_axis = -observed_axis + observed_point = np.asarray(item.point_common_xyz_m, dtype=float) + predicted_moment = np.cross(predicted_point, predicted_axis) + observed_moment = np.cross(observed_point, observed_axis) + residuals.extend(float(value) for value in predicted_axis - observed_axis) + residuals.extend( + float(value) / 0.05 + for value in predicted_moment - observed_moment + ) + return np.asarray(residuals, dtype=float) + + observability_parameters = np.asarray( + [ + *base_rotation.as_rotvec(), + *(base_translation / 0.05), + *( + training_offsets[name] + for name in profile.direct_zero_joints + ), + ], + dtype=float, + ) + observability_base_residual = observability_residual( + observability_parameters + ) + observability_jacobian = np.empty( + ( + observability_base_residual.size, + observability_parameters.size, + ), + dtype=float, + ) + finite_difference_step = 1.0e-6 + for column in range(observability_parameters.size): + positive = observability_parameters.copy() + negative = observability_parameters.copy() + positive[column] += finite_difference_step + negative[column] -= finite_difference_step + observability_jacobian[:, column] = ( + observability_residual(positive) + - observability_residual(negative) + ) / (2.0 * finite_difference_step) + singular_values = np.linalg.svd( + observability_jacobian, compute_uv=False + ) + singular_threshold = ( + 0.0 + if singular_values.size == 0 + else float(singular_values[0]) * 1.0e-7 + ) + observability_rank = int( + np.count_nonzero(singular_values > singular_threshold) + ) + observability_parameter_count = int(observability_parameters.size) + observability_condition_number = ( + float("inf") + if observability_rank < observability_parameter_count + else float(singular_values[0] / singular_values[-1]) + ) + residual_dof = max( + 1, + observability_base_residual.size - observability_parameter_count, + ) + residual_variance = float( + observability_base_residual @ observability_base_residual + ) / residual_dof + covariance = residual_variance * np.linalg.pinv( + observability_jacobian.T @ observability_jacobian, + rcond=1.0e-12, + ) + offset_covariance = { + name: max(0.0, float(covariance[6 + index, 6 + index])) + for index, name in enumerate(profile.direct_zero_joints) + } + + axis_cone_mismatch_by_joint: dict[str, float] = {} + axis_cone_bias_classification_by_joint: dict[str, str] = {} + + def zero_observation_failure_reasons( + selected: Sequence[JointAxisMeasurement], + offsets: Mapping[str, float], + rotation: Rotation, + ) -> dict[str, str]: + """Reject repeatable but geometrically inadmissible zero observers.""" + by_key = {(item.joint, item.cycle): item for item in selected} + failures: dict[str, str] = {} + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + if offset_joint in fixed_offsets: + continue + observer_items = [ + item for item in selected if item.joint == observer_joint + ] + if observer_joint in profile.axis_parent_joint: + parent_joint = profile.axis_parent_joint[observer_joint] + cone_mismatches: list[float] = [] + for item in observer_items: + state = ( + item.condition_state_u8 + if item.condition_command_u8 is None + else item.condition_command_u8 + ) + angles = _angles_from_state( + state, + curves=curves, + motor_by_joint=motor_by_joint, + inherited_zero_joints=( + profile.inherited_zero_joints + ), + ) + predicted_axis, _ = model.axis_line( + observer_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + parent_axis, _ = model.axis_line( + parent_joint, + zero_offsets=offsets, + joint_angles=angles, + ) + cone_mismatches.append( + _axis_cone_mismatch_rad( + rotation.apply(predicted_axis), + item.axis_common_xyz, + rotation.apply(parent_axis), + ) + ) + maximum_cone_mismatch = max(cone_mismatches) + axis_cone_mismatch_by_joint[offset_joint] = ( + maximum_cone_mismatch + ) + if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad: + cone_range = float(np.ptp(cone_mismatches)) + stable_product_bias = bool( + profile.hand.stable_cross_view_cone_bias + and maximum_systematic_axis_cone_bias_rad is not None + and len(cone_mismatches) >= 4 + and maximum_cone_mismatch + <= maximum_systematic_axis_cone_bias_rad + and cone_range <= maximum_cycle_difference_rad + ) + if stable_product_bias: + # A parent zero rotates the downstream direction + # around the parent axis and cannot change their cone + # angle. The zero-sensitive residual above already + # projects both directions onto the parent-normal + # plane, so a repeatable cross-camera/planar-PnP cone + # bias cannot corrupt the written encoder zero. Keep + # it visible in the result while retaining the gross + # gate for a wrong axis, loose Tag, or moved camera. + axis_cone_bias_classification_by_joint[offset_joint] = ( + "stable_cross_view_or_planar_pnp_bias" + ) + else: + failures[offset_joint] = ( + "zero_axis_cone_mismatch_too_large" + ) + else: + parent_joint = profile.phase_parent_joint[observer_joint] + phase_items: list[JointAxisMeasurement] = [] + propagated_angle_uncertainties: list[float] = [] + for item in observer_items: + phase_items.append(item) + parent_item = by_key.get((parent_joint, item.cycle)) + if parent_item is not None: + phase_items.append(parent_item) + parent_axis = np.asarray( + parent_item.axis_common_xyz, dtype=float + ) + parent_axis /= np.linalg.norm(parent_axis) + separation = ( + np.asarray(item.point_common_xyz_m, dtype=float) + - np.asarray( + parent_item.point_common_xyz_m, dtype=float + ) + ) + radial = separation - parent_axis * float( + separation @ parent_axis + ) + effective_distance = float(np.linalg.norm(radial)) + if effective_distance <= 1.0e-6: + propagated_angle_uncertainties.append(float("inf")) + else: + line_uncertainty = math.hypot( + item.pose_axis_line_rms_m, + parent_item.pose_axis_line_rms_m, + ) + propagated_angle_uncertainties.append( + math.atan2( + line_uncertainty, effective_distance + ) + ) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: + angle_limit = ( + maximum_confidence_half_width_rad + if maximum_confidence_half_width_rad is not None + else math.radians(0.5) + ) + if ( + not propagated_angle_uncertainties + or max(propagated_angle_uncertainties) > angle_limit + ): + failures[offset_joint] = ( + "zero_phase_axis_line_angle_uncertainty_too_large" + ) + elif ( + profile.hand.layout_id != G20_RIGHT_19_LAYOUT + and any( + item.pose_axis_line_rms_m + > maximum_pose_axis_line_rms_m + for item in phase_items + ) + ): + failures[offset_joint] = ( + "zero_phase_axis_line_residual_too_large" + ) + return failures + + observation_failures = zero_observation_failure_reasons( + list(measurements), training_offsets, base_rotation + ) + cycle_values: dict[str, list[float]] = { + name: [] for name in profile.direct_zero_joints + } + cycle_models: dict[int, tuple[Rotation, np.ndarray]] = {} + training_cycle_ids = tuple(sorted({int(value) for value in training_cycles})) + for cycle in training_cycle_ids: + selected = [item for item in measurements if item.cycle == cycle] + # Keep one palm pose while comparing cycles. Refitting a base pose from + # only two nearly parallel root axes per cycle makes harmless root-line + # noise appear as a large encoder-zero change. + cycle_rotation, cycle_translation = base_rotation, base_translation + cycle_offsets = optimise_offsets( + selected, + cycle_rotation, + cycle_translation, + initial=training_offsets, + ) + cycle_models[cycle] = (cycle_rotation, cycle_translation) + for name, value in cycle_offsets.items(): + cycle_values[name].append(value) + uncertainties: dict[str, float] = {} + confidence_half_widths: dict[str, float] = {} + cycle_consistent = True + inconsistent_cycles: list[str] = [] + for name, values in cycle_values.items(): + array = np.asarray(values, dtype=float) + spread = float(np.max(array) - np.min(array)) + if spread > maximum_cycle_difference_rad: + cycle_consistent = False + inconsistent_cycles.append(name) + uncertainties[name] = ( + 0.0 + if array.size < 2 + else float(np.std(array, ddof=1) / math.sqrt(array.size)) + ) + confidence_half_widths[name] = ( + float("inf") + if array.size < 2 + else float( + student_t.ppf(0.975, df=array.size - 1) + * uncertainties[name] + ) + ) + + insignificant_large: list[str] = [] + applied_training: dict[str, float] = {} + for name, value in training_offsets.items(): + if name in fixed_offsets: + applied_training[name] = fixed_offsets[name] + continue + uncertainty = uncertainties[name] + confidence_half_width = confidence_half_widths[name] + if ( + abs(value) < minimum_applied_offset_rad + or abs(value) + <= max(significance_sigma * uncertainty, confidence_half_width) + ): + applied_training[name] = 0.0 + if abs(value) >= minimum_applied_offset_rad: + insignificant_large.append(name) + else: + applied_training[name] = value + + candidate_errors = angular_errors( + applied_training, validation, base_rotation, base_translation + ) + original_errors = angular_errors( + zero_offsets, validation, base_rotation, base_translation + ) + validation_error_by_joint: dict[str, float] = {} + original_error_by_joint: dict[str, float] = {} + improvement_by_joint: dict[str, float] = {} + improvement_confidence_lower: dict[str, float] = {} + improvement_passed = True + if profile.same_view_axis_pair_by_offset: + # Validate the same camera/Tag-mount-invariant axis-pair angle used by + # the configured offset. This keeps the side channel out of every + # other joint, including the already stable thumb CMC roll solve. + palm_orientation_validation_errors = tuple( + abs(float(error)) + for offset_joint in profile.same_view_axis_pair_by_offset + for error in same_view_axis_pair_errors( + offset_joint, validation, applied_training + ) + ) + else: + palm_orientation_validation_errors = tuple( + math.acos( + abs( + float( + np.clip( + base_rotation.apply( + predicted_palm_orientation_local( + item, zero_offsets + ) + ) + @ np.asarray(item.axis_common_xyz, dtype=float), + -1.0, + 1.0, + ) + ) + ) + ) + for item in orientation_validation + ) + palm_orientation_validation_limit = ( + maximum_validation_error_rad + if maximum_validation_error_rad is not None + else maximum_axis_cone_mismatch_rad + ) + palm_orientation_validation_passed = bool( + not configured_palm_sources + or ( + palm_orientation_validation_errors + and float(np.median(palm_orientation_validation_errors)) + <= palm_orientation_validation_limit + ) + ) + for offset_joint, observer_joint in profile.offset_observer_joint.items(): + if offset_joint in fixed_offsets: + improvement_by_joint[offset_joint] = 0.0 + improvement_confidence_lower[offset_joint] = 0.0 + continue + candidate = abs(candidate_errors[offset_joint]) + original = abs(original_errors[offset_joint]) + validation_error_by_joint[observer_joint] = candidate + original_error_by_joint[observer_joint] = original + improvement_by_joint[offset_joint] = original - candidate + if applied_training[offset_joint] != 0.0 and not candidate < original: + improvement_passed = False + for offset_joint in profile.direct_zero_joints: + if offset_joint in fixed_offsets: + improvement_confidence_lower[offset_joint] = 0.0 + continue + if applied_training[offset_joint] == 0.0: + improvement_confidence_lower[offset_joint] = 0.0 + continue + cycle_improvements: list[float] = [] + for cycle in training_cycle_ids: + selected = [item for item in measurements if item.cycle == cycle] + cycle_rotation, cycle_translation = cycle_models[cycle] + candidate = abs( + angular_errors( + applied_training, + selected, + cycle_rotation, + cycle_translation, + )[offset_joint] + ) + original = abs( + angular_errors( + zero_offsets, + selected, + cycle_rotation, + cycle_translation, + )[offset_joint] + ) + cycle_improvements.append(original - candidate) + improvement_array = np.asarray(cycle_improvements, dtype=float) + improvement_se = ( + float("inf") + if improvement_array.size < 2 + else float( + np.std(improvement_array, ddof=1) + / math.sqrt(improvement_array.size) + ) + ) + lower = ( + float("-inf") + if not math.isfinite(improvement_se) + else float( + np.mean(improvement_array) + - student_t.ppf( + 0.975, df=improvement_array.size - 1 + ) + * improvement_se + ) + ) + improvement_confidence_lower[offset_joint] = lower + if lower <= 0.0: + improvement_passed = False + + validation_errors = np.asarray( + list(validation_error_by_joint.values()), dtype=float + ) + validation_line_samples: dict[str, list[float]] = {} + for item in validation: + predicted_axis, predicted_point = predicted_local( + item, applied_training + ) + predicted_axis = base_rotation.apply(predicted_axis) + predicted_axis /= np.linalg.norm(predicted_axis) + predicted_point = ( + base_rotation.apply(predicted_point) + base_translation + ) + observed_axis = np.asarray(item.axis_common_xyz, dtype=float) + observed_axis /= np.linalg.norm(observed_axis) + observed_point = np.asarray(item.point_common_xyz_m, dtype=float) + cross = np.cross(predicted_axis, observed_axis) + cross_norm = float(np.linalg.norm(cross)) + separation = observed_point - predicted_point + line_error = ( + abs(float(separation @ cross)) / cross_norm + if cross_norm > 1.0e-6 + else float( + np.linalg.norm( + separation + - predicted_axis * float(separation @ predicted_axis) + ) + ) + ) + validation_line_samples.setdefault(item.joint, []).append(line_error) + validation_line_error_by_joint = { + name: float(np.sqrt(np.mean(np.square(values)))) + for name, values in validation_line_samples.items() + } + all_validation_line_errors = np.asarray( + [ + value + for values in validation_line_samples.values() + for value in values + ], + dtype=float, + ) + axis_line_rms = ( + float("inf") + if all_validation_line_errors.size == 0 + else float( + np.sqrt(np.mean(np.square(all_validation_line_errors))) + ) + ) + finger_roll_common_mode = ( + float( + np.median( + [training_offsets[name] for name in product_finger_rolls] + ) + ) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT + and product_finger_rolls + else 0.0 + ) + configured_limit_exceeded: list[str] = [] + for name, limit in zip(profile.direct_zero_joints, offset_limits): + if name in fixed_offsets: + continue + # A post-solve mechanical endpoint datum is the value that will be + # published for this joint. The visual root-axis scalar remains a + # nuisance gauge used for holdout geometry and must not be compared + # with the safety bound of a different, endpoint-anchored output. + checked_offset = output_offsets.get(name, training_offsets[name]) + if name in product_finger_rolls: + # The four roll motors share the same electrical centre and the + # absolute palm axial datum is recovered from the root-line + # pattern. Protect the independently assembled finger-to-finger + # deviations with the strict finger bound; protect their shared + # common mode with the unchanged global zero bound. Treating the + # same common datum as four independent failures is both + # over-counting and sensitive to the palm-frame gauge. + checked_offset -= finger_roll_common_mode + if abs(checked_offset) > limit + math.radians(0.01): + configured_limit_exceeded.append(name) + if ( + product_finger_rolls + and abs(finger_roll_common_mode) + > maximum_offset_rad + math.radians(0.01) + ): + configured_limit_exceeded.append("finger_mcp_roll_common_mode") + diagnostic_bound_hits: list[str] = [] + for name, limit in zip( + profile.direct_zero_joints, diagnostic_offset_limits + ): + if name in fixed_offsets: + continue + checked_offset = output_offsets.get(name, training_offsets[name]) + if name in product_finger_rolls: + # Match the configured-limit and publication convention above. + # The raw common roll is a fitted-palm-frame gauge; only the + # finger-to-finger deviation is a physical zero correction. + checked_offset -= finger_roll_common_mode + if abs(checked_offset) >= limit - math.radians(0.01): + diagnostic_bound_hits.append(name) + failure_reasons: dict[str, str] = {} + if not palm_orientation_validation_passed: + failure_reasons["palm_orientation"] = ( + "palm_orientation_holdout_too_large" + ) + requires_full_observability = bool( + profile.hand.layout_id == G20_RIGHT_19_LAYOUT + and profile.base_pose_strategy == "full_hand" + ) + if ( + requires_full_observability + and observability_rank < observability_parameter_count + ): + failure_reasons["palm_and_static_zero"] = ( + "zero_observation_jacobian_rank_deficient" + ) + elif ( + requires_full_observability + and observability_condition_number + > maximum_observability_condition_number + ): + failure_reasons["palm_and_static_zero"] = ( + "zero_observation_jacobian_ill_conditioned" + ) + for name in configured_limit_exceeded: + failure_reasons[name] = "zero_offset_exceeds_configured_limit" + for name in diagnostic_bound_hits: + failure_reasons[name] = "zero_offset_reached_diagnostic_bound" + for name in inconsistent_cycles: + failure_reasons[name] = "zero_offset_cycle_difference_too_large" + for name in insignificant_large: + failure_reasons[name] = "zero_offset_not_statistically_significant" + if maximum_confidence_half_width_rad is not None: + for name, half_width in confidence_half_widths.items(): + if half_width > maximum_confidence_half_width_rad: + failure_reasons[name] = ( + "zero_offset_confidence_interval_too_wide" + ) + # This solver publishes rotational encoder zeros only. A post-fit CAD to + # measured axis-line displacement is invariant to the joint's own zero + # and cannot be repaired by changing that rotational parameter. Keep the + # per-joint and aggregate values in ZeroSolveResult for geometry audit, + # but do not misclassify a fixed link-origin/Tag-depth discrepancy as a + # failed rotational holdout. Axis-point *fit* quality is still guarded + # above for every phase observation that actually uses line position. + if not improvement_passed: + for name, value in applied_training.items(): + if ( + name not in fixed_offsets + and value != 0.0 + and improvement_confidence_lower.get(name, 0.0) <= 0.0 + ): + failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence" + # Geometry failures are the root cause and must not be hidden by the + # downstream validation symptom produced by the same bad observation. + failure_reasons.update(observation_failures) + passed = bool( + validation_errors.size + == len(profile.direct_zero_joints) - len(fixed_offsets) + and float(np.mean(validation_errors)) <= maximum_validation_mae_rad + and float(np.percentile(validation_errors, 95.0)) + <= maximum_validation_p95_rad + and ( + maximum_validation_error_rad is None + or float(np.max(validation_errors)) + <= maximum_validation_error_rad + ) + and cycle_consistent + and not insignificant_large + and not configured_limit_exceeded + and not diagnostic_bound_hits + and not observation_failures + and ( + not requires_full_observability + or ( + observability_rank == observability_parameter_count + and observability_condition_number + <= maximum_observability_condition_number + ) + ) + and not any( + reason == "zero_offset_confidence_interval_too_wide" + for reason in failure_reasons.values() + ) + and improvement_passed + and palm_orientation_validation_passed + ) + + # Never refit a model that has passed its holdout with the validation + # cycle. The published offsets are exactly the frozen training result + # that produced ``validation_errors`` above. + final_offsets = dict(applied_training) + # Apply independently validated assembly datums only after trajectory + # fitting and holdout validation. A mechanical prior must define the + # written artifact without perturbing downstream yaw/pitch estimates. + final_offsets.update(output_offsets) + if profile.hand.layout_id == G20_RIGHT_19_LAYOUT and product_finger_rolls: + # The camera solve observes the four roll axes in a fitted palm frame. + # Rotation of that frame about their shared datum is a gauge, not four + # independent finger assembly errors. The product command 127/CAD + # pose defines the common straight-ahead datum; publish only each + # finger's robust deviation from the four-finger median. Validation + # above remains in the observation gauge, so no measured residual is + # discarded. + for name in product_finger_rolls: + final_offsets[name] = ( + float(final_offsets[name]) - finger_roll_common_mode + ) + + # Every active joint must be present in the runtime payload/URDF writer, + # but absence of an absolute observation is not evidence for the + # reference finger's assembly offset. Preserve the source-CAD zero for + # those independent motors while continuing to share their dynamic curve. + all_offsets = { + name: 0.0 for name in profile.hand.active_joints + } + all_offsets.update(final_offsets) + for target, source in profile.inherited_static_zero_joints.items(): + all_offsets[target] = final_offsets[source] + return ZeroSolveResult( + direct_offsets_rad=final_offsets, + all_active_offsets_rad=all_offsets, + base_translation_xyz_m=tuple(float(value) for value in base_translation), + base_quaternion_xyzw=tuple(float(value) for value in base_rotation.as_quat()), + validation_errors_rad=tuple(float(value) for value in validation_errors), + validation_error_by_joint_rad=validation_error_by_joint, + validation_line_error_by_joint_m=validation_line_error_by_joint, + axis_line_rms_m=axis_line_rms, + passed=passed, + cycle_offsets_rad={ + name: tuple(float(value) for value in values) + for name, values in cycle_values.items() + }, + offset_uncertainty_rad=uncertainties, + offset_confidence_half_width_rad=confidence_half_widths, + training_cycles=training_cycle_ids, + validation_cycle=int(validation_cycle), + validation_original_error_by_joint_rad=original_error_by_joint, + validation_improvement_by_joint_rad=improvement_by_joint, + validation_improvement_confidence_lower_rad=( + improvement_confidence_lower + ), + observability_rank=observability_rank, + observability_parameter_count=observability_parameter_count, + observability_condition_number=observability_condition_number, + offset_covariance_rad2=offset_covariance, + axis_cone_mismatch_by_joint_rad=axis_cone_mismatch_by_joint, + axis_cone_bias_classification_by_joint=( + axis_cone_bias_classification_by_joint + ), + failure_reasons=failure_reasons, + ) + + +def _files_have_identical_contents(left: Path, right: Path) -> bool: + if left.stat().st_size != right.stat().st_size: + return False + with left.open("rb") as left_stream, right.open("rb") as right_stream: + while True: + left_chunk = left_stream.read(1024 * 1024) + right_chunk = right_stream.read(1024 * 1024) + if left_chunk != right_chunk: + return False + if not left_chunk: + return True + + +def _materialize_relative_mesh_assets( + *, source: Path, output: Path, urdf_root: ET.Element +) -> tuple[Path, ...]: + """Copy relative mesh resources so a session-local URDF remains loadable.""" + filenames = sorted( + { + str(mesh.get("filename", "")).strip() + for mesh in urdf_root.findall(".//mesh") + if str(mesh.get("filename", "")).strip() + } + ) + materialized: list[Path] = [] + for filename in filenames: + # URI-backed resources are resolved by the URDF consumer. Only local + # relative resources need to follow a URDF copied to a session folder. + if "://" in filename or filename.startswith("package:"): + continue + relative = Path(filename) + if relative.is_absolute() or ".." in relative.parts: + raise ValueError( + f"URDF mesh path must be a safe relative path or URI: {filename}" + ) + source_asset = (source.parent / relative).resolve() + if not source_asset.is_file(): + raise ValueError(f"URDF mesh resource does not exist: {source_asset}") + destination_asset = (output / relative).resolve() + try: + destination_asset.relative_to(output) + except ValueError as error: + raise ValueError( + f"URDF mesh destination escapes output directory: {filename}" + ) from error + if destination_asset == source_asset: + materialized.append(destination_asset) + continue + destination_asset.parent.mkdir(parents=True, exist_ok=True) + if destination_asset.exists(): + if not destination_asset.is_file() or not _files_have_identical_contents( + source_asset, destination_asset + ): + raise ValueError( + f"refusing to overwrite a different mesh resource: " + f"{destination_asset}" + ) + materialized.append(destination_asset) + continue + temporary_asset = destination_asset.with_name( + f".{destination_asset.name}.{os.getpid()}.tmp" + ) + if temporary_asset.exists(): + raise ValueError(f"temporary mesh path is occupied: {temporary_asset}") + try: + shutil.copy2(source_asset, temporary_asset) + os.replace(temporary_asset, destination_asset) + finally: + if temporary_asset.exists(): + temporary_asset.unlink() + materialized.append(destination_asset) + return tuple(materialized) + + +def write_zero_corrected_urdf( + *, + source_urdf: str | Path, + output_directory: str | Path, + serial_number: str, + offsets_rad: Mapping[str, float], + endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, + timestamp: str | None = None, + correction_plan: UrdfCorrectionPlan | None = None, +) -> Path: + source = Path(source_urdf).expanduser().resolve() + output = Path(output_directory).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"source URDF does not exist: {source}") + if ( + "zero_calibrated" in source.stem.lower() + or re.search(r"calibrated_20\d{6}", source.stem.lower()) + ): + raise ValueError( + "source_urdf must be the original CAD URDF, not a calibrated URDF" + ) + if not offsets_rad: + raise ValueError("offsets_rad must contain at least one joint") + offsets = {str(name): float(value) for name, value in offsets_rad.items()} + endpoint_offsets = { + str(name): float(value) + for name, value in dict(endpoint_anchored_offsets_rad or {}).items() + } + if not set(endpoint_offsets) <= set(offsets): + raise ValueError("endpoint-anchored offsets must be URDF zero targets") + if correction_plan is not None: + correction_plan.verify_source(source) + correction_plan.authorize_offsets(offsets) + if not set(endpoint_offsets).issubset( + correction_plan.endpoint_limit_joints + ): + raise ValueError( + "endpoint offsets are not authorized by the correction plan" + ) + if any( + not math.isfinite(value) or abs(value) > math.radians(90.0) + for value in offsets.values() + ): + raise ValueError("URDF zero offsets must be finite and within +/-90deg") + output.mkdir(parents=True, exist_ok=True) + stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") + if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: + raise ValueError("URDF zero timestamp must use YYYYMMDD_HHMMSS") + safe_serial = "".join( + character if character.isalnum() or character in "_.-" else "_" + for character in str(serial_number) + ) + if not safe_serial: + raise ValueError("serial_number must not be empty") + destination = output / f"{source.stem}_zero_calibrated_{safe_serial}_{stamp}.urdf" + if destination == source or destination.exists(): + raise ValueError(f"refusing to overwrite URDF: {destination}") + tree = ET.parse(source) + root = tree.getroot() + original_text = source.read_text(encoding="utf-8") + replacement_rpy: dict[str, str] = {} + replacement_upper: dict[str, str] = {} + replacement_mimic_offset: dict[str, str] = {} + found: set[str] = set() + for joint in root.findall("joint"): + name = str(joint.get("name")) + if name not in offsets: + mimic = joint.find("mimic") + source_joint = None if mimic is None else str(mimic.get("joint")) + if source_joint in endpoint_offsets: + multiplier = float(mimic.get("multiplier", "1")) + old_offset = float(mimic.get("offset", "0")) + calibrated_mimic_offset = ( + old_offset + multiplier * endpoint_offsets[source_joint] + ) + replacement_mimic_offset[name] = ( + f"{calibrated_mimic_offset:.15g}" + ) + continue + found.add(name) + if name in endpoint_offsets: + limit = joint.find("limit") + if limit is None or limit.get("upper") is None: + raise ValueError(f"joint {name} has no upper limit") + replacement_upper[name] = f"{float(limit.get('upper')) - endpoint_offsets[name]:.15g}" + # A statistically insignificant correction is represented as exact + # zero. Preserve that joint's source text byte-for-byte instead of + # serialising an equivalent Euler triplet. + if abs(offsets[name]) <= 1.0e-15: + continue + if joint.get("type") not in {"revolute", "continuous"}: + raise ValueError(f"joint {name} is not revolute") + axis_node = joint.find("axis") + axis = _parse_triplet( + "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") + ) + axis_norm = float(np.linalg.norm(axis)) + if axis_norm <= 1.0e-12: + raise ValueError(f"joint {name} has a degenerate axis") + axis /= axis_norm + origin_node = joint.find("origin") + if origin_node is None: + raise ValueError( + f"joint {name} has no origin; refusing a non-minimal rewrite" + ) + rpy = _parse_triplet(origin_node.get("rpy", "0 0 0")) + original = Rotation.from_euler("xyz", rpy).as_matrix() + corrected = original @ Rotation.from_rotvec( + axis * offsets[name] + ).as_matrix() + corrected_rpy = Rotation.from_matrix(corrected).as_euler("xyz") + replacement_rpy[name] = " ".join( + f"{float(value):.15g}" for value in corrected_rpy + ) + missing = sorted(set(offsets) - found) + if missing: + raise ValueError("source URDF is missing target joints: " + ",".join(missing)) + joint_pattern = re.compile( + r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>" + r".*?", + re.DOTALL, + ) + edits: list[tuple[int, int, str]] = [] + for match in joint_pattern.finditer(original_text): + name = match.group("name") + if name not in replacement_rpy: + if name not in replacement_upper and name not in replacement_mimic_offset: + continue + block = match.group(0) + if name in replacement_rpy: + origin_match = re.search( + r"]*\brpy\s*=\s*([\"'])(?P[^\"']*)\1", + block, + re.DOTALL, + ) + if origin_match is None: + raise ValueError(f"joint {name} origin has no rpy attribute") + edits.append(( + match.start() + origin_match.start("rpy"), + match.start() + origin_match.end("rpy"), + replacement_rpy[name], + )) + if name in replacement_upper: + limit_match = re.search( + r"]*\bupper\s*=\s*([\"'])(?P[^\"']*)\1", + block, + re.DOTALL, + ) + if limit_match is None: + raise ValueError(f"joint {name} limit has no upper attribute") + edits.append(( + match.start() + limit_match.start("upper"), + match.start() + limit_match.end("upper"), + replacement_upper[name], + )) + if name in replacement_mimic_offset: + mimic_match = re.search( + r"]*\boffset\s*=\s*([\"'])(?P[^\"']*)\1", + block, + re.DOTALL, + ) + if mimic_match is None: + raise ValueError(f"joint {name} mimic has no offset attribute") + edits.append(( + match.start() + mimic_match.start("offset"), + match.start() + mimic_match.end("offset"), + replacement_mimic_offset[name], + )) + expected_edit_count = ( + len(replacement_rpy) + + len(replacement_upper) + + len(replacement_mimic_offset) + ) + if len(edits) != expected_edit_count: + raise ValueError("could not locate every target joint field in source URDF text") + corrected_text = original_text + for start, end, value in reversed(edits): + corrected_text = corrected_text[:start] + value + corrected_text[end:] + _materialize_relative_mesh_assets(source=source, output=output, urdf_root=root) + temporary = destination.with_suffix(".urdf.tmp") + with temporary.open("w", encoding="utf-8") as stream: + stream.write(corrected_text) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, destination) + return destination diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py index 2ec12a6..7944db5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py @@ -18,6 +18,7 @@ class EngineBindings: preparation_waypoints: Callable[..., tuple[tuple[int, ...], ...]] return_waypoints: Callable[..., tuple[tuple[int, ...], ...]] cli_main: Callable[[list[str] | None], None] + node_main: Callable[[list[str] | None], None] @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/offline_replay.py b/src/linkerhand_calibration/linkerhand_calibration/offline_replay.py index c625e1a..36d6da7 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/offline_replay.py +++ b/src/linkerhand_calibration/linkerhand_calibration/offline_replay.py @@ -1,1941 +1,7 @@ -"""Safely replay a complete three-camera session without moving the hand.""" +"""One-release module alias for the relocated model replay implementation.""" -from __future__ import annotations +import sys -import argparse -from collections import defaultdict -from dataclasses import replace -import hashlib -import json -import math -import os -from pathlib import Path -import re -import tempfile -from typing import Any, Mapping, Sequence -import xml.etree.ElementTree as ET +from .models.g20 import offline_replay as _implementation -import numpy as np -from scipy.spatial.transform import Rotation -import yaml - -from .extrinsics import load_three_camera_extrinsics -from .full_hand import ( - G20_REFERENCE_THUMB_CMC_JOINTS, - G20_RIGHT_19_LAYOUT, - RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, - HandCalibrationProfile, - JointCurveFit, - build_calibration_motion_command, - build_compact_payload, - canonical_zero_direction, - clamp_runtime_fits_to_urdf_limits, - cross_view_roll_diagnostic_metrics, - derive_mimic_passive_fits, - fit_joint_image_curve, - get_hand_calibration_profile, - validate_compact_payload, -) -from .product import get_product_calibration_contract -from .sample_schema import fitting_sample_record, fitting_sample_records -from .storage import atomic_write_json -from .urdf_zero import ( - JointAxisMeasurement, - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, - UrdfKinematicModel, - _angles_from_state, - anchor_right_19_mechanical_endpoint_curves, - axis_line_uses_depth_free_interpretation_plane, - axis_line_cycle_rms_m, - baseline_hysteresis_by_cycle_rad, - circle_direction_is_constrained, - cross_view_side_line_source, - fit_joint_axis_measurement, - fit_partial_palm_orientation_measurements, - fit_rotation_joint_curve, - get_zero_calibration_profile, - get_right_19_thumb_zero_profile, - derive_right_19_mechanical_endpoint_offsets, - joint_curve_holdout_errors, - refit_axis_line_group_with_shared_radius, - RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, - RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, - merge_right_19_thumb_zero_result, - select_cross_view_roll_direction_source, - solve_urdf_zero_offsets, - with_depth_free_axis_projection, - write_zero_corrected_urdf, -) - - -def _sha256(path: Path) -> str: - digest = hashlib.sha256() - with path.open("rb") as stream: - for chunk in iter(lambda: stream.read(1024 * 1024), b""): - digest.update(chunk) - return digest.hexdigest() - - -def _output_suffix(output_tag: str | None) -> str: - """Return a filename-safe suffix for a non-destructive replay variant.""" - if output_tag is None: - return "" - tag = str(output_tag) - if not re.fullmatch(r"[A-Za-z0-9][A-Za-z0-9_.-]{0,63}", tag): - raise ValueError( - "output tag must be 1-64 filename-safe characters, beginning " - "with a letter or digit" - ) - return f"_{tag}" - - -def _load_parameters(path: Path) -> dict[str, Any]: - payload = yaml.safe_load(path.read_text(encoding="utf-8")) - return dict(payload["g20_calibration"]["ros__parameters"]) - - -def _steady_records_in_feedback_domain( - records: Sequence[Mapping[str, Any]], -) -> list[dict[str, Any]]: - """Mirror the live mechanical-backlash domain for offline replay.""" - return fitting_sample_records( - records, - domain="feedback", - allow_legacy_command=True, - snap_requested_endpoints=True, - ) - - -def _latest_attempt_records( - rows: Sequence[Mapping[str, Any]], - *, - kind: str = "sample", -) -> dict[str, list[dict[str, Any]]]: - """Reproduce the online retry buffer from append-only raw samples. - - Online retry clears only the failed joint/cycle/direction from memory, - while JSONL deliberately retains every attempt for audit. Offline replay - must therefore select the greatest attempt independently for each logical - trajectory rather than mixing rejected attempts into the final fit. - """ - samples = [dict(row) for row in rows if row.get("kind") == kind] - latest_attempt: dict[tuple[str, int, str], int] = {} - for row in samples: - key = ( - str(row["joint"]), - int(row["cycle"]), - str(row["direction"]), - ) - latest_attempt[key] = max( - latest_attempt.get(key, 0), int(row.get("attempt", 1)) - ) - records: dict[str, list[dict[str, Any]]] = defaultdict(list) - for row in samples: - key = ( - str(row["joint"]), - int(row["cycle"]), - str(row["direction"]), - ) - if int(row.get("attempt", 1)) == latest_attempt[key]: - records[key[0]].append(row) - return dict(records) - - -def _latest_palm_axis_records( - rows: Sequence[Mapping[str, Any]], -) -> dict[str, list[dict[str, Any]]]: - """Select the latest accepted append-only palm side-channel attempts.""" - samples = [ - dict(row) for row in rows if row.get("kind") == "palm_axis_sample" - ] - latest_attempt: dict[tuple[str, int, str], int] = {} - for row in samples: - key = ( - str(row["source_joint"]), - int(row["cycle"]), - str(row["direction"]), - ) - latest_attempt[key] = max( - latest_attempt.get(key, 0), int(row.get("attempt", 1)) - ) - records: dict[str, list[dict[str, Any]]] = defaultdict(list) - for row in samples: - key = ( - str(row["source_joint"]), - int(row["cycle"]), - str(row["direction"]), - ) - if int(row.get("attempt", 1)) == latest_attempt[key]: - records[key[0]].append(row) - return dict(records) - - -def _load_raw_session( - session_dir: Path, -) -> tuple[ - dict[str, Any], - dict[str, list[dict[str, Any]]], - dict[str, list[dict[str, Any]]], - dict[str, list[dict[str, Any]]], - dict[str, list[dict[str, Any]]], - Path, -]: - raw_path = session_dir / "raw_samples.jsonl" - if not raw_path.is_file(): - raise ValueError(f"raw session does not exist: {raw_path}") - rows = [ - json.loads(line) - for line in raw_path.read_text(encoding="utf-8").splitlines() - if line.strip() - ] - # Both online fitting and replay use the same canonical-to-fit projection. - # raw_samples.jsonl itself never contains the ambiguous compatibility key. - for row in rows: - if row.get("kind") in { - "sample", - "baseline_hold_sample", - "steady_command_sample", - "palm_axis_sample", - }: - projected = fitting_sample_record( - row, allow_legacy_command=True - ) - row.clear() - row.update(projected) - starts = [row for row in rows if row.get("kind") == "session_start"] - if len(starts) != 1: - raise ValueError("raw session must contain exactly one session_start") - return ( - starts[0], - _latest_attempt_records(rows), - _latest_attempt_records(rows, kind="baseline_hold_sample"), - _latest_attempt_records(rows, kind="steady_command_sample"), - _latest_palm_axis_records(rows), - raw_path, - ) - - -def _fit_curve( - name: str, - records: Sequence[Mapping[str, Any]], - *, - profile: HandCalibrationProfile, - baseline: Sequence[int], -) -> JointCurveFit: - motor = profile.record_specs[name].motor_index - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and ( - name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS - or ( - name.endswith("_mcp_roll") - and not name.startswith("thumb_") - ) - ) - ): - return fit_joint_image_curve(records) - return fit_rotation_joint_curve( - records, - zero_command_u8=int(baseline[motor]), - canonical_zero_direction=canonical_zero_direction(profile, name), - ) - - -def _physical_rotation_axis_for_curve( - name: str, - records: Sequence[Mapping[str, Any]], - fit: JointCurveFit, - *, - profile: HandCalibrationProfile, - baseline: Sequence[int], -) -> Sequence[float]: - """Keep baseline pose hysteresis independent of the dynamic curve space.""" - axis = fit.circle.get("axis_xyz") - if axis is not None: - return axis - motor = profile.record_specs[name].motor_index - diagnostic_fit = fit_rotation_joint_curve( - records, - zero_command_u8=int(baseline[motor]), - canonical_zero_direction=canonical_zero_direction(profile, name), - ) - return diagnostic_fit.circle["axis_xyz"] - - -def _fit_axes( - *, - records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], - profile: HandCalibrationProfile, - baseline: Sequence[int], - extrinsics_file: Path, - source_urdf: Path, - repetitions: int, -) -> list[JointAxisMeasurement]: - zero_profile = get_zero_calibration_profile( - profile.side, profile.layout_id - ) - extrinsics = load_three_camera_extrinsics(extrinsics_file) - cache: dict[tuple[str, int], JointAxisMeasurement] = {} - cross_view_direction_source_cache: dict[str, str] = {} - upstream_by_joint = { - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - **{ - f"{finger}_pip": f"{finger}_mcp_pitch" - for finger in ("index", "middle", "ring", "pinky") - }, - **{ - f"{finger}_dip": f"{finger}_pip" - for finger in ("index", "middle", "ring", "pinky") - }, - } - if profile.layout_id == G20_RIGHT_19_LAYOUT: - reference_roll = f"{profile.reference_finger}_mcp_roll" - upstream_by_joint.update( - { - f"{finger}_mcp_roll": reference_roll - for finger in ("index", "middle", "ring", "pinky") - if f"{finger}_mcp_roll" != reference_roll - } - ) - - def fit_raw(name: str, cycle: int) -> JointAxisMeasurement: - key = (name, cycle) - if key in cache: - return cache[key] - upstream = upstream_by_joint.get(name) - constraint = None if upstream is None else fit_raw(upstream, cycle).axis_common_xyz - spec = profile.record_specs[name] - view_normal = extrinsics.transform(spec.view)[:3, :3] @ np.asarray( - [0.0, 0.0, 1.0], dtype=float - ) - result = fit_joint_axis_measurement( - name, - records_by_joint[name], - cycle=cycle, - zero_command_u8=int(baseline[spec.motor_index]), - axis_common_constraint=constraint, - constrained_circle_joints=( - zero_profile.constrained_circle_joints - | ({name} if name.endswith("_side") else set()) - ), - view_normal_common_xyz=view_normal, - canonical_zero_direction=canonical_zero_direction(profile, name), - ) - sweep_spec = next( - sweep for sweep in profile.sweep_specs if name in sweep.joints - ) - condition = build_calibration_motion_command( - sweep_spec, - int(baseline[spec.motor_index]), - baseline=baseline, - profile=profile, - ) - result = replace( - result, - condition_command_u8=( - None - if profile.layout_id == G20_RIGHT_19_LAYOUT - else tuple(float(value) for value in condition) - ), - view_normal_common_xyz=tuple(float(value) for value in view_normal), - ) - result = with_depth_free_axis_projection( - result, extrinsics.transform(spec.view)[:3, 3] - ) - cache[key] = result - return result - - def fit_one(name: str, cycle: int) -> JointAxisMeasurement: - primary = fit_raw(name, cycle) - validation_name = (profile.axis_validation_sources or {}).get(name) - if validation_name is None: - return primary - secondary = fit_raw(validation_name, cycle) - primary_axis = np.asarray(primary.axis_common_xyz, dtype=float) - secondary_axis = np.asarray(secondary.axis_common_xyz, dtype=float) - if float(primary_axis @ secondary_axis) < 0.0: - secondary_axis = -secondary_axis - difference = math.acos( - float(np.clip(primary_axis @ secondary_axis, -1.0, 1.0)) - ) - primary_point = np.asarray(primary.point_common_xyz_m, dtype=float) - secondary_point = np.asarray(secondary.point_common_xyz_m, dtype=float) - line_distance = float( - np.linalg.norm( - np.cross(secondary_point - primary_point, primary_axis) - ) - ) - if ( - profile.record_specs[validation_name].zero_kind - == "axis_cross_view_validation" - ): - point_view = profile.record_specs[name].view - if point_view is None: - raise ValueError( - f"{name}: primary point source has no view" - ) - point_camera_center = extrinsics.transform(point_view)[:3, 3] - point_ray = primary_point - point_camera_center - interpretation_plane_normal = np.cross( - point_ray, primary_axis - ) - interpretation_plane_normal /= np.linalg.norm( - interpretation_plane_normal - ) - return replace( - primary, - point_common_xyz_m=tuple( - float(value) - for value in primary.point_common_xyz_m - ), - pose_axis_line_rms_m=primary.pose_axis_line_rms_m, - pose_axis_line_source_joints=(name,), - axis_point_source=( - "front_interpretation_plane_cross_view_validated" - ), - axis_point_camera_center_common_xyz_m=tuple( - float(value) - for value in point_camera_center - ), - axis_point_interpretation_plane_normal_common_xyz=tuple( - float(value) - for value in interpretation_plane_normal - ), - ) - # Mirror the live node's tiered policy: the side-view planar-tag - # IPPE bias makes sub-degree cross-view agreement unreachable, so - # disagreement above the fusion gate keeps the front-only axis while - # only a gross direction error (wrong link / loose tag) still fails. - # The front screw-driven roll trajectory yields a displaced - # pseudo-line, so its distance to the side physical line is retained - # only as a diagnostic. The live node likewise keeps only the gross - # direction gate here and validates side-line quality independently. - if difference > math.radians(15.0): - raise ValueError(f"{name}: cross-view axis gross disagreement") - if difference > math.radians(0.75) or line_distance > 0.001: - selected_direction = primary_axis - selected_source = primary.axis_direction_source - observer_name = next( - ( - observer - for observer, parent in zero_profile.axis_parent_joint.items() - if parent == name - ), - None, - ) - if observer_name is not None: - model = UrdfKinematicModel(source_urdf) - parent_axis, _ = model.axis_line( - name, zero_offsets={}, joint_angles={} - ) - observer_axis, _ = model.axis_line( - observer_name, zero_offsets={}, joint_angles={} - ) - expected_cone = math.acos( - abs(float(np.clip(parent_axis @ observer_axis, -1.0, 1.0))) - ) - def cone_residual( - candidate: np.ndarray, measured_observer: np.ndarray - ) -> float: - measured_cone = math.acos( - abs( - float( - np.clip( - candidate @ measured_observer, -1.0, 1.0 - ) - ) - ) - ) - return abs(measured_cone - expected_cone) - - if name not in cross_view_direction_source_cache: - primary_residuals: list[float] = [] - secondary_residuals: list[float] = [] - for group_cycle in range(repetitions): - group_observer = np.asarray( - fit_raw( - observer_name, group_cycle - ).axis_common_xyz, - dtype=float, - ) - primary_residuals.append( - cone_residual( - np.asarray( - fit_raw( - name, group_cycle - ).axis_common_xyz, - dtype=float, - ), - group_observer, - ) - ) - secondary_residuals.append( - cone_residual( - np.asarray( - fit_raw( - validation_name, group_cycle - ).axis_common_xyz, - dtype=float, - ), - group_observer, - ) - ) - cross_view_direction_source_cache[name] = ( - select_cross_view_roll_direction_source( - primary_residuals, - secondary_residuals, - math.radians(5.0), - ) - ) - if cross_view_direction_source_cache[name] == "secondary": - selected_direction = secondary_axis - selected_source = "cross_view_cone_selected_secondary" - # Mirror the live node: use the view whose direction satisfies - # the zero-invariant parent/child cone, and take the physical line - # position from the side PIP-link circle. - return replace( - primary, - axis_common_xyz=tuple( - float(value) for value in selected_direction - ), - point_common_xyz_m=tuple( - float(value) - for value in secondary.point_common_xyz_m - ), - pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, - pose_axis_line_source_joints=(validation_name,), - axis_direction_source=selected_source, - axis_point_source="side_circle_cross_view", - ) - primary_variance = max( - primary.radial_rms_m ** 2 + primary.pose_axis_line_rms_m ** 2, - 1.0e-12, - ) - secondary_variance = max( - secondary.radial_rms_m ** 2 + secondary.pose_axis_line_rms_m ** 2, - 1.0e-12, - ) - primary_weight = 1.0 / primary_variance - secondary_weight = 1.0 / secondary_variance - fused_axis = ( - primary_weight * primary_axis + secondary_weight * secondary_axis - ) - fused_axis /= np.linalg.norm(fused_axis) - delta = secondary_point - primary_point - perpendicular = delta - fused_axis * float(delta @ fused_axis) - fused_point = primary_point + ( - secondary_weight / (primary_weight + secondary_weight) - ) * perpendicular - return replace( - primary, - axis_common_xyz=tuple(float(value) for value in fused_axis), - point_common_xyz_m=tuple(float(value) for value in fused_point), - pose_axis_line_rms_m=max( - primary.pose_axis_line_rms_m, - secondary.pose_axis_line_rms_m, - line_distance, - ), - pose_axis_line_source_joints=(name, validation_name), - axis_direction_source="cross_view_weighted_fusion", - ) - - result: list[JointAxisMeasurement] = [] - for name in zero_profile.axis_joints: - group = [fit_one(name, cycle) for cycle in range(repetitions)] - side_sources = { - source - for measurement in group - if (source := cross_view_side_line_source(measurement)) is not None - } - if ( - len(side_sources) == 1 - and all( - cross_view_side_line_source(measurement) is not None - for measurement in group - ) - and not any( - axis_line_uses_depth_free_interpretation_plane(measurement) - for measurement in group - ) - ): - source = next(iter(side_sources)) - source_spec = profile.record_specs[source] - group = list( - refit_axis_line_group_with_shared_radius( - group, - records_by_joint[source], - zero_command_u8=int( - baseline[source_spec.motor_index] - ), - canonical_zero_direction=canonical_zero_direction( - profile, source - ), - ) - ) - result.extend(group) - return result - - -def _maximum_undirected_axis_difference(axes: Sequence[Sequence[float]]) -> float: - maximum = 0.0 - for left in axes: - for right in axes: - maximum = max( - maximum, - math.acos( - abs( - float( - np.clip( - np.asarray(left, dtype=float) - @ np.asarray(right, dtype=float), - -1.0, - 1.0, - ) - ) - ) - ), - ) - return maximum - - -def _quality_failures( - *, - records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], - baseline_records_by_joint: Mapping[ - str, Sequence[Mapping[str, Any]] - ] | None = None, - profile: HandCalibrationProfile, - baseline: Sequence[int], - fits: Mapping[str, JointCurveFit], - axes: Sequence[JointAxisMeasurement], - parameters: Mapping[str, Any], -) -> list[str]: - failures: list[str] = [] - repetitions = int(parameters["repetitions"]) - zero_profile = get_zero_calibration_profile( - profile.side, profile.layout_id - ) - axis_by_key = {(item.joint, item.cycle): item for item in axes} - expected_directions = {"decreasing", "increasing"} - for name in profile.measured_joints: - records = list(records_by_joint.get(name, ())) - if not records: - failures.append(f"{name}: no samples") - continue - attempts_by_direction: dict[tuple[int, str], set[int]] = defaultdict(set) - for record in records: - attempts_by_direction[ - (int(record["cycle"]), str(record["direction"])) - ].add(int(record.get("attempt", 1))) - for cycle in range(repetitions): - for direction in expected_directions: - selected = [ - record - for record in records - if int(record["cycle"]) == cycle - and str(record["direction"]) == direction - ] - attempts = attempts_by_direction.get((cycle, direction), set()) - if len(attempts) != 1: - failures.append( - f"{name} cycle {cycle + 1} {direction}: ambiguous attempts {sorted(attempts)}" - ) - continue - commands = sorted({int(record["command_u8"]) for record in selected}) - if len(selected) < int(parameters["minimum_sweep_frames"]): - failures.append(f"{name} cycle {cycle + 1} {direction}: too few frames") - if not commands or max(commands) - min(commands) < float( - parameters["minimum_state_span_u8"] - ): - failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient span") - if len(commands) < int(parameters["minimum_sweep_bins"]): - failures.append(f"{name} cycle {cycle + 1} {direction}: insufficient bins") - if 0 not in commands or 255 not in commands: - failures.append(f"{name} cycle {cycle + 1} {direction}: endpoint missing") - if commands and max(np.diff(commands), default=0) > int( - parameters["maximum_bin_gap"] - ): - failures.append(f"{name} cycle {cycle + 1} {direction}: bin gap") - - sync_p95 = float( - np.percentile( - [float(record.get("state_image_sync_error_ms", 0.0)) for record in records], - 95.0, - ) - ) - if sync_p95 > float(parameters["maximum_state_image_skew_ms"]): - failures.append(f"{name}: state/image sync p95 {sync_p95:.3f}ms") - spec = profile.joint_specs[name] - fit = fits[name] - if fit.circle.get("space") == "image_2d": - if float(fit.quality["radial_rms_px"]) > float( - parameters["image_trajectory_maximum_radial_rms_px"] - ): - failures.append(f"{name}: image radial RMS") - if float(fit.quality["radial_p95_px"]) > float( - parameters["image_trajectory_maximum_radial_p95_px"] - ): - failures.append(f"{name}: image radial P95") - if float(fit.quality["radius_px"]) < float( - parameters["image_trajectory_minimum_radius_px"] - ): - failures.append(f"{name}: image radius") - else: - orthogonal_limit = math.radians( - float( - parameters[ - "active_maximum_rotation_orthogonal_rms_deg" - if spec.active - else "passive_maximum_rotation_orthogonal_rms_deg" - ] - ) - ) - if ( - float(fit.quality["rotation_orthogonal_rms_rad"]) - > orthogonal_limit - ): - failures.append(f"{name}: rotation orthogonal RMS") - if float(fit.quality["arc_rad"]) < math.radians( - float(parameters["trajectory_minimum_arc_deg"]) - ): - failures.append(f"{name}: trajectory arc") - monotonic_limit = math.radians( - float( - parameters[ - "maximum_monotonic_correction_deg" - if spec.active - else "passive_maximum_monotonic_correction_deg" - ] - ) - ) - hysteresis_limit = math.radians( - float( - parameters[ - "maximum_hysteresis_deg" - if spec.active - else "passive_maximum_hysteresis_deg" - ] - ) - ) - if fit.maximum_monotonic_correction_rad > monotonic_limit: - failures.append(f"{name}: monotonic correction") - if ( - profile.layout_id != G20_RIGHT_19_LAYOUT - and fit.maximum_hysteresis_rad > hysteresis_limit - ): - failures.append(f"{name}: hysteresis") - if profile.layout_id == G20_RIGHT_19_LAYOUT: - hysteresis_records = records - dedicated = (baseline_records_by_joint or {}).get(name, ()) - if int(baseline[spec.motor_index]) not in {0, 255} and dedicated: - hysteresis_records = list(dedicated) - baseline_hysteresis = baseline_hysteresis_by_cycle_rad( - hysteresis_records, - zero_command_u8=int(baseline[spec.motor_index]), - axis_xyz=_physical_rotation_axis_for_curve( - name, - records, - fit, - profile=profile, - baseline=baseline, - ), - ) - if canonical_zero_direction(profile, name) is not None: - gap_limit = math.radians( - float( - parameters.get( - "directional_zero_maximum_branch_gap_deg", 1.5 - ) - ) - ) - gap_range_limit = math.radians( - float( - parameters.get( - "directional_zero_maximum_branch_gap_range_deg", - 0.3, - ) - ) - ) - if max(baseline_hysteresis) > gap_limit: - failures.append(f"{name}: baseline directional gap") - if ( - max(baseline_hysteresis) - min(baseline_hysteresis) - > gap_range_limit - and spec.zero_kind != "axis_cross_view_validation" - ): - failures.append( - f"{name}: baseline directional gap repeatability" - ) - elif max(baseline_hysteresis) > math.radians( - float(parameters.get("baseline_maximum_hysteresis_deg", 0.5)) - ): - failures.append(f"{name}: baseline hysteresis") - - cycle_travels: list[float] = [] - cycle_axes: list[Sequence[float]] = [] - cycle_axis_sources: list[str] = [] - cycle_axis_measurements: list[JointAxisMeasurement] = [] - for cycle in range(repetitions): - cycle_fit = _fit_curve( - name, - [record for record in records if int(record["cycle"]) == cycle], - profile=profile, - baseline=baseline, - ) - cycle_travels.append( - abs(float(cycle_fit.angle_rad[0]) - float(cycle_fit.angle_rad[255])) - ) - axis = axis_by_key[(name, cycle)] - cycle_axes.append(axis.axis_common_xyz) - cycle_axis_sources.append(axis.axis_direction_source) - cycle_axis_measurements.append(axis) - if axis.radial_rms_m > float(parameters["axis_maximum_radial_rms_m"]): - failures.append(f"{name} cycle {cycle + 1}: radial RMS") - if ( - profile.record_specs[name].zero_kind - != "axis_cross_view_validation" - and cross_view_side_line_source(axis) is None - and axis.pose_axis_line_rms_m - > float(parameters["axis_maximum_pose_line_rms_m"]) - ): - failures.append( - f"{name} cycle {cycle + 1}: pose axis-line RMS" - ) - if not circle_direction_is_constrained( - name, zero_profile.constrained_circle_joints - ): - plane_limit = float( - parameters[ - "axis_maximum_plane_rms_m" - if spec.active - else "passive_axis_maximum_plane_rms_m" - ] - ) - if axis.plane_rms_m > plane_limit: - failures.append(f"{name} cycle {cycle + 1}: plane RMS") - if ( - axis.rotation_circle_axis_difference_rad > math.radians( - float(parameters["axis_maximum_rotation_circle_difference_deg"]) - ) - ): - failures.append(f"{name} cycle {cycle + 1}: axis disagreement") - side_line_sources = { - source - for measurement in cycle_axis_measurements - if ( - source := cross_view_side_line_source(measurement) - ) is not None - } - if ( - len(cycle_axis_measurements) == repetitions - and len(side_line_sources) == 1 - and all( - cross_view_side_line_source(measurement) is not None - for measurement in cycle_axis_measurements - ) - and not any( - axis_line_uses_depth_free_interpretation_plane(measurement) - for measurement in cycle_axis_measurements - ) - and axis_line_cycle_rms_m(cycle_axis_measurements) - > float(parameters["axis_maximum_pose_line_rms_m"]) - ): - source = next(iter(side_line_sources)) - failures.append(f"{source}: axis-line cycle RMS") - travel_limit = math.radians( - float( - parameters[ - "trajectory_maximum_cycle_travel_difference_deg" - if spec.active - else "passive_maximum_cycle_travel_difference_deg" - ] - ) - ) - if max(cycle_travels) - min(cycle_travels) > travel_limit: - failures.append(f"{name}: cycle travel difference") - if ( - not all( - source == "upstream_constraint" - for source in cycle_axis_sources - ) - and spec.zero_kind != "axis_cross_view_validation" - and _maximum_undirected_axis_difference(cycle_axes) > math.radians( - float(parameters["zero_maximum_axis_cycle_difference_deg"]) - ) - ): - failures.append(f"{name}: cycle axis difference") - return failures - - -def _joint_xml(path: Path) -> dict[str, ET.Element]: - return { - str(joint.get("name")): joint - for joint in ET.parse(path).getroot().findall("joint") - } - - -def _triplet(value: str) -> np.ndarray: - return np.asarray([float(item) for item in value.split()], dtype=float) - - -def _validate_corrected_urdf( - *, - source: Path, - corrected: Path, - offsets: Mapping[str, float], - endpoint_anchored_offsets: Mapping[str, float], - axes: Sequence[JointAxisMeasurement], - curves: Mapping[str, JointCurveFit], - motor_by_joint: Mapping[str, int], - inherited_zero_joints: Mapping[str, str], -) -> dict[str, float]: - source_joints = _joint_xml(source) - corrected_joints = _joint_xml(corrected) - if set(source_joints) != set(corrected_joints): - raise ValueError("corrected URDF changed the joint set") - maximum_origin_rotation_error = 0.0 - maximum_origin_translation_error = 0.0 - for name, original_joint in source_joints.items(): - corrected_joint = corrected_joints[name] - original_origin = original_joint.find("origin") - corrected_origin = corrected_joint.find("origin") - if original_origin is None or corrected_origin is None: - continue - original_xyz = _triplet(original_origin.get("xyz", "0 0 0")) - corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0")) - maximum_origin_translation_error = max( - maximum_origin_translation_error, - float(np.linalg.norm(corrected_xyz - original_xyz)), - ) - original_rotation = Rotation.from_euler( - "xyz", _triplet(original_origin.get("rpy", "0 0 0")) - ) - corrected_rotation = Rotation.from_euler( - "xyz", _triplet(corrected_origin.get("rpy", "0 0 0")) - ) - expected = original_rotation - if name in offsets: - axis_node = original_joint.find("axis") - axis = _triplet( - "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") - ) - axis /= np.linalg.norm(axis) - expected = original_rotation * Rotation.from_rotvec( - axis * float(offsets[name]) - ) - error = float((expected.inv() * corrected_rotation).magnitude()) - maximum_origin_rotation_error = max(maximum_origin_rotation_error, error) - original_limit = original_joint.find("limit") - corrected_limit = corrected_joint.find("limit") - if original_limit is not None and corrected_limit is not None: - expected_lower = float(original_limit.get("lower")) - expected_upper = float(original_limit.get("upper")) - if name in endpoint_anchored_offsets: - expected_upper -= float(endpoint_anchored_offsets[name]) - if ( - abs(float(corrected_limit.get("lower")) - expected_lower) > 1.0e-12 - or abs(float(corrected_limit.get("upper")) - expected_upper) > 1.0e-12 - ): - raise ValueError(f"corrected URDF unexpectedly changed {name} limits") - original_mimic = original_joint.find("mimic") - corrected_mimic = corrected_joint.find("mimic") - if original_mimic is not None and corrected_mimic is not None: - source_joint = str(original_mimic.get("joint")) - multiplier = float(original_mimic.get("multiplier", "1")) - expected_mimic_offset = float(original_mimic.get("offset", "0")) - expected_mimic_offset += multiplier * float( - endpoint_anchored_offsets.get(source_joint, 0.0) - ) - if abs( - float(corrected_mimic.get("offset", "0")) - - expected_mimic_offset - ) > 1.0e-12: - raise ValueError(f"corrected URDF has invalid {name} mimic offset") - if maximum_origin_translation_error > 1.0e-12: - raise ValueError("corrected URDF changed a joint origin translation") - if maximum_origin_rotation_error > 1.0e-10: - raise ValueError("corrected URDF does not implement T_original * Rot(axis, offset)") - original_model = UrdfKinematicModel(source) - corrected_model = UrdfKinematicModel(corrected) - maximum_axis_error = 0.0 - maximum_point_error = 0.0 - for measurement in axes: - state = ( - measurement.condition_state_u8 - if measurement.condition_command_u8 is None - else measurement.condition_command_u8 - ) - angles = _angles_from_state( - state, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=inherited_zero_joints, - ) - expected_axis, expected_point = original_model.axis_line( - measurement.joint, - zero_offsets=offsets, - joint_angles=angles, - ) - actual_axis, actual_point = corrected_model.axis_line( - measurement.joint, - zero_offsets={}, - joint_angles=angles, - ) - maximum_axis_error = max( - maximum_axis_error, - math.acos(float(np.clip(expected_axis @ actual_axis, -1.0, 1.0))), - ) - maximum_point_error = max( - maximum_point_error, - float(np.linalg.norm(expected_point - actual_point)), - ) - if maximum_axis_error > 1.0e-7 or maximum_point_error > 1.0e-10: - raise ValueError("written URDF kinematics differ from the solved correction") - return { - "maximum_origin_rotation_error_rad": maximum_origin_rotation_error, - "maximum_origin_translation_error_m": maximum_origin_translation_error, - "maximum_axis_equivalence_error_rad": maximum_axis_error, - "maximum_axis_point_equivalence_error_m": maximum_point_error, - } - - -def replay_session( - session_dir: str | Path, - *, - serial_number: str | None = None, - config_file: str | Path | None = None, - write_outputs: bool = False, - output_tag: str | None = None, -) -> dict[str, Any]: - session = Path(session_dir).expanduser().resolve() - package_root = Path(__file__).resolve().parents[1] - config = ( - package_root / "config" / "three_camera_calibration.yaml" - if config_file is None - else Path(config_file).expanduser().resolve() - ) - parameters = _load_parameters(config) - ( - start, - records_by_joint, - baseline_records_by_joint, - command_records_by_joint, - palm_axis_records_by_source, - raw_path, - ) = _load_raw_session(session) - model = str(start.get("model", "G20")).strip().upper() - side = str(start["hand_type"]).lower() - requested_layout_id = str( - start.get("tag_layout", "legacy_11") - ).lower() - contract = get_product_calibration_contract( - model, side, requested_layout_id - ) - profile = contract.profile - # Normalise the former g20_right_15 compatibility alias immediately so - # every downstream product guard uses the actual physical 19-Tag profile - # instead of silently falling through legacy-11 logic. - layout_id = profile.layout_id - zero_profile = contract.zero_profile - baseline = tuple(int(value) for value in start["baseline_command_u8"]) - if len(baseline) != profile.command_count: - raise ValueError( - "session baseline length differs from the registered product schema" - ) - if ( - layout_id == G20_RIGHT_19_LAYOUT - and write_outputs - and bool(parameters.get("combination_validation_enabled", False)) - ): - replay_rows = [ - json.loads(line) - for line in raw_path.read_text(encoding="utf-8").splitlines() - if line.strip() - ] - combinations = [ - row - for row in replay_rows - if row.get("kind") == "combination_validation_sample" - and bool(row.get("passed")) - ] - expected_poses = { - "all_open", "thumb_middle", "index_middle", "middle_middle", - "ring_middle", "pinky_middle", "half_grip", "light_pinch", - } - if {str(row.get("pose_name")) for row in combinations} != expected_poses: - raise ValueError( - "incomplete session cannot publish: eight combination poses are required" - ) - if any( - float(row.get("position_p95_m", float("inf"))) > 0.003 - or float(row.get("orientation_p95_rad", float("inf"))) - > math.radians(2.0) - for row in combinations - ): - raise ValueError("combination pose validation exceeds product limits") - required_record_joints = set(profile.record_joints) - if ( - not required_record_joints.issubset(records_by_joint) - or not set(records_by_joint).issubset(set(profile.record_joints)) - ): - raise ValueError( - "raw session does not contain the required task record set" - ) - - def hysteresis_records( - name: str, - ) -> Sequence[Mapping[str, Any]]: - spec = profile.record_specs[name] - zero = int(baseline[spec.motor_index]) - if ( - layout_id == G20_RIGHT_19_LAYOUT - and zero not in {0, 255} - and start.get("baseline_hysteresis_source") - == "dedicated_mid_sweep_hold" - ): - records = baseline_records_by_joint.get(name, []) - if not records: - raise ValueError( - f"{name} is missing dedicated baseline hold samples" - ) - return records - return records_by_joint[name] - - def model_records( - name: str, *, require_full_pose: bool = False - ) -> list[dict[str, Any]]: - """Merge settled baseline holds into directional model records.""" - records = [ - dict(record) for record in records_by_joint.get(name, ()) - ] - if canonical_zero_direction(profile, name) is None: - return records - required = { - "relative_translation_xyz_m", - "parent_pose_common", - } - records.extend( - dict(record) - for record in baseline_records_by_joint.get(name, ()) - if not require_full_pose or required.issubset(record) - ) - return records - - curve_records_by_joint = { - name: model_records(name) for name in profile.record_joints - } - axis_records_by_joint = { - name: model_records(name, require_full_pose=True) - for name in profile.record_joints - } - source_urdf = Path(start["source_urdf_path"]).expanduser().resolve() - extrinsics_file = Path(start["camera_extrinsics_file"]).expanduser().resolve() - if not source_urdf.is_file() or not extrinsics_file.is_file(): - raise ValueError("session source URDF or camera extrinsics is missing") - if "zero_calibrated" in source_urdf.stem.lower(): - raise ValueError("offline replay requires the original CAD URDF") - hand_serial = str(serial_number or session.parent.name) - output_suffix = _output_suffix(output_tag) - repetitions = int( - parameters.get("g20_right_19_repetitions", 4) - if layout_id == G20_RIGHT_19_LAYOUT - else parameters["repetitions"] - ) - - measured_fits = { - name: _fit_curve( - name, - curve_records_by_joint[name], - profile=profile, - baseline=baseline, - ) - for name in profile.measured_joints - } - validation_cycle = repetitions - 1 - training_cycles = tuple(range(validation_cycle)) - training_cycle_set = set(training_cycles) - if layout_id == G20_RIGHT_19_LAYOUT and len(training_cycles) < 3: - raise ValueError( - "g20_right_19 replay requires at least 3 training cycles and " - "one isolated holdout" - ) - training_fits = { - name: _fit_curve( - name, - [ - record - for record in curve_records_by_joint[name] - if int(record["cycle"]) in training_cycle_set - ], - profile=profile, - baseline=baseline, - ) - for name in profile.measured_joints - } - command_fits = dict(training_fits) - if layout_id == G20_RIGHT_19_LAYOUT: - if set(command_records_by_joint) != required_record_joints: - raise ValueError( - "g20_right_19 session is incomplete: missing steady command checkpoints" - ) - command_fits = {} - for name in profile.measured_joints: - if name in G20_REFERENCE_THUMB_CMC_JOINTS: - command_fits[name] = measured_fits[name] - continue - records = [ - dict(record) - for record in command_records_by_joint[name] - if int(record["cycle"]) == 0 - ] - if canonical_zero_direction(profile, name) is not None: - records.extend( - dict(record) - for record in baseline_records_by_joint.get(name, ()) - if int(record["cycle"]) == 0 - ) - if len(records) < 18: - raise ValueError( - f"{name} has {len(records)}/18 steady command checkpoints" - ) - command_fits[name] = _fit_curve( - name, records, profile=profile, baseline=baseline - ) - command_gap_limit = math.radians( - float(parameters.get("command_maximum_direction_gap_deg", 2.0)) - ) - command_feedback_fits = { - name: _fit_curve( - name, - _steady_records_in_feedback_domain( - [ - dict(record) - for record in command_records_by_joint[name] - if int(record["cycle"]) == 0 - ] - ), - profile=profile, - baseline=baseline, - ) - for name in profile.measured_joints - if name not in G20_REFERENCE_THUMB_CMC_JOINTS - } - failed_command_gaps = { - name: fit.maximum_hysteresis_rad - for name, fit in command_feedback_fits.items() - if fit.maximum_hysteresis_rad > command_gap_limit - } - if failed_command_gaps: - raise ValueError( - "command-direction gap exceeds 2deg: " - + ", ".join( - f"{name}={math.degrees(value):.3f}deg" - for name, value in failed_command_gaps.items() - ) - ) - command_fits = derive_mimic_passive_fits( - source_urdf, - command_fits, - profile=profile, - ) - command_fits = anchor_right_19_mechanical_endpoint_curves( - command_fits, - { - name: ( - [ - record - for record in curve_records_by_joint[name] - if int(record["cycle"]) in training_cycle_set - ] - if name == "thumb_cmc_roll" - else command_records_by_joint[name] - ) - for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - }, - maximum_direction_difference_rad=command_gap_limit, - feedback_endpoint_joints=frozenset({"thumb_cmc_roll"}), - ) - cross_view_roll_metrics: dict[str, dict[str, float]] = {} - for name, validation_name in ( - profile.axis_validation_sources or {} - ).items(): - validation_training = _fit_curve( - validation_name, - [ - record - for record in curve_records_by_joint[validation_name] - if int(record["cycle"]) in training_cycle_set - ], - profile=profile, - baseline=baseline, - ) - validation_all = _fit_curve( - validation_name, - curve_records_by_joint[validation_name], - profile=profile, - baseline=baseline, - ) - training_metrics = cross_view_roll_diagnostic_metrics( - training_fits[name], validation_training - ) - final_metrics = cross_view_roll_diagnostic_metrics( - measured_fits[name], validation_all - ) - cross_view_roll_metrics[name] = { - **{ - f"training_{key}": value - for key, value in training_metrics.items() - }, - **{ - f"final_{key}": value - for key, value in final_metrics.items() - }, - } - axes = _fit_axes( - records_by_joint=axis_records_by_joint, - profile=profile, - baseline=baseline, - extrinsics_file=extrinsics_file, - source_urdf=source_urdf, - repetitions=repetitions, - ) - palm_orientation_measurements, palm_orientation_rejections = ( - fit_partial_palm_orientation_measurements( - sources=profile.palm_orientation_sources, - records_by_joint=palm_axis_records_by_source, - motor_by_source=profile.palm_axis_motor_by_source, - baseline_command_u8=baseline, - cycles=range(repetitions), - minimum_sources=int( - profile.minimum_palm_orientation_sources - ), - minimum_arc_rad=( - profile.palm_orientation_minimum_arc_rad - ), - maximum_rotation_orthogonal_rms_rad=math.radians( - float( - parameters[ - "active_maximum_rotation_orthogonal_rms_deg" - ] - ) - ), - maximum_command_distance_u8=( - profile.palm_orientation_maximum_command_distance_u8 - ), - ) - ) - failures = _quality_failures( - records_by_joint=records_by_joint, - baseline_records_by_joint=( - baseline_records_by_joint - if start.get("baseline_hysteresis_source") - == "dedicated_mid_sweep_hold" - else None - ), - profile=profile, - baseline=baseline, - fits=measured_fits, - axes=axes, - parameters=parameters, - ) - if failures: - raise ValueError("offline trajectory/axis validation failed: " + "; ".join(failures)) - - holdout_by_joint = { - name: joint_curve_holdout_errors( - training_fits[name], - [ - record - for record in curve_records_by_joint[name] - if int(record["cycle"]) == validation_cycle - ], - zero_command_u8=int( - baseline[profile.joint_specs[name].motor_index] - ), - ) - for name in profile.measured_joints - } - joint_dynamic_diagnostics: dict[str, dict[str, Any]] = {} - for name in profile.measured_joints: - cycle_travel: list[float] = [] - for cycle in range(repetitions): - cycle_fit = _fit_curve( - name, - [ - record - for record in records_by_joint[name] - if int(record["cycle"]) == cycle - ], - profile=profile, - baseline=baseline, - ) - cycle_travel.append( - abs( - float(cycle_fit.angle_rad[0]) - - float(cycle_fit.angle_rad[255]) - ) - ) - holdout = np.abs(np.asarray(holdout_by_joint[name], dtype=float)) - baseline_hysteresis = baseline_hysteresis_by_cycle_rad( - hysteresis_records(name), - zero_command_u8=int( - baseline[profile.joint_specs[name].motor_index] - ), - axis_xyz=_physical_rotation_axis_for_curve( - name, - records_by_joint[name], - measured_fits[name], - profile=profile, - baseline=baseline, - ), - ) - joint_dynamic_diagnostics[name] = { - "cycle_travel_rad": [float(value) for value in cycle_travel], - "cycle_travel_range_rad": max(cycle_travel) - min(cycle_travel), - "baseline_hysteresis_by_cycle_rad": [ - float(value) for value in baseline_hysteresis - ], - "holdout_cycle": validation_cycle, - "holdout_cycle_mae_rad": float(np.mean(holdout)), - "holdout_cycle_max_rad": float(np.max(holdout)), - } - trajectory_errors = np.abs( - np.asarray( - [value for values in holdout_by_joint.values() for value in values], - dtype=float, - ) - ) - maximum_validation_mae = math.radians( - float(parameters["maximum_validation_mae_deg"]) - ) - maximum_validation_p95 = math.radians( - float(parameters["maximum_validation_p95_deg"]) - ) - trajectory_mae = float(np.mean(trajectory_errors)) - trajectory_p95 = float(np.percentile(trajectory_errors, 95.0)) - if ( - trajectory_mae > maximum_validation_mae - or trajectory_p95 > maximum_validation_p95 - or ( - layout_id == G20_RIGHT_19_LAYOUT - and float(np.max(trajectory_errors)) - > math.radians( - float(parameters.get("maximum_validation_error_deg", 1.5)) - ) - ) - or ( - layout_id == G20_RIGHT_19_LAYOUT - and any( - float(np.mean(np.abs(np.asarray(values, dtype=float)))) - > maximum_validation_mae - or float(np.max(np.abs(np.asarray(values, dtype=float)))) - > math.radians( - float( - parameters.get("maximum_validation_error_deg", 1.5) - ) - ) - for values in holdout_by_joint.values() - ) - ) - ): - raise ValueError("isolated trajectory holdout failed") - - motor_by_joint = { - name: int(spec.motor_index) for name, spec in profile.joint_specs.items() - } - measured_endpoint_zero_offsets = ( - derive_right_19_mechanical_endpoint_offsets( - source_urdf, - command_fits, - maximum_offset_rad=math.radians( - float( - parameters.get( - "mechanical_endpoint_maximum_offset_deg", 5.0 - ) - ) - ), - ) - if layout_id == G20_RIGHT_19_LAYOUT - else {} - ) - endpoint_zero_offsets = { - name: value - for name, value in measured_endpoint_zero_offsets.items() - if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - } - post_solve_endpoint_offsets = { - name: value - for name, value in measured_endpoint_zero_offsets.items() - if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS - } - command_fits = clamp_runtime_fits_to_urdf_limits( - source_urdf, - command_fits, - endpoint_anchored_offsets_rad=endpoint_zero_offsets, - ) - joint_limits: dict[str, float] = {} - solve_arguments = { - "source_urdf": source_urdf, - "measurements": axes, - "palm_orientation_measurements": palm_orientation_measurements, - "motor_by_joint": motor_by_joint, - "maximum_offset_rad": math.radians(float(parameters["zero_maximum_offset_deg"])), - "finger_maximum_offset_rad": math.radians( - float(parameters.get("zero_finger_maximum_offset_deg", 3.0)) - ), - "joint_maximum_offset_rad": joint_limits, - "maximum_cycle_difference_rad": math.radians( - float(parameters["zero_maximum_axis_cycle_difference_deg"]) - ), - "maximum_axis_cone_mismatch_rad": math.radians( - float(parameters["zero_maximum_axis_cone_mismatch_deg"]) - ), - "maximum_systematic_axis_cone_bias_rad": math.radians( - float( - parameters.get( - "cross_view_roll_maximum_axis_difference_deg", 15.0 - ) - ) - ), - "maximum_observability_condition_number": float( - parameters.get( - "zero_maximum_observability_condition_number", 1.0e10 - ) - ), - "maximum_pose_axis_line_rms_m": float( - parameters["axis_maximum_pose_line_rms_m"] - ), - "maximum_validation_mae_rad": maximum_validation_mae, - "maximum_validation_p95_rad": maximum_validation_p95, - "maximum_validation_error_rad": ( - math.radians( - float(parameters.get("maximum_validation_error_deg", 1.5)) - ) - if layout_id == G20_RIGHT_19_LAYOUT - else None - ), - "maximum_confidence_half_width_rad": ( - math.radians( - float( - parameters.get( - "zero_maximum_confidence_half_width_deg", 0.5 - ) - ) - ) - if layout_id == G20_RIGHT_19_LAYOUT - else None - ), - "hand_type": side, - "tag_layout": layout_id, - "training_cycles": training_cycles, - "validation_cycle": validation_cycle, - "fixed_direct_zero_offsets_rad": { - **zero_profile.fixed_direct_zero_offsets_rad, - **endpoint_zero_offsets, - }, - "static_output_zero_offsets_rad": { - **zero_profile.static_output_zero_offsets_rad, - **post_solve_endpoint_offsets, - }, - } - def solve_with_product_partition( - fits: Mapping[str, JointCurveFit], - arguments: Mapping[str, Any], - ): - if layout_id != G20_RIGHT_19_LAYOUT: - return solve_urdf_zero_offsets(curves=fits, **arguments) - thumb_profile = get_right_19_thumb_zero_profile() - thumb_names = set(thumb_profile.direct_zero_joints) - thumb = solve_urdf_zero_offsets( - curves=fits, - **{ - **arguments, - "measurements": [ - item - for item in arguments["measurements"] - if item.joint in thumb_profile.axis_joints - ], - "joint_maximum_offset_rad": { - name: value - for name, value in arguments[ - "joint_maximum_offset_rad" - ].items() - if name in thumb_names - }, - "fixed_direct_zero_offsets_rad": { - name: value - for name, value in arguments[ - "fixed_direct_zero_offsets_rad" - ].items() - if name in thumb_names - }, - "static_output_zero_offsets_rad": { - name: value - for name, value in arguments[ - "static_output_zero_offsets_rad" - ].items() - if name in thumb_names - }, - "zero_profile": thumb_profile, - }, - ) - companion = solve_urdf_zero_offsets( - curves=fits, - **{ - **arguments, - "fixed_direct_zero_offsets_rad": { - **arguments["fixed_direct_zero_offsets_rad"], - **thumb.direct_offsets_rad, - }, - }, - ) - return merge_right_19_thumb_zero_result( - thumb_result=thumb, - companion_result=companion, - ) - - holdout_zero = solve_with_product_partition(training_fits, solve_arguments) - if not holdout_zero.passed: - failure = { - "reasons": dict(holdout_zero.failure_reasons), - "fitted_offsets_deg": { - name: math.degrees(value) - for name, value in holdout_zero.direct_offsets_rad.items() - }, - "cycle_offsets_deg": { - name: [math.degrees(value) for value in values] - for name, values in holdout_zero.cycle_offsets_rad.items() - }, - } - raise ValueError( - "isolated zero/URDF holdout failed: " - + json.dumps(failure, ensure_ascii=False, sort_keys=True) - ) - # Keep the model frozen after the holdout decision. Re-fitting with the - # held-out cycle would make the published artifact differ from the model - # that was independently evaluated above. - final_zero = holdout_zero - measured_fits = command_fits - for target, source_name in zero_profile.inherited_static_zero_joints.items(): - if final_zero.all_active_offsets_rad[target] != final_zero.direct_offsets_rad[source_name]: - raise ValueError(f"inherited static zero mismatch: {target} <- {source_name}") - for target in ( - set(zero_profile.inherited_zero_joints) - - set(zero_profile.inherited_static_zero_joints) - ): - if final_zero.all_active_offsets_rad[target] != 0.0: - raise ValueError(f"unobserved static zero must retain source CAD: {target}") - - validation_errors = [ - float(value) for values in holdout_by_joint.values() for value in values - ] - validation_errors.extend(float(value) for value in holdout_zero.validation_errors_rad) - stamp = session.name - final_json = session / ( - f"g20_{side}_{hand_serial}_calibration{output_suffix}.json" - ) - expected_urdf_name = ( - f"{source_urdf.stem}_zero_calibrated_{hand_serial}_{stamp}" - f"{output_suffix}.urdf" - ) - final_urdf = session / expected_urdf_name - report_path = session / ( - f"g20_{side}_{hand_serial}_offline_validation{output_suffix}.json" - ) - if write_outputs: - existing = [path for path in (final_json, final_urdf, report_path) if path.exists()] - if existing: - raise ValueError( - "refusing to overwrite replay outputs: " - + ", ".join(str(path) for path in existing) - ) - - source_hash_before = _sha256(source_urdf) - published_zero_offsets = { - name: round(float(value), 8) - for name, value in final_zero.all_active_offsets_rad.items() - } - urdf_offsets = ( - { - name: published_zero_offsets[name] - for name in zero_profile.direct_zero_joints - } - if layout_id == G20_RIGHT_19_LAYOUT - else published_zero_offsets - ) - with tempfile.TemporaryDirectory(prefix="offline_replay_", dir=session) as temporary: - candidate = write_zero_corrected_urdf( - source_urdf=source_urdf, - output_directory=temporary, - serial_number=hand_serial, - offsets_rad=urdf_offsets, - endpoint_anchored_offsets_rad=endpoint_zero_offsets, - timestamp=stamp, - ) - urdf_checks = _validate_corrected_urdf( - source=source_urdf, - corrected=candidate, - offsets=urdf_offsets, - endpoint_anchored_offsets=endpoint_zero_offsets, - axes=axes, - curves=training_fits, - motor_by_joint=motor_by_joint, - inherited_zero_joints=zero_profile.inherited_zero_joints, - ) - residual_zero = solve_with_product_partition( - # Re-check the corrected URDF in the exact frozen trajectory - # domain that produced the accepted zero offsets. Runtime - # requested-command curves are a separate transfer function and - # differ slightly at settled checkpoints; using them here creates - # a fictitious residual zero after a structurally correct write. - training_fits, - { - **solve_arguments, - "static_output_zero_offsets_rad": { - name: 0.0 - for name in ( - set(zero_profile.static_output_zero_offsets_rad) - | set(post_solve_endpoint_offsets) - ) - }, - "source_urdf": candidate, - "fixed_direct_zero_offsets_rad": { - name: 0.0 - for name in ( - set(zero_profile.fixed_direct_zero_offsets_rad) - | set(endpoint_zero_offsets) - ) - }, - }, - ) - maximum_residual_offset = max( - abs(float(value)) for value in residual_zero.direct_offsets_rad.values() - ) - if not residual_zero.passed or maximum_residual_offset > math.radians(0.3): - raise ValueError( - "written URDF retains a significant zero correction: " - + json.dumps( - { - "passed": residual_zero.passed, - "residual_offsets_deg": { - name: math.degrees(value) - for name, value in ( - residual_zero.direct_offsets_rad.items() - ) - }, - "failure_reasons": dict( - residual_zero.failure_reasons - ), - }, - ensure_ascii=False, - sort_keys=True, - ) - ) - candidate_hash = _sha256(candidate) - payload = build_compact_payload( - serial_number=hand_serial, - measured_fits=command_fits, - urdf_zero_offsets_rad=published_zero_offsets, - validation_errors_rad=validation_errors, - passed=True, - baseline=baseline, - side=side, - layout_id=layout_id, - zero_uncertainty_rad=( - final_zero.offset_confidence_half_width_rad - ), - zero_cycle_offsets_rad=final_zero.cycle_offsets_rad, - zero_observers=zero_profile.offset_observer_joint, - artifact_hashes=( - { - "source_urdf_sha256": source_hash_before, - "camera_extrinsics_sha256": _sha256(extrinsics_file), - "corrected_urdf_sha256": candidate_hash, - } - if layout_id == G20_RIGHT_19_LAYOUT - else None - ), - cross_view_roll_metrics=cross_view_roll_metrics, - joint_dynamic_diagnostics=joint_dynamic_diagnostics, - zero_geometry_diagnostics=( - { - "training_cycles": final_zero.training_cycles, - "validation_cycle": final_zero.validation_cycle, - "axis_line_rms_m": final_zero.axis_line_rms_m, - "validation_line_error_by_joint_m": ( - final_zero.validation_line_error_by_joint_m - ), - "observability_rank": final_zero.observability_rank, - "observability_parameter_count": ( - final_zero.observability_parameter_count - ), - "observability_condition_number": ( - final_zero.observability_condition_number - ), - "offset_covariance_rad2": ( - final_zero.offset_covariance_rad2 - ), - "palm_orientation_sources": [ - { - "source_joint": item.source_joint, - "model_joint": item.model_joint, - "cycle": item.cycle, - "observed_arc_rad": item.observed_arc_rad, - "rotation_orthogonal_rms_rad": ( - item.rotation_orthogonal_rms_rad - ), - "axis_estimator": item.axis_estimator, - "incremental_pair_count": ( - item.incremental_pair_count - ), - } - for item in palm_orientation_measurements - ], - "palm_orientation_rejections": dict( - palm_orientation_rejections - ), - } - if layout_id == G20_RIGHT_19_LAYOUT - else None - ), - ) - validate_compact_payload(payload) - if write_outputs: - if _sha256(source_urdf) != source_hash_before: - raise ValueError("source URDF changed during offline replay") - os.replace(candidate, final_urdf) - try: - # As in the live path, the atomically written JSON is the - # commit marker for the already validated URDF/JSON pair. - atomic_write_json(final_json, payload) - except Exception: - if layout_id == G20_RIGHT_19_LAYOUT and final_urdf.exists(): - rejected = final_urdf.with_suffix(".urdf.rejected") - final_urdf.replace(rejected) - raise - - if _sha256(source_urdf) != source_hash_before: - raise ValueError("source URDF changed during offline replay") - - report: dict[str, Any] = { - "passed": True, - "session_dir": str(session), - "side": side, - "serial_number": hand_serial, - "output_tag": output_tag, - "raw_samples_sha256": _sha256(raw_path), - "source_urdf": str(source_urdf), - "source_urdf_sha256": source_hash_before, - "joint_limits_deg": { - "finger_default": float( - parameters.get("zero_finger_maximum_offset_deg", 3.0) - ), - "thumb_default": float(parameters["zero_maximum_offset_deg"]), - }, - "static_zero_policy": ( - "visual_axes_plus_measured_mechanical_endpoint" - if layout_id == G20_RIGHT_19_LAYOUT - else "direct_measurements_only" - ), - "fixed_zero_offsets_deg": { - name: math.degrees(value) - for name, value in zero_profile.fixed_direct_zero_offsets_rad.items() - }, - "endpoint_anchored_zero_offsets_deg": { - name: math.degrees(value) - for name, value in endpoint_zero_offsets.items() - }, - "static_output_zero_offsets_deg": { - name: math.degrees(value) - for name, value in { - **zero_profile.static_output_zero_offsets_rad, - **post_solve_endpoint_offsets, - }.items() - }, - "direct_offsets_deg": { - name: math.degrees(value) - for name, value in final_zero.direct_offsets_rad.items() - }, - "model_base_translation_xyz_m": list( - final_zero.base_translation_xyz_m - ), - "model_base_quaternion_xyzw": list( - final_zero.base_quaternion_xyzw - ), - "cycle_offsets_deg": { - name: [math.degrees(value) for value in values] - for name, values in holdout_zero.cycle_offsets_rad.items() - }, - "axis_cone_mismatch_by_joint_deg": { - name: math.degrees(value) - for name, value in ( - holdout_zero.axis_cone_mismatch_by_joint_rad.items() - ) - }, - "axis_cone_bias_classification_by_joint": dict( - holdout_zero.axis_cone_bias_classification_by_joint - ), - "offset_uncertainty_deg": { - name: math.degrees(value) - for name, value in holdout_zero.offset_uncertainty_rad.items() - }, - "trajectory_holdout_mae_deg": math.degrees(trajectory_mae), - "trajectory_holdout_p95_deg": math.degrees(trajectory_p95), - "zero_holdout_error_deg": { - name: math.degrees(value) - for name, value in holdout_zero.validation_error_by_joint_rad.items() - }, - "zero_original_error_deg": { - name: math.degrees(value) - for name, value in holdout_zero.validation_original_error_by_joint_rad.items() - }, - "zero_improvement_95pct_lower_deg": { - name: math.degrees(value) - for name, value in ( - holdout_zero.validation_improvement_confidence_lower_rad.items() - ) - }, - "corrected_urdf_checks": { - **urdf_checks, - "maximum_residual_zero_offset_deg": math.degrees(maximum_residual_offset), - }, - "corrected_urdf_sha256": candidate_hash, - "final_json": str(final_json) if write_outputs else None, - "corrected_urdf": str(final_urdf) if write_outputs else None, - } - if write_outputs: - report["final_json_sha256"] = _sha256(final_json) - if _sha256(final_urdf) != candidate_hash: - raise ValueError("formal corrected URDF differs from validated candidate") - atomic_write_json(report_path, report) - report["validation_report"] = str(report_path) - return report - - -def main() -> None: - parser = argparse.ArgumentParser( - description="Replay and validate a registered hand-calibration session." - ) - parser.add_argument("session_dir") - parser.add_argument("--serial-number", default=None) - parser.add_argument("--config-file", default=None) - parser.add_argument("--write", action="store_true") - parser.add_argument( - "--output-tag", - default=None, - help="safe suffix for a replay variant; existing outputs are never overwritten", - ) - arguments = parser.parse_args() - result = replay_session( - arguments.session_dir, - serial_number=arguments.serial_number, - config_file=arguments.config_file, - write_outputs=arguments.write, - output_tag=arguments.output_tag, - ) - print(json.dumps(result, ensure_ascii=False, indent=2, sort_keys=True)) - - -if __name__ == "__main__": - main() +sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/product.py b/src/linkerhand_calibration/linkerhand_calibration/product.py index 72c56a5..77d8934 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/product.py +++ b/src/linkerhand_calibration/linkerhand_calibration/product.py @@ -17,9 +17,13 @@ import xml.etree.ElementTree as ET import yaml -from .compat import product_profile_key, resolve_renamed_package_path +from .compat import ( + product_profile_key, + resolve_legacy_profile_alias, + resolve_renamed_package_path, +) from .core import CalibrationProfile, ProfileKey -from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics +from .core.geometry import camera_info_fingerprint, load_camera_extrinsics from .models import RegisteredProfile, get_default_registry @@ -65,8 +69,11 @@ class ProductCalibrationContract: def get_product_calibration_contract( model: str, side: str, layout_id: str, revision: int = 1 ) -> ProductCalibrationContract: + key = resolve_legacy_profile_alias( + ProfileKey(model, side, layout_id, revision) + ) return ProductCalibrationContract( - get_default_registry().resolve(model, side, layout_id, revision) + get_default_registry().get(key) ) @@ -133,12 +140,18 @@ def _custom_pnp_tag_sizes_m_by_id( ) -> dict[int, float]: with path.open("r", encoding="utf-8") as stream: payload = _mapping(yaml.safe_load(stream), str(path)) - parameters = _mapping( - _mapping(payload.get("g20_calibration"), "g20_calibration").get( - "ros__parameters" - ), - "ros__parameters", - ) + candidates = [ + _mapping(node, str(name)).get("ros__parameters") + for name, node in payload.items() + if isinstance(node, Mapping) + and isinstance(node.get("ros__parameters"), Mapping) + and "tag_size_m" in node["ros__parameters"] + ] + if len(candidates) != 1: + raise ValueError( + "calibration config must contain exactly one Tag-size parameter set" + ) + parameters = _mapping(candidates[0], "ros__parameters") default = float(parameters.get("tag_size_m", 0.0)) ids = [int(value) for value in parameters.get("tag_size_override_ids", [])] sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])] @@ -181,6 +194,7 @@ class ProductConfig: workspace: Path schema_version: int profile_key: ProfileKey + namespace: str model: str side: str tag_layout: str @@ -228,6 +242,11 @@ def load_product_config( contract = get_product_calibration_contract( model, side, layout, profile_key.revision ) + namespace = str( + raw.get("namespace", contract.typed_profile.namespace) + ).strip() + if not namespace.startswith("/") or namespace.endswith("/"): + raise ValueError("namespace must be an absolute ROS namespace") can_interface = str(raw.get("can_interface", "")).strip() if not can_interface: raise ValueError("can_interface is required") @@ -301,7 +320,14 @@ def load_product_config( required_views = tuple(contract.views) if set(camera_raw) != set(required_views): raise ValueError(f"cameras must contain {'/'.join(required_views)}") - loaded_extrinsics = load_three_camera_extrinsics(extrinsics) + vision = contract.typed_profile.vision + loaded_extrinsics = load_camera_extrinsics( + extrinsics, + required_views=required_views, + reference_view=vision.extrinsic_reference_view, + quality_limits=vision.extrinsics_quality_limits, + minimum_capture_counts=vision.minimum_capture_counts, + ) cameras: dict[str, dict[str, str]] = {} serials: set[str] = set() for view in required_views: @@ -344,6 +370,7 @@ def load_product_config( workspace=root, schema_version=int(raw["schema_version"]), profile_key=profile_key, + namespace=namespace, model=model, side=side, tag_layout=layout, diff --git a/src/linkerhand_calibration/linkerhand_calibration/publication.py b/src/linkerhand_calibration/linkerhand_calibration/publication.py index 9ce7085..4f836d4 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/publication.py +++ b/src/linkerhand_calibration/linkerhand_calibration/publication.py @@ -1,914 +1,7 @@ -"""Validate and atomically publish one inseparable G20 JSON/URDF session.""" +"""One-release module alias for the relocated model publication policy.""" -from __future__ import annotations +import sys -from datetime import datetime, timezone -import copy -import json -import math -import os -from pathlib import Path -import re -from typing import Any, Mapping, Sequence -import xml.etree.ElementTree as ET +from .models.g20 import publication as _implementation -import numpy as np -from scipy.spatial.transform import Rotation - -from .full_hand import ( - G20_COMBINATION_REQUIRED_TARGET_KEYS, - G20_RIGHT_19_LAYOUT, - get_hand_calibration_profile, - validate_compact_payload, -) -from .product import ProductConfig, sha256_file -from .storage import atomic_write_json -from .core.urdf import UrdfCorrectionPlan, build_correction_plan -from .urdf_zero import ( - RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, - get_zero_calibration_profile, -) - - -PASSIVE_JOINTS = frozenset( - {"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"} -) -VISUALLY_MEASURED_FINGER_DIPS = frozenset( - {"index_dip", "middle_dip", "ring_dip", "pinky_dip"} -) -ACTIVE_ZERO_JOINTS = frozenset( - get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints -) -RETAINED_ACTIVE_ZERO_JOINTS = frozenset( - get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints -) - ACTIVE_ZERO_JOINTS -SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9 - - -def _load_json(path: Path) -> dict[str, Any]: - with path.open("r", encoding="utf-8") as stream: - payload = json.load(stream) - if not isinstance(payload, dict): - raise ValueError(f"{path} must contain a JSON object") - return payload - - -def _joint_blocks(text: str) -> dict[str, str]: - pattern = re.compile( - r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", - re.DOTALL, - ) - return {match.group("name"): match.group(0) for match in pattern.finditer(text)} - - -def _mask_origin_rpy(block: str) -> str: - return re.sub( - r"(]*\brpy\s*=\s*)([\"'])[^\"']*\2", - r"\1\2__CALIBRATED_RPY__\2", - block, - count=1, - ) - - -def _mask_active_origin_rpy_fields(text: str) -> str: - pattern = re.compile( - r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", - re.DOTALL, - ) - - def replace(match: re.Match[str]) -> str: - block = match.group(0) - if match.group("name") not in ACTIVE_ZERO_JOINTS: - return block - masked = _mask_origin_rpy(block) - if masked == block: - raise ValueError( - f"active joint {match.group('name')} has no origin.rpy" - ) - return masked - - return pattern.sub(replace, text) - - -def _mask_endpoint_coordinate_fields(text: str) -> str: - """Mask only limit/mimic fields induced by endpoint zero coordinates.""" - pattern = re.compile( - r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>.*?", - re.DOTALL, - ) - - def replace(match: re.Match[str]) -> str: - block = match.group(0) - name = match.group("name") - if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS: - return re.sub( - r"(]*\bupper\s*=\s*)([\"'])[^\"']*\2", - r"\1\2__CALIBRATED_UPPER__\2", - block, - count=1, - ) - mimic = re.search( - r"]*\bjoint\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>", - block, - re.DOTALL, - ) - if ( - mimic is not None - and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - ): - return re.sub( - r"(]*\boffset\s*=\s*)([\"'])[^\"']*\2", - r"\1\2__CALIBRATED_MIMIC_OFFSET__\2", - block, - count=1, - ) - return block - - return pattern.sub(replace, text) - - -def _triplet(value: str) -> np.ndarray: - result = np.asarray([float(item) for item in value.split()], dtype=float) - if result.shape != (3,) or not np.all(np.isfinite(result)): - raise ValueError(f"invalid URDF triplet: {value}") - return result - - -def _joint_elements(path: str | Path) -> dict[str, ET.Element]: - return { - str(joint.get("name")): joint - for joint in ET.parse(Path(path)).getroot().findall("joint") - } - - -def _verify_expected_origin_offsets( - source: str | Path, - corrected: str | Path, - expected_offsets_rad: Mapping[str, float], -) -> None: - offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()} - if set(offsets) != ACTIVE_ZERO_JOINTS or any( - not math.isfinite(value) for value in offsets.values() - ): - raise ValueError("expected offsets must contain all finite active static-zero values") - before = _joint_elements(source) - after = _joint_elements(corrected) - maximum_rotation_error = 0.0 - for name, original_joint in before.items(): - corrected_joint = after[name] - original_origin = original_joint.find("origin") - corrected_origin = corrected_joint.find("origin") - if original_origin is None or corrected_origin is None: - if original_origin is not corrected_origin: - raise ValueError(f"corrected URDF changed origin presence for {name}") - continue - original_xyz = _triplet(original_origin.get("xyz", "0 0 0")) - corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0")) - if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0): - raise ValueError(f"corrected URDF changed origin.xyz for {name}") - original_rotation = Rotation.from_euler( - "xyz", _triplet(original_origin.get("rpy", "0 0 0")) - ) - corrected_rotation = Rotation.from_euler( - "xyz", _triplet(corrected_origin.get("rpy", "0 0 0")) - ) - expected_rotation = original_rotation - if name in offsets: - axis_node = original_joint.find("axis") - axis = _triplet( - "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") - ) - norm = float(np.linalg.norm(axis)) - if norm <= 1.0e-12: - raise ValueError(f"source URDF joint {name} has a degenerate axis") - expected_rotation = expected_rotation * Rotation.from_rotvec( - axis / norm * offsets[name] - ) - error = float((expected_rotation.inv() * corrected_rotation).magnitude()) - maximum_rotation_error = max(maximum_rotation_error, error) - # Schema v4 intentionally stores zero offsets at eight decimal places. - # Older sessions wrote the corrected URDF from the full-precision solve, - # so comparing that file with the published JSON necessarily permits half - # of one last-place unit. This is about 2.9e-7 degrees and is far below - # any calibration or URDF numerical significance. - if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD: - raise ValueError( - "corrected URDF origin.rpy does not match the published zero offsets" - ) - - -def verify_corrected_urdf( - source: str | Path, - corrected: str | Path, - *, - expected_offsets_rad: Mapping[str, float] | None = None, - endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, - correction_plan: UrdfCorrectionPlan | None = None, -) -> tuple[str, ...]: - """Prove that only active-joint origin.rpy attributes changed. - - Passive joint blocks, including their mimic elements, are compared as raw - UTF-8 text so formatting and numeric spelling are protected as well. - """ - source_text = Path(source).read_text(encoding="utf-8") - corrected_text = Path(corrected).read_text(encoding="utf-8") - if correction_plan is not None: - correction_plan.verify_source(source) - if expected_offsets_rad is None: - raise ValueError( - "correction-plan validation requires expected offsets" - ) - correction_plan.authorize_offsets(expected_offsets_rad) - before = _joint_blocks(source_text) - after = _joint_blocks(corrected_text) - if set(before) != set(after): - raise ValueError("corrected URDF changed the joint set") - # Text patchers may normalize only the final newline. It has no URDF/XML - # semantics; every joint block and every non-rpy byte remains guarded - # below. - masked_source = _mask_endpoint_coordinate_fields( - _mask_active_origin_rpy_fields(source_text) - ).rstrip("\r\n") - masked_corrected = _mask_endpoint_coordinate_fields( - _mask_active_origin_rpy_fields(corrected_text) - ).rstrip("\r\n") - if masked_source != masked_corrected: - raise ValueError( - "corrected URDF changed content outside active origin.rpy or " - "endpoint-calibrated fields" - ) - changed: list[str] = [] - for name in before: - if before[name] == after[name]: - continue - if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_JOINTS: - raise ValueError(f"corrected URDF changed protected joint {name}") - if name in ACTIVE_ZERO_JOINTS: - changed.append(name) - if expected_offsets_rad is not None: - _verify_expected_origin_offsets(source, corrected, expected_offsets_rad) - endpoint_offsets = { - str(name): float(value) - for name, value in dict(endpoint_anchored_offsets_rad or {}).items() - } - if endpoint_offsets: - if correction_plan is not None and not set(endpoint_offsets).issubset( - correction_plan.endpoint_limit_joints - ): - raise ValueError( - "endpoint offsets are not authorized by the correction plan" - ) - source_joints = _joint_elements(source) - corrected_joints = _joint_elements(corrected) - for name, offset in endpoint_offsets.items(): - original_limit = source_joints[name].find("limit") - corrected_limit = corrected_joints[name].find("limit") - expected_upper = float(original_limit.get("upper")) - offset - # Schema v4 stores the corresponding zero at eight decimal - # places, while the URDF is written from the full-precision solve. - # Match the half-last-place tolerance used for origin rotations. - if ( - abs(float(corrected_limit.get("upper")) - expected_upper) - > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD - ): - raise ValueError(f"corrected URDF has invalid {name} upper limit") - for name in PASSIVE_JOINTS: - original_mimic = source_joints[name].find("mimic") - corrected_mimic = corrected_joints[name].find("mimic") - if original_mimic is None or corrected_mimic is None: - continue - source_name = str(original_mimic.get("joint")) - multiplier = float(original_mimic.get("multiplier", "1")) - expected = float(original_mimic.get("offset", "0")) - expected += multiplier * endpoint_offsets.get(source_name, 0.0) - # The active endpoint offset comes from schema-v4 JSON rounded to - # eight decimal places, while the URDF mimic was written from the - # full-precision solve. Propagate exactly the same accepted - # quantization through the mimic multiplier; retain a much - # smaller allowance for XML decimal formatting itself. - tolerance = ( - abs(multiplier) - * SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD - + 1.0e-12 - ) - if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance: - raise ValueError(f"corrected URDF has invalid {name} mimic offset") - return tuple(sorted(changed)) - - -def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]: - """Return every session-local mesh after proving it resolves safely.""" - path = Path(urdf).expanduser().resolve() - root = ET.parse(path).getroot() - resources: dict[str, Path] = {} - for mesh in root.findall(".//mesh"): - filename = str(mesh.get("filename", "")).strip() - if not filename or "://" in filename or filename.startswith("package:"): - continue - relative = Path(filename) - if relative.is_absolute() or ".." in relative.parts: - raise ValueError(f"URDF has an unsafe local mesh path: {filename}") - resolved = (path.parent / relative).resolve() - try: - resolved.relative_to(path.parent) - except ValueError as error: - raise ValueError(f"URDF mesh escapes the session: {filename}") from error - if not resolved.is_file(): - raise ValueError(f"URDF mesh resource is missing: {filename}") - resources[relative.as_posix()] = resolved - return dict(sorted(resources.items())) - - -def validate_runtime_curves_against_urdf_limits( - payload: Mapping[str, Any], runtime_urdf: str | Path -) -> None: - """Reject a curve whose q leaves its runtime URDF coordinate limits. - - Curves in schema v4 are expressed in the corrected URDF joint coordinate, - not in the source-CAD coordinate. Endpoint zero calibration can therefore - move a corrected coordinate limit while preserving the same physical CAD - endpoint; callers publishing a calibrated pair must pass that corrected - URDF here. - """ - validate_compact_payload(payload) - runtime_joints = _joint_elements(runtime_urdf) - for name, calibration in payload["joints"].items(): - joint = runtime_joints.get(str(name)) - if joint is None: - raise ValueError(f"runtime URDF is missing joint {name}") - limit = joint.find("limit") - if limit is None or limit.get("lower") is None or limit.get("upper") is None: - raise ValueError(f"runtime URDF joint {name} has no finite position limit") - lower = float(limit.get("lower")) - upper = float(limit.get("upper")) - if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: - raise ValueError(f"runtime URDF joint {name} has invalid position limits") - curve = np.asarray(calibration["angle_rad"], dtype=float) - minimum = float(np.min(curve)) - maximum = float(np.max(curve)) - tolerance = 1.0e-7 - if minimum < lower - tolerance or maximum > upper + tolerance: - raise ValueError( - f"runtime curve exceeds runtime URDF limit for {name}: " - f"[{minimum:.9g}, {maximum:.9g}] not within " - f"[{lower:.9g}, {upper:.9g}]" - ) - - -def clamp_compact_payload_to_urdf_limits( - payload: Mapping[str, Any], runtime_urdf: str | Path -) -> tuple[dict[str, Any], dict[str, int]]: - """Return a schema-preserving payload bounded in its runtime coordinates.""" - result = copy.deepcopy(dict(payload)) - validate_compact_payload(result) - runtime_joints = _joint_elements(runtime_urdf) - clipped_by_joint: dict[str, int] = {} - for name, calibration in result["joints"].items(): - joint = runtime_joints.get(str(name)) - limit = None if joint is None else joint.find("limit") - if ( - limit is None - or limit.get("lower") is None - or limit.get("upper") is None - ): - raise ValueError(f"runtime URDF joint {name} has no finite position limit") - lower = float(limit.get("lower")) - upper = float(limit.get("upper")) - if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper: - raise ValueError(f"runtime URDF joint {name} has invalid position limits") - source = np.asarray(calibration["angle_rad"], dtype=float) - bounded = np.clip(source, lower, upper) - count = int(np.count_nonzero(bounded != source)) - if count: - clipped_by_joint[str(name)] = count - calibration["angle_rad"] = [ - round(float(value), 8) for value in bounded - ] - validate_compact_payload(result) - return result, clipped_by_joint - - -def build_mujoco_validation_commands( - baseline: Sequence[int], -) -> dict[str, Any]: - if len(baseline) != 20: - raise ValueError("baseline must contain exactly 20 commands") - base = [int(value) for value in baseline] - if any(not 0 <= value <= 255 for value in base): - raise ValueError("baseline commands must be in [0, 255]") - - def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]: - values = list(base) - for index, value in changes.items(): - values[int(index)] = int(value) - return {"name": name, "label_zh": label, "command_u8": values} - - return { - "schema_version": 1, - "topic": "/g20/cb_right_hand_control_cmd", - "serial_number": "G20_RIGHT_001", - "poses": [ - pose("all_open", "全开", {}), - pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}), - pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}), - pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}), - pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}), - pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}), - pose( - "half_grip", - "四指半握", - {1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160}, - ), - pose( - "light_pinch", - "轻捏", - {0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176}, - ), - ], - } - - -def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path: - parent = Path(root).resolve() - target = Path(session).resolve() - if target.parent != parent: - raise ValueError("session pointer target must be a direct child of the serial root") - if not target.is_dir(): - raise ValueError(f"session directory does not exist: {target}") - if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}: - raise ValueError("unsupported session pointer name") - parent.mkdir(parents=True, exist_ok=True) - destination = parent / name - temporary = parent / f".{name}.{os.getpid()}.tmp" - if temporary.is_symlink(): - temporary.unlink() - elif temporary.exists(): - raise ValueError(f"temporary pointer path is occupied: {temporary}") - os.symlink(target.name, temporary, target_is_directory=True) - os.replace(temporary, destination) - return destination - - -def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]: - validate_compact_payload(payload) - result = { - name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"]) - for name in ACTIVE_ZERO_JOINTS - } - if set(result) != ACTIVE_ZERO_JOINTS or any( - not math.isfinite(value) for value in result.values() - ): - raise ValueError( - "payload does not contain all finite observable active zero offsets" - ) - return result - - -def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]: - """Validate a standalone thumb artifact and expand CAD finger zeros.""" - if ( - payload.get("schema_version") != 1 - or payload.get("artifact_type") - != "g20_right_standalone_thumb_calibration" - or payload.get("side") != "right" - or payload.get("non_thumb_zero_policy") != "source_cad_unchanged" - or not bool(payload.get("quality", {}).get("passed")) - ): - raise ValueError("standalone thumb JSON contract is invalid") - joints = payload.get("joints") - expected_joint_names = { - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - "thumb_ip", - } - if not isinstance(joints, Mapping) or set(joints) != expected_joint_names: - raise ValueError("standalone thumb JSON has the wrong joint set") - thumb_names = expected_joint_names - {"thumb_ip"} - result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS} - for name in thumb_names: - try: - value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"]) - except (KeyError, TypeError, ValueError) as error: - raise ValueError( - f"standalone thumb JSON is missing zero for {name}" - ) from error - curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"standalone thumb JSON curve is invalid: {name}") - result[name] = value - passive_curve = np.asarray( - joints["thumb_ip"].get("angle_rad", ()), dtype=float - ) - if ( - passive_curve.shape != (256,) - or not np.all(np.isfinite(passive_curve)) - or not bool(joints["thumb_ip"].get("passive")) - or any(not math.isfinite(value) for value in result.values()) - ): - raise ValueError("standalone thumb JSON passive curve is invalid") - return result - - -def verify_partial_scope_preserves_certified_zeros( - *, - scope: str, - source_session: str | Path, - serial_root: str | Path | None = None, - serial_number: str, - current_offsets: Mapping[str, float], -) -> None: - """Reject a partial artifact that changes any certified non-target zero.""" - selected = str(scope).strip().lower() - if selected not in {"thumb", "fingers"}: - return - source = Path(source_session).expanduser() - if not source.is_absolute(): - if serial_root is None: - raise ValueError( - "relative partial base session requires its serial root" - ) - source = Path(serial_root).expanduser() / source - source = source.resolve() - base_payload = _load_json( - source / f"g20_right_{serial_number}_calibration.json" - ) - base_offsets = ( - standalone_thumb_offsets(base_payload) - if base_payload.get("artifact_type") - == "g20_right_standalone_thumb_calibration" - else active_offsets(base_payload) - ) - preserve_thumb = selected == "fingers" - changed = { - name: abs(float(current_offsets[name]) - float(base_offsets[name])) - for name in ACTIVE_ZERO_JOINTS - if name.startswith("thumb_") == preserve_thumb - and abs(float(current_offsets[name]) - float(base_offsets[name])) - > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD - } - if changed: - raise ValueError( - f"partial {selected} calibration changed certified non-target " - "zeros: " - + ",".join(sorted(changed)) - ) - - -def compare_session_offsets( - first: Mapping[str, Any], - second: Mapping[str, Any], - *, - maximum_difference_rad: float, -) -> dict[str, float]: - first_standalone = ( - first.get("artifact_type") - == "g20_right_standalone_thumb_calibration" - ) - second_standalone = ( - second.get("artifact_type") - == "g20_right_standalone_thumb_calibration" - ) - if first_standalone != second_standalone: - raise ValueError("cannot compare standalone thumb and full-hand artifacts") - left = ( - standalone_thumb_offsets(first) - if first_standalone - else active_offsets(first) - ) - right = ( - standalone_thumb_offsets(second) - if second_standalone - else active_offsets(second) - ) - differences = { - name: abs(left[name] - right[name]) - for name in sorted(left) - } - failed = {name: value for name, value in differences.items() if value > maximum_difference_rad} - if failed: - details = ", ".join( - f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items() - ) - raise ValueError(f"independent-session static-zero mismatch: {details}") - return differences - - -def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]: - directory = Path(session).resolve() - urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf")) - if len(urdfs) != 1: - raise ValueError("session must contain exactly one corrected URDF") - return { - "json": directory / f"g20_right_{serial_number}_calibration.json", - "urdf": urdfs[0], - "summary": directory / "calibration_summary_zh.json", - "commands": directory / "mujoco_validation_commands.json", - "raw": directory / "raw_samples.jsonl", - "log": directory / "calibration.log", - } - - -def find_compatible_prior_session( - config: ProductConfig, - current: str | Path, - current_payload: Mapping[str, Any], -) -> tuple[Path | None, dict[str, float]]: - current_path = Path(current).resolve() - for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True): - if candidate.resolve() == current_path: - continue - json_path = candidate / f"g20_right_{config.serial_number}_calibration.json" - summary_path = candidate / "calibration_summary_zh.json" - if not json_path.is_file() or not summary_path.is_file(): - continue - try: - summary = _load_json(summary_path) - if not bool(summary.get("quality", {}).get("passed")): - continue - payload = _load_json(json_path) - differences = compare_session_offsets( - payload, - current_payload, - maximum_difference_rad=config.static_repeatability_rad, - ) - except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError): - continue - return candidate, differences - return None, {} - - -def _verify_combination_validation(combination: Mapping[str, Any]) -> None: - # The formal product uses the independently held-out fourth sweep cycle. - # The optional eight-pose check remains a developer diagnostic because the - # axis-line zero solve does not establish an absolute Cartesian hand-base - # transform. When explicitly enabled, retain its strict coverage and - # error checks. - if not bool(combination.get("enabled")): - return - position_p95 = float(combination.get("position_p95_m", float("inf"))) - orientation_p95 = float( - combination.get("orientation_p95_rad", float("inf")) - ) - required = {str(value) for value in combination.get("required_targets", ())} - expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS) - observations = combination.get("observation_counts") - validations = combination.get("validation_counts") - if required != expected: - raise ValueError("combination validation has the wrong required-target set") - if not isinstance(observations, Mapping) or not isinstance(validations, Mapping): - raise ValueError("combination validation is missing per-target coverage") - missing_observations = sorted( - key for key in expected if int(observations.get(key, 0)) < 2 - ) - missing_validations = sorted( - key for key in expected if int(validations.get(key, 0)) < 1 - ) - if missing_observations or missing_validations: - raise ValueError( - "combination validation target coverage is incomplete: " - f"observations={','.join(missing_observations) or '-'}; " - f"validations={','.join(missing_validations) or '-'}" - ) - if ( - not bool(combination.get("completed")) - or int(combination.get("completed_poses", 0)) != 8 - or position_p95 > 0.003 - or orientation_p95 > math.radians(2.0) - ): - raise ValueError( - "eight-pose four-finger combination validation is incomplete or failed" - ) - - -def finalize_session_artifacts( - config: ProductConfig, - session: str | Path, - *, - node_status: Mapping[str, Any], -) -> tuple[dict[str, Any], bool]: - """Create diagnostics and update latest_passed only after every guard.""" - directory = Path(session).resolve() - paths = session_artifact_paths(directory, config.serial_number) - for name in ("json", "urdf", "raw", "log"): - if not paths[name].is_file(): - raise ValueError(f"session is missing {name}: {paths[name]}") - payload = _load_json(paths["json"]) - standalone_thumb = bool( - payload.get("artifact_type") - == "g20_right_standalone_thumb_calibration" - ) - if standalone_thumb: - standalone_thumb_offsets(payload) - else: - validate_compact_payload(payload) - if payload.get("schema_version") != 4 or payload.get("side") != "right": - raise ValueError("runtime JSON is not the compact right-hand schema v4") - if not bool(payload.get("quality", {}).get("passed")): - raise ValueError("runtime JSON quality is not passed") - combination = node_status.get("combination_validation") - if not isinstance(combination, Mapping): - raise ValueError("node status is missing combination validation") - _verify_combination_validation(combination) - resume = node_status.get("resume", {}) - if not isinstance(resume, Mapping): - raise ValueError("node status has invalid resume provenance") - calibration_scope = str( - resume.get("recalibration_scope", "full") - ).strip().lower() - if calibration_scope not in {"full", "thumb", "fingers"}: - raise ValueError("node status has an unsupported recalibration scope") - recalibration_tasks = tuple( - str(value) for value in resume.get("recalibration_task_keys", ()) - ) - if calibration_scope == "thumb" and not standalone_thumb and ( - not bool(resume.get("used")) - or not str(resume.get("source_session", "")) - or len(recalibration_tasks) != 4 - or any("thumb_" not in name for name in recalibration_tasks) - ): - raise ValueError( - "thumb recalibration is missing its passed base-session provenance" - ) - if calibration_scope == "fingers" and ( - not bool(resume.get("used")) - or not str(resume.get("source_session", "")) - or len(recalibration_tasks) != 12 - or any("thumb_" in name for name in recalibration_tasks) - ): - raise ValueError( - "finger recalibration is missing its certified thumb-session " - "provenance" - ) - if standalone_thumb and calibration_scope != "thumb": - raise ValueError("standalone thumb artifact has the wrong node scope") - offsets = ( - standalone_thumb_offsets(payload) - if standalone_thumb - else active_offsets(payload) - ) - if calibration_scope != "full" and not standalone_thumb: - verify_partial_scope_preserves_certified_zeros( - scope=calibration_scope, - source_session=str(resume.get("source_session", "")), - serial_root=directory.parent, - serial_number=config.serial_number, - current_offsets=offsets, - ) - endpoint_offsets = { - name: offsets[name] - for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - } - typed_profile = config.calibration_contract.typed_profile - frozen_names = typed_profile.scope.frozen_joints[calibration_scope] - correction_plan = build_correction_plan( - typed_profile, - source_sha256=config.source_urdf_sha256, - scope=calibration_scope, - frozen_offsets_rad={name: offsets[name] for name in frozen_names}, - ) - changed_joints = verify_corrected_urdf( - config.source_urdf, - paths["urdf"], - expected_offsets_rad=offsets, - endpoint_anchored_offsets_rad=endpoint_offsets, - correction_plan=correction_plan, - ) - if standalone_thumb: - clipped_runtime_joints = {} - else: - payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits( - payload, paths["urdf"] - ) - if clipped_runtime_joints: - atomic_write_json(paths["json"], payload) - validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) - mesh_resources = verify_urdf_mesh_resources(paths["urdf"]) - mesh_hashes = { - name: sha256_file(path) for name, path in mesh_resources.items() - } - commands = ( - { - "artifact_type": "standalone_thumb_no_full_hand_validation", - "poses": [], - } - if standalone_thumb - else build_mujoco_validation_commands(payload["baseline_command_u8"]) - ) - atomic_write_json(paths["commands"], commands) - - prior, differences = find_compatible_prior_session(config, directory, payload) - release_ready = config.required_independent_passes == 1 or prior is not None - quality = dict(payload["quality"]) - preserved_certified_zeros = sorted( - name - for name in ACTIVE_ZERO_JOINTS - if ( - calibration_scope == "thumb" - and not name.startswith("thumb_") - ) - or ( - calibration_scope == "fingers" - and name.startswith("thumb_") - ) - ) - if standalone_thumb: - preserved_certified_zeros = [] - calibrated_zero_joints = ( - sorted( - name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_") - ) - if standalone_thumb - else sorted(ACTIVE_ZERO_JOINTS) - ) - summary: dict[str, Any] = { - "schema_version": 1, - "serial_number": config.serial_number, - "session_id": f"{config.serial_number}_{directory.name}", - "calibration_scope": calibration_scope, - "inherited_base_session": ( - None - if calibration_scope == "full" or standalone_thumb - else str(resume.get("source_session")) - ), - "freshly_calibrated_task_keys": list(recalibration_tasks), - "preserved_certified_zero_joints": preserved_certified_zeros, - "result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION", - "quality": quality, - "runtime_limit_clipped_bins": clipped_runtime_joints, - "runtime_curve_domain": ( - "thumb_diagnostic_only" - if standalone_thumb - else "requested_command_u8" - ), - "static_zero_calibrated_joints": calibrated_zero_joints, - "non_thumb_zero_policy": ( - "source_cad_unchanged" if standalone_thumb else None - ), - "retained_active_urdf_zero_joints": sorted( - RETAINED_ACTIVE_ZERO_JOINTS - ), - "retained_passive_urdf_joints": sorted(PASSIVE_JOINTS), - "mimic_derived_dynamic_joints": [], - "visually_measured_passive_joints": sorted( - VISUALLY_MEASURED_FINGER_DIPS - ), - "changed_urdf_joint_origins": list(changed_joints), - "hashes": { - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - "calibration_config_sha256": config.calibration_config_sha256, - "corrected_urdf_sha256": sha256_file(paths["urdf"]), - "mesh_resources_sha256": mesh_hashes, - "calibration_json_sha256": sha256_file(paths["json"]), - "raw_samples_sha256": sha256_file(paths["raw"]), - }, - "holdout": { - "training_cycles": [0, 1, 2], - "validation_cycle": 3, - "isolated": True, - }, - "combination_validation": dict(combination), - "formal_release": { - "passed": release_ready, - "required_independent_passes": config.required_independent_passes, - "comparison_session": None if prior is None else prior.name, - "maximum_static_difference_deg": ( - None if not differences else math.degrees(max(differences.values())) - ), - }, - "node_status": dict(node_status), - "created_at_utc": datetime.now(timezone.utc).isoformat(), - "artifacts": {name: path.name for name, path in paths.items()}, - } - atomic_write_json(paths["summary"], summary) - # Recompute hashes after all files exist and validate the pair once more - # immediately before the one atomic publication operation. - if sha256_file(config.source_urdf) != config.source_urdf_sha256: - raise ValueError("source URDF changed during calibration") - verify_corrected_urdf( - config.source_urdf, - paths["urdf"], - expected_offsets_rad=offsets, - endpoint_anchored_offsets_rad=endpoint_offsets, - ) - # Revalidate the same coordinate contract immediately before publication: - # verify_corrected_urdf proves that corrected endpoint limits map back to - # the original physical CAD endpoints, while the runtime curves must stay - # inside those corrected-coordinate limits. - if not standalone_thumb: - validate_runtime_curves_against_urdf_limits(payload, paths["urdf"]) - final_mesh_hashes = { - name: sha256_file(path) - for name, path in verify_urdf_mesh_resources(paths["urdf"]).items() - } - if final_mesh_hashes != mesh_hashes: - raise ValueError("URDF mesh resources changed during publication") - if release_ready: - atomic_session_pointer( - config.session_root, - "latest_thumb_passed" if standalone_thumb else "latest_passed", - directory, - ) - return summary, release_ready +sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py index 5a82088..580fb6c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py @@ -1 +1 @@ -"""ROS message/service shells are migrated here after controller extraction.""" +"""ROS message/service shells selected through the local profile registry.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/calibration.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/calibration.py new file mode 100644 index 0000000..b167647 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/calibration.py @@ -0,0 +1,23 @@ +"""Generic ROS-node dispatcher selected by the registered profile identity.""" + +from __future__ import annotations + +import argparse +import sys + +from ...compat import legacy_default_profile_key +from ...core import ProfileKey +from ...models import get_default_registry + + +def main(args: list[str] | None = None) -> None: + arguments = list(sys.argv[1:] if args is None else args) + selector = argparse.ArgumentParser(add_help=False) + selector.add_argument("--profile-id", default=None) + selected, remaining = selector.parse_known_args(arguments) + key = ( + legacy_default_profile_key() + if selected.profile_id is None + else ProfileKey.parse(selected.profile_id) + ) + get_default_registry().get(key).engine.node_main(remaining) diff --git a/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py b/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py index e7ffa2d..9465c69 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py @@ -1,13060 +1,7 @@ -"""One-command three-camera calibration for either complete G20 hand.""" +"""One-release module alias for the relocated model-specific ROS node.""" -from __future__ import annotations +import sys -from collections import deque -from dataclasses import dataclass, field, replace -import hashlib -import json -import math -from pathlib import Path -import random -import re -import time -import traceback -from typing import Any, Mapping, Sequence +from .models.g20 import node as _implementation -import numpy as np -import rclpy -from apriltag_msgs.msg import AprilTagDetectionArray -from rclpy.exceptions import ParameterUninitializedException -from rclpy.node import Node -from rclpy.parameter import Parameter -from rclpy.qos import qos_profile_sensor_data -from scipy.spatial.transform import Rotation -from sensor_msgs.msg import CameraInfo, JointState -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from .acquisition import ( - StateSample, - TagQuality, - interpolate_state_u8, - required_resume_views, - tag_quality_is_valid, - update_pnp_reset_watchdog, -) -from .core import ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - robust_rotation_summary, -) -from .core.urdf import build_correction_plan -from .extrinsics import ( - ThreeCameraExtrinsics, - camera_info_fingerprint, - load_three_camera_extrinsics, - matrix_payload, - transform_matrix, -) -from .full_hand import ( - G20_COMBINATION_REQUIRED_TARGET_KEYS, - G20_REFERENCE_THUMB_CMC_JOINTS, - G20_RIGHT_19_LAYOUT, - LEFT_HAND_PROFILE, - RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, - RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, - THREE_CAMERA_BASELINE_COMMAND, - HandCalibrationProfile, - JointCurveFit, - PalmAxisObserver, - SweepSpec, - build_calibration_motion_command, - build_calibration_preparation_waypoints, - build_calibration_return_waypoints, - build_calibration_speed_profile, - build_compact_payload, - calibration_auxiliary_commands, - canonical_zero_direction, - clamp_runtime_fits_to_urdf_limits, - compare_cross_view_roll_curves, - cross_view_roll_diagnostic_metrics, - derive_mimic_passive_fits, - fit_joint_image_curve, - get_hand_calibration_profile, -) -from .hikrobot_camera import configure_fastdds_large_image_transport -from .models.g20.command_layout import G20_COMMAND_NAMES as COMMAND_NAMES -from .pnp import ( - SquareTagGroupPoseTracker, - SquareTagPose, - SquareTagPoseTracker, -) -from .product import get_product_calibration_contract -from .sample_schema import ( - SampleDataContractError, - canonical_sample_record, - fitting_sample_record, - fitting_sample_records, - validate_sample_records, -) -from .storage import append_jsonl, append_jsonl_many, atomic_write_json -from .three_camera_diagnostics import render_three_camera_status_text_zh -from .urdf_zero import ( - LEFT_ZERO_PROFILE, - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, - RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, - RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, - JointAxisMeasurement, - PalmOrientationMeasurement, - UrdfKinematicModel, - ZeroCalibrationProfile, - ZeroSolveResult, - anchor_right_19_mechanical_endpoint_curves, - axis_line_cycle_rms_m, - axis_line_uses_depth_free_interpretation_plane, - baseline_hysteresis_by_cycle_rad, - circle_direction_is_constrained, - cross_view_side_line_source, - fit_joint_axis_measurement, - fit_partial_palm_orientation_measurements, - fit_rotation_joint_curve, - derive_right_19_mechanical_endpoint_offsets, - expand_right_19_thumb_zero_result_with_cad_fingers, - joint_curve_holdout_errors, - maximum_axis_line_cycle_spread_m, - measure_joint_curve_observation, - merge_right_19_thumb_zero_result, - refit_axis_line_group_with_shared_radius, - select_cross_view_roll_direction_source, - solve_urdf_zero_offsets, - get_zero_calibration_profile, - get_right_19_thumb_zero_profile, - write_zero_corrected_urdf, - with_depth_free_axis_projection, -) - - -STATE_PREFLIGHT = "PREFLIGHT" -STATE_WAIT_START = "WAIT_START" -STATE_IMPORTING_BASE = "IMPORTING_BASE" -STATE_REVALIDATING_INHERITED = "REVALIDATING_INHERITED" -STATE_RETURN_BASELINE = "RETURN_BASELINE" -STATE_PREPARE_SWEEP = "PREPARE_SWEEP" -STATE_SWEEP = "SWEEP" -STATE_FITTING = "FITTING" -STATE_VALIDATION_MOVE = "VALIDATION_MOVE" -STATE_VALIDATION_CAPTURE = "VALIDATION_CAPTURE" -STATE_PAUSED = "PAUSED" -STATE_ABORTED = "ABORTED" -STATE_COMPLETE = "COMPLETE" - -STEADY_COMMAND_CHECKPOINTS: tuple[int, ...] = ( - 255, 224, 192, 160, 128, 96, 64, 32, 0 -) - -COMBINATION_TAG_TARGETS_BY_VIEW: Mapping[ - str, tuple[tuple[str, str], ...] -] = { - "front": tuple( - (name, name) - for name in ( - "thumb_cmc_pitch", "thumb_mcp", "thumb_ip", - "index_mcp_roll", "middle_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", - ) - ), - "side": tuple( - (f"{finger}_{joint}", f"{finger}_{joint}") - for finger in ("index", "middle", "ring", "pinky") - for joint in ("pip", "dip") - ), - # The top-view Tag observes yaw but is mounted on the final CMC link, - # downstream of pitch in the URDF chain. - "top": (("thumb_cmc_yaw", "thumb_cmc_pitch"),), -} - -COMBINATION_BASE_OBSERVER_BY_VIEW = { - "front": "thumb_cmc_pitch", - "side": "index_pip", - "top": "thumb_cmc_yaw", -} - -COMBINATION_BASE_ROLE_BY_VIEW = { - "front": "front_base", - "side": "side_base", - "top": "top_base", -} - - -def combination_target_coverage( - observation_counts: Mapping[str, int], - validation_counts: Mapping[str, int], -) -> dict[str, Any]: - required = G20_COMBINATION_REQUIRED_TARGET_KEYS - observations = { - key: int(observation_counts.get(key, 0)) for key in required - } - validations = { - key: int(validation_counts.get(key, 0)) for key in required - } - missing_observations = sorted( - key for key, count in observations.items() if count < 2 - ) - missing_validations = sorted( - key for key, count in validations.items() if count < 1 - ) - return { - "required_targets": list(required), - "minimum_observations_per_target": 2, - "minimum_validations_per_target": 1, - "observation_counts": observations, - "validation_counts": validations, - "missing_observation_targets": missing_observations, - "missing_validation_targets": missing_validations, - "coverage_passed": not missing_observations and not missing_validations, - } - - -def _combination_observable_joints( - profile: HandCalibrationProfile, - view: str, - roles: Sequence[str], -) -> tuple[str, ...]: - """Return combination targets measurable from the currently visible Tags. - - Combination poses deliberately do not require all four adjacent fingers - to be visible at once. A joint is usable only when both Tags that define - its relative pose are present in the same camera frame. - """ - visible = {str(role) for role in roles} - targets = { - observation_name - for observation_name, _ in COMBINATION_TAG_TARGETS_BY_VIEW[view] - } - return tuple( - name - for name, spec in profile.record_specs.items() - if name in targets - and spec.measured - and spec.view == view - and spec.parent_role in visible - and spec.child_role in visible - ) - - -def _overall_progress( - state: str, - *, - scan_progress: float, - validation_index: int = 0, - validation_total: int = 0, -) -> float: - """Keep 100% reserved for a validated, written calibration result.""" - scan = float(np.clip(scan_progress, 0.0, 1.0)) - if state == STATE_COMPLETE: - return 1.0 - if state in {STATE_FITTING}: - return 0.92 - if state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE}: - validation = ( - 0.0 - if validation_total <= 0 - else float(np.clip(validation_index / validation_total, 0.0, 1.0)) - ) - return 0.92 + 0.07 * validation - return min(0.90, 0.90 * scan) - - -def _stamp_ns(stamp: Any) -> int: - return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) - - -def _safe_name(value: str) -> str: - safe = re.sub(r"[^A-Za-z0-9_.-]+", "_", str(value).strip()) - return safe or "UNSET" - - -def _sweep_storage_key(spec: SweepSpec) -> str | int: - """Keep legacy motor-keyed retry state while separating product tasks.""" - return spec.key if spec.task_name else int(spec.motor_index) - - -def _sweep_views( - profile: HandCalibrationProfile, spec: SweepSpec -) -> tuple[str, ...]: - """Return every camera that contributes a joint to one physical sweep.""" - views: list[str] = [] - for joint_name in spec.joints: - joint = profile.record_specs[joint_name] - if joint.view is not None and joint.view not in views: - views.append(joint.view) - return tuple(views or [spec.view]) - - -def _palm_axis_observer_for_sweep( - profile: HandCalibrationProfile, - spec: SweepSpec, - view: str, -) -> PalmAxisObserver | None: - """Return the one non-blocking palm observer attached to this task/view.""" - matches = tuple( - observer - for observer in profile.palm_axis_observers - if observer.task_name == spec.key and observer.view == str(view) - ) - if len(matches) > 1: - raise ValueError( - f"multiple palm-axis observers configured for {spec.key}:{view}" - ) - return matches[0] if matches else None - - -def _palm_axis_observer_schema( - profile: HandCalibrationProfile, -) -> list[dict[str, Any]]: - return [ - { - "source_name": item.source_name, - "task_name": item.task_name, - "view": item.view, - "parent_role": item.parent_role, - "child_role": item.child_role, - "model_joint": item.model_joint, - "motor_index": int(item.motor_index), - } - for item in profile.palm_axis_observers - ] - - -def _palm_axis_resume_policy( - profile: HandCalibrationProfile, - session_start: Mapping[str, Any], - compatibility_tokens: frozenset[str] | None = None, -) -> tuple[bool, tuple[str, ...]]: - """Validate optional palm-axis checkpoint data when a model uses it.""" - capability = "palm_axis_relative_motion_v3" - previous = {str(value) for value in session_start.get("capabilities", [])} - current = ( - frozenset(profile.capabilities) - if compatibility_tokens is None - else frozenset(compatibility_tokens) - ) - if not profile.palm_axis_observers and capability not in current: - return True, () - required_previous = set(current) - {capability} - if not required_previous.issubset(previous): - raise ValueError("resume checkpoint lacks required capabilities") - if capability in previous: - if session_start.get("palm_axis_observers") != ( - _palm_axis_observer_schema(profile) - ): - raise ValueError("resume palm-axis observer schema differs") - return True, () - # A new side-channel version changes the zero solver's observation model. - # Do not combine its fresh CMC axes with any trajectory captured under the - # previous PnP transaction semantics. This full invalidation happens only - # once; same-version checkpoints keep their normal task-level resume. - return False, tuple(spec.key for spec in profile.sweep_specs) - - -def _node_profile(node: Any) -> HandCalibrationProfile: - return getattr(node, "profile", LEFT_HAND_PROFILE) - - -def _command_names(node: Any) -> tuple[str, ...]: - profile = _node_profile(node) - names = tuple(getattr(profile, "command_names", ()) or COMMAND_NAMES) - if not names or len(set(names)) != len(names): - raise ValueError("product command names must be non-empty and unique") - return names - - -def _command_count(node: Any) -> int: - return len(_command_names(node)) - - -def _sweep_joints_for_view( - profile: HandCalibrationProfile, - spec: SweepSpec, - view: str, -) -> tuple[str, ...]: - return tuple( - joint_name - for joint_name in spec.joints - if profile.record_specs[joint_name].view == str(view) - ) - - -def resume_tasks_invalidated_by_tag_size_changes( - profile: HandCalibrationProfile, - previous_sizes_m_by_id: Mapping[int, float], - current_sizes_m_by_id: Mapping[int, float], -) -> tuple[tuple[int, ...], tuple[str, ...]]: - """Localise a corrected physical Tag size to dependent motion tasks. - - PnP translations stored in a checkpoint are already scaled by the Tag - size used during acquisition and cannot be repaired exactly afterwards. - A size-only product correction therefore invalidates every task that used - one of those Tags, while complete tasks whose required Tags are unchanged - remain valid sparse checkpoints. - """ - previous = { - int(key): float(value) - for key, value in previous_sizes_m_by_id.items() - } - current = { - int(key): float(value) - for key, value in current_sizes_m_by_id.items() - } - if set(previous) != set(current): - raise ValueError("resume Tag-size maps contain different Tag IDs") - if any( - not math.isfinite(value) or value <= 0.0 - for value in (*previous.values(), *current.values()) - ): - raise ValueError("resume Tag sizes must be finite and positive") - changed_ids = { - tag_id - for tag_id in current - if not math.isclose( - previous[tag_id], current[tag_id], rel_tol=0.0, abs_tol=1.0e-12 - ) - } - affected: list[str] = [] - base_role_by_view = { - "front": "front_base", - "side": "side_base", - "top": "top_base", - } - for spec in profile.sweep_specs: - used_ids: set[int] = set() - for view in _sweep_views(profile, spec): - used_ids.add(int(profile.view_tags[view][base_role_by_view[view]])) - for joint_name in _sweep_joints_for_view(profile, spec, view): - joint = profile.record_specs[joint_name] - for role in (joint.parent_role, joint.child_role): - if role is not None: - used_ids.add(int(profile.view_tags[view][role])) - if used_ids & changed_ids: - affected.append(spec.key) - return tuple(sorted(changed_ids)), tuple(affected) - - -def _fit_retry_joint_names( - profile: HandCalibrationProfile, - spec: SweepSpec, - failures: Sequence[Mapping[str, Any]], -) -> set[str]: - """Resolve the independently failed measurements in a physical task.""" - selected: set[str] = set() - validation_sources = profile.axis_validation_sources or {} - for failure in failures: - quality_sources = failure.get("quality_source_joints", ()) - if ( - isinstance(quality_sources, Sequence) - and not isinstance(quality_sources, (str, bytes)) - ): - selected_sources = { - str(source) - for source in quality_sources - if str(source) in spec.joints - } - if selected_sources: - selected.update(selected_sources) - continue - name = str(failure.get("joint", "")) - if str(failure.get("metric", "")) == "cross_view_roll_curve": - name = str(validation_sources.get(name, name)) - if name in spec.joints: - selected.add(name) - return selected or set(spec.joints) - - -def _isolated_axis_cycle_outliers( - axes: Sequence[np.ndarray], limit_rad: float -) -> set[int]: - """Return one unambiguous outlier outside an all-other-cycle cluster. - - At least three mutually consistent inlier cycles are required. Ambiguous - or generally scattered axes deliberately return an empty set so callers - retain the conservative full-task retry. - """ - if len(axes) < 4: - return set() - candidates: list[tuple[float, int]] = [] - for excluded in range(len(axes)): - inliers = [ - np.asarray(axis, dtype=float) - for index, axis in enumerate(axes) - if index != excluded - ] - consistent = True - maximum_inlier_difference = 0.0 - for left_index, left in enumerate(inliers): - for right in inliers[left_index + 1 :]: - difference = math.acos( - abs(float(np.clip(left @ right, -1.0, 1.0))) - ) - maximum_inlier_difference = max( - maximum_inlier_difference, difference - ) - if difference > float(limit_rad): - consistent = False - break - if not consistent: - break - if consistent: - candidates.append((maximum_inlier_difference, excluded)) - if not candidates: - return set() - candidates.sort() - best_spread, best_excluded = candidates[0] - # A gradual end-to-end drift can produce two different all-but-one - # subsets just under the formal limit. Localize only when one three-cycle - # cluster is substantially tighter than the acceptance band. - if best_spread > 0.5 * float(limit_rad): - return set() - if ( - len(candidates) > 1 - and abs(candidates[1][0] - best_spread) <= math.radians(0.01) - ): - return set() - return {best_excluded} - - -def _isolated_scalar_cycle_outlier( - values: Sequence[float], limit_rad: float -) -> set[int]: - """Return one clearly separated scalar outlier among three rounds. - - Zero-offset fits have three training cycles, so the axis helper above - cannot localize them (it deliberately requires three inliers). Use the - unique closest pair as a two-cycle consensus only when that pair is tight - and clearly better than either alternative. Symmetric drift and - generally scattered values retain the conservative full-task retry. - """ - samples = np.asarray(values, dtype=float) - limit = float(limit_rad) - if ( - samples.ndim != 1 - or samples.size != 3 - or not np.all(np.isfinite(samples)) - or not math.isfinite(limit) - or limit <= 0.0 - ): - return set() - pair_candidates = sorted( - ( - abs(float(samples[left] - samples[right])), - left, - right, - ) - for left in range(samples.size) - for right in range(left + 1, samples.size) - ) - pair_spread, left, right = pair_candidates[0] - second_pair_spread = pair_candidates[1][0] - overall_spread = pair_candidates[-1][0] - if overall_spread <= limit or pair_spread > 0.5 * limit: - return set() - # Do not arbitrarily pick one side of a gradual or nearly symmetric - # three-cycle drift. The winning pair must be materially tighter than - # the next candidate, not merely win due to floating-point noise. - if second_pair_spread - pair_spread < 0.1 * limit: - return set() - excluded = [ - index for index in range(samples.size) if index not in {left, right} - ] - if len(excluded) != 1: - return set() - outlier = excluded[0] - inlier_center = 0.5 * float(samples[left] + samples[right]) - if abs(float(samples[outlier]) - inlier_center) < 0.75 * limit: - return set() - return {outlier} - - -def _thumb_yaw_zero_repeatability_failures( - zero_result: ZeroSolveResult, - *, - maximum_cycle_range_rad: float, - maximum_confidence_half_width_rad: float, -) -> list[dict[str, Any]]: - """Apply a stricter, retry-aware publication gate to thumb CMC yaw.""" - joint_name = "thumb_cmc_yaw" - cycle_offsets = tuple( - float(value) - for value in zero_result.cycle_offsets_rad.get(joint_name, ()) - ) - failures: list[dict[str, Any]] = [] - if len(cycle_offsets) >= 2 and all( - math.isfinite(value) for value in cycle_offsets - ): - cycle_range = max(cycle_offsets) - min(cycle_offsets) - if cycle_range > float(maximum_cycle_range_rad): - failure: dict[str, Any] = { - "joint": joint_name, - "metric": "zero_cycle_offset_range_deg", - "actual": round(math.degrees(cycle_range), 6), - "limit": round( - math.degrees(maximum_cycle_range_rad), 6 - ), - "comparison": "maximum", - "cycle_offset_deg": [ - round(math.degrees(value), 6) - for value in cycle_offsets - ], - } - outliers = _isolated_scalar_cycle_outlier( - cycle_offsets, maximum_cycle_range_rad - ) - if outliers: - outlier = next(iter(outliers)) - failure["cycle"] = outlier + 1 - failure["inlier_cycles"] = [ - index + 1 - for index in range(len(cycle_offsets)) - if index != outlier - ] - failures.append(failure) - - # A range failure is more actionable: it can often identify exactly one - # acquisition cycle. Avoid adding a second, non-localized failure that - # would unnecessarily turn that retry back into a full yaw rescan. - if failures: - return failures - confidence = zero_result.offset_confidence_half_width_rad.get(joint_name) - if ( - confidence is not None - and math.isfinite(float(confidence)) - and float(confidence) > float(maximum_confidence_half_width_rad) - ): - failures.append( - { - "joint": joint_name, - "metric": "zero_confidence_95_half_width_deg", - "actual": round(math.degrees(float(confidence)), 6), - "limit": round( - math.degrees(maximum_confidence_half_width_rad), 6 - ), - "comparison": "maximum", - } - ) - return failures - - -def _previous_passed_joint_zero_offset( - session_dir: Path, serial_number: str, joint_name: str -) -> tuple[Path, float] | None: - """Read one zero from the previous passed sibling session, if present.""" - pointer = session_dir.parent / "latest_passed" - try: - previous = pointer.resolve(strict=True) - except OSError: - return None - if previous == session_dir or previous.parent != session_dir.parent: - return None - payload_path = previous / ( - f"g20_right_{_safe_name(serial_number)}_calibration.json" - ) - try: - payload = json.loads(payload_path.read_text(encoding="utf-8")) - offset = float( - payload["joints"][joint_name]["zero_angles"][ - "urdf_zero_offset_rad" - ] - ) - except (KeyError, OSError, TypeError, ValueError, json.JSONDecodeError): - return None - if not math.isfinite(offset): - return None - return previous, offset - - -def _isolated_axis_line_cycle_outliers( - measurements: Sequence[JointAxisMeasurement], limit_m: float -) -> set[int]: - """Return one cycle whose removal restores line-position repeatability. - - Four independently fitted near-parallel lines can contain one bad PnP - branch or one mechanically unsettled pass. When three cycles form a - clearly tighter cluster below the unchanged formal RMS limit, reacquire - only the excluded cycle. Ambiguous gradual drift still returns no - outlier and therefore keeps the conservative full four-cycle retry. - """ - if len(measurements) < 4 or float(limit_m) <= 0.0: - return set() - full_rms = axis_line_cycle_rms_m(measurements) - if full_rms <= float(limit_m): - return set() - candidates: list[tuple[float, int]] = [] - for excluded in range(len(measurements)): - inliers = [ - measurement - for index, measurement in enumerate(measurements) - if index != excluded - ] - inlier_rms = axis_line_cycle_rms_m(inliers) - if inlier_rms <= float(limit_m): - candidates.append((inlier_rms, excluded)) - if not candidates: - return set() - candidates.sort() - best_rms, best_excluded = candidates[0] - # Require a strong three-cycle cluster and a unique choice. This rejects - # the common gradual-drift case where excluding either endpoint happens - # to move a marginal RMS just below the gate. - if best_rms > 0.75 * float(limit_m): - return set() - if ( - len(candidates) > 1 - and candidates[1][0] - best_rms <= 0.10 * float(limit_m) - ): - return set() - return {best_excluded} - - -def _preserve_pnp_task_reference_for_sweep( - item: SweepItem, *, is_fit_retry: bool, has_precheck_anchor: bool -) -> bool: - """Keep the accepted IPPE branch after precheck and across retries.""" - return bool( - not item.precheck - and ( - item.cycle > 0 - or bool(is_fit_retry) - or bool(has_precheck_anchor) - ) - ) - - -def _requires_pnp_tracker_reset_for_sweep( - profile: HandCalibrationProfile, - item: SweepItem, - *, - is_fit_retry: bool, - preserve_retry_continuity: bool = False, -) -> bool: - """Reset G20-right PnP once per task, not once per formal cycle. - - The low-speed decreasing precheck establishes a branch with a static - endpoint window. Clearing frame-to-frame state at every later cycle made - four measurements of one rigid installation depend on four independent - planar-PnP initializations. Keep the tracker continuous for the whole - normal task transaction; a bounded retry remains a deliberately fresh - initialization while retaining the accepted task reference. - """ - if item.direction != DIRECTION_DECREASING: - return False - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return True - if is_fit_retry and preserve_retry_continuity: - # A dependent retry (currently the top-view thumb-yaw axis pair) - # replaces motion samples inside an already accepted task-relative - # PnP generation. Resetting here can move the two planar Tags to a - # different, internally smooth pose branch and shift every yaw zero - # by several degrees. Keep the exact live tracker/reference while - # replacing only the localized physical acquisition. - return False - if is_fit_retry: - return True - if profile.precheck_sweeps: - return bool(item.precheck) - return not item.precheck and item.cycle == 0 - - -def _fixed_base_role(view: str) -> str: - return { - "front": "front_base", - "side": "side_base", - "top": "top_base", - }[str(view)] - - -def _sweep_uses_locked_base_reference( - profile: HandCalibrationProfile, - spec: SweepSpec, - view: str, -) -> bool: - """Use one session-fixed base pose wherever task geometry needs it. - - Pinky/ring flexion is required to expose the middle/index side Tag, but - that same safe pose physically covers front Tag 0. The clearance remains - parked through that finger's roll, pitch and PIP tasks, including stages - where front is only an inactive status camera. Tag 0 is therefore locked - from baseline for every four-finger task; any moving Tag used by the - actual capture remains live and mandatory. - - Top Tag 8 has a different contract: it remains live and visible, but its - pose is frozen after preflight. Pitch and roll are collected in separate - tasks and jointly define thumb yaw. Re-solving a near-frontal planar base - Tag independently in those tasks expresses the two axes in subtly - different frames and turns image-position-dependent IPPE tilt into a yaw - zero shift. One session reference makes any fixed pose bias common to - both axes, where it cancels geometrically. - """ - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return False - selected_view = str(view) - if selected_view == "front": - return spec.key.startswith( - ("pinky_", "ring_", "middle_", "index_") - ) - return bool( - selected_view == "top" - and spec.key - in { - "thumb_cmc_pitch_front", - "thumb_cmc_roll_front", - "thumb_cmc_yaw_top", - } - ) - - -def _maximum_corner_drift_px( - reference: Sequence[Sequence[float]], - current: Sequence[Sequence[float]], -) -> float: - """Return the largest same-corner displacement for one fixed Tag.""" - reference_array = np.asarray(reference, dtype=float) - current_array = np.asarray(current, dtype=float) - if ( - reference_array.shape != (4, 2) - or current_array.shape != (4, 2) - or not np.all(np.isfinite(reference_array)) - or not np.all(np.isfinite(current_array)) - ): - return float("inf") - return float( - np.max(np.linalg.norm(current_array - reference_array, axis=1)) - ) - - -def _selected_pose_qualities( - selected: Mapping[str, SquareTagPose], - live_qualities: Mapping[str, TagQuality], - *, - locked_base_role: str | None, - locked_base_quality: TagQuality | None, -) -> dict[str, TagQuality]: - """Attach PnP residuals without pretending a cached Tag is live. - - A hidden fixed palm Tag has no entry in ``live_qualities``. Its cached - pose can still be selected together with the live moving Tag, so quality - propagation must explicitly use the quality captured at baseline instead - of indexing a missing live detection. - """ - updated = dict(live_qualities) - for role, pose in selected.items(): - source = ( - locked_base_quality - if role == locked_base_role - else live_qualities.get(role) - ) - if source is None: - raise KeyError(f"selected role has no quality source: {role}") - updated[role] = TagQuality( - hamming=source.hamming, - decision_margin=source.decision_margin, - edge_pixels=source.edge_pixels, - reprojection_error_px=pose.reprojection_error_px, - ) - return updated - - -def _frames_for_joint( - frames: Sequence[Any], joint_name: str -) -> list[Any]: - """Filter an asynchronous multi-camera buffer to one joint observation.""" - return [ - frame - for frame in frames - if ( - not frame.joint_quaternions_xyzw - and not frame.joint_vectors_xyz_m - and not frame.image_vectors_xy_px - and not frame.parent_poses_common - and not frame.child_poses_common - ) - or ( - joint_name in frame.joint_quaternions_xyzw - and joint_name in frame.joint_vectors_xyz_m - and joint_name in frame.image_vectors_xy_px - and joint_name in frame.parent_poses_common - and joint_name in frame.child_poses_common - ) - ] - - -def _frames_cover_sweep_joints( - frames: Sequence[Any], spec: SweepSpec, minimum_per_joint: int -) -> bool: - minimum = max(1, int(minimum_per_joint)) - return all( - len(_frames_for_joint(frames, joint_name)) >= minimum - for joint_name in spec.joints - ) - - -def _records_have_formal_sweep_coverage( - records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], - joint_names: Sequence[str], - *, - repetitions: int, - minimum_bins: int, - maximum_bin_gap: int, -) -> bool: - """Return whether every persisted direction has formal bin coverage.""" - expected_groups = { - (cycle, direction) - for cycle in range(int(repetitions)) - for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) - } - if not joint_names or not expected_groups: - return False - for joint_name in joint_names: - grouped: dict[tuple[int, str], set[int]] = {} - for record in records_by_joint.get(joint_name, ()): - direction = str(record.get("direction", "")) - if direction not in { - DIRECTION_DECREASING, - DIRECTION_INCREASING, - }: - continue - command = int( - np.clip( - np.rint( - float( - record.get( - "command_u8", - record.get("feedback_u8", -1), - ) - ) - ), - 0, - 255, - ) - ) - grouped.setdefault( - (int(record.get("cycle", -1)), direction), set() - ).add(command) - if not expected_groups.issubset(grouped): - return False - for key in expected_groups: - commands = sorted(grouped[key]) - if ( - not commands - or commands[0] != 0 - or commands[-1] != 255 - or len(commands) < int(minimum_bins) - or max(np.diff(commands), default=0) > int(maximum_bin_gap) - ): - return False - return True - - -def _frames_cover_sweep_motion( - frames: Sequence[Any], - spec: SweepSpec, - *, - motor_index: int, - minimum_per_joint: int, - minimum_span_u8: float, -) -> bool: - for joint_name in spec.joints: - selected = _frames_for_joint(frames, joint_name) - if len(selected) < int(minimum_per_joint): - return False - values = [float(frame.state_u8[motor_index]) for frame in selected] - if max(values) - min(values) < float(minimum_span_u8): - return False - return True - - -def _joint_failure_reason( - reason: str, spec: SweepSpec, joint_name: str -) -> str: - return ( - str(reason) - if len(spec.joints) == 1 - else f"{reason}:{joint_name}" - ) - - -def _steady_checkpoint_commands( - profile: HandCalibrationProfile, item: "SweepItem" -) -> tuple[int, ...]: - """Return first-training-round steady commands after the start pose.""" - if ( - profile.layout_id != G20_RIGHT_19_LAYOUT - or item.precheck - or item.cycle != 0 - ): - return () - ordered = ( - STEADY_COMMAND_CHECKPOINTS - if item.direction == DIRECTION_DECREASING - else tuple(reversed(STEADY_COMMAND_CHECKPOINTS)) - ) - if int(ordered[0]) != item.start_u8 or int(ordered[-1]) != item.target_u8: - raise RuntimeError("steady command checkpoint direction is inconsistent") - return tuple(int(value) for value in ordered[1:]) - - -RESUMABLE_SAMPLE_KINDS = frozenset( - {"sample", "baseline_hold_sample", "steady_command_sample"} -) - -RECALIBRATION_SCOPES = frozenset({"full", "thumb", "fingers"}) - - -def recalibration_task_keys( - profile: HandCalibrationProfile, scope: str -) -> tuple[str, ...]: - """Return tasks that must be freshly acquired for a partial session.""" - selected = str(scope).strip().lower() - if selected not in RECALIBRATION_SCOPES: - raise ValueError( - "recalibration_scope must be one of: " - + ", ".join(sorted(RECALIBRATION_SCOPES)) - ) - if selected == "full": - return () - if ( - profile.side != "right" - or profile.layout_id != G20_RIGHT_19_LAYOUT - ): - raise ValueError( - "partial recalibration is supported only for the G20 right " - "19-Tag product" - ) - thumb_tasks = tuple( - spec.key - for spec in profile.sweep_specs - if any(str(name).startswith("thumb_") for name in spec.joints) - ) - if len(thumb_tasks) != 4: - raise ValueError( - "G20 right thumb recalibration must resolve exactly four tasks" - ) - if selected == "thumb": - return thumb_tasks - thumb_task_set = set(thumb_tasks) - return tuple( - spec.key - for spec in profile.sweep_specs - if spec.key not in thumb_task_set - ) - - -def recalibration_quality_joints( - profile: HandCalibrationProfile, scope: str -) -> tuple[str, ...]: - """Return joints whose quality is decided by freshly acquired data.""" - selected = str(scope).strip().lower() - if selected == "full": - return tuple(profile.measured_joints) - # Reuse the scope validation and product guard in the task selector. - recalibration_task_keys(profile, selected) - want_thumb = selected == "thumb" - return tuple( - name - for name in profile.measured_joints - if str(name).startswith("thumb_") == want_thumb - ) - - -def build_standalone_thumb_payload( - *, - profile: HandCalibrationProfile, - serial_number: str, - measured_fits: Mapping[str, JointCurveFit], - thumb_offsets_rad: Mapping[str, float], - validation_errors_rad: Sequence[float], - baseline_command_u8: Sequence[int], - source_urdf_sha256: str, - camera_extrinsics_sha256: str, - corrected_urdf_sha256: str, -) -> dict[str, Any]: - """Build a non-runtime artifact for a fully standalone thumb solve.""" - if profile.side != "right" or profile.layout_id != G20_RIGHT_19_LAYOUT: - raise ValueError("standalone thumb payload requires G20 right 19-Tag") - expected_fits = set(recalibration_quality_joints(profile, "thumb")) - if set(measured_fits) != expected_fits: - raise ValueError("standalone thumb fits have the wrong joint set") - expected_offsets = { - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - } - offsets = { - str(name): float(value) for name, value in thumb_offsets_rad.items() - } - if set(offsets) != expected_offsets or any( - not math.isfinite(value) for value in offsets.values() - ): - raise ValueError("standalone thumb offsets must contain four finite zeros") - if len(baseline_command_u8) != 20: - raise ValueError("standalone thumb baseline must contain 20 commands") - joints: dict[str, dict[str, Any]] = {} - for name in sorted(expected_fits): - fit = measured_fits[name] - curve = np.asarray(fit.angle_rad, dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"standalone thumb curve is invalid: {name}") - spec = profile.joint_specs[name] - item: dict[str, Any] = { - "motor_index": int(spec.motor_index), - "angle_rad": [round(float(value), 8) for value in curve], - } - if name in offsets: - item["zero_command_u8"] = int( - baseline_command_u8[spec.motor_index] - ) - item["zero_angles"] = { - "urdf_zero_offset_rad": round(offsets[name], 8) - } - else: - item["passive"] = True - joints[name] = item - absolute_errors = np.abs( - np.asarray(validation_errors_rad, dtype=float) - ) - if absolute_errors.size == 0 or not np.all(np.isfinite(absolute_errors)): - raise ValueError("standalone thumb validation errors are missing") - return { - "schema_version": 1, - "artifact_type": "g20_right_standalone_thumb_calibration", - "model": "G20", - "side": "right", - "serial_number": str(serial_number), - "angle_unit": "rad", - "command_range": [0, 255], - "baseline_command_u8": [int(value) for value in baseline_command_u8], - "non_thumb_zero_policy": "source_cad_unchanged", - "joints": joints, - "quality": { - "passed": True, - "validation_mae_rad": round(float(np.mean(absolute_errors)), 8), - "validation_p95_rad": round( - float(np.percentile(absolute_errors, 95.0)), 8 - ), - }, - "hashes": { - "source_urdf_sha256": str(source_urdf_sha256), - "camera_extrinsics_sha256": str(camera_extrinsics_sha256), - "corrected_urdf_sha256": str(corrected_urdf_sha256), - }, - } - - -def _partial_scope_frozen_zero_offsets( - profile: HandCalibrationProfile, - scope: str, - payload: Mapping[str, Any], -) -> dict[str, float]: - """Return certified non-target zeros that a partial run must preserve.""" - selected = str(scope).strip().lower() - if selected == "full": - return {} - if selected not in {"thumb", "fingers"}: - raise ValueError(f"unsupported partial recalibration scope: {scope}") - joints = payload.get("joints") - if not isinstance(joints, Mapping): - raise ValueError("base calibration payload is missing joints") - freeze_thumb = selected == "fingers" - result: dict[str, float] = {} - for name in get_zero_calibration_profile( - profile.side, profile.layout_id - ).direct_zero_joints: - is_thumb = str(name).startswith("thumb_") - if is_thumb != freeze_thumb: - continue - try: - value = float( - joints[name]["zero_angles"]["urdf_zero_offset_rad"] - ) - except (KeyError, TypeError, ValueError) as error: - raise ValueError( - f"base calibration is missing certified zero for {name}" - ) from error - if not math.isfinite(value): - raise ValueError(f"base calibration zero is invalid for {name}") - result[name] = value - expected = 4 if freeze_thumb else 12 - if len(result) != expected: - raise ValueError( - f"partial {selected} scope must freeze exactly {expected} zeros" - ) - return result - - -def _normalise_legacy_split_roll_resume_rows( - profile: HandCalibrationProfile, - rows: Sequence[Mapping[str, Any]], -) -> tuple[dict[str, Any], ...]: - """Map the former front/side roll tasks onto one multiview checkpoint. - - This lets a calibration that was already running during the task-layout - upgrade donate all fully completed data if it later needs to resume. The - two historical motions remain independent observations; they are never - presented as timestamp-synchronised frames or accepted unless both joint - record sets pass the normal completeness checks. - """ - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return tuple(dict(row) for row in rows) - aliases = { - f"{finger}_roll_{view}": f"{finger}_roll_multiview" - for finger in ("pinky", "ring", "middle", "index") - for view in ("front", "side") - } - normalised: list[dict[str, Any]] = [] - for source in rows: - row = dict(source) - old_task = str(row.get("task_name", "")) - replacement = aliases.get(old_task) - if ( - replacement is not None - and row.get("kind") != "synchronised_frame" - ): - row["task_name"] = replacement - row["resume_source_task_name"] = old_task - normalised.append(row) - return tuple(normalised) - - -def _discard_automatically_retried_sweep_rows( - rows: Sequence[Mapping[str, Any]], -) -> tuple[dict[str, Any], ...]: - """Replay automatic-sweep retry tombstones before checkpoint selection. - - An automatic acquisition retry clears that cycle/direction from the - online in-memory stores. The raw JSONL is append-only, so the superseded - steady checkpoints remain physically present before the - ``automatic_sweep_retry`` event. Treat that event as a write-ahead - tombstone: records before it must never be merged with the replacement - acquisition written afterwards. - - Older sessions did not include ``task_name`` on retry events. Their - motor/cycle/direction and joint list still identify the invalidated - acquisition unambiguously. New sessions include both task and attempt - for a stronger durable identity. - """ - materialised = tuple(dict(row) for row in rows) - retry_events = [ - (index, row) - for index, row in enumerate(materialised) - if str(row.get("kind", "")) == "automatic_sweep_retry" - ] - if not retry_events: - return materialised - - durable_kinds = set(RESUMABLE_SAMPLE_KINDS) | { - "palm_axis_sample", - "synchronised_frame", - } - - def invalidated( - row_index: int, row: Mapping[str, Any] - ) -> bool: - if str(row.get("kind", "")) not in durable_kinds: - return False - row_task = str(row.get("task_name", "")) - row_cycle = int(row.get("cycle", -999)) - row_direction = str(row.get("direction", "")) - row_motor = int(row.get("motor_index", -1)) - row_joints = { - str(name) - for name in ( - row.get("joint"), - row.get("model_joint"), - *(row.get("joints", ()) or ()), - ) - if name - } - for retry_index, retry in retry_events: - if retry_index <= row_index: - continue - if int(retry.get("cycle", -998)) != row_cycle: - continue - if str(retry.get("direction", "")) != row_direction: - continue - retry_task = str(retry.get("task_name", "")) - if retry_task and row_task != retry_task: - continue - retry_motor = int(retry.get("motor_index", -1)) - if ( - row_motor >= 0 - and retry_motor >= 0 - and row_motor != retry_motor - ): - continue - retry_joints = { - str(name) for name in (retry.get("joints", ()) or ()) if name - } - if ( - row_joints - and retry_joints - and row_joints.isdisjoint(retry_joints) - ): - continue - return True - return False - - return tuple( - row - for index, row in enumerate(materialised) - if not invalidated(index, row) - ) - - -def _latest_resume_rows( - rows: Sequence[Mapping[str, Any]], - *, - kind: str, - minimum_sweep_bins: int = 0, -) -> dict[tuple[str, str, int, str], list[dict[str, Any]]]: - """Select the latest *complete* persisted attempt for each group. - - An interrupted retry must not hide the previous complete scan. This is - especially important for the nine steady checkpoints, which are written - before the continuous trajectory starts. - """ - selected = [ - dict(row) - for row in rows - if row.get("kind") == kind - and row.get("task_name") - and row.get("joint") - and row.get("direction") in {DIRECTION_DECREASING, DIRECTION_INCREASING} - ] - grouped: dict[ - tuple[str, str, int, str], dict[int, list[dict[str, Any]]] - ] = {} - for row in selected: - key = ( - str(row["task_name"]), - str(row["joint"]), - int(row.get("cycle", -1)), - str(row["direction"]), - ) - grouped.setdefault(key, {}).setdefault( - int(row.get("attempt", 1)), [] - ).append(row) - result: dict[tuple[str, str, int, str], list[dict[str, Any]]] = {} - for key, attempts in grouped.items(): - for _attempt, candidate in sorted( - attempts.items(), reverse=True - ): - if kind == "steady_command_sample": - requested = { - int(row.get("requested_command_u8", -1)) - for row in candidate - } - complete = requested == set(STEADY_COMMAND_CHECKPOINTS) - elif kind == "sample": - feedback_bins = { - int(round(float(row.get("feedback_u8", -1)))) - for row in candidate - } - complete = ( - 0 in feedback_bins - and 255 in feedback_bins - and len(feedback_bins) >= int(minimum_sweep_bins) - ) - else: - complete = bool(candidate) - if complete: - result[key] = candidate - break - return result - - -def _unresolved_fit_failure_tasks( - profile: HandCalibrationProfile, - rows: Sequence[Mapping[str, Any]], -) -> set[str]: - """Return tasks whose newest complete acquisition was still rejected. - - A newer model may deliberately retire a metric for a constrained joint. - Such historical failures are safe to replay because the final fit will - evaluate the imported raw samples against all current quality gates. - """ - sample_attempts: dict[str, int] = {} - for row in rows: - task_name = str(row.get("task_name", "")) - if row.get("kind") == "sample" and task_name: - sample_attempts[task_name] = max( - sample_attempts.get(task_name, 0), - int(row.get("attempt", 1)), - ) - zero_profile = get_zero_calibration_profile( - profile.side, profile.layout_id - ) - unresolved: set[str] = set() - for row in rows: - if row.get("kind") != "fit_failure": - continue - task_name = str(row.get("task_name", "")) - if not task_name: - matches = [ - spec - for spec in profile.sweep_specs - if spec.view == str(row.get("view", "")) - and spec.motor_index == int(row.get("motor_index", -1)) - and set(spec.joints) - == {str(name) for name in row.get("joints", [])} - ] - if len(matches) != 1: - continue - task_name = matches[0].key - failure_attempt = int(row.get("attempt", 1)) - if failure_attempt < sample_attempts.get(task_name, 0): - continue - failures = [ - item - for item in row.get("failures", []) - if isinstance(item, Mapping) - ] - def retired(item: Mapping[str, Any]) -> bool: - metric = str(item.get("metric", "")) - joint_name = str(item.get("joint", "")) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and metric == "rotation_orthogonal_rms_deg" - and joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS - ): - return True - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and metric in { - "hysteresis_deg", - "command_direction_gap_deg", - } - ): - # Product curves now gate direction dependence with settled - # feedback-domain samples and final-cycle holdout. Historical - # failures that mixed velocity lag or firmware tracking - # deadband with mechanical hysteresis are safe to revalidate. - return True - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and joint_name - in RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS - and metric == "axis_pose_line_rms_mm" - ): - # Product v19 originally treated the correlated per-frame - # pose-line residual of an incorrectly scaled passive-DIP Tag as the - # uncertainty of its fitted axis centre. Current code - # revalidates those complete raw trajectories using radial, - # cross-cycle and holdout gates, so the old rejection itself - # must not force another eight-direction scan. - return True - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and metric == "cross_view_roll_curve" - and joint_name in (profile.axis_validation_sources or {}) - ): - # The side roll alias is a validation-only observation in the - # current product model. Its raw samples are still - # revalidated through side-line radial/cycle/holdout gates, - # but a historical front/side curve-shape warning must not - # make a complete task look unfinished on every restart. - return True - return metric in { - "rotation_circle_axis_difference_deg", - "axis_plane_rms_mm", - } and circle_direction_is_constrained( - joint_name, - zero_profile.constrained_circle_joints, - ) - - retired_by_current_model = bool(failures) and all( - retired(item) for item in failures - ) - if not retired_by_current_model: - unresolved.add(task_name) - return unresolved - - -def resumable_completed_task_prefix( - profile: HandCalibrationProfile, - repetitions: int, - baseline: Sequence[int], - rows: Sequence[Mapping[str, Any]], - *, - minimum_sweep_bins: int = 32, - allow_sparse: bool = False, -) -> tuple[tuple[str, ...], tuple[dict[str, Any], ...]]: - """Return quality-preserving complete tasks and their raw records. - - A partially written task is never reused. Every joint, all four cycles, - both directions, both endpoints, the two nine-point command curves and - any required directional baseline holds must be present before the whole - task becomes a durable restart checkpoint. The compatibility default - returns only a contiguous prefix; product resume can opt into sparse - recovery so a failed early task does not discard independent later tasks. - """ - rows = _discard_automatically_retried_sweep_rows( - _normalise_legacy_split_roll_resume_rows(profile, rows) - ) - samples = _latest_resume_rows( - rows, - kind="sample", - minimum_sweep_bins=minimum_sweep_bins, - ) - baselines = _latest_resume_rows(rows, kind="baseline_hold_sample") - checkpoints = _latest_resume_rows(rows, kind="steady_command_sample") - directions = (DIRECTION_DECREASING, DIRECTION_INCREASING) - expected_checkpoints = set(STEADY_COMMAND_CHECKPOINTS) - unresolved_fit_failures = _unresolved_fit_failure_tasks(profile, rows) - completed: list[str] = [] - selected_group_keys: set[tuple[str, tuple[str, str, int, str]]] = set() - - for spec in profile.sweep_specs: - if spec.key in unresolved_fit_failures: - if allow_sparse: - continue - break - complete = True - task_group_keys: set[ - tuple[str, tuple[str, str, int, str]] - ] = set() - for joint_name in spec.joints: - joint = profile.record_specs[joint_name] - needs_baseline_hold = int(baseline[joint.motor_index]) not in { - 0, - 255, - } - for cycle in range(int(repetitions)): - for direction in directions: - key = (spec.key, joint_name, cycle, direction) - sample_rows = samples.get(key, []) - feedback_bins = { - int(round(float(row.get("feedback_u8", -1)))) - for row in sample_rows - } - if ( - len(feedback_bins) < int(minimum_sweep_bins) - or 0 not in feedback_bins - or 255 not in feedback_bins - ): - complete = False - break - task_group_keys.add(("sample", key)) - if needs_baseline_hold: - if not baselines.get(key): - complete = False - break - task_group_keys.add(("baseline_hold_sample", key)) - if not complete: - break - if not complete: - break - for direction in directions: - key = (spec.key, joint_name, 0, direction) - requested = { - int(row.get("requested_command_u8", -1)) - for row in checkpoints.get(key, []) - } - if requested != expected_checkpoints: - complete = False - break - task_group_keys.add(("steady_command_sample", key)) - if not complete: - break - if not complete: - if allow_sparse: - continue - break - completed.append(spec.key) - selected_group_keys.update(task_group_keys) - - reusable: list[dict[str, Any]] = [] - grouped = { - "sample": samples, - "baseline_hold_sample": baselines, - "steady_command_sample": checkpoints, - } - for kind, key in selected_group_keys: - reusable.extend(grouped[kind][key]) - completed_set = set(completed) - latest_task_attempt = { - (key[0], key[2], key[3]): max( - int(row.get("attempt", 1)) for row in sample_rows - ) - for key, sample_rows in samples.items() - if key[0] in completed_set and sample_rows - } - reusable.extend( - dict(row) - for row in rows - if row.get("kind") == "synchronised_frame" - and str(row.get("task_name", "")) in completed_set - and int(row.get("attempt", 1)) - == latest_task_attempt.get( - ( - str(row.get("task_name", "")), - int(row.get("cycle", -1)), - str(row.get("direction", "")), - ), - -1, - ) - ) - reusable.extend( - dict(row) - for row in rows - if row.get("kind") == "palm_axis_sample" - and str(row.get("task_name", "")) in completed_set - and int(row.get("attempt", 1)) - == latest_task_attempt.get( - ( - str(row.get("task_name", "")), - int(row.get("cycle", -1)), - str(row.get("direction", "")), - ), - -1, - ) - ) - reusable.sort( - key=lambda row: ( - profile.sweep_specs.index( - next(spec for spec in profile.sweep_specs if spec.key == row["task_name"]) - ), - int(row.get("cycle", -1)), - 0 if row.get("direction") == DIRECTION_DECREASING else 1, - 0 if row.get("kind") == "synchronised_frame" else 1, - str(row.get("joint", "")), - float(row.get("feedback_u8", row.get("requested_command_u8", 0))), - ) - ) - return tuple(completed), tuple(reusable) - - -def _file_sha256(path: str | Path) -> str: - digest = hashlib.sha256() - with Path(path).open("rb") as stream: - for chunk in iter(lambda: stream.read(1024 * 1024), b""): - digest.update(chunk) - return digest.hexdigest() - - -def _publish_import_status(node: Any) -> None: - """Publish synchronous import progress when running inside a ROS node.""" - publisher = getattr(node, "_publish_status", None) - if callable(publisher): - publisher(time.monotonic()) - - -def _steady_records_in_feedback_domain( - records: Sequence[Mapping[str, Any]], -) -> list[dict[str, Any]]: - """Re-index settled poses by feedback for mechanical backlash checks. - - Runtime curves remain indexed by requested command. This companion - representation separates intrinsic hysteresis from the firmware's - direction-dependent command/feedback deadband. Endpoint records have - already passed their motor-specific reach gate, so they are snapped to - the exact endpoints required by the curve fitter. - """ - return fitting_sample_records( - records, - domain="feedback", - allow_legacy_command=True, - snap_requested_endpoints=True, - ) - - -def _classify_cross_view_roll_hysteresis( - front_values_deg: Sequence[float], - side_values_deg: Sequence[float], - *, - limit_deg: float, -) -> str: - """Classify a diagnostic without turning it into a formal acceptance.""" - front = np.asarray(front_values_deg, dtype=float) - side = np.asarray(side_values_deg, dtype=float) - if front.size == 0 or side.size == 0: - raise ValueError("cross-view diagnostic requires both camera results") - front_max = float(np.max(front)) - side_max = float(np.max(side)) - if front_max <= limit_deg and side_max <= limit_deg: - return "both_views_within_formal_hysteresis_limit" - if front_max > limit_deg and side_max > limit_deg: - return "both_views_confirm_direction_dependent_pose" - if front_max > limit_deg: - return "front_only_difference_check_roll_tag_bracket_or_front_pnp" - return "side_only_difference_check_side_tag_chain_or_side_pnp" - - -def _fit_failure_is_systematic( - failures: Sequence[Mapping[str, Any]], repetitions: int -) -> bool: - """Identify a repeatable model conflict that more motion cannot repair.""" - if not failures or int(repetitions) <= 1: - return False - if all(bool(item.get("systematic", False)) for item in failures): - return True - if any( - str(item.get("metric", "")) - != "rotation_circle_axis_difference_deg" - for item in failures - ): - return False - cycles = {int(item.get("cycle", -1)) for item in failures} - if cycles != set(range(1, int(repetitions) + 1)): - return False - actual = np.asarray([float(item["actual"]) for item in failures]) - limits = np.asarray([float(item["limit"]) for item in failures]) - if ( - not np.all(np.isfinite(actual)) - or not np.all(np.isfinite(limits)) - or np.any(limits <= 0.0) - ): - return False - # Every independent round is far outside the limit and agrees with the - # others. This is a fixed geometry/PnP model conflict, not random capture - # loss, so two full four-round retries would only reproduce it. - return bool( - np.all(actual >= 1.5 * limits) - and float(np.ptp(actual)) <= max(0.5, 0.2 * float(np.mean(actual))) - ) - - -def _cross_view_curve_failure( - primary_name: str, - validation_name: str, - primary: JointCurveFit, - validation: JointCurveFit, - *, - scope: str, - maximum_rms_difference_rad: float, - maximum_branch_gap_difference_rad: float, - allow_projection_scale: bool = False, - maximum_projection_scale_ratio: float = 1.5, - cycle_fits: Sequence[tuple[JointCurveFit, JointCurveFit]] = (), -) -> dict[str, Any] | None: - """Return a localized, retry-aware cross-view failure. - - A stable offset repeated in every cycle is an installation/model conflict; - reacquiring the same four motions cannot change it. A single bad cycle is - instead localized so only that side-view cycle is reacquired. - """ - try: - compare_cross_view_roll_curves( - primary, - validation, - maximum_rms_difference_rad=maximum_rms_difference_rad, - maximum_branch_gap_difference_rad=( - maximum_branch_gap_difference_rad - ), - allow_projection_scale=allow_projection_scale, - maximum_projection_scale_ratio=( - maximum_projection_scale_ratio - ), - ) - return None - except ValueError as error: - reason = str(error) - - metrics: dict[str, float] = {} - if reason != "cross_view_roll_curve_direction_disagrees": - metrics = compare_cross_view_roll_curves( - primary, - validation, - maximum_rms_difference_rad=float("inf"), - maximum_branch_gap_difference_rad=float("inf"), - allow_projection_scale=allow_projection_scale, - maximum_projection_scale_ratio=float("inf"), - ) - failure: dict[str, Any] = { - "joint": primary_name, - "metric": "cross_view_roll_curve", - "reason": reason, - "scope": scope, - "quality_source_joints": [validation_name], - } - metric_key: str | None = None - metric_limit: float | None = None - metric_in_degrees = True - if reason.startswith("cross_view_roll_curve_difference_too_large:"): - field_name = reason.split(":", 2)[1] - metric_key = f"{field_name}_rms_difference_rad" - metric_limit = float(maximum_rms_difference_rad) - elif reason.startswith( - "cross_view_roll_branch_gap_difference_too_large:" - ): - metric_key = "baseline_branch_gap_difference_rad" - metric_limit = float(maximum_branch_gap_difference_rad) - elif reason.startswith( - "cross_view_roll_projection_scale_ratio_too_large:" - ): - metric_key = "projection_scale_ratio" - metric_limit = float(maximum_projection_scale_ratio) - metric_in_degrees = False - - cycle_values_rad: list[float] = [] - cycle_direction_disagrees: list[bool] = [] - for primary_cycle, validation_cycle in cycle_fits: - try: - cycle_metrics = compare_cross_view_roll_curves( - primary_cycle, - validation_cycle, - maximum_rms_difference_rad=float("inf"), - maximum_branch_gap_difference_rad=float("inf"), - allow_projection_scale=allow_projection_scale, - maximum_projection_scale_ratio=float("inf"), - ) - except ValueError as cycle_error: - if str(cycle_error) != "cross_view_roll_curve_direction_disagrees": - raise - cycle_metrics = {} - if metric_key is not None: - value = cycle_metrics.get(metric_key) - if value is not None: - cycle_values_rad.append(float(value)) - primary_travel = float( - primary_cycle.angle_rad[0] - primary_cycle.angle_rad[255] - ) - validation_travel = float( - validation_cycle.angle_rad[0] - - validation_cycle.angle_rad[255] - ) - cycle_direction_disagrees.append( - primary_travel * validation_travel <= 0.0 - ) - - if metric_key is not None and metric_limit is not None: - actual_value = float(metrics[metric_key]) - rendered_actual = ( - math.degrees(actual_value) - if metric_in_degrees - else actual_value - ) - rendered_limit = ( - math.degrees(metric_limit) - if metric_in_degrees - else metric_limit - ) - failure.update( - { - "actual": round(rendered_actual, 6), - "limit": round(rendered_limit, 6), - "comparison": "maximum", - "unit": "deg" if metric_in_degrees else "ratio", - } - ) - if cycle_values_rad: - values = np.asarray(cycle_values_rad, dtype=float) - cycle_key = ( - "cycle_values_deg" - if metric_in_degrees - else "cycle_values_ratio" - ) - failure[cycle_key] = [ - round( - math.degrees(value) if metric_in_degrees else value, - 6, - ) - for value in values - ] - over = np.flatnonzero(values > metric_limit) - if ( - over.size == 1 - and np.all( - np.delete(values, over[0]) <= 0.9 * metric_limit - ) - ): - failure["cycle"] = int(over[0]) + 1 - # All cycles tell the same near-threshold story: this is a fixed - # view/mount/model bias, not random data that another scan heals. - if ( - np.all(values >= 0.85 * metric_limit) - and float(np.ptp(values)) <= 0.35 * metric_limit - ): - failure["systematic"] = True - failure["classification"] = ( - "stable_cross_view_installation_or_model_bias" - ) - elif reason == "cross_view_roll_curve_direction_disagrees" and ( - cycle_direction_disagrees - and all(cycle_direction_disagrees) - ): - failure["systematic"] = True - failure["classification"] = "stable_cross_view_direction_conflict" - return failure - - -def _fit_failure_repeats_branch_clusters( - previous: Sequence[Mapping[str, Any]], - current: Sequence[Mapping[str, Any]], -) -> bool: - """Detect the same cross-cycle PnP branch split on two full retries.""" - - def travel_failure( - failures: Sequence[Mapping[str, Any]], - ) -> Mapping[str, Any] | None: - return next( - ( - item - for item in failures - if str(item.get("metric")) == "cycle_travel_range_deg" - and isinstance(item.get("cycle_travel_deg"), Sequence) - ), - None, - ) - - left = travel_failure(previous) - right = travel_failure(current) - if left is None or right is None: - return False - left_values = np.asarray(left["cycle_travel_deg"], dtype=float) - right_values = np.asarray(right["cycle_travel_deg"], dtype=float) - if ( - left_values.shape != right_values.shape - or left_values.size < 3 - or not np.all(np.isfinite(left_values)) - or not np.all(np.isfinite(right_values)) - or np.max(np.abs(left_values - right_values)) > 1.0 - ): - return False - limit = float(right.get("limit", 0.0)) - if limit <= 0.0 or float(np.ptp(right_values)) <= limit: - return False - - previous_by_metric = { - (str(item.get("joint")), str(item.get("metric"))): item - for item in previous - if "actual" in item and "limit" in item - } - stable_model_failures = 0 - for item in current: - key = (str(item.get("joint")), str(item.get("metric"))) - old = previous_by_metric.get(key) - if old is None or key[1] == "cycle_travel_range_deg": - continue - actual = float(item.get("actual", float("nan"))) - old_actual = float(old.get("actual", float("nan"))) - item_limit = float(item.get("limit", 0.0)) - if ( - np.isfinite(actual) - and np.isfinite(old_actual) - and item_limit > 0.0 - and actual > item_limit - and old_actual > item_limit - and abs(actual - old_actual) <= max(0.25, 0.1 * item_limit) - ): - stable_model_failures += 1 - return stable_model_failures >= 1 - - -def _robust_pose_payload( - poses: Sequence[Mapping[str, Sequence[float]]], -) -> dict[str, list[float]]: - if not poses: - raise ValueError("at least one pose is required") - translation = np.median( - np.asarray([pose["translation_xyz_m"] for pose in poses], dtype=float), - axis=0, - ) - quaternion = robust_rotation_summary( - [pose["quaternion_xyzw"] for pose in poses] - )[0] - return { - "translation_xyz_m": [float(value) for value in translation], - "quaternion_xyzw": [float(value) for value in quaternion], - } - - -def _view_pairs( - view: str, reference_finger: str = "index" -) -> tuple[tuple[str, str], ...]: - if view == "front": - return ( - ("front_base", "thumb_cmc"), - ("thumb_cmc", "thumb_mcp"), - ("thumb_mcp", "thumb_ip"), - ("front_base", f"{reference_finger}_roll"), - ) - if view == "side": - return ( - ("side_base", "thumb_cmc"), - ("side_base", f"{reference_finger}_mcp"), - (f"{reference_finger}_mcp", f"{reference_finger}_pip"), - (f"{reference_finger}_pip", f"{reference_finger}_dip"), - ) - if view == "top": - return (("top_base", "thumb_yaw"),) - raise ValueError(f"unknown view {view}") - - -@dataclass(frozen=True) -class FrameObservation: - stamp_ns: int - received_at: float - view: str - state_u8: tuple[float, ...] - state_sync_error_ns: int - joint_vectors_xyz_m: Mapping[str, tuple[float, float, float]] - image_vectors_xy_px: Mapping[str, tuple[float, float]] - joint_quaternions_xyzw: Mapping[str, tuple[float, float, float, float]] - parent_poses_common: Mapping[str, Mapping[str, list[float]]] - child_poses_common: Mapping[str, Mapping[str, list[float]]] - joint_reprojection_error_px: Mapping[str, float] - - -@dataclass(frozen=True) -class SweepItem: - spec: SweepSpec - cycle: int - direction: str - precheck: bool = False - - @property - def start_u8(self) -> int: - return 255 if self.direction == DIRECTION_DECREASING else 0 - - @property - def target_u8(self) -> int: - return 0 if self.direction == DIRECTION_DECREASING else 255 - - -def _build_sweep_plan( - profile: HandCalibrationProfile, repetitions: int -) -> tuple[SweepItem, ...]: - """Build one deterministic qualification-and-measurement transaction. - - Each product task owns a low-speed outbound/return qualification followed - by fixed-order formal cycles. Keeping this construction in one place - prevents resume, progress and runtime transitions from silently inventing - different task lifecycles. - """ - plan: list[SweepItem] = [] - for spec in profile.sweep_specs: - if profile.precheck_sweeps: - plan.extend( - SweepItem(spec, -1, direction, precheck=True) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ) - plan.extend( - SweepItem(spec, cycle, direction) - for cycle in range(int(repetitions)) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ) - return tuple(plan) - - -def _sweep_plan_transition( - profile: HandCalibrationProfile, - completed: SweepItem, - following: SweepItem, -) -> str: - """Return the only allowed boundary between two planned sweep items.""" - if completed.spec != following.spec: - return "task_change" - # Both qualification directions and the qualification-to-formal handoff - # are one continuous transaction. The physical endpoint and task - # clearance pose are unchanged, so a baseline detour can only discard - # good observations and perturb the PnP branch. - if completed.precheck: - return "immediate_reverse" - if completed.direction == DIRECTION_INCREASING: - # Formal cycles deliberately re-establish the same mechanical approach - # between repetitions so hysteresis measurements remain comparable. - return "cycle_reset" - return "immediate_reverse" - - -@dataclass(frozen=True) -class ValidationItem: - spec: SweepSpec - command_u8: int - - -@dataclass(frozen=True) -class CombinationValidationItem: - name: str - label_zh: str - command_u8: tuple[int, ...] - - -def _combination_validation_items( - baseline: Sequence[int], -) -> tuple[CombinationValidationItem, ...]: - if len(baseline) != 20: - raise ValueError("combination baseline must contain 20 commands") - base = [int(value) for value in baseline] - - def item( - name: str, label: str, changes: Mapping[int, int] - ) -> CombinationValidationItem: - values = list(base) - for index, value in changes.items(): - values[int(index)] = int(value) - return CombinationValidationItem(name, label, tuple(values)) - - return ( - item("all_open", "全开", {}), - item("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}), - item("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}), - item("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}), - item("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}), - item("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}), - item( - "half_grip", - "四指半握", - {1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160}, - ), - item( - "light_pinch", - "轻捏", - {0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176}, - ), - ) - - -def _combination_joint_angles( - profile: HandCalibrationProfile, - measured_fits: Mapping[str, JointCurveFit], - command_u8: Sequence[int], - motor_directions: Sequence[str], -) -> dict[str, float]: - """Evaluate a combination pose on the branch used to reach it. - - Using ``angle_rad`` here averages the increasing and decreasing branches; - on a serial chain (especially the thumb) those half-hysteresis errors - accumulate and can reject an otherwise correct pose. - """ - if len(command_u8) != 20 or len(motor_directions) != 20: - raise ValueError("combination commands and directions require 20 motors") - angles: dict[str, float] = {} - for name, spec in profile.joint_specs.items(): - target = int(command_u8[spec.motor_index]) - fit = measured_fits[name] - direction = str(motor_directions[spec.motor_index]) - if direction == DIRECTION_DECREASING: - curve = fit.decreasing_rad - elif direction == DIRECTION_INCREASING: - curve = fit.increasing_rad - else: - raise ValueError(f"invalid combination direction: {direction}") - angles[name] = float(curve[target]) - return angles - - -def _combination_motor_directions( - items: Sequence[CombinationValidationItem], - pose_index: int, - baseline_u8: Sequence[int], -) -> tuple[str, ...]: - """Reconstruct the direction in which every motor reached a pose. - - Pose zero establishes the all-open Tag mounts. Each later pose starts - from that baseline. A motor moved by an earlier pose returns along the - opposite branch and stays on that branch until another pose moves it. - """ - if len(baseline_u8) != 20: - raise ValueError("combination baseline must contain 20 motors") - if pose_index < 0 or pose_index >= len(items): - raise ValueError("combination pose index is out of range") - baseline = tuple(int(value) for value in baseline_u8) - directions = [DIRECTION_DECREASING] * 20 - for previous in items[1:pose_index]: - for motor, target in enumerate(previous.command_u8): - if int(target) < baseline[motor]: - directions[motor] = DIRECTION_INCREASING - elif int(target) > baseline[motor]: - directions[motor] = DIRECTION_DECREASING - current = items[pose_index] - for motor, target in enumerate(current.command_u8): - if int(target) < baseline[motor]: - directions[motor] = DIRECTION_DECREASING - elif int(target) > baseline[motor]: - directions[motor] = DIRECTION_INCREASING - return tuple(directions) - - -def _model_link_in_observer_base( - observer_base_common: Sequence[Sequence[float]], - model_base_common: Sequence[Sequence[float]], - model_link_from_base: Sequence[Sequence[float]], -) -> np.ndarray: - """Express a URDF link in the fixed palm Tag's coordinate frame.""" - observer = np.asarray(observer_base_common, dtype=float) - model_base = np.asarray(model_base_common, dtype=float) - link = np.asarray(model_link_from_base, dtype=float) - if any(value.shape != (4, 4) for value in (observer, model_base, link)): - raise ValueError("combination transforms must be 4x4 matrices") - return np.linalg.inv(observer) @ model_base @ link - - -@dataclass -class ViewRuntime: - name: str - tag_size_m: float - preflight_frames: int - tracker: SquareTagPoseTracker - group_tracker: SquareTagGroupPoseTracker - view_tags: Mapping[str, int] = field(default_factory=dict) - tag_sizes_m_by_role: Mapping[str, float] = field(default_factory=dict) - preflight_roles: tuple[str, ...] = () - role_by_id: dict[int, str] = field(default_factory=dict) - group_trackers: dict[ - tuple[str, ...], SquareTagGroupPoseTracker - ] = field(default_factory=dict) - current_required_roles: tuple[str, ...] = () - camera_matrix: np.ndarray | None = None - camera_info_valid: bool = False - camera_frame: str = "" - image_width: int = 0 - image_height: int = 0 - intrinsics_sha256: str = "" - extrinsics_valid: bool = False - valid_flags: deque[bool] = field(default_factory=deque) - detection_times: deque[float] = field(default_factory=deque) - # Task-scoped capture validity: only frames judged against an active - # task's required roles are counted. The rolling valid_flags window is - # cleared whenever the required-role set changes, so right after a task - # finishes it silently switches to scoring idle frames against the full - # preflight role set — the wrong measurement for a per-task data gate. - task_valid_frames: int = 0 - task_total_frames: int = 0 - latest_tag_quality: dict[str, TagQuality] = field(default_factory=dict) - latest_pnp_rejections: dict[str, str] = field(default_factory=dict) - latest_group_pnp_reason: str = "" - latest_group_missing_candidate_roles: tuple[str, ...] = () - # Task-scoped counters preserve the reasons that occurred before the last - # camera frame. A final ``group_initializing:5/8`` frame must not hide a - # recurring per-Tag rejection or a completed-window geometry rejection. - pnp_rejection_counts: dict[str, int] = field(default_factory=dict) - group_pnp_rejection_counts: dict[str, int] = field(default_factory=dict) - pnp_initialization_progress: tuple[int, int] | None = None - latest_pnp_valid: bool = False - last_pnp_diagnostic_signature: tuple[Any, ...] | None = None - pnp_invalid_since: float | None = None - pnp_reset_count: int = 0 - last_message_at: float = 0.0 - last_valid_at: float = 0.0 - fixed_base_observations: deque[ - tuple[SquareTagPose, tuple[float, float], TagQuality] - ] = field(default_factory=deque) - fixed_base_corner_observations: deque[np.ndarray] = field( - default_factory=deque - ) - locked_base_pose: SquareTagPose | None = None - locked_base_center_xy_px: tuple[float, float] | None = None - locked_base_corners_xy: tuple[ - tuple[float, float], - tuple[float, float], - tuple[float, float], - tuple[float, float], - ] | None = None - locked_base_quality: TagQuality | None = None - locked_base_corner_drift_count: int = 0 - latest_locked_base_corner_drift_px: float = 0.0 - - def __post_init__(self) -> None: - self.role_by_id = { - tag_id: role for role, tag_id in self.view_tags.items() - } - self.valid_flags = deque(maxlen=self.preflight_frames) - self.detection_times = deque(maxlen=self.preflight_frames) - self.fixed_base_observations = deque(maxlen=self.preflight_frames) - self.fixed_base_corner_observations = deque( - maxlen=self.preflight_frames - ) - if not self.preflight_roles: - self.preflight_roles = tuple(self.view_tags) - if any(role not in self.view_tags for role in self.preflight_roles): - raise ValueError("preflight roles must be configured for the view") - if any(role not in self.view_tags for role in self.tag_sizes_m_by_role): - raise ValueError("Tag-size roles must be configured for the view") - if any( - not math.isfinite(float(size)) or float(size) <= 0.0 - for size in self.tag_sizes_m_by_role.values() - ): - raise ValueError("Tag sizes must be finite and positive") - self.current_required_roles = tuple(self.preflight_roles) - self.group_trackers[tuple(self.preflight_roles)] = self.group_tracker - - @property - def roles(self) -> tuple[str, ...]: - return tuple(self.view_tags) - - @property - def valid_rate(self) -> float: - if not self.valid_flags: - return 0.0 - return float(sum(self.valid_flags) / len(self.valid_flags)) - - @property - def detection_hz(self) -> float: - if len(self.detection_times) < 2: - return 0.0 - elapsed = self.detection_times[-1] - self.detection_times[0] - return 0.0 if elapsed <= 0.0 else (len(self.detection_times) - 1) / elapsed - - -class G20ThreeCameraCalibrationNode(Node): - """Own all hand commands while observing three fixed Hikrobot views.""" - - def __init__(self) -> None: - super().__init__("g20_calibration") - self._declare_parameters() - self._load_parameters() - - self.session_dir.mkdir(parents=True, exist_ok=True) - self.raw_path = self.session_dir / "raw_samples.jsonl" - self.final_path = self.session_dir / ( - f"g20_{self.hand_type}_{_safe_name(self.serial_number)}_calibration.json" - ) - - self.views = { - name: self._make_view_runtime(name) - for name in ("front", "side", "top") - } - self.latest_state_u8: tuple[float, ...] = () - self.last_state_at = 0.0 - self.state_history: deque[StateSample] = deque(maxlen=1200) - self.state_receive_times: deque[float] = deque(maxlen=300) - self.latest_hand_info: dict[str, Any] = {} - self.commanded_speed_profile: tuple[int, ...] = () - self.speed_commanded_at = 0.0 - - self.state = STATE_PREFLIGHT - self.reason = "waiting_for_devices_and_sdk" - self.sample_schema_version = 1 - self.base_import_progress: dict[str, Any] = {} - self.paused_reason = "" - self.started = False - # Startup has two deliberately separate gates. Camera/SDK health is - # enough to let us recover the hand from an interrupted pose; fixed - # palm Tags are checked only after that recovery reaches baseline. - self.startup_baseline_recovered = False - self.abort_original_reason = "" - self.baseline_after = "" - self.position_hold_since: float | None = None - self.motion_stage_started_at = 0.0 - self.return_waypoints: deque[tuple[int, ...]] = deque() - self.preparation_waypoints: deque[tuple[int, ...]] = deque() - self.preparation_command_u8: tuple[int, ...] = tuple( - self.baseline_command - ) - - self.sweep_items: list[SweepItem] = [] - self.sweep_index = 0 - self.active_sweep: SweepItem | None = None - # Owns required Tag roles and PnP continuity across the brief - # active_sweep=None interval used for same-task cycle resets. - self.pnp_task_spec: SweepSpec | None = None - self.sweep_frames: list[FrameObservation] = [] - # Roll joints use command 127 as their zero. A frame acquired while - # merely passing 127 contains velocity/latency error and is not a - # valid backlash observation. Keep a separate buffer populated only - # after the motor has reached 127 and the commanded mid-sweep hold has - # started. - self.sweep_baseline_frames: list[FrameObservation] = [] - self.sweep_baseline_pending = False - self.sweep_baseline_hold_since: float | None = None - self.sweep_checkpoint_commands: deque[int] = deque() - self.sweep_checkpoint_mode = False - self.sweep_checkpoint_target_u8: int | None = None - self.sweep_checkpoint_hold_since: float | None = None - self.sweep_checkpoint_frames: list[FrameObservation] = [] - # Keep synchronised endpoint observations acquired while the motor is - # held at the sweep start. If these frames are discarded and the - # target is commanded immediately, a fast motor can leave the endpoint - # before the next camera/state pair arrives. - self.sweep_start_frames: list[FrameObservation] = [] - # A completed direction already supplies fresh, synchronised frames at - # the physical endpoint where the immediately reversed direction - # starts. Stage those frames across the state transition instead of - # discarding them and waiting again at the unchanged pose. - self.carried_sweep_start_frames: list[FrameObservation] = [] - self.sweep_started_at = 0.0 - self.sweep_last_valid_at = 0.0 - self.sweep_last_valid_at_by_view: dict[str, float] = {} - self.sweep_endpoint_since: float | None = None - self.sweep_detection_total_frames = 0 - self.sweep_detection_valid_frames = 0 - self.sweep_detection_total_by_view: dict[str, int] = {} - self.sweep_detection_valid_by_view: dict[str, int] = {} - self.precheck_speed_metrics: dict[ - str, dict[str, dict[str, float | int]] - ] = {} - self.formal_speed_scales: dict[str, float] = {} - self.motion_progress_reference_error_u8 = float("inf") - self.motion_last_progress_at = 0.0 - self.motion_stall_details: dict[str, Any] = {} - self.retry_sweep_spec: SweepSpec | None = None - self.retry_resume_index: int | None = None - self.retry_sweep_items: list[SweepItem] = [] - self.retry_joint_names: set[str] = set() - # Some fitted quantities are observed by data collected during other - # physical tasks. In particular, right-hand thumb yaw zero comes from - # the two top-view side channels captured during CMC pitch and roll. - self.retry_source_failure_task_key: str | None = None - self.retry_source_task_keys: tuple[str, ...] = () - self.retry_cycle_override: set[int] = set() - self.retry_preserve_pnp_continuity = False - self.active_sweep_is_fit_retry = False - self.fit_failure: dict[str, Any] = {} - self.fit_failure_history_by_task: dict[ - str, list[list[dict[str, Any]]] - ] = {} - self.cross_view_roll_diagnostic_result: dict[str, Any] = {} - self.sweep_attempts: dict[str | int, int] = { - _sweep_storage_key(spec): 1 for spec in self.profile.sweep_specs - } - self.sweep_retry_counts: dict[tuple[str | int, int, str], int] = {} - self.motion_retry_counts: dict[str, int] = {} - self.validation_retry_counts: dict[tuple[str | int, int], int] = {} - self.records_by_joint: dict[str, list[dict[str, Any]]] = { - name: [] for name in self.profile.record_joints - } - self.baseline_records_by_joint: dict[str, list[dict[str, Any]]] = { - name: [] for name in self.profile.record_joints - } - self.command_records_by_joint: dict[str, list[dict[str, Any]]] = { - name: [] for name in self.profile.record_joints - } - self.palm_axis_records_by_source: dict[ - str, list[dict[str, Any]] - ] = { - observer.source_name: [] - for observer in self.profile.palm_axis_observers - } - - self.measured_fits: dict[str, JointCurveFit] = {} - self.axis_measurements: list[JointAxisMeasurement] = [] - self.palm_orientation_measurements: list[ - PalmOrientationMeasurement - ] = [] - self.palm_orientation_rejections: dict[str, str] = {} - self.zero_result: ZeroSolveResult | None = None - self.validated_endpoint_zero_offsets_rad: dict[str, float] = {} - self.corrected_urdf_path: Path | None = None - self.fit_quality_passed = False - - self.validation_items: list[ValidationItem] = [] - self.validation_index = 0 - self.active_validation: ValidationItem | None = None - self.active_validation_direction: str | None = None - self.validation_frames_buffer: list[FrameObservation] = [] - self.validation_errors_rad: list[float] = [] - self.validation_stage_started_at = 0.0 - self.combination_validation_items: list[CombinationValidationItem] = [] - self.combination_validation_index = 0 - self.active_combination_validation: CombinationValidationItem | None = None - self.combination_validation_frames_buffer: dict[ - str, list[FrameObservation] - ] = {view: [] for view in ("front", "side", "top")} - self.combination_validation_completed = False - self.combination_tag_mounts: dict[str, np.ndarray] = {} - self.combination_tag_observation_counts: dict[str, int] = {} - self.combination_tag_validation_counts: dict[str, int] = {} - self.combination_position_errors_m: list[float] = [] - self.combination_orientation_errors_rad: list[float] = [] - self.completed_payload: dict[str, Any] | None = None - - self.command_publisher = self.create_publisher( - JointState, self.command_topic, 1 - ) - self.setting_publisher = self.create_publisher( - String, self.setting_topic, 10 - ) - self.status_publisher = self.create_publisher(String, "~/status", 10) - self.status_text_publisher = self.create_publisher( - String, "~/status_text", 10 - ) - self.create_subscription( - JointState, self.state_topic, self._state_callback, 10 - ) - self.create_subscription( - String, self.info_topic, self._info_callback, 10 - ) - for name, runtime in self.views.items(): - del runtime - self.create_subscription( - CameraInfo, - self.camera_info_topics[name], - lambda message, view=name: self._camera_info_callback( - view, message - ), - qos_profile_sensor_data, - ) - self.create_subscription( - AprilTagDetectionArray, - self.detections_topics[name], - lambda message, view=name: self._detections_callback( - view, message - ), - qos_profile_sensor_data, - ) - - self.create_service(Trigger, "~/start", self._start_callback) - self.create_service(Trigger, "~/pause", self._pause_callback) - self.create_service(Trigger, "~/resume", self._resume_callback) - self.create_service(Trigger, "~/abort", self._abort_callback) - self.last_status_publish = 0.0 - self.timer = self.create_timer(0.05, self._timer_callback) - self.get_logger().info( - f"Three-camera calibration session: {self.session_dir}" - ) - - def _declare_parameters(self) -> None: - self.declare_parameter("model", "G20") - self.declare_parameter("hand_type", "left") - self.declare_parameter("tag_layout", "legacy_11") - self.declare_parameter("serial_number", "UNSET") - self.declare_parameter("session_dir", "calibration_output/session") - self.declare_parameter("resume_raw_samples_path", "") - self.declare_parameter("recalibration_scope", "full") - self.declare_parameter("camera_extrinsics_file", "") - self.declare_parameter("source_urdf_path", "") - self.declare_parameter("source_urdf_expected_sha256", "") - self.declare_parameter("corrected_urdf_output_dir", "") - self.declare_parameter("commands_enabled", True) - self.declare_parameter("command_topic", "/g20/cb_left_hand_control_cmd") - self.declare_parameter("state_topic", "/g20/cb_left_hand_state") - self.declare_parameter("info_topic", "/g20/cb_left_hand_info") - self.declare_parameter( - "setting_topic", "/g20/cb_hand_setting_cmd" - ) - for view in ("front", "side", "top"): - self.declare_parameter(f"{view}_camera_serial", "") - self.declare_parameter( - f"{view}_camera_info_topic", - f"/g20_calibration/{view}/camera/camera_info", - ) - self.declare_parameter( - f"{view}_detections_topic", - f"/g20_calibration/{view}/apriltag/detections", - ) - self.declare_parameter("tag_size_m", 0.016) - self.declare_parameter( - "tag_size_override_ids", Parameter.Type.INTEGER_ARRAY - ) - self.declare_parameter( - "tag_size_overrides_m", Parameter.Type.DOUBLE_ARRAY - ) - self.declare_parameter( - "baseline_command_u8", list(THREE_CAMERA_BASELINE_COMMAND) - ) - self.declare_parameter("normal_calibration_speed", 15) - self.declare_parameter("index_roll_calibration_speed", 5) - self.declare_parameter("index_flex_calibration_speed", 10) - self.declare_parameter("adaptive_formal_speed_enabled", True) - self.declare_parameter("adaptive_formal_speed_max_scale", 1.5) - self.declare_parameter("adaptive_formal_speed_minimum_bins", 64) - self.declare_parameter("adaptive_formal_speed_maximum_bin_gap", 8) - self.declare_parameter("speed_setting_settle_seconds", 0.25) - self.declare_parameter("parallel_pose_transitions", True) - self.declare_parameter("repetitions", 3) - self.declare_parameter("g20_right_19_repetitions", 4) - self.declare_parameter("preflight_frames", 60) - self.declare_parameter("minimum_detection_rate", 0.95) - self.declare_parameter("minimum_detection_hz", 15.0) - self.declare_parameter("minimum_feedback_hz", 25.0) - self.declare_parameter("maximum_hamming", 0) - self.declare_parameter("minimum_decision_margin", 30.0) - self.declare_parameter("minimum_edge_pixels", 30.0) - self.declare_parameter("pnp_maximum_reprojection_error_px", 1.5) - self.declare_parameter("pnp_reprojection_tie_px", 1.5) - self.declare_parameter("pnp_maximum_pose_jump_deg", 35.0) - self.declare_parameter("pnp_maximum_translation_jump_m", 0.04) - self.declare_parameter("pnp_maximum_tag_tilt_deg", 75.0) - self.declare_parameter("pnp_tracker_reset_seconds", 5.0) - self.declare_parameter("pnp_group_initialization_frames", 8) - self.declare_parameter("pnp_group_normal_alignment_scale_deg", 5.0) - self.declare_parameter("pnp_group_maximum_normal_alignment_deg", 15.0) - self.declare_parameter("fixed_base_maximum_corner_drift_px", 2.0) - self.declare_parameter("fixed_base_movement_confirmation_frames", 5) - self.declare_parameter("thumb_ip_pnp_coupling_multiplier", 1.03) - self.declare_parameter("thumb_ip_pnp_coupling_scale_deg", 3.0) - self.declare_parameter( - "thumb_ip_pnp_maximum_coupling_residual_deg", 7.5 - ) - self.declare_parameter("top_pnp_invalid_reset_seconds", 1.0) - self.declare_parameter("maximum_state_image_skew_ms", 50.0) - self.declare_parameter("axis_maximum_plane_rms_m", 0.003) - self.declare_parameter("passive_axis_maximum_plane_rms_m", 0.004) - self.declare_parameter("axis_maximum_radial_rms_m", 0.003) - self.declare_parameter("axis_maximum_pose_line_rms_m", 0.001) - self.declare_parameter("axis_maximum_rotation_circle_difference_deg", 1.0) - self.declare_parameter( - "active_maximum_rotation_orthogonal_rms_deg", 2.5 - ) - self.declare_parameter( - "passive_maximum_rotation_orthogonal_rms_deg", 7.5 - ) - self.declare_parameter("zero_maximum_axis_cycle_difference_deg", 0.75) - self.declare_parameter( - "thumb_yaw_maximum_zero_cycle_difference_deg", 0.5 - ) - self.declare_parameter( - "thumb_yaw_maximum_confidence_half_width_deg", 0.75 - ) - self.declare_parameter( - "thumb_yaw_cross_session_diagnostic_deg", 0.75 - ) - self.declare_parameter("zero_maximum_axis_cone_mismatch_deg", 5.0) - self.declare_parameter( - "zero_maximum_observability_condition_number", 1.0e10 - ) - self.declare_parameter("zero_maximum_offset_deg", 20.0) - self.declare_parameter("zero_finger_maximum_offset_deg", 3.0) - self.declare_parameter("mechanical_endpoint_maximum_offset_deg", 5.0) - self.declare_parameter("endpoint_tolerance_u8", 2.0) - self.declare_parameter( - "synchronised_endpoint_tolerance_margin_u8", 1.0 - ) - self.declare_parameter( - "steady_checkpoint_command_feedback_tolerance_u8", 8.0 - ) - self.declare_parameter( - "steady_checkpoint_maximum_feedback_range_u8", 2.0 - ) - self.declare_parameter("thumb_yaw_zero_endpoint_tolerance_u8", 4.0) - self.declare_parameter( - "right_thumb_yaw_255_endpoint_tolerance_u8", 5.0 - ) - self.declare_parameter("pinky_pip_zero_endpoint_tolerance_u8", 5.0) - self.declare_parameter("endpoint_hold_seconds", 0.5) - self.declare_parameter("baseline_hold_seconds", 0.5) - self.declare_parameter("minimum_baseline_hold_frames", 10) - self.declare_parameter("task_precheck_hold_seconds", 2.0) - self.declare_parameter("position_timeout_seconds", 30.0) - self.declare_parameter("sweep_timeout_seconds", 90.0) - self.declare_parameter("motor_stall_timeout_seconds", 5.0) - self.declare_parameter("motor_stall_startup_grace_seconds", 1.0) - self.declare_parameter("motor_stall_minimum_progress_u8", 1.0) - self.declare_parameter("invalid_timeout_seconds", 3.0) - self.declare_parameter("minimum_sweep_frames", 40) - self.declare_parameter("minimum_state_span_u8", 240.0) - self.declare_parameter("minimum_sweep_bins", 32) - self.declare_parameter("maximum_bin_gap", 16) - self.declare_parameter("automatic_sweep_retry_limit", 3) - self.declare_parameter("automatic_fit_retry_limit", 2) - self.declare_parameter("automatic_motion_retry_limit", 2) - self.declare_parameter("cross_view_roll_diagnostic_finger", "") - self.declare_parameter("provisional_warning_ratio", 1.25) - self.declare_parameter("retry_minimum_speed", 3) - self.declare_parameter("retry_speed_scales", [0.8, 0.6, 0.5]) - self.declare_parameter( - "retry_endpoint_hold_seconds", [0.75, 1.0, 1.25] - ) - self.declare_parameter("trajectory_maximum_plane_rms_m", 0.004) - self.declare_parameter("trajectory_maximum_radial_rms_m", 0.004) - self.declare_parameter("trajectory_minimum_radius_m", 0.003) - self.declare_parameter("trajectory_minimum_arc_deg", 15.0) - self.declare_parameter("image_trajectory_maximum_radial_rms_px", 2.0) - self.declare_parameter("image_trajectory_maximum_radial_p95_px", 3.5) - self.declare_parameter("image_trajectory_minimum_radius_px", 20.0) - self.declare_parameter( - "trajectory_maximum_cycle_travel_difference_deg", 3.0 - ) - self.declare_parameter( - "passive_maximum_cycle_travel_difference_deg", 10.0 - ) - self.declare_parameter("maximum_monotonic_correction_deg", 2.0) - self.declare_parameter("maximum_hysteresis_deg", 5.0) - self.declare_parameter("baseline_maximum_hysteresis_deg", 0.5) - self.declare_parameter( - "directional_zero_maximum_branch_gap_deg", 1.5 - ) - self.declare_parameter( - "directional_zero_maximum_branch_gap_range_deg", 0.3 - ) - self.declare_parameter( - "cross_view_roll_maximum_branch_gap_difference_deg", 0.3 - ) - self.declare_parameter( - "cross_view_roll_maximum_shape_rms_deg", 1.25 - ) - self.declare_parameter( - "cross_view_roll_maximum_projection_scale_ratio", 1.5 - ) - self.declare_parameter( - "cross_view_roll_maximum_axis_difference_deg", 15.0 - ) - self.declare_parameter("passive_maximum_monotonic_correction_deg", 3.0) - self.declare_parameter("passive_maximum_hysteresis_deg", 7.5) - self.declare_parameter("command_maximum_direction_gap_deg", 2.0) - self.declare_parameter("validation_enabled", False) - self.declare_parameter("combination_validation_enabled", True) - self.declare_parameter("combination_validation_frames", 10) - self.declare_parameter("combination_maximum_position_p95_m", 0.003) - self.declare_parameter("combination_maximum_orientation_p95_deg", 2.0) - self.declare_parameter("validation_command_count", 3) - self.declare_parameter("validation_frames", 10) - self.declare_parameter("validation_seed", 20260804) - self.declare_parameter("validation_timeout_seconds", 20.0) - self.declare_parameter("maximum_validation_mae_deg", 1.0) - self.declare_parameter("maximum_validation_p95_deg", 2.0) - self.declare_parameter("maximum_validation_error_deg", 1.5) - self.declare_parameter("zero_maximum_confidence_half_width_deg", 0.5) - - def _load_parameters(self) -> None: - def value(name: str) -> Any: - return self.get_parameter(name).value - - def optional_array(name: str) -> Sequence[Any]: - # An empty YAML sequence has no element type. With a typed ROS 2 - # declaration it can therefore remain NOT_SET and ``.value`` - # raises instead of returning an empty list. These overrides are - # optional, so preserve the default-size semantics in that case. - try: - result = value(name) - except ParameterUninitializedException: - return () - return () if result is None else result - - self.model = str(value("model")).strip().upper() - self.hand_type = str(value("hand_type")).lower() - self.tag_layout = str(value("tag_layout")).lower() - product_contract = get_product_calibration_contract( - self.model, self.hand_type, self.tag_layout - ) - self.profile = product_contract.profile - self.zero_profile = product_contract.zero_profile - self.calibration_profile = product_contract.typed_profile - self.serial_number = str(value("serial_number")) - if self.serial_number == "UNSET": - raise ValueError("serial_number is required") - self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() - resume_value = str(value("resume_raw_samples_path")).strip() - self.resume_raw_samples_path = ( - None - if not resume_value - else Path(resume_value).expanduser().resolve() - ) - self.recalibration_scope = str( - value("recalibration_scope") - ).strip().lower() - self.recalibration_task_keys = recalibration_task_keys( - self.profile, self.recalibration_scope - ) - self.standalone_thumb_calibration = bool( - self.recalibration_scope == "thumb" - and self.resume_raw_samples_path is None - ) - if ( - self.recalibration_scope == "fingers" - and self.resume_raw_samples_path is None - ): - raise ValueError( - "finger recalibration requires resume_raw_samples_path from " - "a passed thumb or complete session" - ) - self.partial_scope_fixed_zero_offsets_rad: dict[str, float] = {} - if ( - self.recalibration_scope != "full" - and self.resume_raw_samples_path is not None - ): - assert self.resume_raw_samples_path is not None - base_payload_path = self.resume_raw_samples_path.parent / ( - f"g20_right_{_safe_name(self.serial_number)}_calibration.json" - ) - try: - base_payload = json.loads( - base_payload_path.read_text(encoding="utf-8") - ) - except (OSError, json.JSONDecodeError) as error: - raise ValueError( - "partial recalibration base calibration JSON is invalid" - ) from error - if not isinstance(base_payload, Mapping): - raise ValueError( - "partial recalibration base calibration JSON must be an object" - ) - self.partial_scope_fixed_zero_offsets_rad = ( - _partial_scope_frozen_zero_offsets( - self.profile, - self.recalibration_scope, - base_payload, - ) - ) - self.resumed_task_keys: tuple[str, ...] = () - self.resume_source_session = "" - self.camera_extrinsics_file = Path( - str(value("camera_extrinsics_file")) - ).expanduser().resolve() - self.source_urdf_path = Path( - str(value("source_urdf_path")) - ).expanduser().resolve() - self.source_urdf_expected_sha256 = str( - value("source_urdf_expected_sha256") - ).strip().lower() - output_value = str(value("corrected_urdf_output_dir")) - self.corrected_urdf_output_dir = ( - Path(output_value).expanduser().resolve() - if output_value - else self.source_urdf_path.parent - ) - self.camera_serials = { - view: str(value(f"{view}_camera_serial")) - for view in ("front", "side", "top") - } - self.extrinsics: ThreeCameraExtrinsics | None = None - self.extrinsics_error = "" - try: - self.extrinsics = load_three_camera_extrinsics( - self.camera_extrinsics_file - ) - except Exception as error: - self.extrinsics_error = str(error) - self.commands_enabled = bool(value("commands_enabled")) - self.command_topic = str(value("command_topic")) - self.state_topic = str(value("state_topic")) - self.info_topic = str(value("info_topic")) - if self.hand_type == "right": - if self.command_topic == "/g20/cb_left_hand_control_cmd": - self.command_topic = "/g20/cb_right_hand_control_cmd" - if self.state_topic == "/g20/cb_left_hand_state": - self.state_topic = "/g20/cb_right_hand_state" - if self.info_topic == "/g20/cb_left_hand_info": - self.info_topic = "/g20/cb_right_hand_info" - self.setting_topic = str(value("setting_topic")) - self.camera_info_topics = { - view: str(value(f"{view}_camera_info_topic")) - for view in ("front", "side", "top") - } - self.detections_topics = { - view: str(value(f"{view}_detections_topic")) - for view in ("front", "side", "top") - } - self.tag_size_m = float(value("tag_size_m")) - override_ids = tuple( - int(item) for item in optional_array("tag_size_override_ids") - ) - override_sizes = tuple( - float(item) for item in optional_array("tag_size_overrides_m") - ) - if len(override_ids) != len(override_sizes): - raise ValueError( - "tag_size_override_ids and tag_size_overrides_m must have " - "the same length" - ) - if len(set(override_ids)) != len(override_ids): - raise ValueError("tag_size_override_ids must be unique") - configured_tag_ids = { - int(tag_id) - for tags in self.profile.view_tags.values() - for tag_id in tags.values() - } - if ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and any(tag_id not in configured_tag_ids for tag_id in override_ids) - ): - raise ValueError("Tag-size override contains an unconfigured Tag ID") - if ( - not math.isfinite(self.tag_size_m) - or self.tag_size_m <= 0.0 - or any( - not math.isfinite(size) or size <= 0.0 - for size in override_sizes - ) - ): - raise ValueError("Tag sizes must be finite and positive") - self.tag_sizes_m_by_id = { - tag_id: self.tag_size_m for tag_id in configured_tag_ids - } - self.tag_sizes_m_by_id.update( - (tag_id, size) - for tag_id, size in zip(override_ids, override_sizes) - if tag_id in configured_tag_ids - ) - self.baseline_command = tuple( - int(item) for item in value("baseline_command_u8") - ) - self.return_command_u8 = tuple(self.baseline_command) - self.normal_calibration_speed = int(value("normal_calibration_speed")) - self.index_roll_calibration_speed = int( - value("index_roll_calibration_speed") - ) - self.index_flex_calibration_speed = int( - value("index_flex_calibration_speed") - ) - self.adaptive_formal_speed_enabled = bool( - value("adaptive_formal_speed_enabled") - ) - self.adaptive_formal_speed_max_scale = float( - value("adaptive_formal_speed_max_scale") - ) - self.adaptive_formal_speed_minimum_bins = int( - value("adaptive_formal_speed_minimum_bins") - ) - self.adaptive_formal_speed_maximum_bin_gap = int( - value("adaptive_formal_speed_maximum_bin_gap") - ) - self.speed_setting_settle_seconds = float( - value("speed_setting_settle_seconds") - ) - self.repetitions = int(value("repetitions")) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - self.repetitions = int(value("g20_right_19_repetitions")) - self.preflight_frames = int(value("preflight_frames")) - self.minimum_detection_rate = float(value("minimum_detection_rate")) - self.minimum_detection_hz = float(value("minimum_detection_hz")) - self.minimum_feedback_hz = float(value("minimum_feedback_hz")) - self.maximum_hamming = int(value("maximum_hamming")) - self.minimum_decision_margin = float(value("minimum_decision_margin")) - self.minimum_edge_pixels = float(value("minimum_edge_pixels")) - self.pnp_maximum_reprojection_error_px = float( - value("pnp_maximum_reprojection_error_px") - ) - self.pnp_reprojection_tie_px = float(value("pnp_reprojection_tie_px")) - self.pnp_maximum_pose_jump_rad = math.radians( - float(value("pnp_maximum_pose_jump_deg")) - ) - self.pnp_maximum_translation_jump_m = float( - value("pnp_maximum_translation_jump_m") - ) - self.pnp_maximum_tag_tilt_rad = math.radians( - float(value("pnp_maximum_tag_tilt_deg")) - ) - self.pnp_tracker_reset_seconds = float(value("pnp_tracker_reset_seconds")) - self.pnp_group_initialization_frames = int( - value("pnp_group_initialization_frames") - ) - self.pnp_group_normal_alignment_scale_rad = math.radians( - float(value("pnp_group_normal_alignment_scale_deg")) - ) - self.pnp_group_maximum_normal_alignment_rad = math.radians( - float(value("pnp_group_maximum_normal_alignment_deg")) - ) - self.fixed_base_maximum_corner_drift_px = float( - value("fixed_base_maximum_corner_drift_px") - ) - self.fixed_base_movement_confirmation_frames = int( - value("fixed_base_movement_confirmation_frames") - ) - self.thumb_ip_pnp_coupling_multiplier = float( - value("thumb_ip_pnp_coupling_multiplier") - ) - self.thumb_ip_pnp_coupling_scale_rad = math.radians( - float(value("thumb_ip_pnp_coupling_scale_deg")) - ) - self.thumb_ip_pnp_maximum_coupling_residual_rad = math.radians( - float(value("thumb_ip_pnp_maximum_coupling_residual_deg")) - ) - self.top_pnp_invalid_reset_seconds = float( - value("top_pnp_invalid_reset_seconds") - ) - self.maximum_state_image_skew_ns = int( - float(value("maximum_state_image_skew_ms")) * 1_000_000.0 - ) - self.axis_maximum_plane_rms_m = float( - value("axis_maximum_plane_rms_m") - ) - self.passive_axis_maximum_plane_rms_m = float( - value("passive_axis_maximum_plane_rms_m") - ) - self.axis_maximum_radial_rms_m = float( - value("axis_maximum_radial_rms_m") - ) - self.axis_maximum_pose_line_rms_m = float( - value("axis_maximum_pose_line_rms_m") - ) - self.axis_maximum_rotation_circle_difference_rad = math.radians( - float(value("axis_maximum_rotation_circle_difference_deg")) - ) - self.active_maximum_rotation_orthogonal_rms_rad = math.radians( - float(value("active_maximum_rotation_orthogonal_rms_deg")) - ) - self.passive_maximum_rotation_orthogonal_rms_rad = math.radians( - float(value("passive_maximum_rotation_orthogonal_rms_deg")) - ) - self.zero_maximum_axis_cycle_difference_rad = math.radians( - float(value("zero_maximum_axis_cycle_difference_deg")) - ) - self.thumb_yaw_maximum_zero_cycle_difference_rad = math.radians( - float(value("thumb_yaw_maximum_zero_cycle_difference_deg")) - ) - self.thumb_yaw_maximum_confidence_half_width_rad = math.radians( - float(value("thumb_yaw_maximum_confidence_half_width_deg")) - ) - self.thumb_yaw_cross_session_diagnostic_rad = math.radians( - float(value("thumb_yaw_cross_session_diagnostic_deg")) - ) - self.zero_maximum_axis_cone_mismatch_rad = math.radians( - float(value("zero_maximum_axis_cone_mismatch_deg")) - ) - self.zero_maximum_observability_condition_number = float( - value("zero_maximum_observability_condition_number") - ) - self.zero_maximum_offset_rad = math.radians( - float(value("zero_maximum_offset_deg")) - ) - self.zero_finger_maximum_offset_rad = math.radians( - float(value("zero_finger_maximum_offset_deg")) - ) - self.mechanical_endpoint_maximum_offset_rad = math.radians( - float(value("mechanical_endpoint_maximum_offset_deg")) - ) - self.zero_joint_maximum_offsets_rad = {} - self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) - self.synchronised_endpoint_tolerance_margin_u8 = float( - value("synchronised_endpoint_tolerance_margin_u8") - ) - self.steady_checkpoint_command_feedback_tolerance_u8 = float( - value("steady_checkpoint_command_feedback_tolerance_u8") - ) - self.steady_checkpoint_maximum_feedback_range_u8 = float( - value("steady_checkpoint_maximum_feedback_range_u8") - ) - self.thumb_yaw_zero_endpoint_tolerance_u8 = float( - value("thumb_yaw_zero_endpoint_tolerance_u8") - ) - self.right_thumb_yaw_255_endpoint_tolerance_u8 = float( - value("right_thumb_yaw_255_endpoint_tolerance_u8") - ) - self.pinky_pip_zero_endpoint_tolerance_u8 = float( - value("pinky_pip_zero_endpoint_tolerance_u8") - ) - self.endpoint_hold_seconds = float(value("endpoint_hold_seconds")) - self.baseline_hold_seconds = float(value("baseline_hold_seconds")) - self.minimum_baseline_hold_frames = int( - value("minimum_baseline_hold_frames") - ) - self.task_precheck_hold_seconds = float( - value("task_precheck_hold_seconds") - ) - self.position_timeout_seconds = float(value("position_timeout_seconds")) - self.parallel_pose_transitions = bool( - value("parallel_pose_transitions") - ) - self.sweep_timeout_seconds = float(value("sweep_timeout_seconds")) - self.motor_stall_timeout_seconds = float( - value("motor_stall_timeout_seconds") - ) - self.motor_stall_startup_grace_seconds = float( - value("motor_stall_startup_grace_seconds") - ) - self.motor_stall_minimum_progress_u8 = float( - value("motor_stall_minimum_progress_u8") - ) - self.invalid_timeout_seconds = float(value("invalid_timeout_seconds")) - self.minimum_sweep_frames = int(value("minimum_sweep_frames")) - self.minimum_state_span_u8 = float(value("minimum_state_span_u8")) - self.minimum_sweep_bins = int(value("minimum_sweep_bins")) - self.maximum_bin_gap = int(value("maximum_bin_gap")) - self.automatic_sweep_retry_limit = int( - value("automatic_sweep_retry_limit") - ) - self.automatic_fit_retry_limit = int( - value("automatic_fit_retry_limit") - ) - self.cross_view_roll_diagnostic_finger = str( - value("cross_view_roll_diagnostic_finger") - ).strip().lower() - self.automatic_motion_retry_limit = int( - value("automatic_motion_retry_limit") - ) - self.provisional_warning_ratio = float( - value("provisional_warning_ratio") - ) - self.retry_minimum_speed = int(value("retry_minimum_speed")) - self.retry_speed_scales = tuple( - float(item) for item in value("retry_speed_scales") - ) - self.retry_endpoint_hold_seconds = tuple( - float(item) for item in value("retry_endpoint_hold_seconds") - ) - self.trajectory_maximum_plane_rms_m = float( - value("trajectory_maximum_plane_rms_m") - ) - self.trajectory_maximum_radial_rms_m = float( - value("trajectory_maximum_radial_rms_m") - ) - self.trajectory_minimum_radius_m = float( - value("trajectory_minimum_radius_m") - ) - self.trajectory_minimum_arc_rad = math.radians( - float(value("trajectory_minimum_arc_deg")) - ) - self.image_trajectory_maximum_radial_rms_px = float( - value("image_trajectory_maximum_radial_rms_px") - ) - self.image_trajectory_maximum_radial_p95_px = float( - value("image_trajectory_maximum_radial_p95_px") - ) - self.image_trajectory_minimum_radius_px = float( - value("image_trajectory_minimum_radius_px") - ) - self.trajectory_maximum_cycle_travel_difference_rad = math.radians( - float(value("trajectory_maximum_cycle_travel_difference_deg")) - ) - self.passive_maximum_cycle_travel_difference_rad = math.radians( - float(value("passive_maximum_cycle_travel_difference_deg")) - ) - self.maximum_monotonic_correction_rad = math.radians( - float(value("maximum_monotonic_correction_deg")) - ) - self.maximum_hysteresis_rad = math.radians( - float(value("maximum_hysteresis_deg")) - ) - self.baseline_maximum_hysteresis_rad = math.radians( - float(value("baseline_maximum_hysteresis_deg")) - ) - self.directional_zero_maximum_branch_gap_rad = math.radians( - float(value("directional_zero_maximum_branch_gap_deg")) - ) - self.directional_zero_maximum_branch_gap_range_rad = math.radians( - float(value("directional_zero_maximum_branch_gap_range_deg")) - ) - self.cross_view_roll_maximum_branch_gap_difference_rad = math.radians( - float(value("cross_view_roll_maximum_branch_gap_difference_deg")) - ) - self.cross_view_roll_maximum_shape_rms_rad = math.radians( - float(value("cross_view_roll_maximum_shape_rms_deg")) - ) - self.cross_view_roll_maximum_projection_scale_ratio = float( - value("cross_view_roll_maximum_projection_scale_ratio") - ) - self.cross_view_roll_maximum_axis_difference_rad = math.radians( - float(value("cross_view_roll_maximum_axis_difference_deg")) - ) - self.passive_maximum_monotonic_correction_rad = math.radians( - float(value("passive_maximum_monotonic_correction_deg")) - ) - self.passive_maximum_hysteresis_rad = math.radians( - float(value("passive_maximum_hysteresis_deg")) - ) - self.command_maximum_direction_gap_rad = math.radians( - float(value("command_maximum_direction_gap_deg")) - ) - self.validation_enabled = bool(value("validation_enabled")) - self.combination_validation_enabled = bool( - value("combination_validation_enabled") - ) - if self.standalone_thumb_calibration: - # A standalone thumb artifact has no four-finger curves or - # Cartesian combination model to validate. - self.combination_validation_enabled = False - self.combination_validation_frames = int( - value("combination_validation_frames") - ) - self.combination_maximum_position_p95_m = float( - value("combination_maximum_position_p95_m") - ) - self.combination_maximum_orientation_p95_rad = math.radians( - float(value("combination_maximum_orientation_p95_deg")) - ) - self.validation_command_count = int(value("validation_command_count")) - self.validation_frames = int(value("validation_frames")) - self.validation_seed = int(value("validation_seed")) - self.validation_timeout_seconds = float( - value("validation_timeout_seconds") - ) - self.maximum_validation_mae_rad = math.radians( - float(value("maximum_validation_mae_deg")) - ) - self.maximum_validation_p95_rad = math.radians( - float(value("maximum_validation_p95_deg")) - ) - self.maximum_validation_error_rad = math.radians( - float(value("maximum_validation_error_deg")) - ) - self.zero_maximum_confidence_half_width_rad = math.radians( - float(value("zero_maximum_confidence_half_width_deg")) - ) - if len(self.baseline_command) != self.profile.command_count: - raise ValueError( - "baseline_command_u8 length does not match the registered " - f"product command schema ({self.profile.command_count})" - ) - if any(value < 0 or value > 255 for value in self.baseline_command): - raise ValueError("baseline_command_u8 values must be in [0, 255]") - if self.baseline_command != tuple(self.profile.baseline_command): - raise ValueError( - "baseline_command_u8 differs from the registered product " - "calibration baseline" - ) - if any(not serial for serial in self.camera_serials.values()): - raise ValueError("front/side/top camera serial parameters are required") - if not self.source_urdf_path.is_file(): - raise ValueError( - f"source_urdf_path does not exist: {self.source_urdf_path}" - ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - if re.fullmatch( - r"[0-9a-f]{64}", self.source_urdf_expected_sha256 - ) is None: - raise ValueError( - "g20_right_19 requires source_urdf_expected_sha256 from " - "the CAD/hardware owner; unconfirmed source CAD cannot " - "produce a formal corrected URDF" - ) - actual_source_hash = _file_sha256(self.source_urdf_path) - if actual_source_hash != self.source_urdf_expected_sha256: - raise ValueError( - "source_urdf_path SHA-256 does not match the confirmed " - "G20 CAD hash" - ) - source_model = UrdfKinematicModel(self.source_urdf_path) - thumb_ip_model = source_model.joints.get("thumb_ip") - if ( - thumb_ip_model is None - or thumb_ip_model.mimic_joint != "thumb_mcp" - or abs(thumb_ip_model.mimic_offset) > 1.0e-9 - ): - raise ValueError( - "source URDF thumb_ip mimic structure is invalid" - ) - source_name = self.source_urdf_path.name.lower() - opposite = "right" if self.hand_type == "left" else "left" - if f"g20_{opposite}" in source_name: - raise ValueError( - "source_urdf_path hand side does not match hand_type" - ) - if ( - "zero_calibrated" in self.source_urdf_path.stem.lower() - or re.search( - r"calibrated_20\d{6}", - self.source_urdf_path.stem.lower(), - ) - ): - raise ValueError( - "source_urdf_path must be the original CAD URDF" - ) - if not 0 <= self.normal_calibration_speed <= 255: - raise ValueError("normal_calibration_speed must be in [0, 255]") - if not 0 <= self.index_roll_calibration_speed <= 255: - raise ValueError("index_roll_calibration_speed must be in [0, 255]") - if not 0 <= self.index_flex_calibration_speed <= 255: - raise ValueError("index_flex_calibration_speed must be in [0, 255]") - if not 1.0 <= self.adaptive_formal_speed_max_scale <= 2.0: - raise ValueError( - "adaptive_formal_speed_max_scale must be in [1, 2]" - ) - if ( - self.adaptive_formal_speed_minimum_bins - < self.minimum_sweep_bins - ): - raise ValueError( - "adaptive_formal_speed_minimum_bins cannot be below the " - "formal minimum_sweep_bins" - ) - if not ( - 1 - <= self.adaptive_formal_speed_maximum_bin_gap - <= self.maximum_bin_gap - ): - raise ValueError( - "adaptive_formal_speed_maximum_bin_gap must be in " - "[1, maximum_bin_gap]" - ) - if self.speed_setting_settle_seconds < 0.0: - raise ValueError("speed_setting_settle_seconds must be non-negative") - if not 0.0 <= self.endpoint_tolerance_u8 <= 10.0: - raise ValueError("endpoint_tolerance_u8 must be in [0, 10]") - if not 0.0 <= self.synchronised_endpoint_tolerance_margin_u8 <= 4.0: - raise ValueError( - "synchronised_endpoint_tolerance_margin_u8 must be in [0, 4]" - ) - if not ( - self.endpoint_tolerance_u8 - <= self.steady_checkpoint_command_feedback_tolerance_u8 - <= 16.0 - ): - raise ValueError( - "steady_checkpoint_command_feedback_tolerance_u8 must be " - "between endpoint_tolerance_u8 and 16" - ) - if not 0.0 < self.steady_checkpoint_maximum_feedback_range_u8 <= 4.0: - raise ValueError( - "steady_checkpoint_maximum_feedback_range_u8 must be in (0, 4]" - ) - if not ( - self.endpoint_tolerance_u8 - <= self.thumb_yaw_zero_endpoint_tolerance_u8 - <= 10.0 - ): - raise ValueError( - "thumb_yaw_zero_endpoint_tolerance_u8 must be between the " - "default endpoint tolerance and 10" - ) - if not ( - self.endpoint_tolerance_u8 - <= self.right_thumb_yaw_255_endpoint_tolerance_u8 - <= 10.0 - ): - raise ValueError( - "right_thumb_yaw_255_endpoint_tolerance_u8 must be between " - "the default endpoint tolerance and 10" - ) - if self.top_pnp_invalid_reset_seconds <= 0.0: - raise ValueError("top_pnp_invalid_reset_seconds must be positive") - if not 3 <= self.pnp_group_initialization_frames <= 30: - raise ValueError( - "pnp_group_initialization_frames must be in [3, 30]" - ) - if not 0.5 <= self.fixed_base_maximum_corner_drift_px <= 10.0: - raise ValueError( - "fixed_base_maximum_corner_drift_px must be in [0.5, 10]" - ) - if not 1 <= self.fixed_base_movement_confirmation_frames <= 30: - raise ValueError( - "fixed_base_movement_confirmation_frames must be in [1, 30]" - ) - if not ( - self.endpoint_tolerance_u8 - <= self.pinky_pip_zero_endpoint_tolerance_u8 - <= 8.0 - ): - raise ValueError( - "pinky_pip_zero_endpoint_tolerance_u8 must be between " - "endpoint_tolerance_u8 and 8" - ) - if self.motor_stall_timeout_seconds <= 0.0: - raise ValueError("motor_stall_timeout_seconds must be positive") - if not 0.0 <= self.motor_stall_startup_grace_seconds <= 5.0: - raise ValueError("motor_stall_startup_grace_seconds must be in [0, 5]") - if not 0.0 < self.motor_stall_minimum_progress_u8 <= 10.0: - raise ValueError( - "motor_stall_minimum_progress_u8 must be in (0, 10]" - ) - if not all( - value > 0.0 - for value in ( - self.image_trajectory_maximum_radial_rms_px, - self.image_trajectory_maximum_radial_p95_px, - self.image_trajectory_minimum_radius_px, - self.pnp_group_normal_alignment_scale_rad, - self.pnp_group_maximum_normal_alignment_rad, - self.thumb_ip_pnp_coupling_multiplier, - self.thumb_ip_pnp_coupling_scale_rad, - self.thumb_ip_pnp_maximum_coupling_residual_rad, - self.axis_maximum_plane_rms_m, - self.passive_axis_maximum_plane_rms_m, - self.axis_maximum_pose_line_rms_m, - self.zero_maximum_axis_cycle_difference_rad, - self.thumb_yaw_maximum_zero_cycle_difference_rad, - self.thumb_yaw_maximum_confidence_half_width_rad, - self.thumb_yaw_cross_session_diagnostic_rad, - self.zero_maximum_axis_cone_mismatch_rad, - self.zero_maximum_offset_rad, - self.zero_finger_maximum_offset_rad, - self.mechanical_endpoint_maximum_offset_rad, - self.maximum_validation_error_rad, - self.zero_maximum_confidence_half_width_rad, - self.task_precheck_hold_seconds, - self.active_maximum_rotation_orthogonal_rms_rad, - self.passive_maximum_rotation_orthogonal_rms_rad, - self.trajectory_maximum_cycle_travel_difference_rad, - self.passive_maximum_cycle_travel_difference_rad, - self.baseline_maximum_hysteresis_rad, - self.directional_zero_maximum_branch_gap_rad, - self.directional_zero_maximum_branch_gap_range_rad, - self.cross_view_roll_maximum_branch_gap_difference_rad, - self.cross_view_roll_maximum_shape_rms_rad, - self.passive_maximum_monotonic_correction_rad, - self.passive_maximum_hysteresis_rad, - self.command_maximum_direction_gap_rad, - ) - ): - raise ValueError("trajectory quality thresholds must be positive") - if ( - self.thumb_yaw_maximum_zero_cycle_difference_rad - > self.zero_maximum_axis_cycle_difference_rad - ): - raise ValueError( - "thumb_yaw_maximum_zero_cycle_difference_deg must not " - "exceed zero_maximum_axis_cycle_difference_deg" - ) - if ( - self.thumb_yaw_maximum_confidence_half_width_rad - > self.zero_maximum_confidence_half_width_rad - ): - raise ValueError( - "thumb_yaw_maximum_confidence_half_width_deg must not " - "exceed zero_maximum_confidence_half_width_deg" - ) - if not 1.0 < self.cross_view_roll_maximum_projection_scale_ratio <= 2.0: - raise ValueError( - "cross-view roll projection-scale ratio must be in (1, 2]" - ) - if self.zero_finger_maximum_offset_rad > self.zero_maximum_offset_rad: - raise ValueError( - "zero_finger_maximum_offset_deg cannot exceed " - "zero_maximum_offset_deg" - ) - if ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and self.repetitions < 4 - ): - raise ValueError( - "g20_right_19 requires at least 4 repetitions: at least 3 " - "training cycles and one isolated holdout" - ) - if ( - self.profile.layout_id != G20_RIGHT_19_LAYOUT - and self.repetitions != 3 - ): - raise ValueError( - "legacy three-camera calibration requires exactly 3 repetitions" - ) - if self.preflight_frames < 10 or self.minimum_sweep_bins < 3: - raise ValueError("preflight_frames or minimum_sweep_bins is too small") - if self.minimum_feedback_hz <= 0.0: - raise ValueError("minimum_feedback_hz must be positive") - if self.minimum_baseline_hold_frames < 3: - raise ValueError("minimum_baseline_hold_frames must be at least 3") - if not 0 <= self.automatic_sweep_retry_limit <= 5: - raise ValueError("automatic_sweep_retry_limit must be in [0, 5]") - if not 0 <= self.automatic_fit_retry_limit <= 2: - raise ValueError("automatic_fit_retry_limit must be in [0, 2]") - if not 0 <= self.automatic_motion_retry_limit <= 5: - raise ValueError("automatic_motion_retry_limit must be in [0, 5]") - if self.cross_view_roll_diagnostic_finger not in { - "", "pinky", "ring", "middle", "index" - }: - raise ValueError( - "cross_view_roll_diagnostic_finger must be empty or one of " - "pinky/ring/middle/index" - ) - if ( - self.cross_view_roll_diagnostic_finger - and self.profile.layout_id != G20_RIGHT_19_LAYOUT - ): - raise ValueError( - "cross-view roll diagnostic requires g20_right_19" - ) - if not 1.0 <= self.provisional_warning_ratio <= 2.0: - raise ValueError("provisional_warning_ratio must be in [1, 2]") - if not 1 <= self.retry_minimum_speed <= 255: - raise ValueError("retry_minimum_speed must be in [1, 255]") - if ( - len(self.retry_speed_scales) != self.automatic_sweep_retry_limit - or any(not 0.0 < item <= 1.0 for item in self.retry_speed_scales) - ): - raise ValueError("retry_speed_scales must match the sweep retry limit") - if ( - len(self.retry_endpoint_hold_seconds) - != self.automatic_sweep_retry_limit - or any(item < self.endpoint_hold_seconds for item in self.retry_endpoint_hold_seconds) - ): - raise ValueError( - "retry endpoint holds must match retries and not shorten the hold" - ) - if not 1 <= self.validation_command_count <= 10: - raise ValueError("validation_command_count must be in [1, 10]") - if self.validation_frames < 3: - raise ValueError("validation_frames must be at least 3") - if self.combination_validation_frames < 3: - raise ValueError("combination_validation_frames must be at least 3") - if ( - self.combination_maximum_position_p95_m <= 0.0 - or self.combination_maximum_orientation_p95_rad <= 0.0 - ): - raise ValueError("combination pose validation thresholds must be positive") - - def _make_group_pose_tracker( - self, name: str, roles: Sequence[str] - ) -> SquareTagGroupPoseTracker: - role_tuple = tuple(str(role) for role in roles) - role_set = set(role_tuple) - adjacent_pairs = tuple( - pair - for pair in dict.fromkeys( - (spec.parent_role, spec.child_role) - for spec in self.profile.record_specs.values() - if spec.measured - and spec.view == name - and spec.parent_role is not None - and spec.child_role is not None - ) - if pair[0] in role_set and pair[1] in role_set - ) - # Do not impose an absolute face-normal relationship between the - # fixed palm Tag and an articulated finger Tag. That angle depends on - # the physical Tag bracket and the endpoint joint pose; it is not a - # product invariant. The old "nearly parallel" prior repeatedly - # cleared otherwise stable 8-frame initialisation windows for ID4/ID5 - # on ring MCP flexion. Multi-frame relative rigidity, reprojection - # error and the task reference retained across cycles provide the - # valid branch evidence without assuming a particular mounting plane. - normal_alignment_pairs: tuple[tuple[str, str], ...] = () - thumb_mcp_ip_roles = {"thumb_cmc", "thumb_mcp", "thumb_ip"} - thumb_mcp_ip_coupling = () - if name == "front" and thumb_mcp_ip_roles.issubset(role_set): - thumb_mcp_ip_coupling = ( - ( - "thumb_cmc", - "thumb_mcp", - "thumb_mcp", - "thumb_ip", - self.thumb_ip_pnp_coupling_multiplier, - ), - ) - return SquareTagGroupPoseTracker( - roles=role_tuple, - adjacent_pairs=adjacent_pairs, - maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, - maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, - relative_rotation_scale_rad=math.radians(5.0), - relative_translation_scale_m=0.01, - reprojection_scale_px=0.1, - reprojection_weight=0.05, - reset_after_seconds=self.pnp_tracker_reset_seconds, - # Five-tag front preflight would make the exhaustive multi-frame - # search unnecessarily expensive. Four-tag side groups remain - # small enough and need the robust initializer so the fixed base - # Tag can anchor the three reference-finger IPPE branches. - initialization_frames=( - 1 - if len(role_tuple) > 4 - else self.pnp_group_initialization_frames - ), - normal_alignment_pairs=normal_alignment_pairs, - normal_alignment_scale_rad=( - self.pnp_group_normal_alignment_scale_rad - ), - maximum_normal_alignment_rad=( - self.pnp_group_maximum_normal_alignment_rad - if normal_alignment_pairs - else None - ), - # The source-URDF thumb_ip mimic ratio is used only to disambiguate - # the two planar-IPPE candidates while motor 15 drives MCP and IP - # together. If no candidate matches this weak prior, selection - # falls back to visual continuity instead of dropping the frame. - # Accepted Tag poses still fit both visual curves independently. - coupled_rotation_pairs=thumb_mcp_ip_coupling, - coupled_rotation_scale_rad=self.thumb_ip_pnp_coupling_scale_rad, - maximum_coupled_rotation_residual_rad=( - self.thumb_ip_pnp_maximum_coupling_residual_rad - if thumb_mcp_ip_coupling - else None - ), - ) - - def _make_view_runtime(self, name: str) -> ViewRuntime: - tracker = SquareTagPoseTracker( - maximum_reprojection_error_px=self.pnp_maximum_reprojection_error_px, - reprojection_tie_px=self.pnp_reprojection_tie_px, - maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, - maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, - maximum_tag_tilt_rad=self.pnp_maximum_tag_tilt_rad, - reset_after_seconds=self.pnp_tracker_reset_seconds, - ) - preflight_roles = tuple(self.profile.preflight_view_roles[name]) - if getattr(self, "standalone_thumb_calibration", False): - base_role = _fixed_base_role(name) - preflight_roles = tuple( - role - for role in self.profile.view_tags[name] - if role == base_role or role.startswith("thumb_") - ) - group = self._make_group_pose_tracker( - name, - preflight_roles, - ) - return ViewRuntime( - name=name, - tag_size_m=self.tag_size_m, - preflight_frames=self.preflight_frames, - tracker=tracker, - group_tracker=group, - view_tags=self.profile.view_tags[name], - tag_sizes_m_by_role={ - role: self.tag_sizes_m_by_id[int(tag_id)] - for role, tag_id in self.profile.view_tags[name].items() - }, - preflight_roles=preflight_roles, - ) - - def _required_roles_for_view(self, view: str) -> tuple[str, ...]: - """Return only the roles required by the active task in this view.""" - runtime = self.views[view] - if getattr(self, "active_combination_validation", None) is not None: - # Combination validation consumes every reliable visible target, - # but only the fixed palm reference is a blocking requirement. - # Four adjacent fingers cannot expose all of their Tags to one - # side camera at the same instant. - return tuple(runtime.preflight_roles) - active_spec: SweepSpec | None = None - if ( - self.active_sweep is not None - and view in _sweep_views(_node_profile(self), self.active_sweep.spec) - ): - active_spec = self.active_sweep.spec - elif ( - self.active_validation is not None - and view in _sweep_views( - self.profile, self.active_validation.spec - ) - ): - active_spec = self.active_validation.spec - elif ( - self.retry_sweep_spec is not None - and view in _sweep_views(_node_profile(self), self.retry_sweep_spec) - ): - active_spec = self.retry_sweep_spec - elif ( - getattr(self, "pnp_task_spec", None) is not None - and view - in _sweep_views( - _node_profile(self), self.pnp_task_spec - ) - ): - active_spec = self.pnp_task_spec - if active_spec is None: - return tuple(runtime.preflight_roles) - required: set[str] = set() - for joint_name in _sweep_joints_for_view( - self.profile, active_spec, view - ): - joint = self.profile.record_specs[joint_name] - if joint.parent_role is not None: - required.add(joint.parent_role) - if joint.child_role is not None: - required.add(joint.child_role) - # Every task keeps its view's fixed palm Tag. Besides suppressing an - # unanchored IPPE mirror branch, this makes the thumb MCP/IP task obey - # its explicit front 0/1/2/3 visibility contract. - base_role = { - "front": "front_base", - "side": "side_base", - "top": "top_base", - }[view] - required.add(base_role) - return tuple(role for role in runtime.roles if role in required) - - def _locked_base_role_for_active_capture(self, view: str) -> str | None: - runtime = self.views[view] - if ( - getattr(runtime, "locked_base_pose", None) is None - or getattr(runtime, "locked_base_center_xy_px", None) is None - or getattr(runtime, "locked_base_quality", None) is None - ): - return None - if getattr(self, "active_combination_validation", None) is not None: - return _fixed_base_role(view) - spec: SweepSpec | None = None - if self.active_sweep is not None: - spec = self.active_sweep.spec - elif self.active_validation is not None: - spec = self.active_validation.spec - elif self.retry_sweep_spec is not None: - spec = self.retry_sweep_spec - elif getattr(self, "pnp_task_spec", None) is not None: - spec = self.pnp_task_spec - if spec is None or not _sweep_uses_locked_base_reference( - _node_profile(self), spec, view - ): - return None - return _fixed_base_role(view) - - def _live_required_roles_for_view( - self, view: str, required_roles: Sequence[str] - ) -> tuple[str, ...]: - locked = self._locked_base_role_for_active_capture(view) - # Front Tag 0 may be intentionally hidden by finger clearance. Top - # Tag 8 must remain visible so its raw corners can prove that the - # session reference has not moved, even though its PnP pose is frozen. - if str(view) == "top": - return tuple(required_roles) - return tuple(role for role in required_roles if role != locked) - - def _lock_fixed_base_references(self) -> bool: - """Freeze robust palm references captured at the confirmed baseline.""" - minimum = min(30, int(self.preflight_frames)) - pending = False - for view, runtime in self.views.items(): - if runtime.locked_base_pose is not None: - continue - observations = list(runtime.fixed_base_observations) - if len(observations) < minimum: - pending = True - continue - poses = [item[0] for item in observations] - centres = np.asarray([item[1] for item in observations], dtype=float) - qualities = [item[2] for item in observations] - translation = np.median( - np.asarray( - [pose.translation_xyz_m for pose in poses], dtype=float - ), - axis=0, - ) - quaternion = robust_rotation_summary( - [pose.quaternion_xyzw for pose in poses] - )[0] - reprojection = float( - np.percentile( - [pose.reprojection_error_px for pose in poses], 95.0 - ) - ) - runtime.locked_base_pose = SquareTagPose( - tuple(float(value) for value in quaternion), - tuple(float(value) for value in translation), - reprojection, - ) - runtime.locked_base_center_xy_px = tuple( - float(value) for value in np.median(centres, axis=0) - ) - corner_observations = list( - getattr(runtime, "fixed_base_corner_observations", ()) - ) - if len(corner_observations) >= minimum: - locked_corners = np.median( - np.asarray(corner_observations, dtype=float), axis=0 - ) - runtime.locked_base_corners_xy = tuple( - tuple(float(value) for value in point) - for point in locked_corners - ) - else: - runtime.locked_base_corners_xy = None - runtime.locked_base_corner_drift_count = 0 - runtime.latest_locked_base_corner_drift_px = 0.0 - runtime.locked_base_quality = TagQuality( - hamming=max(item.hamming for item in qualities), - decision_margin=min(item.decision_margin for item in qualities), - edge_pixels=min(item.edge_pixels for item in qualities), - reprojection_error_px=reprojection, - ) - translation_rms_m = float( - np.sqrt( - np.mean( - np.sum( - ( - np.asarray( - [pose.translation_xyz_m for pose in poses], - dtype=float, - ) - - translation - ) - ** 2, - axis=1, - ) - ) - ) - ) - rotation_p95_deg = math.degrees( - float( - np.percentile( - [ - ( - Rotation.from_quat(quaternion).inv() - * Rotation.from_quat(pose.quaternion_xyzw) - ).magnitude() - for pose in poses - ], - 95.0, - ) - ) - ) - append_jsonl( - self.raw_path, - { - "kind": "fixed_base_reference_locked", - "view": view, - "role": _fixed_base_role(view), - "tag_id": runtime.view_tags[_fixed_base_role(view)], - "sample_count": len(observations), - "translation_rms_m": round(translation_rms_m, 9), - "rotation_p95_deg": round(rotation_p95_deg, 6), - "reprojection_p95_px": round(reprojection, 6), - "corner_reference_xy": ( - None - if runtime.locked_base_corners_xy is None - else [ - list(point) - for point in runtime.locked_base_corners_xy - ] - ), - }, - ) - return not pending and all( - runtime.locked_base_pose is not None - for runtime in self.views.values() - ) - - def _group_tracker_for_roles( - self, runtime: ViewRuntime, roles: Sequence[str] - ) -> SquareTagGroupPoseTracker: - key = tuple(str(role) for role in roles) - group = runtime.group_trackers.get(key) - if group is None: - group = self._make_group_pose_tracker(runtime.name, key) - runtime.group_trackers[key] = group - return group - - @staticmethod - def _reset_view_trackers( - runtime: ViewRuntime, *, preserve_task_reference: bool = False - ) -> None: - runtime.tracker.reset() - for group in runtime.group_trackers.values(): - group.reset( - preserve_task_reference=preserve_task_reference - ) - - @staticmethod - def _reset_view_pnp_diagnostics(runtime: ViewRuntime) -> None: - """Clear PnP evidence only when a new capture scope begins.""" - runtime.latest_pnp_rejections.clear() - runtime.latest_group_pnp_reason = "" - runtime.latest_group_missing_candidate_roles = () - runtime.pnp_rejection_counts.clear() - runtime.group_pnp_rejection_counts.clear() - runtime.pnp_initialization_progress = None - runtime.latest_pnp_valid = False - runtime.last_pnp_diagnostic_signature = None - - def _record_group_pnp_candidate_event( - self, - *, - runtime: ViewRuntime, - stamp_ns: int, - required_roles: Sequence[str], - pose_roles: Sequence[str], - corners_by_role: Mapping[str, np.ndarray], - qualities: Mapping[str, TagQuality], - matched_tracking: tuple[tuple[float, ...], int] | None, - selected: Mapping[str, SquareTagPose] | None, - pnp_rejections: Mapping[str, str], - group_pnp_reason: str, - ) -> None: - """Persist bounded, replayable evidence for group candidate decisions. - - Valid FrameObservation records intentionally contain only accepted - poses. Without a separate event, a deterministic PnP dropout leaves - no corners, per-candidate tilt, reprojection error or missing role in - raw_samples.jsonl, making the next run guess at the cause. Record one - event per eight-count motor bucket and decision signature. This is - dense enough to locate a repeatable geometry boundary without writing - every 30 Hz rejected camera message. - """ - item = self.active_sweep - if ( - item is None - or self.state not in {STATE_PREPARE_SWEEP, STATE_SWEEP} - or runtime.name not in _sweep_views(_node_profile(self), item.spec) - ): - return - candidate_diagnostics = { - role: dict( - runtime.tracker.last_candidate_diagnostics_by_role.get( - role, {} - ) - ) - for role in pose_roles - if runtime.tracker.last_candidate_diagnostics_by_role.get(role) - } - oblique_group_roles = tuple( - role - for role, diagnostics in candidate_diagnostics.items() - if int(diagnostics.get("reprojection_candidate_count", 0)) > 0 - and int( - diagnostics.get("independent_tilt_candidate_count", 0) - ) - == 0 - ) - if ( - not group_pnp_reason - and not pnp_rejections - and not oblique_group_roles - ): - return - if selected is not None and not oblique_group_roles: - return - state_u8 = ( - matched_tracking[0] - if matched_tracking is not None - else self.latest_state_u8 - ) - motor_value = ( - None - if len(state_u8) != 20 - else float(state_u8[item.spec.motor_index]) - ) - motor_bucket = ( - -1 - if motor_value is None or not math.isfinite(motor_value) - else int(np.clip(math.floor(motor_value / 8.0), 0, 31)) - ) - missing_roles = tuple(runtime.latest_group_missing_candidate_roles) - signature = ( - item.spec.key, - item.cycle, - item.direction, - motor_bucket, - selected is not None, - str(group_pnp_reason), - tuple( - sorted( - (str(key), str(value)) - for key, value in pnp_rejections.items() - ) - ), - missing_roles, - oblique_group_roles, - ) - if runtime.last_pnp_diagnostic_signature == signature: - return - runtime.last_pnp_diagnostic_signature = signature - append_jsonl( - self.raw_path, - { - "kind": "group_pnp_candidate_event", - "outcome": ( - "selected_with_oblique_group_constraint" - if selected is not None - else "group_pose_rejected" - ), - "view": runtime.name, - "task_name": item.spec.key, - "motor_index": item.spec.motor_index, - "cycle": item.cycle, - "direction": item.direction, - "feedback_u8": motor_value, - "feedback_bucket_u8": ( - None if motor_bucket < 0 else motor_bucket * 8 - ), - "image_stamp_ns": int(stamp_ns), - "required_roles": list(required_roles), - "required_tag_ids": { - role: runtime.view_tags[role] - for role in required_roles - }, - "group_pnp_reason": str(group_pnp_reason), - "group_missing_candidate_roles": list(missing_roles), - "pnp_rejections": dict(pnp_rejections), - "oblique_group_roles": list(oblique_group_roles), - "candidate_diagnostics": candidate_diagnostics, - "tag_quality": { - role: { - "hamming": int(quality.hamming), - "decision_margin": float(quality.decision_margin), - "mean_edge_pixels": float(quality.edge_pixels), - } - for role, quality in qualities.items() - if role in required_roles - }, - "corners_xy": { - role: np.asarray(corners, dtype=float).tolist() - for role, corners in corners_by_role.items() - if role in required_roles - }, - "camera_intrinsics_sha256": runtime.intrinsics_sha256, - "camera_matrix": ( - None - if runtime.camera_matrix is None - else runtime.camera_matrix.tolist() - ), - }, - ) - - def _camera_info_callback(self, view: str, message: CameraInfo) -> None: - runtime = self.views[view] - valid = ( - int(message.width) > 0 - and int(message.height) > 0 - and len(message.p) == 12 - and float(message.p[0]) > 0.0 - and float(message.p[5]) > 0.0 - ) - matrix = None - if valid: - matrix = np.asarray(message.p, dtype=float).reshape(3, 4)[:, :3] - valid = bool( - np.all(np.isfinite(matrix)) - and matrix[0, 0] > 0.0 - and matrix[1, 1] > 0.0 - ) - if ( - matrix is not None - and runtime.camera_matrix is not None - and not np.allclose(matrix, runtime.camera_matrix) - ): - self._reset_view_trackers(runtime) - runtime.camera_info_valid = valid - runtime.camera_matrix = matrix if valid else None - runtime.camera_frame = str(message.header.frame_id) - runtime.image_width = int(message.width) - runtime.image_height = int(message.height) - runtime.intrinsics_sha256 = ( - "" - if matrix is None - else camera_info_fingerprint( - width=message.width, - height=message.height, - camera_matrix=message.k, - distortion=message.d, - rectification=message.r, - projection=message.p, - ) - ) - runtime.extrinsics_valid = False - if valid and self.extrinsics is not None: - runtime.extrinsics_valid = self.extrinsics.camera_matches( - view, - serial_number=self.camera_serials[view], - width=runtime.image_width, - height=runtime.image_height, - intrinsics_sha256=runtime.intrinsics_sha256, - ) - - def _state_callback(self, message: JointState) -> None: - command_names = _command_names(self) - if len(message.position) != len(command_names): - return - if ( - len(message.name) == len(command_names) - and set(message.name) == set(command_names) - ): - lookup = dict(zip(message.name, message.position)) - state = tuple(float(lookup[name]) for name in command_names) - else: - state = tuple(float(value) for value in message.position) - stamp = _stamp_ns(message.header.stamp) - if stamp <= 0: - stamp = int(self.get_clock().now().nanoseconds) - self.latest_state_u8 = state - self.last_state_at = time.monotonic() - self.state_receive_times.append(self.last_state_at) - if not self.state_history or stamp > self.state_history[-1].stamp_ns: - self.state_history.append(StateSample(stamp, state)) - - def _info_callback(self, message: String) -> None: - try: - value = json.loads(message.data) - except json.JSONDecodeError: - return - if isinstance(value, dict): - self.latest_hand_info = value - - def _quality_valid(self, quality: TagQuality, *, include_pnp: bool) -> bool: - return tag_quality_is_valid( - quality, - maximum_hamming=self.maximum_hamming, - minimum_decision_margin=self.minimum_decision_margin, - minimum_edge_pixels=self.minimum_edge_pixels, - maximum_reprojection_error_px=( - self.pnp_maximum_reprojection_error_px if include_pnp else None - ), - ) - - def _detections_callback( - self, view: str, message: AprilTagDetectionArray - ) -> None: - runtime = self.views[view] - now = time.monotonic() - stamp = _stamp_ns(message.header.stamp) - required_roles = self._required_roles_for_view(view) - locked_base_role = self._locked_base_role_for_active_capture(view) - live_required_roles = self._live_required_roles_for_view( - view, required_roles - ) - palm_observer: PalmAxisObserver | None = None - if ( - self.active_sweep is not None - and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} - ): - palm_observer = _palm_axis_observer_for_sweep( - _node_profile(self), self.active_sweep.spec, view - ) - palm_observer_roles = ( - () - if palm_observer is None - else (palm_observer.parent_role, palm_observer.child_role) - ) - combination_capture = bool( - self.active_combination_validation is not None - and self.state in {STATE_VALIDATION_MOVE, STATE_VALIDATION_CAPTURE} - ) - if required_roles != runtime.current_required_roles: - runtime.current_required_roles = required_roles - runtime.valid_flags.clear() - runtime.detection_times.clear() - self._reset_view_pnp_diagnostics(runtime) - runtime.task_valid_frames = 0 - runtime.task_total_frames = 0 - self._reset_view_trackers(runtime) - qualities: dict[str, TagQuality] = {} - corners_by_role: dict[str, np.ndarray] = {} - centres_by_role: dict[str, np.ndarray] = {} - for detection in message.detections: - role = runtime.role_by_id.get(int(detection.id)) - if role is None: - continue - corners = np.asarray( - [[float(point.x), float(point.y)] for point in detection.corners], - dtype=float, - ) - if corners.shape != (4, 2): - continue - edges = np.linalg.norm(corners - np.roll(corners, -1, axis=0), axis=1) - qualities[role] = TagQuality( - hamming=int(detection.hamming), - decision_margin=float(detection.decision_margin), - edge_pixels=float(np.mean(edges)), - ) - corners_by_role[role] = corners - centres_by_role[role] = np.mean(corners, axis=0) - - if ( - view == "top" - and locked_base_role is not None - and locked_base_role in corners_by_role - and runtime.locked_base_corners_xy is not None - ): - corner_drift = _maximum_corner_drift_px( - runtime.locked_base_corners_xy, - corners_by_role[locked_base_role], - ) - runtime.latest_locked_base_corner_drift_px = corner_drift - if corner_drift > self.fixed_base_maximum_corner_drift_px: - runtime.locked_base_corner_drift_count += 1 - else: - runtime.locked_base_corner_drift_count = 0 - if ( - runtime.locked_base_corner_drift_count - >= self.fixed_base_movement_confirmation_frames - ): - append_jsonl( - self.raw_path, - { - "kind": "fixed_base_reference_moved", - "view": view, - "role": locked_base_role, - "tag_id": runtime.view_tags[locked_base_role], - "corner_drift_px": round(corner_drift, 6), - "maximum_corner_drift_px": ( - self.fixed_base_maximum_corner_drift_px - ), - "confirmation_frames": ( - runtime.locked_base_corner_drift_count - ), - "task_name": ( - "" - if self.active_sweep is None - else self.active_sweep.spec.key - ), - }, - ) - self._pause("fixed_base_reference_moved") - return - - pose_roles = ( - tuple( - role - for role in runtime.roles - if role in qualities - and role != locked_base_role - and self._quality_valid(qualities[role], include_pnp=False) - ) - if combination_capture - else tuple( - dict.fromkeys( - ( - *live_required_roles, - *( - role - for role in palm_observer_roles - if role in qualities - and role != locked_base_role - and self._quality_valid( - qualities[role], include_pnp=False - ) - ), - ) - ) - ) - ) - if locked_base_role is not None: - # The fixed pose is injected below. A live top base remains a - # detection/movement requirement but is never re-solved into the - # pitch/roll/yaw geometry after the session reference is locked. - pose_roles = tuple( - role for role in pose_roles if role != locked_base_role - ) - observable_combination_joints = ( - _combination_observable_joints( - self.profile, - view, - ( - *pose_roles, - *((locked_base_role,) if locked_base_role else ()), - ), - ) - if combination_capture - else () - ) - detection_good = bool( - all(role in qualities for role in live_required_roles) - and all( - self._quality_valid(qualities[role], include_pnp=False) - for role in live_required_roles - ) - and ( - not combination_capture - or bool(observable_combination_joints) - ) - ) - tracking_command_u8: int | None = None - tracking_direction: str | None = None - matched_tracking: tuple[tuple[float, ...], int] | None = None - if ( - self.active_sweep is not None - and ( - view in _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - or palm_observer is not None - ) - and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} - ): - matched_tracking = interpolate_state_u8( - list(self.state_history), - stamp, - maximum_skew_ns=self.maximum_state_image_skew_ns, - ) - if matched_tracking is not None: - matched_tracking_state = matched_tracking[0] - if ( - self.state == STATE_SWEEP - or self._motion_command_reached( - self.active_sweep.spec, - self.active_sweep.start_u8, - matched_tracking_state, - ) - ): - # During PREPARE, learn only the final held start pose. - # In particular a roll task approaches 255 from its 127 - # baseline; caching that preparation motion as the formal - # decreasing trajectory would contaminate the same-command - # return reference before the real scan overwrites it. - tracking_command_u8 = int( - np.clip( - np.rint( - matched_tracking_state[ - self.active_sweep.spec.motor_index - ] - ), - 0, - 255, - ) - ) - tracking_direction = self.active_sweep.direction - selected: dict[str, SquareTagPose] | None = None - pnp_rejections: dict[str, str] = {} - group_pnp_reason = "" - if detection_good and runtime.camera_matrix is not None: - candidates: dict[str, tuple[SquareTagPose, ...]] = {} - individual_selected: dict[str, SquareTagPose] = {} - for role in pose_roles: - pose, rejection = runtime.tracker.estimate( - role, - corners_by_role[role], - tag_size_m=float( - runtime.tag_sizes_m_by_role.get( - role, runtime.tag_size_m - ) - ), - camera_matrix=runtime.camera_matrix, - stamp_ns=stamp, - ) - if pose is not None: - individual_selected[role] = pose - candidates[role] = runtime.tracker.last_candidates_by_role.get( - role, () - ) - # A group-constrained task deliberately retains an oblique - # candidate that the independent per-Tag selector would not - # use. Report a rejection only if candidate generation itself - # failed; otherwise the group tracker owns the decision. - if rejection and ( - combination_capture or not candidates[role] - ): - pnp_rejections[role] = rejection - if combination_capture: - # Each visible target is tracked independently. This avoids - # an exponential all-Tag IPPE search and, more importantly, - # lets an occluded neighbouring finger remain optional. - selected = individual_selected - else: - if locked_base_role is not None: - assert runtime.locked_base_pose is not None - candidates[locked_base_role] = ( - runtime.locked_base_pose, - ) - group_tracker = self._group_tracker_for_roles( - runtime, required_roles - ) - selected, group_pnp_reason = group_tracker.select( - candidates, - stamp_ns=stamp, - trajectory_command_u8=tracking_command_u8, - trajectory_direction=tracking_direction, - ) - runtime.latest_group_missing_candidate_roles = ( - group_tracker.last_missing_roles - ) - if selected is not None and palm_observer_roles: - selected = dict(selected) - for role in palm_observer_roles: - pose = individual_selected.get(role) - quality = qualities.get(role) - if pose is None or quality is None: - continue - pose_quality = TagQuality( - hamming=quality.hamming, - decision_margin=quality.decision_margin, - edge_pixels=quality.edge_pixels, - reprojection_error_px=pose.reprojection_error_px, - ) - if self._quality_valid( - pose_quality, include_pnp=True - ): - selected[role] = pose - if selected is not None: - qualities = _selected_pose_qualities( - selected, - qualities, - locked_base_role=locked_base_role, - locked_base_quality=runtime.locked_base_quality, - ) - # Preserve exactly which Tags were present in this camera message. - # The cached palm reference participates in geometry and quality - # checks below, but must never be reported as a live detection. - live_qualities = { - role: quality - for role, quality in qualities.items() - if role in corners_by_role - } - if selected is not None and locked_base_role is not None: - assert runtime.locked_base_pose is not None - assert runtime.locked_base_center_xy_px is not None - assert runtime.locked_base_quality is not None - selected = dict(selected) - selected[locked_base_role] = runtime.locked_base_pose - centres_by_role[locked_base_role] = np.asarray( - runtime.locked_base_center_xy_px, dtype=float - ) - qualities[locked_base_role] = runtime.locked_base_quality - valid = bool( - selected is not None - and all(role in selected for role in required_roles) - and all( - self._quality_valid(qualities[role], include_pnp=True) - for role in ( - selected - if combination_capture - else required_roles - ) - ) - and ( - not combination_capture - or bool( - _combination_observable_joints( - self.profile, view, tuple(selected) - ) - ) - ) - ) - if ( - self.state == STATE_SWEEP - and self.active_sweep is not None - and view in _sweep_views(_node_profile(self), self.active_sweep.spec) - ): - self.sweep_detection_total_frames += 1 - total_by_view = getattr( - self, "sweep_detection_total_by_view", None - ) - if total_by_view is None: - self.sweep_detection_total_by_view = {} - total_by_view = self.sweep_detection_total_by_view - total_by_view[view] = total_by_view.get(view, 0) + 1 - if valid: - self.sweep_detection_valid_frames += 1 - valid_by_view = getattr( - self, "sweep_detection_valid_by_view", None - ) - if valid_by_view is None: - self.sweep_detection_valid_by_view = {} - valid_by_view = self.sweep_detection_valid_by_view - valid_by_view[view] = valid_by_view.get(view, 0) + 1 - runtime.latest_pnp_rejections = pnp_rejections - runtime.latest_group_pnp_reason = group_pnp_reason - if not group_pnp_reason: - runtime.latest_group_missing_candidate_roles = () - runtime.latest_pnp_valid = valid - task_pnp_capture = bool( - self.active_sweep is not None - and self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} - and view in _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - ) - if task_pnp_capture: - for role, rejection in pnp_rejections.items(): - key = f"{role}:{rejection}" - runtime.pnp_rejection_counts[key] = ( - runtime.pnp_rejection_counts.get(key, 0) + 1 - ) - if group_pnp_reason.startswith("group_initializing:"): - try: - progress = group_pnp_reason.split(":", 1)[1] - accepted, required = progress.split("/", 1) - runtime.pnp_initialization_progress = ( - int(accepted), - int(required), - ) - except (IndexError, TypeError, ValueError): - runtime.pnp_initialization_progress = None - elif group_pnp_reason: - runtime.group_pnp_rejection_counts[group_pnp_reason] = ( - runtime.group_pnp_rejection_counts.get( - group_pnp_reason, 0 - ) - + 1 - ) - elif selected is not None and runtime.pnp_initialization_progress: - required = runtime.pnp_initialization_progress[1] - runtime.pnp_initialization_progress = (required, required) - self._record_group_pnp_candidate_event( - runtime=runtime, - stamp_ns=stamp, - required_roles=required_roles, - pose_roles=pose_roles, - corners_by_role=corners_by_role, - qualities=qualities, - matched_tracking=matched_tracking, - selected=selected, - pnp_rejections=pnp_rejections, - group_pnp_reason=group_pnp_reason, - ) - if view == "top": - runtime.pnp_invalid_since, reset_due = update_pnp_reset_watchdog( - detection_good=bool( - detection_good and runtime.camera_matrix is not None - ), - pnp_valid=valid, - now=now, - invalid_since=runtime.pnp_invalid_since, - reset_after_seconds=self.top_pnp_invalid_reset_seconds, - ) - if reset_due: - self._reset_view_trackers(runtime) - runtime.pnp_reset_count += 1 - self.get_logger().warning( - "Top-view PnP was continuously invalid for " - f"{self.top_pnp_invalid_reset_seconds:.1f}s; reset trackers " - f"(count={runtime.pnp_reset_count}, " - f"group_reason={group_pnp_reason or 'none'}, " - f"tag_rejections={pnp_rejections})" - ) - if ( - valid - and selected is not None - and self.state == STATE_PREFLIGHT - and self.started - and self.startup_baseline_recovered - ): - base_role = _fixed_base_role(view) - if ( - base_role in selected - and base_role in centres_by_role - and base_role in live_qualities - ): - runtime.fixed_base_observations.append( - ( - selected[base_role], - tuple( - float(value) - for value in centres_by_role[base_role] - ), - live_qualities[base_role], - ) - ) - runtime.fixed_base_corner_observations.append( - np.asarray(corners_by_role[base_role], dtype=float).copy() - ) - runtime.latest_tag_quality = live_qualities - runtime.last_message_at = now - runtime.detection_times.append(now) - runtime.valid_flags.append(valid) - task_sweep_capture = bool( - self.state == STATE_SWEEP - and self.active_sweep is not None - and view in _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - ) - if ( - runtime.current_required_roles - != tuple(runtime.preflight_roles) - and task_sweep_capture - ): - # Attempt-level validity is a motion-trajectory metric. PREPARE - # deliberately rejects the first N frames while the group PnP - # tracker initializes; counting those expected frames made clean - # side tasks report about 94% and triggered full four-round - # rescans despite 100% visibility prechecks and dense trajectories. - runtime.task_total_frames += 1 - if valid: - runtime.task_valid_frames += 1 - if not valid or selected is None: - return - runtime.last_valid_at = now - - # Preflight and motion-only states need pose quality/status, but do not - # consume FrameObservation objects. Building transforms and payloads - # for every measured joint on all three 30 Hz streams was pure work - # and could starve the detection subscriptions. Only materialize an - # observation for the one view used by an active capture stage. - sweep_capture = bool( - self.state in {STATE_PREPARE_SWEEP, STATE_SWEEP} - and self.active_sweep is not None - and view in _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - ) - palm_axis_capture = bool( - self.state == STATE_SWEEP - and self.active_sweep is not None - and not self.active_sweep.precheck - and self.active_sweep.cycle >= 0 - and palm_observer is not None - ) - validation_capture = bool( - self.state == STATE_VALIDATION_CAPTURE - and ( - ( - self.active_validation is not None - and view in _sweep_views( - self.profile, self.active_validation.spec - ) - ) - or self.active_combination_validation is not None - ) - ) - if not sweep_capture and not validation_capture and not palm_axis_capture: - return - - matched = matched_tracking - if matched is None: - matched = interpolate_state_u8( - list(self.state_history), - stamp, - maximum_skew_ns=self.maximum_state_image_skew_ns, - ) - if matched is None: - return - state_u8, sync_error_ns = matched - joint_vectors: dict[str, tuple[float, float, float]] = {} - image_vectors: dict[str, tuple[float, float]] = {} - joint_quaternions: dict[str, tuple[float, float, float, float]] = {} - parent_poses_common: dict[str, Mapping[str, list[float]]] = {} - child_poses_common: dict[str, Mapping[str, list[float]]] = {} - joint_reprojection: dict[str, float] = {} - if self.extrinsics is None or not runtime.extrinsics_valid: - return - front_from_view = self.extrinsics.transform(view) - if palm_axis_capture and palm_observer is not None: - self._record_palm_axis_sample( - palm_observer, - selected, - front_from_view, - state_u8, - sync_error_ns=int(sync_error_ns), - stamp_ns=stamp, - ) - if not sweep_capture and not validation_capture: - return - for name, spec in self.profile.record_specs.items(): - if not spec.measured or spec.view != view: - continue - assert spec.parent_role is not None and spec.child_role is not None - if ( - spec.parent_role not in selected - or spec.child_role not in selected - ): - continue - parent = selected[spec.parent_role] - child = selected[spec.child_role] - parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) - relative = parent_rotation.inv().apply( - np.asarray(child.translation_xyz_m, dtype=float) - - np.asarray(parent.translation_xyz_m, dtype=float) - ) - image_relative = ( - centres_by_role[spec.child_role] - - centres_by_role[spec.parent_role] - ) - parent_matrix = front_from_view @ transform_matrix( - parent.translation_xyz_m, parent.quaternion_xyzw - ) - child_matrix = front_from_view @ transform_matrix( - child.translation_xyz_m, child.quaternion_xyzw - ) - relative_rotation = Rotation.from_matrix( - parent_matrix[:3, :3] - ).inv() * Rotation.from_matrix(child_matrix[:3, :3]) - joint_vectors[name] = tuple(float(value) for value in relative) - image_vectors[name] = tuple(float(value) for value in image_relative) - joint_quaternions[name] = tuple( - float(value) for value in relative_rotation.as_quat() - ) - parent_poses_common[name] = matrix_payload(parent_matrix) - child_poses_common[name] = matrix_payload(child_matrix) - joint_reprojection[name] = max( - float(parent.reprojection_error_px), - float(child.reprojection_error_px), - ) - observation = FrameObservation( - stamp_ns=stamp, - received_at=now, - view=view, - state_u8=state_u8, - state_sync_error_ns=int(sync_error_ns), - joint_vectors_xyz_m=joint_vectors, - image_vectors_xy_px=image_vectors, - joint_quaternions_xyzw=joint_quaternions, - parent_poses_common=parent_poses_common, - child_poses_common=child_poses_common, - joint_reprojection_error_px=joint_reprojection, - ) - self._accept_frame(observation) - - def _record_palm_axis_sample( - self, - observer: PalmAxisObserver, - selected: Mapping[str, SquareTagPose], - common_from_view: np.ndarray, - state_u8: Sequence[float], - *, - sync_error_ns: int, - stamp_ns: int, - ) -> None: - """Buffer one direction-only sample without touching sweep validity.""" - item = self.active_sweep - if ( - item is None - or item.spec.key != observer.task_name - or observer.parent_role not in selected - or observer.child_role not in selected - ): - return - parent = selected[observer.parent_role] - child = selected[observer.child_role] - parent_matrix = common_from_view @ transform_matrix( - parent.translation_xyz_m, parent.quaternion_xyzw - ) - child_matrix = common_from_view @ transform_matrix( - child.translation_xyz_m, child.quaternion_xyzw - ) - relative_rotation = ( - Rotation.from_matrix(parent_matrix[:3, :3]).inv() - * Rotation.from_matrix(child_matrix[:3, :3]) - ) - feedback = float(state_u8[observer.motor_index]) - durable = canonical_sample_record({ - "kind": "palm_axis_sample", - "attempt": self.sweep_attempts.get( - _sweep_storage_key(item.spec), 1 - ), - "task_name": item.spec.key, - "view": observer.view, - "source_joint": observer.source_name, - "model_joint": observer.model_joint, - "motor_index": int(observer.motor_index), - "cycle": int(item.cycle), - "direction": item.direction, - "requested_command_u8": int(item.target_u8), - "feedback_u8": feedback, - "relative_quaternion_xyzw": [ - float(value) for value in relative_rotation.as_quat() - ], - "parent_pose_common": matrix_payload(parent_matrix), - "child_pose_common": matrix_payload(child_matrix), - "state_u8": [float(value) for value in state_u8], - "state_image_sync_error_ms": round( - abs(int(sync_error_ns)) / 1_000_000.0, 6 - ), - "pnp_reprojection_error_px": round( - max( - float(parent.reprojection_error_px), - float(child.reprojection_error_px), - ), - 6, - ), - "image_stamp_ns": int(stamp_ns), - }) - record = fitting_sample_record(durable) - self.palm_axis_records_by_source.setdefault( - observer.source_name, [] - ).append(record) - - def _persist_palm_axis_samples(self, item: SweepItem) -> None: - """Commit the side channel only after its authoritative sweep passes.""" - observer = next( - ( - candidate - for candidate in self.profile.palm_axis_observers - if candidate.task_name == item.spec.key - ), - None, - ) - if observer is None: - return - attempt = int( - self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) - ) - rows = [ - record - for record in self.palm_axis_records_by_source.get( - observer.source_name, () - ) - if int(record.get("attempt", 1)) == attempt - and int(record.get("cycle", -1)) == int(item.cycle) - and str(record.get("direction", "")) == item.direction - ] - append_jsonl_many( - self.raw_path, - (canonical_sample_record(record) for record in rows), - ) - - def _accept_frame(self, observation: FrameObservation) -> None: - if self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: - if observation.view not in _sweep_views( - _node_profile(self), self.active_sweep.spec - ): - return - if not self._motion_command_reached( - self.active_sweep.spec, - self.active_sweep.start_u8, - observation.state_u8, - ): - return - self.sweep_start_frames.append(observation) - maximum_start_frames = max( - 1, int(getattr(self, "preflight_frames", 30)) - ) * len( - _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - ) - if len(self.sweep_start_frames) > maximum_start_frames: - del self.sweep_start_frames[:-maximum_start_frames] - return - if self.state == STATE_SWEEP and self.active_sweep is not None: - if observation.view not in _sweep_views( - self.profile, self.active_sweep.spec - ): - return - motor = self.active_sweep.spec.motor_index - value = float(observation.state_u8[motor]) - if not -3.0 <= value <= 258.0: - return - if not self._motion_command_reached( - self.active_sweep.spec, - int(np.clip(np.rint(value), 0, 255)), - observation.state_u8, - ): - return - self.sweep_frames.append(observation) - checkpoint_target = getattr( - self, "sweep_checkpoint_target_u8", None - ) - if ( - checkpoint_target is not None - and getattr(self, "sweep_checkpoint_hold_since", None) - is not None - and self._steady_checkpoint_reached( - self.active_sweep.spec, - int(checkpoint_target), - observation.state_u8, - ) - ): - self.sweep_checkpoint_frames.append(observation) - if ( - getattr(self, "sweep_baseline_pending", False) - and getattr(self, "sweep_baseline_hold_since", None) - is not None - and observation.received_at - - float(self.sweep_baseline_hold_since) - >= float(self.baseline_hold_seconds) - and self._motion_command_reached( - self.active_sweep.spec, - int( - self.baseline_command[ - self.active_sweep.spec.motor_index - ] - ), - observation.state_u8, - ) - ): - self.sweep_baseline_frames.append(observation) - self.sweep_last_valid_at = observation.received_at - last_by_view = getattr( - self, "sweep_last_valid_at_by_view", None - ) - if last_by_view is None: - self.sweep_last_valid_at_by_view = {} - last_by_view = self.sweep_last_valid_at_by_view - last_by_view[observation.view] = observation.received_at - return - if ( - self.state == STATE_VALIDATION_CAPTURE - and self.active_validation is not None - and observation.view in _sweep_views( - self.profile, self.active_validation.spec - ) - ): - if self._motion_command_reached( - self.active_validation.spec, - self.active_validation.command_u8, - observation.state_u8, - ): - self.validation_frames_buffer.append(observation) - return - if ( - self.state == STATE_VALIDATION_CAPTURE - and self.active_combination_validation is not None - and self._command_vector_reached( - self.active_combination_validation.command_u8 - ) - ): - self.combination_validation_frames_buffer[ - observation.view - ].append(observation) - - def _view_ready(self, runtime: ViewRuntime, now: float) -> bool: - minimum_frames = min(30, self.preflight_frames) - return bool( - runtime.camera_info_valid - and runtime.extrinsics_valid - and len(runtime.valid_flags) >= minimum_frames - and runtime.valid_rate >= self.minimum_detection_rate - and runtime.detection_hz >= self.minimum_detection_hz - and now - runtime.last_message_at <= 1.0 - ) - - def _all_preflight_ready(self, now: float) -> bool: - feedback_hz = ( - 0.0 - if len(self.state_receive_times) < 2 - or self.state_receive_times[-1] <= self.state_receive_times[0] - else (len(self.state_receive_times) - 1) - / (self.state_receive_times[-1] - self.state_receive_times[0]) - ) - return bool( - self.extrinsics is not None - and len(self.latest_state_u8) == 20 - and now - self.last_state_at <= 1.0 - and feedback_hz >= self.minimum_feedback_hz - and all(self._view_ready(runtime, now) for runtime in self.views.values()) - ) - - def _all_devices_ready(self, now: float) -> bool: - """Check only hardware transport needed for a safe baseline return. - - Tag visibility must not gate this check: an interrupted calibration can - leave the fingers in a pose that occludes a fixed palm Tag. Requiring - that Tag before commanding baseline would create a startup deadlock. - """ - feedback_hz = ( - 0.0 - if len(self.state_receive_times) < 2 - or self.state_receive_times[-1] <= self.state_receive_times[0] - else (len(self.state_receive_times) - 1) - / (self.state_receive_times[-1] - self.state_receive_times[0]) - ) - return bool( - self.extrinsics is not None - and len(self.latest_state_u8) == 20 - and now - self.last_state_at <= 1.0 - and feedback_hz >= self.minimum_feedback_hz - and all( - runtime.camera_info_valid - and runtime.extrinsics_valid - and runtime.last_message_at > 0.0 - and now - runtime.last_message_at <= 1.0 - for runtime in self.views.values() - ) - ) - - def _finish_startup_baseline_recovery(self) -> None: - """Start a fresh fixed-palm-Tag window at confirmed baseline.""" - for runtime in self.views.values(): - runtime.valid_flags.clear() - runtime.detection_times.clear() - observations = getattr(runtime, "fixed_base_observations", None) - if observations is not None: - observations.clear() - corner_observations = getattr( - runtime, "fixed_base_corner_observations", None - ) - if corner_observations is not None: - corner_observations.clear() - runtime.locked_base_pose = None - runtime.locked_base_center_xy_px = None - runtime.locked_base_corners_xy = None - runtime.locked_base_quality = None - runtime.locked_base_corner_drift_count = 0 - runtime.latest_locked_base_corner_drift_px = 0.0 - self._reset_view_trackers(runtime) - self.startup_baseline_recovered = True - self.state = STATE_PREFLIGHT - self.reason = "waiting_for_baseline_tags_after_recovery" - - def _active_view_for_resume(self) -> str | None: - if self.active_sweep is not None: - return self.active_sweep.spec.view - if self.active_validation is not None: - return self.active_validation.spec.view - if self.retry_sweep_spec is not None: - return self.retry_sweep_spec.view - return None - - def _active_views_for_resume(self) -> tuple[str, ...]: - if self.active_sweep is not None: - return _sweep_views(_node_profile(self), self.active_sweep.spec) - if self.active_validation is not None: - return _sweep_views(_node_profile(self), self.active_validation.spec) - if self.retry_sweep_spec is not None: - return _sweep_views(_node_profile(self), self.retry_sweep_spec) - return required_resume_views(None) - - def _resume_preflight_ready(self, now: float) -> bool: - if ( - len(self.latest_state_u8) != _command_count(self) - or now - self.last_state_at > 1.0 - ): - return False - return all( - self._view_ready(self.views[view], now) - for view in self._active_views_for_resume() - ) - - def _prepare_failed_sweep_retry(self) -> SweepSpec: - """Discard only failed measurements/cycles and queue two directions.""" - if self.retry_sweep_spec is None: - raise RuntimeError("no failed sweep is pending retry") - spec = self.retry_sweep_spec - repetitions = int(getattr(self, "repetitions", 3)) - cycles = set(getattr(self, "retry_cycles", set(range(repetitions)))) - profile = _node_profile(self) - source_task_keys = ( - tuple(getattr(self, "retry_source_task_keys", ())) - if getattr(self, "retry_source_failure_task_key", None) == spec.key - else () - ) - source_key_set = set(source_task_keys) - source_specs = [ - candidate - for candidate in profile.sweep_specs - if candidate.key in source_key_set - ] - # Yaw zero is a same-view relationship between the pitch and roll - # axes. A retry must remain in the original top-camera PnP reference - # generation; otherwise the retry changes the datum it is intended to - # verify. Ordinary single-task retries still reset their camera to - # recover from a genuinely bad branch. - self.retry_preserve_pnp_continuity = bool(source_specs) - data_specs = source_specs or [spec] - retry_joint_names = set(getattr(self, "retry_joint_names", set())) - if source_specs: - retry_joint_names = { - joint_name - for candidate in source_specs - for joint_name in candidate.joints - } - else: - retry_joint_names = retry_joint_names or set(spec.joints) - retry_joint_names.intersection_update(spec.joints) - if not retry_joint_names: - retry_joint_names = set(spec.joints) - self.retry_joint_names = retry_joint_names - baseline_records_by_joint = getattr( - self, "baseline_records_by_joint", {} - ) - for joint_name in retry_joint_names: - self.records_by_joint[joint_name][:] = [ - record - for record in self.records_by_joint[joint_name] - if int(record.get("cycle", -1)) not in cycles - ] - if joint_name in baseline_records_by_joint: - baseline_records_by_joint[joint_name][:] = [ - record - for record in baseline_records_by_joint[joint_name] - if int(record.get("cycle", -1)) not in cycles - ] - command_records = getattr(self, "command_records_by_joint", {}) - if joint_name in command_records: - command_records[joint_name][:] = [ - record - for record in command_records[joint_name] - if int(record.get("cycle", -1)) not in cycles - ] - data_task_keys = {candidate.key for candidate in data_specs} - observers = [ - item - for item in profile.palm_axis_observers - if item.task_name in data_task_keys - ] - for observer in observers: - records = self.palm_axis_records_by_source.get( - observer.source_name, [] - ) - records[:] = [ - record - for record in records - if int(record.get("cycle", -1)) not in cycles - ] - all_items = getattr(self, "sweep_items", []) - self.retry_sweep_items = [ - item - for item in all_items - if item.spec in data_specs and item.cycle in cycles - ] - if not self.retry_sweep_items: - self.retry_sweep_items = [ - SweepItem(candidate, cycle, direction) - for candidate in data_specs - for cycle in sorted(cycles) - for direction in ( - DIRECTION_DECREASING, - DIRECTION_INCREASING, - ) - ] - # A planar Tag PnP tracker can occasionally remain on a continuous but - # biased pose branch for an otherwise clean sweep. Starting a full - # fit retry with that tracker state intact tends to reproduce the same - # orientation residual, so reset only the affected camera before the - # retry. Raw samples remain logged and all final quality gates stay - # unchanged. - reset_trackers = getattr(self, "_reset_view_trackers", None) - retry_views = { - profile.record_specs[name].view - for name in retry_joint_names - } - for observer in observers: - retry_views.add(observer.view) - for view in retry_views: - if view is None: - continue - runtime = getattr(self, "views", {}).get(view) - if runtime is None or reset_trackers is None: - continue - if self.retry_preserve_pnp_continuity: - # Frame-quality counters are attempt-local even though the - # geometric PnP state deliberately remains continuous. - runtime.task_valid_frames = 0 - runtime.task_total_frames = 0 - continue - if _node_profile(self).layout_id == G20_RIGHT_19_LAYOUT: - reset_trackers(runtime, preserve_task_reference=True) - else: - reset_trackers(runtime) - runtime.pnp_invalid_since = None - runtime.pnp_reset_count += 1 - # Detection quality is attempt-local. Carrying the previous - # attempt into this retry made its percentage describe two PnP - # branches instead of the data currently being admitted. - runtime.task_valid_frames = 0 - runtime.task_total_frames = 0 - storage_key = _sweep_storage_key(spec) - self.sweep_attempts[storage_key] = ( - self.sweep_attempts.get(storage_key, 1) + 1 - ) - source_attempts: dict[str, int] = {} - for source_spec in source_specs: - source_key = _sweep_storage_key(source_spec) - self.sweep_attempts[source_key] = ( - self.sweep_attempts.get(source_key, 1) + 1 - ) - source_attempts[str(source_spec.key)] = self.sweep_attempts[ - source_key - ] - invalidated_storage_keys = { - storage_key, - *(_sweep_storage_key(candidate) for candidate in source_specs), - } - for key in list(getattr(self, "sweep_retry_counts", {})): - if key[0] in invalidated_storage_keys: - self.sweep_retry_counts.pop(key, None) - append_jsonl( - self.raw_path, - { - "kind": "retry", - "view": spec.view, - **({"task_name": spec.key} if spec.task_name else {}), - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "joints_to_rescan": sorted(retry_joint_names), - **( - {"source_task_names": list(source_task_keys)} - if source_task_keys - else {} - ), - **( - {"source_attempts": source_attempts} - if source_attempts - else {} - ), - **( - {"pnp_reference_policy": "preserve_existing_generation"} - if self.retry_preserve_pnp_continuity - else {} - ), - "attempt": self.sweep_attempts[storage_key], - "reason": self.paused_reason, - "cycles": [cycle + 1 for cycle in sorted(cycles)], - }, - ) - self.fit_failure = {} - G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) - getattr(self, "motion_stall_details", {}).clear() - return spec - - def _invalidate_fitted_calibration_state(self) -> None: - """Invalidate every artifact derived from the current motion records. - - A task rescan changes the observation set, so curves, zero offsets and - validation diagnostics must be treated as one generation. Keeping - this transition in one place prevents final publication from mixing a - newly collected task with state left by an earlier fit attempt. - """ - self.measured_fits = {} - self.axis_measurements = [] - self.palm_orientation_measurements = [] - self.palm_orientation_rejections = {} - self.zero_result = None - self.validated_endpoint_zero_offsets_rad = {} - self.corrected_urdf_path = None - self.fit_quality_passed = False - self.validation_errors_rad = [] - self.cross_view_roll_metrics = {} - self.validation_only_fits = {} - self.joint_dynamic_diagnostics = {} - - def _endpoint_zero_offsets_for_publication(self) -> dict[str, float]: - """Return the endpoint anchors belonging to the validated zero fit.""" - stored = { - str(name): float(value) - for name, value in getattr( - self, "validated_endpoint_zero_offsets_rad", {} - ).items() - } - if self.profile.layout_id != G20_RIGHT_19_LAYOUT: - if stored: - raise RuntimeError( - "validated_endpoint_zero_state_unexpected_for_layout" - ) - return {} - expected = set(RIGHT_19_MECHANICAL_ENDPOINT_JOINTS) - if getattr(self, "standalone_thumb_calibration", False): - expected = { - name for name in expected if name.startswith("thumb_") - } - actual = set(stored) - if actual != expected: - missing = ",".join(sorted(expected - actual)) or "-" - extra = ",".join(sorted(actual - expected)) or "-" - raise RuntimeError( - "validated_endpoint_zero_state_incomplete:" - f"missing={missing};extra={extra}" - ) - if not all(math.isfinite(value) for value in stored.values()): - raise RuntimeError("validated_endpoint_zero_state_non_finite") - if self.zero_result is None: - raise RuntimeError("URDF zero solution is missing") - solved = self.zero_result.direct_offsets_rad - diverged = sorted( - name - for name, value in stored.items() - if name not in solved - or not math.isclose( - value, - float(solved[name]), - rel_tol=0.0, - abs_tol=1.0e-12, - ) - ) - if diverged: - raise RuntimeError( - "validated_endpoint_zero_state_diverged:" - + ",".join(diverged) - ) - return stored - - def _restore_durable_task_checkpoint(self) -> int: - """Restore independently validated complete tasks from a failed session.""" - source = self.resume_raw_samples_path - if source is None: - return 0 - if not source.is_file(): - raise RuntimeError(f"resume raw samples do not exist: {source}") - if source.resolve() == self.raw_path.resolve(): - raise RuntimeError("resume raw samples must come from an older session") - rows: list[dict[str, Any]] = [] - source_size = max(1, int(source.stat().st_size)) - bytes_read = 0 - with source.open("rb") as stream: - for line_number, raw_line in enumerate(stream, start=1): - bytes_read += len(raw_line) - if not raw_line.strip(): - continue - try: - line = raw_line.decode("utf-8") - value = json.loads(line) - except (UnicodeDecodeError, json.JSONDecodeError) as error: - raise RuntimeError( - f"resume raw JSON is invalid at line {line_number}" - ) from error - if isinstance(value, dict): - rows.append(value) - if line_number % 1000 == 0: - self.base_import_progress = { - "phase": "reading", - "records_read": len(rows), - "bytes_read": bytes_read, - "total_bytes": source_size, - "fraction": min(1.0, bytes_read / source_size), - } - self.reason = "reading_base_session_records" - _publish_import_status(self) - starts = [row for row in rows if row.get("kind") == "session_start"] - if len(starts) != 1: - raise RuntimeError("resume raw must contain exactly one session_start") - start = starts[0] - previous_tag_sizes = { - int(tag_id): float(size) - for tag_id, size in dict( - start.get("tag_sizes_m_by_id", {}) - ).items() - } - current_tag_sizes = dict(getattr(self, "tag_sizes_m_by_id", {})) - try: - ( - palm_axis_schema_compatible, - palm_axis_invalidated_tasks, - ) = _palm_axis_resume_policy( - self.profile, - start, - getattr( - getattr( - getattr(self, "calibration_profile", None), - "artifacts", - None, - ), - "session_compatibility_tokens", - frozenset(self.profile.capabilities), - ), - ) - except ValueError as error: - raise RuntimeError( - "resume checkpoint algorithm capabilities differ" - ) from error - if ( - str(start.get("model", "G20")).upper() - != str(getattr(self, "model", "G20")).upper() - or str(start.get("hand_type")) != self.hand_type - or str(start.get("tag_layout")) != self.profile.layout_id - or start.get("view_tags") - != {view: dict(tags) for view, tags in self.profile.view_tags.items()} - or tuple(int(value) for value in start.get("baseline_command_u8", [])) - != tuple(self.baseline_command) - or str(start.get("source_urdf_sha256", "")) - != _file_sha256(self.source_urdf_path) - ): - raise RuntimeError( - "resume checkpoint geometry, algorithm capabilities, Tag " - "layout, baseline or source URDF differs" - ) - try: - changed_tag_size_ids, size_invalidated_tasks = ( - resume_tasks_invalidated_by_tag_size_changes( - self.profile, - previous_tag_sizes, - current_tag_sizes, - ) - ) - except ValueError as error: - raise RuntimeError( - "resume checkpoint Tag-size geometry differs incompatibly" - ) from error - invalidated_task_set = set(size_invalidated_tasks) - invalidated_task_set.update(palm_axis_invalidated_tasks) - scope_invalidated_tasks = tuple( - getattr(self, "recalibration_task_keys", ()) or () - ) - invalidated_task_set.update(scope_invalidated_tasks) - if invalidated_task_set: - rows = [ - row - for row in rows - if str(row.get("task_name", "")) not in invalidated_task_set - ] - completed, reusable = resumable_completed_task_prefix( - self.profile, - self.repetitions, - self.baseline_command, - rows, - minimum_sweep_bins=self.minimum_sweep_bins, - allow_sparse=True, - ) - validate_sample_records( - row - for row in reusable - if str(row.get("kind", "")) in RESUMABLE_SAMPLE_KINDS - or row.get("kind") == "palm_axis_sample" - ) - self.state = STATE_REVALIDATING_INHERITED - self.reason = "revalidating_inherited_tasks" - self.base_import_progress = { - "phase": "revalidating", - "records_read": len(rows), - "completed_tasks": 0, - "total_tasks": len(completed), - "fraction": 0.0, - } - _publish_import_status(self) - for durable in reusable: - kind = str(durable["kind"]) - if kind == "synchronised_frame": - continue - if kind == "palm_axis_sample": - source_name = str(durable.get("source_joint", "")) - if source_name not in self.palm_axis_records_by_source: - raise RuntimeError( - "resume checkpoint contains unknown palm-axis source " - + source_name - ) - record = fitting_sample_record(durable) - self.palm_axis_records_by_source[source_name].append(record) - continue - joint_name = str(durable["joint"]) - if joint_name not in self.records_by_joint: - raise RuntimeError( - f"resume checkpoint contains unknown joint {joint_name}" - ) - if kind not in { - "sample", "baseline_hold_sample", "steady_command_sample" - }: - continue - record = fitting_sample_record(durable) - if kind == "sample": - self.records_by_joint[joint_name].append(record) - elif kind == "baseline_hold_sample": - self.baseline_records_by_joint[joint_name].append(record) - else: - self.command_records_by_joint[joint_name].append(record) - completed, dropped = G20ThreeCameraCalibrationNode._revalidate_imported_tasks( - self, completed, allow_sparse=True - ) - completed_set = set(completed) - reusable = tuple( - row - for row in reusable - if str(row.get("task_name", "")) in completed_set - ) - # Attempts are durable identities, not counters local to one process. - # Without this floor a resumed task whose accepted data used attempt 2 - # would label its next retry as attempt 2 again. The raw log then - # contained two different acquisitions with the same identity and an - # offline replay merged them into one invalid trajectory. - attempt_floor_by_task: dict[str, int] = {} - for row in reusable: - task_key = str(row.get("task_name", "")) - if task_key not in completed_set: - continue - try: - attempt = max(1, int(row.get("attempt", 1))) - except (TypeError, ValueError): - attempt = 1 - attempt_floor_by_task[task_key] = max( - attempt_floor_by_task.get(task_key, 1), attempt - ) - attempts = getattr(self, "sweep_attempts", None) - if attempts is None: - self.sweep_attempts = { - _sweep_storage_key(spec): 1 - for spec in self.profile.sweep_specs - } - attempts = self.sweep_attempts - for spec in self.profile.sweep_specs: - if spec.key not in completed_set: - continue - storage_key = _sweep_storage_key(spec) - attempts[storage_key] = max( - int(attempts.get(storage_key, 1)), - attempt_floor_by_task.get(spec.key, 1), - ) - 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 - if spec.key not in completed_set - ] - append_jsonl( - self.raw_path, - { - "kind": "resume_checkpoint_import", - "source_session": source.parent.name, - "completed_task_count": len(completed), - "completed_task_keys": list(completed), - "discarded_incomplete_task": ( - missing_tasks[0] if missing_tasks else None - ), - "pending_task_keys": missing_tasks, - "revalidation_dropped_tasks": dropped, - "tag_size_changed_ids": list(changed_tag_size_ids), - "tag_size_invalidated_task_keys": list(size_invalidated_tasks), - "palm_axis_schema_compatible": palm_axis_schema_compatible, - "palm_axis_invalidated_task_keys": list( - palm_axis_invalidated_tasks - ), - "recalibration_scope": str( - getattr(self, "recalibration_scope", "full") - ), - "scope_invalidated_task_keys": list( - scope_invalidated_tasks - ), - "imported_record_count": len(reusable), - "imported_attempt_floor_by_task": attempt_floor_by_task, - "source_raw_samples_sha256": _file_sha256(source), - }, - ) - append_jsonl_many(self.raw_path, reusable) - return len(completed) - - def _revalidate_imported_tasks( - self, completed: Sequence[str], *, allow_sparse: bool = False - ) -> tuple[list[str], list[dict[str, Any]]]: - """Re-apply the final hard gates to imported tasks at import time. - - A task that only provisionally passed (warning band) in its source - session would otherwise survive until the final fit re-applies the - unmodified thresholds after every other task is collected, forcing - the session to go back and re-scan it at the very end. Failing - tasks are dropped here. Sparse product resume independently checks - later complete tasks so only the failed task has to be re-collected; - the compatibility mode still drops the remaining suffix. - """ - def discard_task_records(task_key: str) -> None: - spec = next( - ( - candidate - for candidate in self.profile.sweep_specs - if candidate.key == task_key - ), - None, - ) - if spec is None: - return - for joint_name in spec.joints: - for store in ( - self.records_by_joint, - self.baseline_records_by_joint, - self.command_records_by_joint, - ): - if joint_name in store: - store[joint_name].clear() - observer = next( - ( - item - for item in self.profile.palm_axis_observers - if item.task_name == task_key - ), - None, - ) - if observer is not None: - self.palm_axis_records_by_source[ - observer.source_name - ].clear() - - accepted: list[str] = [] - dropped: list[dict[str, Any]] = [] - for task_index, task_key in enumerate(completed, start=1): - if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED: - total = max(1, len(completed)) - self.base_import_progress.update( - { - "current_task": task_key, - "completed_tasks": task_index - 1, - "total_tasks": len(completed), - "fraction": (task_index - 1) / total, - } - ) - _publish_import_status(self) - spec = next( - ( - item.spec - for item in self.sweep_items - if item.spec.key == task_key - ), - None, - ) - failures = ( - G20ThreeCameraCalibrationNode._provisional_fit_failures( - self, spec, include_view_validity=False - ) - if spec is not None - else [{"metric": "missing_task_spec"}] - ) - if failures: - dropped.append( - { - "task": task_key, - "failures": [ - { - key: value - for key, value in failure.items() - if key in {"joint", "metric", "actual", "limit"} - } - for failure in failures[:6] - ], - } - ) - # Make the rejection visible before checking later tasks. - # Some fits intentionally depend on an earlier reference - # task (for example every non-reference finger roll uses the - # pinky roll direction). Delaying this clear until the end - # let a dependent task pass with records that were about to - # be removed, producing a checkpoint that failed only on the - # next restart. - discard_task_records(task_key) - if allow_sparse: - continue - break - accepted.append(task_key) - if getattr(self, "state", "") == STATE_REVALIDATING_INHERITED: - self.base_import_progress.update( - { - "current_task": "", - "completed_tasks": len(completed), - "total_tasks": len(completed), - "fraction": 1.0, - } - ) - _publish_import_status(self) - if dropped and not allow_sparse: - accepted_set = set(accepted) - for task_key in completed: - if task_key not in accepted_set: - discard_task_records(task_key) - if isinstance(completed, tuple): - accepted = tuple(accepted) - return accepted, dropped - - def _start_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state != STATE_WAIT_START: - response.success = False - response.message = f"not ready: state={self.state} reason={self.reason}" - return response - if not self.commands_enabled: - response.success = False - response.message = "commands_enabled=false; preview cannot move the hand" - return response - if self.command_publisher.get_subscription_count() < 1: - response.success = False - response.message = "hand SDK is not subscribed to the command topic" - return response - if self.setting_publisher.get_subscription_count() < 1: - response.success = False - response.message = "hand SDK is not subscribed to the setting topic" - return response - if len(self.get_publishers_info_by_topic(self.command_topic)) > 1: - response.success = False - response.message = "another node is publishing hand commands" - return response - self.started = True - self.startup_baseline_recovered = False - self.sweep_items = [] - self.pnp_task_spec = None - selected_sweep_specs = list(self.profile.sweep_specs) - if self.standalone_thumb_calibration: - standalone_task_keys = set(self.recalibration_task_keys) - selected_sweep_specs = [ - spec - for spec in selected_sweep_specs - if spec.key in standalone_task_keys - ] - if self.cross_view_roll_diagnostic_finger: - selected_sweep_specs = [ - spec - for spec in selected_sweep_specs - if spec.key - == f"{self.cross_view_roll_diagnostic_finger}_roll_multiview" - ] - if len(selected_sweep_specs) != 1: - raise RuntimeError( - "cross-view diagnostic could not resolve multiview task" - ) - selected_profile = replace( - self.profile, - sweep_specs=tuple(selected_sweep_specs), - ) - self.sweep_items.extend( - _build_sweep_plan(selected_profile, self.repetitions) - ) - self.sweep_index = 0 - self.retry_sweep_spec = None - self.retry_resume_index = None - self.retry_sweep_items.clear() - self.retry_joint_names.clear() - self.retry_source_failure_task_key = None - self.retry_source_task_keys = () - self.retry_cycle_override.clear() - self.retry_preserve_pnp_continuity = False - self.active_sweep_is_fit_retry = False - self.fit_failure = {} - self.fit_failure_history_by_task.clear() - self.cross_view_roll_diagnostic_result = {} - self.motion_stall_details.clear() - self.sweep_attempts = { - _sweep_storage_key(spec): 1 for spec in self.profile.sweep_specs - } - self.sweep_retry_counts.clear() - self.motion_retry_counts.clear() - self.validation_retry_counts.clear() - self.precheck_speed_metrics.clear() - self.formal_speed_scales.clear() - self.records_by_joint = { - name: [] for name in self.profile.record_joints - } - self.baseline_records_by_joint = { - name: [] for name in self.profile.record_joints - } - self.command_records_by_joint = { - name: [] for name in self.profile.record_joints - } - self.palm_axis_records_by_source = { - observer.source_name: [] - for observer in self.profile.palm_axis_observers - } - G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) - self.combination_validation_items.clear() - self.combination_validation_index = 0 - self.active_combination_validation = None - self.combination_validation_completed = False - self.combination_tag_mounts.clear() - self.combination_tag_observation_counts.clear() - self.combination_tag_validation_counts.clear() - self.combination_position_errors_m.clear() - self.combination_orientation_errors_rad.clear() - for frames in self.combination_validation_frames_buffer.values(): - frames.clear() - self.completed_payload = None - self.sweep_frames.clear() - self.sweep_baseline_frames.clear() - self.sweep_baseline_pending = False - self.sweep_baseline_hold_since = None - self.sweep_start_frames.clear() - self.carried_sweep_start_frames.clear() - append_jsonl( - self.raw_path, - { - "kind": "session_start", - "model": self.model, - "hand_type": self.hand_type, - "tag_layout": self.profile.layout_id, - "sample_schema_version": self.sample_schema_version, - "recalibration_scope": self.recalibration_scope, - "recalibration_task_keys": list( - self.recalibration_task_keys - ), - "command_names": list(_command_names(self)), - # Retained only as an old-session serialization token. Live - # decisions use the typed motion/measurement policies. - "capabilities": sorted( - getattr( - getattr( - getattr(self, "calibration_profile", None), - "artifacts", - None, - ), - "session_compatibility_tokens", - frozenset(self.profile.capabilities), - ) - ), - "reference_finger": self.profile.reference_finger, - "view_tags": { - view: dict(tags) - for view, tags in self.profile.view_tags.items() - }, - "palm_axis_observers": _palm_axis_observer_schema( - self.profile - ), - "tag_family": "36h11", - "tag_size_m": float(self.tag_size_m), - "tag_sizes_m_by_id": { - str(tag_id): float(self.tag_sizes_m_by_id[tag_id]) - for tag_id in sorted(self.tag_sizes_m_by_id) - }, - "baseline_command_u8": [ - int(value) for value in self.baseline_command - ], - "camera_extrinsics_file": str(self.camera_extrinsics_file), - "source_urdf_path": str(self.source_urdf_path), - "source_urdf_sha256": _file_sha256(self.source_urdf_path), - "curve_input_domain": ( - "requested_command_u8" - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else "command_u8" - ), - "baseline_hysteresis_source": ( - "dedicated_mid_sweep_hold" - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else "trajectory_endpoint" - ), - "directional_zero_policy": ( - "canonical_decreasing_255_to_127" - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else "independent_direction_rezero" - ), - "thumb_ip_pnp_coupling": { - "usage": "candidate_prior_with_visual_fallback", - }, - "cross_view_roll_diagnostic_finger": ( - self.cross_view_roll_diagnostic_finger - ), - "training_cycles": list(range(self.repetitions - 1)), - "validation_cycle": self.repetitions - 1, - "adaptive_formal_speed": { - "enabled": bool(self.adaptive_formal_speed_enabled), - "maximum_scale": float( - self.adaptive_formal_speed_max_scale - ), - "minimum_bins": int( - self.adaptive_formal_speed_minimum_bins - ), - "maximum_bin_gap": int( - self.adaptive_formal_speed_maximum_bin_gap - ), - }, - }, - ) - try: - if self.resume_raw_samples_path is not None: - 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()) - restored_tasks = self._restore_durable_task_checkpoint() - except Exception as error: - self.started = False - response.success = False - response.message = f"CFG-RESUME-009:{error}" - return response - self._begin_return_baseline("startup_tag_preflight") - response.success = True - response.message = ( - "three-camera calibration started" - if restored_tasks == 0 - else f"calibration resumed with {restored_tasks} completed tasks" - ) - return response - - def _pause_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state in {STATE_COMPLETE, STATE_ABORTED, STATE_PREFLIGHT, STATE_WAIT_START}: - response.success = False - response.message = f"cannot pause from {self.state}" - return response - self._pause("operator_pause") - response.success = True - response.message = "calibration paused and current pose held" - return response - - def _resume_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - if self.state != STATE_PAUSED: - response.success = False - response.message = "calibration is not paused" - return response - if self.paused_reason == "zero_model_validation_failed": - response.success = False - response.message = ( - "zero/URDF model validation failed; samples are retained but " - "resume cannot repair this result by rescanning. Inspect the " - "reported zero guards and start a new session after correcting " - "the camera/tag/URDF geometry" - ) - return response - if self.paused_reason == "fixed_base_reference_moved": - response.success = False - response.message = ( - "the locked top base reference moved during acquisition; " - "start a new session so preflight can establish a new " - "reference before any motion" - ) - return response - if self.paused_reason == "cross_view_roll_diagnostic_complete": - response.success = False - response.message = ( - "cross-view roll diagnostic is complete and URDF publication " - "is locked; inspect the front/side result and start a formal " - "session only after choosing the mechanical or vision remedy" - ) - return response - now = time.monotonic() - if not self._resume_preflight_ready(now): - response.success = False - required = ",".join(self._active_views_for_resume()) - response.message = ( - f"resume preflight is not ready; required_views={required}" - ) - return response - for view in self._active_views_for_resume(): - runtime = self.views[view] - self._reset_view_trackers(runtime) - runtime.pnp_invalid_since = None - self.sweep_frames.clear() - self.sweep_start_frames.clear() - self.validation_frames_buffer.clear() - if self.retry_sweep_spec is not None: - self._prepare_failed_sweep_retry() - self._begin_return_baseline("resume_sweep") - response.success = True - response.message = ( - "calibration resumed; only the failed six-direction joint " - "sweep will restart" - ) - return response - if self.active_sweep is not None and self.sweep_index < len(self.sweep_items): - self._begin_return_baseline("resume_sweep") - elif self.active_combination_validation is not None: - item = self.active_combination_validation - active = { - "kind": "combination_validation", - "pose_name": item.name, - "label_zh": item.label_zh, - "pose_index": self.combination_validation_index + 1, - "pose_total": len(self.combination_validation_items), - "requested_command_u8": list(item.command_u8), - "valid_frames_by_view": { - view: len(frames) - for view, frames in self.combination_validation_frames_buffer.items() - }, - "automatic_retry_count": self.validation_retry_counts.get( - ("combination", self.combination_validation_index), 0 - ), - "automatic_retry_limit": self.automatic_sweep_retry_limit, - } - elif self.active_validation is not None: - self._begin_return_baseline("resume_validation") - else: - self._begin_return_baseline("next_sweep") - response.success = True - response.message = "calibration resumed; active step will restart" - return response - - def _abort_callback( - self, request: Trigger.Request, response: Trigger.Response - ) -> Trigger.Response: - del request - self._publish_speed_profile(self._normal_speed_profile()) - unsafe_to_return = bool( - self.motion_stall_details - or "stall" in str(self.reason) - or len(self.latest_state_u8) != 20 - or time.monotonic() - self.last_state_at > 1.0 - ) - if unsafe_to_return: - self._publish_hold_current() - self.state = STATE_ABORTED - self.reason = "operator_abort_held_current_for_safety" - else: - self.abort_original_reason = str(self.reason or "operator_abort") - self._begin_return_baseline("abort") - response.success = True - response.message = ( - "calibration aborted and current pose held" - if unsafe_to_return - else "calibration stopping after safe baseline return" - ) - return response - - def _publish_command(self, values: list[int]) -> None: - if not self.commands_enabled: - return - message = JointState() - message.header.stamp = self.get_clock().now().to_msg() - message.name = list(_command_names(self)) - message.position = [float(value) for value in values] - self.command_publisher.publish(message) - - def _publish_speed_profile(self, speeds: list[int]) -> None: - if not self.commands_enabled: - return - profile = tuple(int(value) for value in speeds) - if profile == self.commanded_speed_profile: - return - message = String() - message.data = json.dumps( - { - "setting_cmd": "set_speed", - "params": { - "hand_type": self.hand_type, - "speed": list(profile), - }, - } - ) - self.setting_publisher.publish(message) - self.commanded_speed_profile = profile - self.speed_commanded_at = time.monotonic() - - def _normal_speed_profile(self) -> list[int]: - return [self.normal_calibration_speed] * 5 - - def _transition_speed_profile( - self, target_command: Sequence[int] - ) -> list[int]: - """Use conservative joint-class speeds for one safe pose waypoint.""" - speeds = list(self._normal_speed_profile()) - current = getattr(self, "latest_state_u8", ()) - if len(current) != 20 or len(target_command) != 20: - return speeds - roll_speed = int( - getattr(self, "index_roll_calibration_speed", min(speeds)) - ) - flex_speed = int( - getattr(self, "index_flex_calibration_speed", min(speeds)) - ) - for motor, (actual, target) in enumerate( - zip(current, target_command) - ): - if abs(float(actual) - float(target)) <= 0.5: - continue - if motor in range(6, 10): - slot = motor - 5 - speeds[slot] = min(speeds[slot], roll_speed) - elif motor in range(1, 5): - slot = motor - speeds[slot] = min(speeds[slot], flex_speed) - elif motor in range(16, 20): - slot = motor - 15 - speeds[slot] = min(speeds[slot], flex_speed) - return speeds - - @staticmethod - def _speed_slot_for_spec(spec: SweepSpec) -> int: - motor = int(spec.motor_index) - if motor in {0, 5, 10, 15}: - return 0 - if motor in range(1, 5): - return motor - if motor in range(6, 10): - return motor - 5 - if motor in range(16, 20): - return motor - 15 - raise ValueError(f"unsupported G20 calibration motor: {motor}") - - def _base_speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return build_calibration_speed_profile( - spec, - normal_speed=self.normal_calibration_speed, - index_roll_speed=self.index_roll_calibration_speed, - index_flex_speed=self.index_flex_calibration_speed, - profile=profile, - ) - - def _speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: - speeds = G20ThreeCameraCalibrationNode._base_speed_profile_for_spec( - self, spec - ) - active = getattr(self, "active_sweep", None) - precheck = bool(active is not None and active.precheck) - fit_retry = bool(getattr(self, "active_sweep_is_fit_retry", False)) - if not precheck and not fit_retry: - scale = float( - getattr(self, "formal_speed_scales", {}).get(spec.key, 1.0) - ) - slot = G20ThreeCameraCalibrationNode._speed_slot_for_spec(spec) - speeds[slot] = min( - 255, int(math.floor(float(speeds[slot]) * scale + 1e-9)) - ) - retry = 0 - if active is not None: - key = ( - _sweep_storage_key(active.spec), - active.cycle, - active.direction, - ) - retry = self.sweep_retry_counts.get(key, 0) - if retry: - scale = self.retry_speed_scales[min(retry, len(self.retry_speed_scales)) - 1] - speeds = [ - max(self.retry_minimum_speed, int(round(speed * scale))) - for speed in speeds - ] - return speeds - - def _record_precheck_speed_metric( - self, - item: SweepItem, - *, - bin_count: int, - maximum_bin_gap: int, - valid_frames: int, - ) -> None: - """Use the accepted low-speed precheck to bound formal-scan speed.""" - key = ( - _sweep_storage_key(item.spec), item.cycle, item.direction - ) - retry = int(getattr(self, "sweep_retry_counts", {}).get(key, 0)) - base_speeds = ( - G20ThreeCameraCalibrationNode._base_speed_profile_for_spec( - self, item.spec - ) - ) - actual_speeds = G20ThreeCameraCalibrationNode._speed_profile_for_spec( - self, item.spec - ) - slot = G20ThreeCameraCalibrationNode._speed_slot_for_spec(item.spec) - base_speed = int(base_speeds[slot]) - actual_speed = int(actual_speeds[slot]) - acquisition_scale = ( - 1.0 - if base_speed <= 0 - else float(actual_speed) / float(base_speed) - ) - metrics = getattr(self, "precheck_speed_metrics", None) - if metrics is None: - self.precheck_speed_metrics = {} - metrics = self.precheck_speed_metrics - by_direction = metrics.setdefault(item.spec.key, {}) - by_direction[item.direction] = { - "bin_count": int(bin_count), - "maximum_bin_gap": int(maximum_bin_gap), - "valid_frames": int(valid_frames), - "retry": retry, - "acquisition_speed": actual_speed, - "acquisition_speed_scale": acquisition_scale, - } - required = { - DIRECTION_DECREASING, - DIRECTION_INCREASING, - } - if not required.issubset(by_direction): - return - - profile = _node_profile(self) - configured_enabled = bool( - getattr(self, "adaptive_formal_speed_enabled", False) - ) - # G20 right repeatability is evaluated across independent sessions. - # Letting camera-dependent precheck density alter the physical scan - # speed changes velocity lag and backlash, so two runs of the same hand - # no longer measure the same process. Keep the reviewed base speed for - # every product task; adaptive speed remains available to legacy tools. - stability_speed_lock = profile.layout_id == G20_RIGHT_19_LAYOUT - enabled = configured_enabled and not stability_speed_lock - # MCP roll is both the fastest native mechanism and the one guarded by - # the strict 0.5-degree baseline backlash limit. Field data at speed - # 7 exceeded that limit after a clean speed-5 precheck, so sampling - # density alone is not sufficient evidence to accelerate roll. - eligible = int(item.spec.motor_index) not in range(6, 10) - selected_scale = 1.0 - if enabled and eligible and base_speed > 0: - caps: list[float] = [] - target_bins = int(self.adaptive_formal_speed_minimum_bins) - target_gap = int(self.adaptive_formal_speed_maximum_bin_gap) - for direction in sorted(required): - metric = by_direction[direction] - observed_scale = float(metric["acquisition_speed_scale"]) - caps.append( - observed_scale - * float(metric["bin_count"]) - / float(target_bins) - ) - caps.append( - observed_scale - * float(target_gap) - / float(max(1, int(metric["maximum_bin_gap"]))) - ) - selected_scale = max( - 1.0, - min(float(self.adaptive_formal_speed_max_scale), *caps), - ) - selected_speed = min( - 255, - int(math.floor(float(base_speed) * selected_scale + 1e-9)), - ) - if selected_speed <= base_speed or base_speed <= 0: - selected_speed = base_speed - selected_scale = 1.0 - else: - selected_scale = float(selected_speed) / float(base_speed) - scales = getattr(self, "formal_speed_scales", None) - if scales is None: - self.formal_speed_scales = {} - scales = self.formal_speed_scales - scales[item.spec.key] = selected_scale - raw_path = getattr(self, "raw_path", None) - if raw_path is not None: - append_jsonl( - raw_path, - { - "kind": "task_formal_speed_selected", - "task_name": item.spec.key, - "motor_index": int(item.spec.motor_index), - "enabled": enabled, - "configured_enabled": configured_enabled, - "stability_speed_lock": stability_speed_lock, - "eligible": eligible, - "ineligible_reason": ( - "g20_right_deterministic_acquisition_speed" - if stability_speed_lock - else "roll_baseline_hysteresis_sensitive" - if configured_enabled and not eligible - else "" - ), - "base_speed": base_speed, - "formal_speed": selected_speed, - "formal_speed_scale": round(selected_scale, 6), - "minimum_retained_bins": int( - self.adaptive_formal_speed_minimum_bins - ), - "maximum_retained_bin_gap": int( - self.adaptive_formal_speed_maximum_bin_gap - ), - "precheck": { - direction: dict(by_direction[direction]) - for direction in sorted(required) - }, - }, - ) - - def _active_endpoint_hold_seconds(self) -> float: - if self.active_sweep is None: - return float(self.endpoint_hold_seconds) - if self.active_sweep.precheck: - return max( - float(self.endpoint_hold_seconds), - float(self.task_precheck_hold_seconds), - ) - key = ( - _sweep_storage_key(self.active_sweep.spec), - self.active_sweep.cycle, - self.active_sweep.direction, - ) - retry = self.sweep_retry_counts.get(key, 0) - if retry <= 0: - return float(self.endpoint_hold_seconds) - return float( - self.retry_endpoint_hold_seconds[ - min(retry, len(self.retry_endpoint_hold_seconds)) - 1 - ] - ) - - def _publish_hold_current(self) -> None: - if len(self.latest_state_u8) != 20: - return - values = [ - int(np.clip(np.rint(value), 0, 255)) - for value in self.latest_state_u8 - ] - self._publish_command(values) - - def _snap_reached_state_to_command( - self, - current_state: Sequence[float], - target_command: Sequence[int], - ) -> tuple[int, ...]: - """Remove no-op transition steps already inside feedback tolerance.""" - if len(current_state) != 20 or len(target_command) != 20: - raise ValueError("state and target command must contain 20 values") - result = [ - int(np.clip(np.rint(value), 0, 255)) for value in current_state - ] - controlled = { - int(spec.motor_index) for spec in self.profile.joint_specs.values() - } - for index in controlled: - target = int(target_command[index]) - if abs(float(current_state[index]) - target) <= ( - G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, target - ) - ): - result[index] = target - return tuple(result) - - def _begin_return_baseline(self, after: str) -> None: - self.baseline_after = str(after) - if str(after) not in {"next_cycle", "resume_sweep"}: - # A mechanical task boundary is also the PnP ownership boundary. - # Same-task cycle resets and localized fit recovery retain it. - self.pnp_task_spec = None - target_command = ( - G20ThreeCameraCalibrationNode._return_command_for_transition( - self, after - ) - ) - current_state = getattr(self, "latest_state_u8", ()) - if len(current_state) != 20: - current_state = target_command - else: - current_state = self._snap_reached_state_to_command( - current_state, target_command - ) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - self.return_waypoints = deque( - build_calibration_return_waypoints( - target_command, - current_command=current_state, - profile=profile, - anchor_roll_motor=( - G20ThreeCameraCalibrationNode._return_anchor_roll_motor( - self - ) - ), - parallel=self.parallel_pose_transitions, - ) - ) - self.return_command_u8 = self.return_waypoints.popleft() - self.position_hold_since = None - self.motion_stage_started_at = time.monotonic() - getattr(self, "motion_stall_details", {}).clear() - self._reset_motion_progress( - self.motion_stage_started_at, self._baseline_error_u8() - ) - self.state = STATE_RETURN_BASELINE - self.reason = ( - "holding_same_finger_clearance_before_next_task" - if str(after) == "next_task_same_finger" - else f"return_baseline_before_{after}" - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, self.return_command_u8 - ) - ) - self._publish_command(list(self.return_command_u8)) - - def _return_anchor_roll_motor(self) -> int | None: - """Resolve the finger that must leave a fan pose first.""" - retry_items = getattr(self, "retry_sweep_items", []) - item = retry_items[0] if retry_items else None - if item is None: - items = getattr(self, "sweep_items", []) - index = int(getattr(self, "sweep_index", len(items))) - if index < len(items): - item = items[index] - if item is None: - item = getattr(self, "active_sweep", None) - if item is None: - return None - motor = int(item.spec.motor_index) - if motor in range(6, 10): - return motor - if motor in range(1, 5): - return motor + 5 - if motor in range(16, 20): - return motor - 10 - return None - - @staticmethod - def _four_finger_task_group(spec: SweepSpec) -> str | None: - """Return the finger whose consecutive tasks share one avoidance pose.""" - for joint_name in spec.joints: - finger = str(joint_name).split("_", 1)[0] - if finger in {"pinky", "ring", "middle", "index"}: - return finger - return None - - def _next_pending_sweep_item(self) -> SweepItem | None: - retry_items = getattr(self, "retry_sweep_items", []) - if retry_items: - return retry_items[0] - items = getattr(self, "sweep_items", []) - index = int(getattr(self, "sweep_index", len(items))) - return items[index] if index < len(items) else None - - def _advance_past_resumed_sweeps(self) -> None: - """Skip every independently revalidated task restored from disk.""" - completed = set(getattr(self, "resumed_task_keys", ()) or ()) - items = getattr(self, "sweep_items", []) - while ( - self.sweep_index < len(items) - and items[self.sweep_index].spec.key in completed - ): - self.sweep_index += 1 - - def _transition_after_completed_spec(self, spec: SweepSpec) -> str: - """Keep clearance parked between consecutive tasks of one finger.""" - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item(self) - current_group = ( - G20ThreeCameraCalibrationNode._four_finger_task_group(spec) - ) - next_group = ( - None - if next_item is None - else G20ThreeCameraCalibrationNode._four_finger_task_group( - next_item.spec - ) - ) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and current_group is not None - and current_group == next_group - ): - return "next_task_same_finger" - return "next_sweep" - - def _return_command_for_transition(self, after: str) -> tuple[int, ...]: - """Choose a global or task-local safe transition target.""" - if str(after) in {"next_cycle", "next_task_same_finger"}: - next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item( - self - ) - if next_item is None: - return tuple(self.baseline_command) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return tuple( - build_calibration_motion_command( - next_item.spec, - int(self.baseline_command[next_item.spec.motor_index]), - baseline=self.baseline_command, - profile=profile, - ) - ) - if str(after) == "next_sweep": - merged = ( - G20ThreeCameraCalibrationNode._cross_group_return_command( - self - ) - ) - if merged is not None: - return merged - if str(after) not in {"retry_sweep", "resume_sweep"}: - return tuple(self.baseline_command) - retry_items = getattr(self, "retry_sweep_items", []) - item = retry_items[0] if retry_items else getattr(self, "active_sweep", None) - if item is None: - return tuple(self.baseline_command) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - # A four-finger retry immediately repeats the same task. Preserve its - # reviewed occlusion-clearance pose and return only to the queued - # direction start; a global baseline would unfold the parked fingers - # and make the next preparation flex them again. The retry setup has - # already reset every contributing PnP tracker. - keep_task_clearance = bool( - item.spec.motor_index == 10 - or ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and G20ThreeCameraCalibrationNode._four_finger_task_group( - item.spec - ) - is not None - ) - ) - if not keep_task_clearance: - return tuple(self.baseline_command) - return tuple( - build_calibration_motion_command( - item.spec, - item.start_u8, - baseline=self.baseline_command, - profile=profile, - ) - ) - - def _cross_group_return_command(self) -> tuple[int, ...] | None: - """Keep already-positioned clearance motors through a finger change. - - Crossing from one finger's task group to the next used to unfold every - auxiliary motor back to baseline and then re-flex it for the next - avoidance pose. Motors the next group still wants at a non-baseline - value and that are already in place (within their endpoint deadband) - now simply stay put; everything else still returns to baseline, so the - reviewed roll-before-unfold and open-before-flex orderings keep - holding and only genuinely unused clearance unfolds. - """ - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if profile.layout_id != G20_RIGHT_19_LAYOUT: - return None - next_item = G20ThreeCameraCalibrationNode._next_pending_sweep_item( - self - ) - if next_item is None: - return None - current_state = getattr(self, "latest_state_u8", ()) - if len(current_state) != 20: - return None - baseline = [int(value) for value in self.baseline_command] - # Match what the next preparation will actually command: the measured - # motor enters at its sweep start, not at the baseline value. - next_start = int( - getattr( - next_item, - "start_u8", - baseline[int(next_item.spec.motor_index)], - ) - ) - next_final = [ - int(value) - for value in build_calibration_motion_command( - next_item.spec, - next_start, - baseline=baseline, - profile=profile, - ) - ] - controlled = { - int(spec.motor_index) for spec in profile.joint_specs.values() - } - target = list(baseline) - for motor in sorted(controlled): - next_value = next_final[motor] - if next_value == baseline[motor]: - continue - actual = float(current_state[motor]) - tolerance = ( - G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, motor, next_value - ) - ) - if abs(actual - next_value) <= tolerance: - target[motor] = next_value - return tuple(target) - - def _current_return_command(self) -> tuple[int, ...]: - command = getattr(self, "return_command_u8", self.baseline_command) - return tuple(int(value) for value in command) - - def _baseline_reached(self) -> bool: - if len(self.latest_state_u8) != 20: - return False - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return_command = G20ThreeCameraCalibrationNode._current_return_command( - self - ) - indices = sorted( - {spec.motor_index for spec in profile.joint_specs.values()} - ) - return all( - abs(float(self.latest_state_u8[index]) - return_command[index]) - <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(return_command[index]) - ) - for index in indices - ) - - def _baseline_error_details(self) -> dict[str, float | int | str]: - if len(self.latest_state_u8) != 20: - return { - "stage": "return_baseline", - "motor_index": -1, - "target_u8": float("nan"), - "actual_u8": float("nan"), - "error_u8": float("inf"), - "tolerance_u8": float(self.endpoint_tolerance_u8), - } - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return_command = G20ThreeCameraCalibrationNode._current_return_command( - self - ) - indices = sorted({spec.motor_index for spec in profile.joint_specs.values()}) - values: list[dict[str, float | int | str]] = [] - for index in indices: - target = float(return_command[index]) - actual = float(self.latest_state_u8[index]) - tolerance = ( - G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(target) - ) - ) - values.append( - { - "stage": "return_baseline", - "motor_index": int(index), - "target_u8": target, - "actual_u8": actual, - "error_u8": abs(actual - target), - "tolerance_u8": tolerance, - } - ) - outside = [ - item - for item in values - if float(item["error_u8"]) > float(item["tolerance_u8"]) - ] - candidates = outside or values - return max( - candidates, - key=lambda item: ( - float(item["error_u8"]) - float(item["tolerance_u8"]), - float(item["error_u8"]), - ), - ) - - def _baseline_error_u8(self) -> float: - if len(self.latest_state_u8) != 20: - return float("inf") - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - return_command = G20ThreeCameraCalibrationNode._current_return_command( - self - ) - indices = {spec.motor_index for spec in profile.joint_specs.values()} - return max( - abs( - float(self.latest_state_u8[index]) - - float(return_command[index]) - ) - for index in indices - ) - - def _command_vector_reached(self, command: Sequence[int]) -> bool: - if len(self.latest_state_u8) != 20 or len(command) != 20: - return False - controlled = { - int(spec.motor_index) for spec in self.profile.joint_specs.values() - } - return all( - abs(float(self.latest_state_u8[index]) - float(command[index])) - <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(command[index]) - ) - for index in controlled - ) - - def _command_vector_error_u8(self, command: Sequence[int]) -> float: - if len(self.latest_state_u8) != 20 or len(command) != 20: - return float("inf") - controlled = { - int(spec.motor_index) for spec in self.profile.joint_specs.values() - } - return max( - abs(float(self.latest_state_u8[index]) - float(command[index])) - for index in controlled - ) - - def _command_vector_error_details( - self, command: Sequence[int], context: str - ) -> dict[str, float | int | str]: - """Identify the actual controlled motor blocking one safe waypoint.""" - if len(self.latest_state_u8) != 20 or len(command) != 20: - return { - "stage": str(context), - "motor_index": -1, - "target_u8": float("nan"), - "actual_u8": float("nan"), - "error_u8": float("inf"), - "tolerance_u8": float(self.endpoint_tolerance_u8), - } - controlled = sorted( - {int(spec.motor_index) for spec in self.profile.joint_specs.values()} - ) - values: list[dict[str, float | int | str]] = [] - for index in controlled: - target = float(command[index]) - actual = float(self.latest_state_u8[index]) - tolerance = G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(target) - ) - values.append( - { - "stage": str(context), - "motor_index": index, - "target_u8": target, - "actual_u8": actual, - "error_u8": abs(actual - target), - "tolerance_u8": float(tolerance), - } - ) - outside = [ - item - for item in values - if float(item["error_u8"]) > float(item["tolerance_u8"]) - ] - return max( - outside or values, - key=lambda item: ( - float(item["error_u8"]) - float(item["tolerance_u8"]), - float(item["error_u8"]), - ), - ) - - def _motion_command_reached( - self, - spec: SweepSpec, - command_u8: int, - state_u8: tuple[float, ...] | None = None, - ) -> bool: - """Check the swept motor and all clearance motors together.""" - state = self.latest_state_u8 if state_u8 is None else state_u8 - if len(state) != 20: - return False - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - expected = build_calibration_motion_command( - spec, - command_u8, - baseline=self.baseline_command, - profile=profile, - ) - indices = {spec.motor_index} - indices.update(calibration_auxiliary_commands(spec, profile=profile)) - return all( - abs(float(state[index]) - expected[index]) - <= ( - self._endpoint_tolerance_for_spec(spec, command_u8) - if index == spec.motor_index - else G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, expected[index] - ) - ) - for index in indices - ) - - def _steady_checkpoint_reached( - self, - spec: SweepSpec, - command_u8: int, - state_u8: tuple[float, ...] | None = None, - ) -> bool: - """Accept a settled command/feedback offset without hiding a stall. - - The checkpoint exists specifically to identify requested-command to - feedback/angle offsets, so applying the ordinary ±2-u8 equality gate - here is circular. Only the swept motor gets the bounded calibration - deadband; every clearance motor keeps its normal strict tolerance. - """ - state = self.latest_state_u8 if state_u8 is None else state_u8 - if len(state) != 20: - return False - expected = build_calibration_motion_command( - spec, - command_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - swept_tolerance = max( - float(self.steady_checkpoint_command_feedback_tolerance_u8), - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - self, spec, command_u8 - ), - ) - if ( - abs(float(state[spec.motor_index]) - expected[spec.motor_index]) - > swept_tolerance - ): - return False - return all( - abs(float(state[index]) - expected[index]) - <= G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, expected[index] - ) - for index in calibration_auxiliary_commands( - spec, profile=self.profile - ) - ) - - def _steady_checkpoint_feedback_is_stable( - self, - spec: SweepSpec, - frames: Sequence[FrameObservation], - ) -> bool: - """Require the held feedback range to be small before recording it.""" - if len(frames) < 3: - return False - values = [ - float(frame.state_u8[spec.motor_index]) for frame in frames - ] - return bool( - max(values) - min(values) - <= float(self.steady_checkpoint_maximum_feedback_range_u8) - ) - - def _motion_command_error_u8( - self, - spec: SweepSpec, - command_u8: int, - state_u8: tuple[float, ...] | None = None, - ) -> float: - """Return the worst swept/auxiliary motor error for stall detection.""" - state = self.latest_state_u8 if state_u8 is None else state_u8 - if len(state) != 20: - return float("inf") - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - expected = build_calibration_motion_command( - spec, - command_u8, - baseline=self.baseline_command, - profile=profile, - ) - indices = {spec.motor_index} - indices.update(calibration_auxiliary_commands(spec, profile=profile)) - return max( - abs(float(state[index]) - float(expected[index])) - for index in indices - ) - - def _motion_command_error_details( - self, - spec: SweepSpec, - command_u8: int, - context: str, - ) -> dict[str, float | int | str]: - """Identify the swept or clearance motor blocking a motion.""" - if len(self.latest_state_u8) != 20: - return { - "stage": str(context), - "motor_index": -1, - "target_u8": float(command_u8), - "actual_u8": float("nan"), - "error_u8": float("inf"), - "tolerance_u8": float(self.endpoint_tolerance_u8), - } - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - expected = build_calibration_motion_command( - spec, - command_u8, - baseline=self.baseline_command, - profile=profile, - ) - indices = {spec.motor_index} - indices.update(calibration_auxiliary_commands(spec, profile=profile)) - values: list[dict[str, float | int | str]] = [] - for index in indices: - target = float(expected[index]) - actual = float(self.latest_state_u8[index]) - tolerance = ( - G20ThreeCameraCalibrationNode._endpoint_tolerance_for_spec( - self, spec, command_u8 - ) - if index == spec.motor_index - else G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, index, int(target) - ) - ) - values.append( - { - "stage": str(context), - "motor_index": int(index), - "target_u8": target, - "actual_u8": actual, - "error_u8": abs(actual - target), - "tolerance_u8": float(tolerance), - } - ) - outside = [ - item - for item in values - if float(item["error_u8"]) > float(item["tolerance_u8"]) - ] - return max( - outside or values, - key=lambda item: ( - float(item["error_u8"]) - float(item["tolerance_u8"]), - float(item["error_u8"]), - ), - ) - - def _reset_motion_progress(self, now: float, error_u8: float) -> None: - self.motion_progress_reference_error_u8 = float(error_u8) - self.motion_last_progress_at = float(now) - - def _pause_if_motion_stalled( - self, - *, - now: float, - error_u8: float, - context: str, - details: Mapping[str, Any] | None = None, - ) -> bool: - """Pause immediately when feedback stops moving toward its target.""" - error = float(error_u8) - if not math.isfinite(error): - return False - reference = float( - getattr(self, "motion_progress_reference_error_u8", float("inf")) - ) - last_progress = float(getattr(self, "motion_last_progress_at", now)) - minimum_progress = float( - getattr(self, "motor_stall_minimum_progress_u8", 1.0) - ) - if not math.isfinite(reference): - self._reset_motion_progress(now, error) - return False - if reference - error >= minimum_progress: - self._reset_motion_progress(now, error) - return False - motion_started_value = getattr(self, "motion_stage_started_at", None) - startup_grace = float( - getattr(self, "motor_stall_startup_grace_seconds", 1.0) - ) - if ( - motion_started_value is not None - and now - float(motion_started_value) < startup_grace - ): - return False - timeout = float(getattr(self, "motor_stall_timeout_seconds", 8.0)) - if now - last_progress < timeout: - return False - detail_payload = {} if details is None else dict(details) - reason_parts = ["motor_state_stalled", str(context)] - for key in ("motor_index", "target_u8", "actual_u8", "tolerance_u8"): - if key in detail_payload: - reason_parts.append(f"{key}={detail_payload[key]}") - reason_parts.append(f"timeout_seconds={timeout:.3f}") - reason_parts.append(f"error_u8={error:.3f}") - reason = ":".join(reason_parts) - self.motion_stall_details = { - "kind": "motion_stall", - "stage": str(context), - "error_u8": error, - "timeout_seconds": timeout, - **detail_payload, - } - append_jsonl( - self.raw_path, - { - "kind": "mechanical_motion_stall", - "context": context, - "error_u8": error, - "timeout_seconds": timeout, - "minimum_progress_u8": minimum_progress, - "state_u8": [float(value) for value in self.latest_state_u8], - "details": detail_payload, - }, - ) - self._pause(reason) - return True - - def _active_sweep_timeout_seconds(self) -> float: - """Scale retry timeout so a deliberately slower retry can finish.""" - if self.active_sweep is None: - return float(self.sweep_timeout_seconds) - key = ( - _sweep_storage_key(self.active_sweep.spec), - self.active_sweep.cycle, - self.active_sweep.direction, - ) - retry = self.sweep_retry_counts.get(key, 0) - if retry <= 0: - return float(self.sweep_timeout_seconds) - scale = self.retry_speed_scales[ - min(retry, len(self.retry_speed_scales)) - 1 - ] - return float(self.sweep_timeout_seconds) / float(scale) - - def _endpoint_tolerance_for_spec( - self, spec: SweepSpec, endpoint_u8: int - ) -> float: - """Return the measured feedback deadband for one sweep endpoint.""" - if spec.motor_index == 10 and int(endpoint_u8) == 0: - return float( - getattr( - self, - "thumb_yaw_zero_endpoint_tolerance_u8", - self.endpoint_tolerance_u8, - ) - ) - if spec.motor_index == 19 and int(endpoint_u8) == 0: - return float( - getattr( - self, - "pinky_pip_zero_endpoint_tolerance_u8", - self.endpoint_tolerance_u8, - ) - ) - return G20ThreeCameraCalibrationNode._motor_endpoint_tolerance( - self, spec.motor_index, int(endpoint_u8) - ) - - def _synchronised_endpoint_tolerance_for_spec( - self, spec: SweepSpec, endpoint_u8: int - ) -> float: - """Return the endpoint bin deadband for image-timestamped feedback. - - Motion completion deliberately keeps the physical endpoint deadband. - A synchronised image observation uses interpolated feedback at the - camera timestamp and can trail that latest feedback by a fraction of - one u8 count. One additional count prevents a reached endpoint from - being discarded solely at this continuous-to-integer bin boundary. - """ - mechanical_tolerance = self._endpoint_tolerance_for_spec( - spec, endpoint_u8 - ) - return mechanical_tolerance + float( - getattr(self, "synchronised_endpoint_tolerance_margin_u8", 1.0) - ) - - def _motor_endpoint_tolerance( - self, motor_index: int, endpoint_u8: int - ) -> float: - """Return a side/motor-specific feedback deadband when established.""" - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if int(motor_index) == 10 and int(endpoint_u8) == 0: - return float( - getattr( - self, - "thumb_yaw_zero_endpoint_tolerance_u8", - 4.0, - ) - ) - if ( - profile.side == "right" - and int(motor_index) == 10 - and int(endpoint_u8) == 255 - ): - return float( - getattr( - self, - "right_thumb_yaw_255_endpoint_tolerance_u8", - 5.0, - ) - ) - if ( - profile.side == "right" - and int(motor_index) == 19 - and int(endpoint_u8) == 0 - ): - return float( - getattr( - self, - "pinky_pip_zero_endpoint_tolerance_u8", - 5.0, - ) - ) - return float(self.endpoint_tolerance_u8) - - def _requires_mid_sweep_baseline_hold(self, item: SweepItem) -> bool: - """Return whether this formal sweep crosses a non-endpoint zero. - - The four MCP-roll motors use command 127 as zero. Their zero-pose - hysteresis must be measured after settling at 127 from each direction, - not inferred from frames captured while the motor is moving through - 127. Endpoint-zero joints already have static start/end holds. - """ - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if ( - profile.layout_id != G20_RIGHT_19_LAYOUT - or item.precheck - or item.cycle < 0 - ): - return False - baseline = int(self.baseline_command[item.spec.motor_index]) - return baseline not in {item.start_u8, item.target_u8} - - def _start_next_sweep(self) -> None: - carried_start_frames = list( - getattr(self, "carried_sweep_start_frames", ()) - ) - if hasattr(self, "carried_sweep_start_frames"): - self.carried_sweep_start_frames.clear() - retry_items = getattr(self, "retry_sweep_items", []) - if retry_items: - self.active_sweep = retry_items.pop(0) - self.active_sweep_is_fit_retry = True - else: - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) - if self.sweep_index >= len(self.sweep_items): - self.active_sweep = None - self._begin_return_baseline("fit") - return - self.active_sweep = self.sweep_items[self.sweep_index] - self.active_sweep_is_fit_retry = False - item = self.active_sweep - profile = _node_profile(self) - if profile.layout_id == G20_RIGHT_19_LAYOUT: - self.pnp_task_spec = item.spec - is_fit_retry = bool( - getattr(self, "active_sweep_is_fit_retry", False) - ) - if _requires_pnp_tracker_reset_for_sweep( - profile, - item, - is_fit_retry=is_fit_retry, - preserve_retry_continuity=bool( - getattr(self, "retry_preserve_pnp_continuity", False) - ), - ): - reset_trackers = getattr(self, "_reset_view_trackers", None) - preserve_task_reference = _preserve_pnp_task_reference_for_sweep( - item, - is_fit_retry=is_fit_retry, - has_precheck_anchor=( - profile.layout_id == G20_RIGHT_19_LAYOUT - ), - ) - reset_views = set(_sweep_views(profile, item.spec)) - observer = next( - ( - candidate - for candidate in profile.palm_axis_observers - if candidate.task_name == item.spec.key - ), - None, - ) - if observer is not None: - reset_views.add(observer.view) - if is_fit_retry: - scoped = set(getattr(self, "retry_joint_names", set())) - if scoped: - reset_views = { - _node_profile(self).record_specs[name].view - for name in scoped - } - reset_views.discard(None) - if observer is not None: - reset_views.add(observer.view) - for view in reset_views: - runtime = getattr(self, "views", {}).get(view) - if runtime is None or reset_trackers is None: - continue - # A normal G20-right task enters here only for its first - # decreasing precheck. All formal cycles keep this exact - # frame-to-frame branch instead of independently selecting a - # new stable IPPE solution. A fit retry deliberately starts - # fresh but retains the task-relative anchor. - if preserve_task_reference: - reset_trackers( - runtime, preserve_task_reference=True - ) - else: - reset_trackers(runtime) - self._reset_view_pnp_diagnostics(runtime) - runtime.pnp_invalid_since = None - runtime.pnp_reset_count += 1 - raw_path = getattr(self, "raw_path", None) - if raw_path is not None: - append_jsonl( - raw_path, - { - "kind": "pnp_task_initialization", - "view": view, - "task_name": item.spec.key, - "motor_index": item.spec.motor_index, - "cycle": item.cycle, - "fit_retry": is_fit_retry, - "task_reference_preserved": ( - preserve_task_reference - ), - }, - ) - getattr(self, "sweep_frames", []).clear() - getattr(self, "sweep_baseline_frames", []).clear() - self.sweep_baseline_pending = False - self.sweep_baseline_hold_since = None - getattr(self, "sweep_start_frames", []).clear() - getattr(self, "sweep_start_frames", []).extend(carried_start_frames) - self.sweep_detection_total_frames = 0 - self.sweep_detection_valid_frames = 0 - self.sweep_detection_total_by_view = {} - self.sweep_detection_valid_by_view = {} - self.position_hold_since = None - self.motion_stage_started_at = time.monotonic() - current_state = getattr(self, "latest_state_u8", ()) - if len(current_state) != 20: - current_state = self.baseline_command - else: - preparation_target = build_calibration_motion_command( - item.spec, - item.start_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - current_state = self._snap_reached_state_to_command( - current_state, preparation_target - ) - self.preparation_waypoints = deque( - build_calibration_preparation_waypoints( - item.spec, - item.start_u8, - current_command=current_state, - baseline=self.baseline_command, - profile=self.profile, - parallel=self.parallel_pose_transitions, - ) - ) - self.preparation_command_u8 = self.preparation_waypoints.popleft() - self._reset_motion_progress( - self.motion_stage_started_at, - self._command_vector_error_u8(self.preparation_command_u8), - ) - self.state = STATE_PREPARE_SWEEP - self.reason = ( - f"prepare_{item.spec.view}_motor_{item.spec.motor_index}_" - f"cycle_{item.cycle}_{item.direction}" - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, self.preparation_command_u8 - ) - ) - self._publish_command(list(self.preparation_command_u8)) - - def _stage_immediate_reverse_start_frames( - self, completed: SweepItem, following: SweepItem - ) -> int: - """Carry a proven terminal endpoint into an immediate reverse pass.""" - carried = getattr(self, "carried_sweep_start_frames", None) - if carried is None: - self.carried_sweep_start_frames = [] - carried = self.carried_sweep_start_frames - carried.clear() - if ( - completed.spec != following.spec - or completed.target_u8 != following.start_u8 - or completed.direction == following.direction - ): - return 0 - tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, following.spec, following.start_u8 - ) - ) - limit_per_view = max(1, int(getattr(self, "preflight_frames", 30))) - selected: list[FrameObservation] = [] - for view in _sweep_views(_node_profile(self), following.spec): - view_frames = [ - frame - for frame in self.sweep_frames - if frame.view == view - and abs( - float(frame.state_u8[following.spec.motor_index]) - - following.start_u8 - ) - <= tolerance - ] - selected.extend(view_frames[-limit_per_view:]) - selected.sort(key=lambda frame: frame.stamp_ns) - carried.extend(selected) - if selected and getattr(self, "raw_path", None) is not None: - append_jsonl( - self.raw_path, - { - "kind": "sweep_endpoint_frames_carried", - "task_name": completed.spec.key, - "from_cycle": completed.cycle, - "from_direction": completed.direction, - "to_cycle": following.cycle, - "to_direction": following.direction, - "endpoint_u8": following.start_u8, - "valid_frames": len(selected), - "valid_frames_by_joint": { - name: len(_frames_for_joint(selected, name)) - for name in following.spec.joints - }, - }, - ) - return len(selected) - - def _begin_active_sweep(self, now: float) -> None: - assert self.active_sweep is not None - self.sweep_frames.clear() - self.sweep_frames.extend(self.sweep_start_frames) - self.sweep_start_frames.clear() - if not hasattr(self, "sweep_baseline_frames"): - self.sweep_baseline_frames = [] - self.sweep_baseline_frames.clear() - self.sweep_baseline_pending = ( - G20ThreeCameraCalibrationNode._requires_mid_sweep_baseline_hold( - self, self.active_sweep - ) - ) - self.sweep_baseline_hold_since = None - self.sweep_started_at = now - self.sweep_last_valid_at = now - capture_views = _sweep_views( - _node_profile(self), self.active_sweep.spec - ) - self.sweep_last_valid_at_by_view = { - view: now for view in capture_views - } - self.sweep_endpoint_since = None - self.sweep_detection_total_frames = 0 - self.sweep_detection_valid_frames = 0 - self.sweep_detection_total_by_view = { - view: 0 for view in capture_views - } - self.sweep_detection_valid_by_view = { - view: 0 for view in capture_views - } - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - self.sweep_checkpoint_commands = deque( - _steady_checkpoint_commands(profile, self.active_sweep) - ) - self.sweep_checkpoint_mode = bool(self.sweep_checkpoint_commands) - self.sweep_checkpoint_target_u8 = None - self.sweep_checkpoint_hold_since = None - if not hasattr(self, "sweep_checkpoint_frames"): - self.sweep_checkpoint_frames = [] - self.sweep_checkpoint_frames.clear() - if self.sweep_checkpoint_commands: - # PREPARE_SWEEP already held the start endpoint long enough to be - # a steady command-domain observation. - if hasattr(self, "command_records_by_joint"): - self._record_command_checkpoint( - self.active_sweep, - self.active_sweep.start_u8, - list(self.sweep_frames), - ) - self._reset_motion_progress( - now, - self._motion_command_error_u8( - self.active_sweep.spec, self.active_sweep.target_u8 - ), - ) - self.state = STATE_SWEEP - self.reason = ( - "collecting_dedicated_baseline_hold" - if self.sweep_baseline_pending - else "collecting_timestamp_synchronised_tag_centres" - ) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - target_u8 = ( - int(self.baseline_command[self.active_sweep.spec.motor_index]) - if self.sweep_baseline_pending - else ( - self.sweep_checkpoint_commands.popleft() - if self.sweep_checkpoint_commands - else self.active_sweep.target_u8 - ) - ) - if not self.sweep_baseline_pending and self.sweep_checkpoint_mode: - self.sweep_checkpoint_target_u8 = int(target_u8) - self._publish_command( - build_calibration_motion_command( - self.active_sweep.spec, - target_u8, - baseline=self.baseline_command, - profile=profile, - ) - ) - - def _record_command_checkpoint( - self, - item: SweepItem, - requested_command_u8: int, - frames: Sequence[FrameObservation], - ) -> None: - """Persist one steady requested-command/feedback/vision observation.""" - motor = int(item.spec.motor_index) - record_joint_names = tuple(item.spec.joints) - if getattr(self, "active_sweep_is_fit_retry", False): - scoped = set(getattr(self, "retry_joint_names", set())) - if scoped: - record_joint_names = tuple( - name for name in item.spec.joints if name in scoped - ) - for joint_name in record_joint_names: - selected = _frames_for_joint(frames, joint_name) - if len(selected) < 3: - raise RuntimeError( - f"steady command checkpoint {joint_name} has fewer than 3 frames" - ) - feedback = float( - np.median([float(frame.state_u8[motor]) for frame in selected]) - ) - state = np.median( - np.asarray( - [frame.state_u8 for frame in selected], dtype=float - ), - axis=0, - ) - durable = canonical_sample_record({ - "kind": "steady_command_sample", - "attempt": self.sweep_attempts.get( - _sweep_storage_key(item.spec), 1 - ), - "task_name": item.spec.key, - "view": self.profile.record_specs[joint_name].view, - "joint": joint_name, - "motor_index": motor, - "cycle": item.cycle, - "direction": item.direction, - "requested_command_u8": int(requested_command_u8), - "feedback_u8": round(feedback, 6), - "image_stamp_ns": int( - np.median([frame.stamp_ns for frame in selected]) - ), - "relative_quaternion_xyzw": [ - float(value) - for value in robust_rotation_summary( - [ - frame.joint_quaternions_xyzw[joint_name] - for frame in selected - ] - )[0] - ], - "relative_translation_xyz_m": [ - float(value) - for value in np.median( - np.asarray( - [ - frame.joint_vectors_xyz_m[joint_name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - ], - "image_relative_xy_px": [ - float(value) - for value in np.median( - np.asarray( - [ - frame.image_vectors_xy_px[joint_name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - ], - "parent_pose_common": _robust_pose_payload( - [frame.parent_poses_common[joint_name] for frame in selected] - ), - "child_pose_common": _robust_pose_payload( - [frame.child_poses_common[joint_name] for frame in selected] - ), - "state_u8": [float(value) for value in state], - "valid_frames": len(selected), - }) - record = fitting_sample_record(durable) - self.command_records_by_joint[joint_name].append(record) - append_jsonl(self.raw_path, durable) - - def _publish_next_checkpoint(self, now: float) -> bool: - """Advance a first-round steady scan; return False at its endpoint.""" - if not self.sweep_checkpoint_commands: - self.sweep_checkpoint_target_u8 = None - # The final checkpoint is also the normal sweep endpoint. Leave - # checkpoint mode here so _advance can run its common endpoint - # hold/completion path on this and subsequent timer ticks. Keeping - # the mode latched made the next tick treat the intentionally - # cleared target as an internal error. - self.sweep_checkpoint_mode = False - return False - assert self.active_sweep is not None - target = int(self.sweep_checkpoint_commands.popleft()) - self.sweep_checkpoint_target_u8 = target - self.sweep_checkpoint_hold_since = None - self.sweep_checkpoint_frames.clear() - self.sweep_endpoint_since = None - self.motion_stage_started_at = now - self._reset_motion_progress( - now, self._motion_command_error_u8(self.active_sweep.spec, target) - ) - self._publish_command( - build_calibration_motion_command( - self.active_sweep.spec, - target, - baseline=self.baseline_command, - profile=self.profile, - ) - ) - return True - - def _sweep_spec_start_index(self, spec: SweepSpec) -> int: - return next( - index - for index, item in enumerate(self.sweep_items) - if item.spec == spec - ) - - def _fit_joint_records( - self, - joint_name: str, - records: list[dict[str, Any]], - *, - relaxed: bool = False, - ) -> JointCurveFit: - del relaxed - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - zero_command = int( - self.baseline_command[ - profile.record_specs[joint_name].motor_index - ] - ) - model_records = G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, joint_name, records - ) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and ( - joint_name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS - or ( - joint_name.endswith("_mcp_roll") - and not joint_name.startswith("thumb_") - ) - ) - ): - return fit_joint_image_curve( - model_records, - maximum_radial_rms_px=( - self.image_trajectory_maximum_radial_rms_px - ), - maximum_radial_p95_px=( - self.image_trajectory_maximum_radial_p95_px - ), - minimum_radius_px=self.image_trajectory_minimum_radius_px, - minimum_arc_rad=self.trajectory_minimum_arc_rad, - ) - return fit_rotation_joint_curve( - model_records, - zero_command_u8=zero_command, - canonical_zero_direction=canonical_zero_direction( - profile, joint_name - ), - ) - - def _hysteresis_axis_for_fit( - self, - joint_name: str, - records: Sequence[Mapping[str, Any]], - fit: JointCurveFit, - ) -> Sequence[float]: - """Return a physical rotation axis for directional zero checks. - - End-on finger flexion uses its much more stable projected circle for - the dynamic curve. Baseline hysteresis is still a pose-domain check, - so fit a separate diagnostic rotation axis instead of accidentally - treating the image circle as a 3-D pose model. - """ - axis = fit.circle.get("axis_xyz") - if axis is not None: - return axis - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - zero_command = int( - self.baseline_command[ - profile.record_specs[joint_name].motor_index - ] - ) - diagnostic_fit = fit_rotation_joint_curve( - G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, joint_name, records - ), - zero_command_u8=zero_command, - canonical_zero_direction=canonical_zero_direction( - profile, joint_name - ), - ) - return diagnostic_fit.circle["axis_xyz"] - - def _records_with_baseline_holds( - self, - joint_name: str, - records: Sequence[Mapping[str, Any]], - ) -> list[dict[str, Any]]: - """Add settled mid-range zero poses to directional model fitting.""" - samples = [dict(record) for record in records] - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - if canonical_zero_direction(profile, joint_name) is None: - return samples - included_groups = { - (int(record["cycle"]), str(record["direction"])) - for record in samples - } - holds = getattr(self, "baseline_records_by_joint", {}).get( - joint_name, () - ) - samples.extend( - dict(record) - for record in holds - if ( - int(record["cycle"]), str(record["direction"]) - ) in included_groups - ) - return samples - - def _fit_axis_measurement_raw( - self, joint_name: str, cycle: int - ) -> JointAxisMeasurement: - constraint: Sequence[float] | None = None - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) - upstream_joint = { - # These neighbouring axes are parallel in the fixed source URDF. - # A small planar Tag's monocular PnP orientation can have a stable - # field-dependent bias, so use the well-observed upstream axis - # direction and let each Tag-centre trajectory independently fit - # its physical axis line. This changes no URDF geometry and is - # valid only because the source-URDF axes are deliberately locked. - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - **{ - f"{finger}_pip": f"{finger}_mcp_pitch" - for finger in ("index", "middle", "ring", "pinky") - }, - **{ - f"{finger}_dip": f"{finger}_pip" - for finger in ("index", "middle", "ring", "pinky") - }, - }.get(joint_name) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and joint_name.endswith("_mcp_roll") - and not joint_name.endswith("_mcp_roll_side") - and not joint_name.startswith("thumb_") - ): - reference_roll = f"{profile.reference_finger}_mcp_roll" - if joint_name != reference_roll: - # The four finger roll axes are exactly parallel in the - # product URDF. The reference-finger task is acquired first, - # so reuse its measured common-frame direction for the other - # three fingers. This removes session-to-session planar-PnP - # branch tilt without changing any fitted axis-line position. - upstream_joint = reference_roll - if upstream_joint is not None: - # Resolve recursively so thumb_ip receives the already constrained - # thumb_mcp direction (and index_dip the constrained index_pip - # direction), rather than reintroducing the raw PnP orientation at - # the last passive joint. - upstream = ( - G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - self, upstream_joint, cycle - ) - ) - constraint = upstream.axis_common_xyz - spec = profile.record_specs[joint_name] - extrinsics = getattr(self, "extrinsics", None) - view_normal = None - if extrinsics is not None: - view_transform = extrinsics.transform(spec.view) - view_normal = view_transform[:3, :3] @ np.asarray( - [0.0, 0.0, 1.0], dtype=float - ) - measurement_records = ( - G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, - joint_name, - self.records_by_joint[joint_name], - ) - ) - measurement = fit_joint_axis_measurement( - joint_name, - measurement_records, - cycle=cycle, - zero_command_u8=int( - self.baseline_command[spec.motor_index] - ), - axis_common_constraint=constraint, - constrained_circle_joints=( - zero_profile.constrained_circle_joints - | ({joint_name} if joint_name.endswith("_side") else set()) - ), - view_normal_common_xyz=view_normal, - canonical_zero_direction=canonical_zero_direction( - profile, joint_name - ), - ) - sweep_spec = next( - sweep - for sweep in profile.sweep_specs - if joint_name in sweep.joints - ) - condition_command = build_calibration_motion_command( - sweep_spec, - int(self.baseline_command[spec.motor_index]), - baseline=self.baseline_command, - profile=profile, - ) - if profile.layout_id != G20_RIGHT_19_LAYOUT: - measurement = replace( - measurement, - condition_command_u8=tuple( - float(value) for value in condition_command - ), - ) - if extrinsics is None: - return measurement - return with_depth_free_axis_projection( - replace( - measurement, - view_normal_common_xyz=tuple( - float(value) for value in view_normal - ), - ), - extrinsics.transform(spec.view)[:3, 3], - ) - - def _fit_axis_measurement( - self, joint_name: str, cycle: int - ) -> JointAxisMeasurement: - """Fit one axis, fusing a separately accepted cross-view estimate.""" - primary = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - self, joint_name, cycle - ) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - validation_sources = profile.axis_validation_sources or {} - validation_name = validation_sources.get(joint_name) - if validation_name is None: - return primary - validation_records = self.records_by_joint.get(validation_name, []) - if not any( - int(record.get("cycle", -1)) == int(cycle) - for record in validation_records - ): - # Front roll is provisionally checked before the independent side - # task has been acquired. Fusion is allowed only in the final fit - # after both complete raw datasets have independently passed. - return primary - secondary = G20ThreeCameraCalibrationNode._fit_axis_measurement_raw( - self, validation_name, cycle - ) - primary_axis = np.asarray(primary.axis_common_xyz, dtype=float) - secondary_axis = np.asarray(secondary.axis_common_xyz, dtype=float) - if float(primary_axis @ secondary_axis) < 0.0: - secondary_axis = -secondary_axis - axis_difference = math.acos( - float(np.clip(primary_axis @ secondary_axis, -1.0, 1.0)) - ) - primary_point = np.asarray(primary.point_common_xyz_m, dtype=float) - secondary_point = np.asarray(secondary.point_common_xyz_m, dtype=float) - line_distance = float( - np.linalg.norm( - np.cross(secondary_point - primary_point, primary_axis) - ) - ) - validation_spec = profile.record_specs[validation_name] - if validation_spec.zero_kind == "axis_cross_view_validation": - # This Tag is attached to the downstream PIP link. During MCP - # roll its orientation contains linkage/pitch coupling and is not - # an independent observation of the published MCP angle or axis - # direction. It remains the physical axis-line-position source; - # its own radial, line-repeatability, visibility and trajectory - # quality gates are all applied before reaching this point. - decision = ( - "diagnostic_only_gross_pose_disagreement_use_primary" - if axis_difference - > getattr( - self, - "cross_view_roll_maximum_axis_difference_rad", - math.radians(15.0), - ) - else "diagnostic_only_pose_disagreement_use_primary" - ) - G20ThreeCameraCalibrationNode._record_cross_view_roll_axis_diagnostic( - self, - joint_name, - cycle, - primary=primary, - secondary=secondary, - axis_difference_rad=axis_difference, - line_distance_m=line_distance, - decision=decision, - ) - extrinsics = getattr(self, "extrinsics", None) - if extrinsics is None or validation_spec.view is None: - raise ValueError( - f"{validation_name}: cross-view camera geometry is unavailable" - ) - primary_view = profile.record_specs[joint_name].view - if primary_view is None: - raise ValueError( - f"{joint_name}: primary point source has no view" - ) - point_camera_center = extrinsics.transform(primary_view)[:3, 3] - point_ray = primary_point - point_camera_center - interpretation_plane_normal = np.cross( - point_ray, primary_axis - ) - interpretation_plane_normal /= np.linalg.norm( - interpretation_plane_normal - ) - return replace( - primary, - point_common_xyz_m=tuple( - float(value) for value in primary.point_common_xyz_m - ), - axis_point_source=( - "front_interpretation_plane_cross_view_validated" - ), - pose_axis_line_rms_m=primary.pose_axis_line_rms_m, - pose_axis_line_source_joints=(joint_name,), - axis_point_camera_center_common_xyz_m=tuple( - float(value) - for value in point_camera_center - ), - axis_point_interpretation_plane_normal_common_xyz=tuple( - float(value) - for value in interpretation_plane_normal - ), - ) - # Monocular planar-tag orientation on the side view carries a - # systematic line-of-sight IPPE bias of several degrees through roll - # sweeps (tags tilted ~13-20 deg from the ray), which sub-pixel - # reprojection cannot expose. Session 20260820_105535 showed a - # stable 11.4 deg front/side disagreement from exactly this effect, - # so sub-degree cross-view agreement is not achievable at this - # geometry. Disagreement above the fusion gate now falls back to the - # trusted front-only axis with a recorded diagnostic instead of - # failing the joint; only a gross direction error (wrong-link or a - # loose Tag that changes the observed motion axis) still fails here. - gross_axis_limit = getattr( - self, - "cross_view_roll_maximum_axis_difference_rad", - math.radians(15.0), - ) - # Do not apply a universal front/side line-distance gate here. The - # front roll link rides the splay screw, so its pure-revolute pose fit - # is a displaced pseudo-line whose offset varies by finger (about - # 15 mm for pinky and 37 mm for ring on the same fixed setup). It is - # not the physical line ultimately published. Retain the distance in - # diagnostics; validate the selected side physical line through its - # own radial quality, cycle RMS and final URDF geometry instead. - if axis_difference > gross_axis_limit: - raise ValueError( - "cross_view_roll_axis_gross_disagreement:" - f"{math.degrees(axis_difference):.6f}deg," - f"{1000.0 * line_distance:.6f}mm" - ) - if ( - axis_difference > self.zero_maximum_axis_cycle_difference_rad - or line_distance > self.axis_maximum_pose_line_rms_m - ): - selected_direction = primary - decision = "skip_fusion_use_primary" - zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) - observer_name = next( - ( - observer - for observer, parent in zero_profile.axis_parent_joint.items() - if parent == joint_name - ), - None, - ) - observer_records = ( - [] - if observer_name is None - else self.records_by_joint.get(observer_name, []) - ) - if observer_records: - model = UrdfKinematicModel(self.source_urdf_path) - parent_axis, _ = model.axis_line( - joint_name, zero_offsets={}, joint_angles={} - ) - observer_axis, _ = model.axis_line( - observer_name, zero_offsets={}, joint_angles={} - ) - expected_cone = math.acos( - abs( - float( - np.clip(parent_axis @ observer_axis, -1.0, 1.0) - ) - ) - ) - - def cone_residual( - candidate_axis: np.ndarray, - measured_observer_axis: np.ndarray, - ) -> float: - measured_cone = math.acos( - abs( - float( - np.clip( - candidate_axis @ measured_observer_axis, - -1.0, - 1.0, - ) - ) - ) - ) - return abs(measured_cone - expected_cone) - - # A zero rotates the downstream axis around this parent and - # cannot change their mutual cone angle. Select the camera - # once for the complete repeated-sweep group. Per-cycle - # selection is invalid because a residual close to the gate - # can alternate views and turn their fixed systematic bias - # into a false cycle-axis spread. - group_cycles = tuple(range(int(self.repetitions))) - required_names = ( - joint_name, - validation_name, - observer_name, - ) - group_is_complete = all( - any( - int(record.get("cycle", -1)) == group_cycle - for record in self.records_by_joint.get(name, ()) - ) - for name in required_names - for group_cycle in group_cycles - ) - group_source = "primary" - if group_is_complete: - group_signature = tuple( - ( - name, - group_cycle, - len(cycle_records), - max( - ( - int(record.get("attempt", 0)) - for record in cycle_records - ), - default=0, - ), - max( - ( - int(record.get("image_stamp_ns", 0)) - for record in cycle_records - ), - default=0, - ), - ) - for name in required_names - for group_cycle in group_cycles - for cycle_records in ( - [ - record - for record in self.records_by_joint.get( - name, () - ) - if int(record.get("cycle", -1)) - == group_cycle - ], - ) - ) - cache_key = ( - joint_name, - validation_name, - observer_name, - group_signature, - ) - source_cache = getattr( - self, "_cross_view_roll_group_source_cache", {} - ) - cached_source = source_cache.get(cache_key) - if cached_source is not None: - group_source = str(cached_source) - else: - primary_residuals: list[float] = [] - secondary_residuals: list[float] = [] - fit_raw = ( - G20ThreeCameraCalibrationNode - ._fit_axis_measurement_raw - ) - try: - for group_cycle in group_cycles: - group_primary = fit_raw( - self, joint_name, group_cycle - ) - group_secondary = fit_raw( - self, validation_name, group_cycle - ) - group_observer = fit_raw( - self, observer_name, group_cycle - ) - observer_axis_common = np.asarray( - group_observer.axis_common_xyz, - dtype=float, - ) - primary_residuals.append( - cone_residual( - np.asarray( - group_primary.axis_common_xyz, - dtype=float, - ), - observer_axis_common, - ) - ) - secondary_residuals.append( - cone_residual( - np.asarray( - group_secondary.axis_common_xyz, - dtype=float, - ), - observer_axis_common, - ) - ) - except (KeyError, ValueError): - primary_residuals = [] - secondary_residuals = [] - group_source = ( - select_cross_view_roll_direction_source( - primary_residuals, - secondary_residuals, - self.zero_maximum_axis_cone_mismatch_rad, - ) - ) - source_cache[cache_key] = group_source - if len(source_cache) > 32: - source_cache.pop(next(iter(source_cache))) - self._cross_view_roll_group_source_cache = source_cache - if group_source == "secondary": - selected_direction = replace( - primary, - axis_common_xyz=tuple( - float(value) for value in secondary_axis - ), - axis_direction_source=( - "cross_view_cone_selected_secondary" - ), - ) - decision = ( - "use_secondary_zero_invariant_cone_consensus" - ) - G20ThreeCameraCalibrationNode._record_cross_view_roll_axis_diagnostic( - self, - joint_name, - cycle, - primary=primary, - secondary=secondary, - axis_difference_rad=axis_difference, - line_distance_m=line_distance, - decision=decision, - ) - # The front roll link rides the splay screw: its orientation - # tracks the rotation faithfully, but its centre trajectory - # carries the screw translation, displacing the fitted axis - # line by ~21 mm from the finger's physical MCP axis. The side - # PIP-link circle recovers that physical line. Keep the one - # direction selected consistently for all cycles and take the - # line position from the side view. - return replace( - selected_direction, - point_common_xyz_m=tuple( - float(value) - for value in secondary.point_common_xyz_m - ), - axis_point_source="side_circle_cross_view", - pose_axis_line_rms_m=secondary.pose_axis_line_rms_m, - pose_axis_line_source_joints=(validation_name,), - ) - primary_variance = max( - primary.radial_rms_m ** 2 + primary.pose_axis_line_rms_m ** 2, - 1.0e-12, - ) - secondary_variance = max( - secondary.radial_rms_m ** 2 + secondary.pose_axis_line_rms_m ** 2, - 1.0e-12, - ) - primary_weight = 1.0 / primary_variance - secondary_weight = 1.0 / secondary_variance - total_weight = primary_weight + secondary_weight - fused_axis = ( - primary_weight * primary_axis - + secondary_weight * secondary_axis - ) - fused_axis /= np.linalg.norm(fused_axis) - # Axis-line points have a free coordinate along the axis. Fuse only - # the observable perpendicular displacement and retain the primary - # point's along-axis gauge. - delta = secondary_point - primary_point - perpendicular_delta = delta - fused_axis * float(delta @ fused_axis) - fused_point = ( - primary_point - + secondary_weight / total_weight * perpendicular_delta - ) - return replace( - primary, - axis_common_xyz=tuple(float(value) for value in fused_axis), - point_common_xyz_m=tuple(float(value) for value in fused_point), - plane_rms_m=max(primary.plane_rms_m, secondary.plane_rms_m), - radial_rms_m=max(primary.radial_rms_m, secondary.radial_rms_m), - pose_axis_line_rms_m=max( - primary.pose_axis_line_rms_m, - secondary.pose_axis_line_rms_m, - line_distance, - ), - pose_axis_line_source_joints=(joint_name, validation_name), - axis_direction_source="cross_view_weighted_fusion", - ) - - def _refit_cross_view_axis_line_group( - self, - joint_name: str, - measurements: Sequence[JointAxisMeasurement], - ) -> list[JointAxisMeasurement]: - """Apply the repeated-cycle physical-radius constraint when eligible.""" - group = list(measurements) - sources = { - source - for measurement in group - if (source := cross_view_side_line_source(measurement)) is not None - } - if ( - len(group) != int(self.repetitions) - or len(sources) != 1 - or not all( - cross_view_side_line_source(measurement) is not None - for measurement in group - ) - or any( - axis_line_uses_depth_free_interpretation_plane(measurement) - for measurement in group - ) - ): - return group - source = next(iter(sources)) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - source_spec = profile.record_specs[source] - source_records = G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, - source, - self.records_by_joint[source], - ) - return list( - refit_axis_line_group_with_shared_radius( - group, - source_records, - zero_command_u8=int( - self.baseline_command[source_spec.motor_index] - ), - canonical_zero_direction=canonical_zero_direction( - profile, source - ), - ) - ) - - def _record_cross_view_roll_axis_diagnostic( - self, - joint_name: str, - cycle: int, - *, - primary: Any, - secondary: Any, - axis_difference_rad: float, - line_distance_m: float, - decision: str = "skip_fusion_use_primary", - ) -> None: - """Log the deterministic cross-view direction decision.""" - record = { - "kind": "cross_view_roll_axis_diagnostic", - "joint": joint_name, - "cycle": int(cycle), - "decision": str(decision), - "axis_difference_deg": round( - math.degrees(axis_difference_rad), 6 - ), - "line_distance_mm": round(1000.0 * line_distance_m, 6), - "primary_axis_common_xyz": [ - round(float(value), 9) for value in primary.axis_common_xyz - ], - "secondary_axis_common_xyz": [ - round(float(value), 9) for value in secondary.axis_common_xyz - ], - "primary_axis_direction_source": primary.axis_direction_source, - } - try: - append_jsonl(self.raw_path, record) - except Exception as error: - logger_factory = getattr(self, "get_logger", None) - if callable(logger_factory): - logger_factory().warning( - f"failed to record cross-view roll diagnostic: {error}" - ) - logger_factory = getattr(self, "get_logger", None) - if callable(logger_factory): - selected_text = ( - "using the side axis selected by the zero-invariant cone" - if decision.startswith( - "use_secondary_zero_invariant_cone" - ) - else "keeping the front-only axis" - ) - logger_factory().warning( - f"{joint_name} cycle {cycle}: front/side roll axes disagree " - f"by {math.degrees(axis_difference_rad):.2f} deg; skipping " - f"fusion and {selected_text}" - ) - - def _provisional_fit_failures( - self, spec: SweepSpec, *, include_view_validity: bool = True - ) -> list[dict[str, Any]]: - """Check full-pose curve and 3-D axis after one six-way task.""" - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - zero_profile = getattr(self, "zero_profile", LEFT_ZERO_PROFILE) - failures: list[dict[str, Any]] = [] - if include_view_validity and hasattr(self, "views"): - for view in _sweep_views(profile, spec): - runtime = self.views[view] - view_joints = _sweep_joints_for_view(profile, spec, view) - validation_only_view = bool(view_joints) and all( - profile.record_specs[name].zero_kind - == "axis_cross_view_validation" - for name in view_joints - ) - retry_scope = set( - getattr(self, "retry_joint_names", set()) - if getattr(self, "retry_sweep_spec", None) == spec - else () - ) - if retry_scope and not retry_scope.intersection(view_joints): - # This view's accepted records were intentionally kept; - # only the independently failed view is being measured. - continue - valid_rate = float(runtime.valid_rate) - # Prefer the task-scoped counters: after the last sweep the - # required-role set flips back to the full preflight set and - # the rolling window restarts on idle frames, which is not - # what a per-task capture-quality gate should measure. - total = int(getattr(runtime, "task_total_frames", 0)) - if total > 0: - valid_rate = ( - float(getattr(runtime, "task_valid_frames", 0)) - / total - ) - imported_without_capture = ( - total == 0 - and spec.key - in set(getattr(self, "resumed_task_keys", ()) or ()) - ) - dense_trajectory_coverage = bool( - profile.layout_id == G20_RIGHT_19_LAYOUT - and _records_have_formal_sweep_coverage( - self.records_by_joint, - view_joints, - repetitions=self.repetitions, - minimum_bins=int( - getattr(self, "minimum_sweep_bins", 32) - ), - maximum_bin_gap=int( - getattr(self, "maximum_bin_gap", 16) - ), - ) - ) - if ( - not imported_without_capture - and valid_rate < self.minimum_detection_rate - and not validation_only_view - and not dense_trajectory_coverage - ): - failures.append( - { - "joint": view_joints[0], - "view": view, - "metric": "tag_valid_rate_percent", - "actual": round(100.0 * valid_rate, 3), - "limit": round( - 100.0 * self.minimum_detection_rate, 3 - ), - "comparison": "minimum", - "task_valid_frames": int( - getattr(runtime, "task_valid_frames", 0) - ), - "task_total_frames": total, - } - ) - for joint_name in spec.joints: - records = self.records_by_joint[joint_name] - sync_p95 = float( - np.percentile( - [ - float(record.get("state_image_sync_error_ms", 0.0)) - for record in records - ], - 95.0, - ) - ) - sync_limit_ms = ( - getattr(self, "maximum_state_image_skew_ns", 50_000_000) - / 1_000_000.0 - ) - if sync_p95 > sync_limit_ms: - failures.append( - { - "joint": joint_name, - "metric": "state_image_sync_p95_ms", - "actual": round(sync_p95, 6), - "limit": round(sync_limit_ms, 6), - "comparison": "maximum", - } - ) - try: - fit = self._fit_joint_records( - joint_name, - records, - ) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "trajectory_fit", - "reason": str(error), - } - ) - continue - if profile.steady_command_checkpoints: - command_store = getattr( - self, "command_records_by_joint", None - ) - if command_store is not None: - command_records = [ - dict(record) - for record in command_store.get(joint_name, ()) - if int(record.get("cycle", 0)) == 0 - ] - if len(command_records) < 18: - failures.append( - { - "joint": joint_name, - "metric": "steady_command_checkpoints", - "actual": len(command_records), - "limit": 18, - "comparison": "minimum", - } - ) - else: - try: - self._fit_joint_records( - joint_name, command_records - ) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "command_trajectory_fit", - "reason": str(error), - } - ) - else: - feedback_fit = self._fit_joint_records( - joint_name, - _steady_records_in_feedback_domain( - command_records - ), - ) - command_gap = float( - feedback_fit.maximum_hysteresis_rad - ) - command_gap_limit = float( - self.command_maximum_direction_gap_rad - ) - if command_gap > command_gap_limit: - failures.append( - { - "joint": joint_name, - "metric": ( - "feedback_direction_gap_deg" - ), - "actual": round( - math.degrees(command_gap), 6 - ), - "limit": round( - math.degrees(command_gap_limit), 6 - ), - "comparison": "maximum", - } - ) - try: - baseline_hysteresis = baseline_hysteresis_by_cycle_rad( - G20ThreeCameraCalibrationNode._baseline_hysteresis_records( - self, joint_name, records - ), - zero_command_u8=int( - self.baseline_command[ - profile.record_specs[joint_name].motor_index - ] - ), - axis_xyz=( - G20ThreeCameraCalibrationNode - ._hysteresis_axis_for_fit( - self, joint_name, records, fit - ) - ), - ) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "baseline_hysteresis", - "reason": str(error), - } - ) - continue - maximum_baseline_hysteresis = max(baseline_hysteresis) - canonical_direction = canonical_zero_direction( - profile, joint_name - ) - if canonical_direction is not None: - validation_only_pose = ( - profile.record_specs[joint_name].zero_kind - == "axis_cross_view_validation" - ) - branch_gap_range = ( - maximum_baseline_hysteresis - - min(baseline_hysteresis) - ) - branch_gap_limit = getattr( - self, - "directional_zero_maximum_branch_gap_rad", - math.radians(1.5), - ) - branch_gap_range_limit = getattr( - self, - "directional_zero_maximum_branch_gap_range_rad", - math.radians(0.3), - ) - if maximum_baseline_hysteresis > branch_gap_limit: - failures.append( - { - "joint": joint_name, - "metric": "baseline_directional_gap_deg", - "actual": round( - math.degrees( - maximum_baseline_hysteresis - ), - 6, - ), - "limit": round( - math.degrees(branch_gap_limit), 6 - ), - "comparison": "maximum", - "cycle_values_deg": [ - round(math.degrees(value), 6) - for value in baseline_hysteresis - ], - } - ) - if ( - branch_gap_range > branch_gap_range_limit - and not validation_only_pose - ): - failures.append( - { - "joint": joint_name, - "metric": ( - "baseline_directional_gap_range_deg" - ), - "actual": round( - math.degrees(branch_gap_range), 6 - ), - "limit": round( - math.degrees(branch_gap_range_limit), 6 - ), - "comparison": "maximum", - "cycle_values_deg": [ - round(math.degrees(value), 6) - for value in baseline_hysteresis - ], - } - ) - elif ( - branch_gap_range > branch_gap_range_limit - and validation_only_pose - ): - # This side-view alias is never published as a joint - # angle. Its orientation is reconstructed from a - # near-grazing planar Tag, so IPPE pose drift can move - # the apparent branch gap between rounds even when - # the image trajectory and the authoritative front - # axis are repeatable. Keep the production threshold - # as an audit reference, but do not rescan identical - # motion based on a non-published pose component. - append_jsonl( - self.raw_path, - { - "kind": "validation_only_quality_diagnostic", - "joint": joint_name, - "metric": ( - "baseline_directional_gap_range_deg" - ), - "actual": round( - math.degrees(branch_gap_range), 6 - ), - "reference_limit": round( - math.degrees(branch_gap_range_limit), 6 - ), - "cycle_values_deg": [ - round(math.degrees(value), 6) - for value in baseline_hysteresis - ], - "decision": "diagnostic_only", - }, - ) - elif ( - maximum_baseline_hysteresis - > self.baseline_maximum_hysteresis_rad - ): - failures.append( - { - "joint": joint_name, - "metric": "baseline_hysteresis_deg", - "actual": round( - math.degrees(maximum_baseline_hysteresis), 6 - ), - "limit": round( - math.degrees( - self.baseline_maximum_hysteresis_rad - ), - 6, - ), - "comparison": "maximum", - "cycle_values_deg": [ - round(math.degrees(value), 6) - for value in baseline_hysteresis - ], - } - ) - if fit.circle.get("space") == "image_2d": - checks = ( - ( - "image_radial_rms_px", - float(fit.quality["radial_rms_px"]), - self.image_trajectory_maximum_radial_rms_px, - "maximum", - ), - ( - "image_radial_p95_px", - float(fit.quality["radial_p95_px"]), - self.image_trajectory_maximum_radial_p95_px, - "maximum", - ), - ( - "image_radius_px", - float(fit.quality["radius_px"]), - self.image_trajectory_minimum_radius_px, - "minimum", - ), - ( - "arc_deg", - math.degrees(float(fit.quality["arc_rad"])), - math.degrees(self.trajectory_minimum_arc_rad), - "minimum", - ), - ) - else: - checks = ( - ( - "rotation_orthogonal_rms_deg", - math.degrees( - float( - fit.quality["rotation_orthogonal_rms_rad"] - ) - ), - math.degrees( - self.active_maximum_rotation_orthogonal_rms_rad - if profile.record_specs[joint_name].active - else self.passive_maximum_rotation_orthogonal_rms_rad - ), - "maximum", - ), - ( - "arc_deg", - math.degrees(float(fit.quality["arc_rad"])), - math.degrees(self.trajectory_minimum_arc_rad), - "minimum", - ), - ) - joint_spec = profile.record_specs[joint_name] - monotonic_limit = ( - self.maximum_monotonic_correction_rad - if joint_spec.active - else self.passive_maximum_monotonic_correction_rad - ) - hysteresis_limit = ( - self.maximum_hysteresis_rad - if joint_spec.active - else self.passive_maximum_hysteresis_rad - ) - checks = checks + ( - ( - "monotonic_correction_deg", - math.degrees(fit.maximum_monotonic_correction_rad), - math.degrees(monotonic_limit), - "maximum", - ), - ) - if not profile.directional_zero: - checks = checks + ( - ( - "hysteresis_deg", - math.degrees(fit.maximum_hysteresis_rad), - math.degrees(hysteresis_limit), - "maximum", - ), - ) - for metric, actual, limit, comparison in checks: - failed = ( - actual > limit - if comparison == "maximum" - else actual < limit - ) - if failed: - failures.append( - { - "joint": joint_name, - "metric": metric, - "actual": round(float(actual), 6), - "limit": round(float(limit), 6), - "comparison": comparison, - } - ) - - cycle_travels: list[float] = [] - cycle_axes: list[np.ndarray] = [] - cycle_axis_sources: list[str] = [] - cycle_axis_measurements: list[JointAxisMeasurement] = [] - for cycle in range(self.repetitions): - cycle_records = [ - record - for record in self.records_by_joint[joint_name] - if int(record["cycle"]) == cycle - ] - try: - cycle_fit = self._fit_joint_records( - joint_name, cycle_records - ) - except Exception as error: - failures.append( - { - "joint": joint_name, - "metric": "cycle_fit", - "cycle": cycle + 1, - "reason": str(error), - } - ) - continue - cycle_travels.append( - abs( - float(cycle_fit.angle_rad[0]) - - float(cycle_fit.angle_rad[255]) - ) - ) - try: - axis = self._fit_axis_measurement(joint_name, cycle) - except Exception as error: - reason = str(error) - failure: dict[str, Any] = { - "joint": joint_name, - "metric": "axis_fit", - "cycle": cycle + 1, - "reason": reason, - } - if reason.startswith( - "cross_view_roll_axis_gross_disagreement:" - ): - validation_name = ( - profile.axis_validation_sources or {} - ).get(joint_name) - if validation_name in spec.joints: - # A gross direction disagreement does not identify - # which independently observed view is wrong. A - # retry must therefore reacquire both sources. - failure["quality_source_joints"] = [ - joint_name, - validation_name, - ] - failures.append(failure) - continue - cycle_axes.append(np.asarray(axis.axis_common_xyz, dtype=float)) - cycle_axis_sources.append(axis.axis_direction_source) - cycle_axis_measurements.append(axis) - plane_limit = ( - self.axis_maximum_plane_rms_m - if joint_spec.active - else self.passive_axis_maximum_plane_rms_m - ) - axis_checks = [ - ( - "axis_radial_rms_mm", - 1000.0 * axis.radial_rms_m, - 1000.0 * self.axis_maximum_radial_rms_m, - ), - ] - if ( - joint_spec.zero_kind != "axis_cross_view_validation" - and cross_view_side_line_source(axis) is None - ): - # A cross-view fallback deliberately takes its direction - # from the front and its line position from the side. The - # side Tag's monocular orientation is not the published - # direction, so its per-frame ideal-revolute residual is - # not an accuracy measure for the combined line. Those - # lines are checked for independent-cycle position - # repeatability below, at this same threshold. - axis_checks.append( - ( - "axis_pose_line_rms_mm", - 1000.0 * axis.pose_axis_line_rms_m, - 1000.0 * self.axis_maximum_pose_line_rms_m, - ) - ) - circle_is_constrained = circle_direction_is_constrained( - joint_name, zero_profile.constrained_circle_joints - ) - if not circle_is_constrained: - axis_checks.extend( - [ - ( - "axis_plane_rms_mm", - 1000.0 * axis.plane_rms_m, - 1000.0 * plane_limit, - ), - ] - ) - if not circle_is_constrained: - axis_checks.append( - ( - "rotation_circle_axis_difference_deg", - math.degrees( - axis.rotation_circle_axis_difference_rad - ), - math.degrees( - self.axis_maximum_rotation_circle_difference_rad - ), - ) - ) - # With a trusted orientation-constrained axis, plane_rms is - # scatter *along* that infinite axis. It cannot change the - # axis line or joint zero, so only the perpendicular/radial - # error is an admissibility check for constrained circles. - for metric, actual, limit in axis_checks: - if actual > limit: - failure_joint = joint_name - quality_sources: tuple[str, ...] = () - if metric == "axis_pose_line_rms_mm": - quality_sources = tuple( - str(source) - for source in getattr( - axis, - "pose_axis_line_source_joints", - (), - ) - if str(source) in spec.joints - ) - if len(quality_sources) == 1: - failure_joint = quality_sources[0] - failure = { - "joint": failure_joint, - "metric": metric, - "cycle": cycle + 1, - "actual": round(float(actual), 6), - "limit": round(float(limit), 6), - "comparison": "maximum", - } - if quality_sources: - failure["quality_source_joints"] = list( - quality_sources - ) - if failure_joint != joint_name: - failure["model_joint"] = joint_name - failures.append(failure) - side_line_sources = { - source - for measurement in cycle_axis_measurements - if ( - source := cross_view_side_line_source(measurement) - ) is not None - } - if side_line_sources: - try: - cycle_axis_measurements = ( - G20ThreeCameraCalibrationNode - ._refit_cross_view_axis_line_group( - self, joint_name, cycle_axis_measurements - ) - ) - except Exception as error: - source = next(iter(side_line_sources)) - failures.append( - { - "joint": source, - "model_joint": joint_name, - "quality_source_joints": [source], - "metric": "axis_line_shared_radius_fit", - "reason": str(error), - } - ) - side_line_sources = { - source - for measurement in cycle_axis_measurements - if ( - source := cross_view_side_line_source(measurement) - ) is not None - } - if ( - len(cycle_axis_measurements) == self.repetitions - and len(side_line_sources) == 1 - and all( - cross_view_side_line_source(measurement) is not None - for measurement in cycle_axis_measurements - ) - and not any( - axis_line_uses_depth_free_interpretation_plane(measurement) - for measurement in cycle_axis_measurements - ) - ): - line_rms = axis_line_cycle_rms_m( - cycle_axis_measurements - ) - if line_rms > self.axis_maximum_pose_line_rms_m: - source = next(iter(side_line_sources)) - outliers = _isolated_axis_line_cycle_outliers( - cycle_axis_measurements, - self.axis_maximum_pose_line_rms_m, - ) - failure = { - "joint": source, - "model_joint": joint_name, - "quality_source_joints": [source], - "metric": "axis_line_cycle_rms_mm", - "actual": round(1000.0 * line_rms, 6), - "limit": round( - 1000.0 * self.axis_maximum_pose_line_rms_m, - 6, - ), - "maximum_pairwise_mm": round( - 1000.0 - * maximum_axis_line_cycle_spread_m( - cycle_axis_measurements - ), - 6, - ), - "comparison": "maximum", - } - if outliers: - outlier = next(iter(outliers)) - failure["cycle"] = outlier + 1 - failure["inlier_cycles"] = [ - index + 1 - for index in range(len(cycle_axis_measurements)) - if index != outlier - ] - failures.append(failure) - if len(cycle_travels) == self.repetitions: - travel_range = max(cycle_travels) - min(cycle_travels) - cycle_limit = ( - self.trajectory_maximum_cycle_travel_difference_rad - if joint_spec.active - else self.passive_maximum_cycle_travel_difference_rad - ) - if travel_range > cycle_limit: - failures.append( - { - "joint": joint_name, - "metric": "cycle_travel_range_deg", - "actual": round(math.degrees(travel_range), 6), - "limit": round(math.degrees(cycle_limit), 6), - "comparison": "maximum", - "cycle_travel_deg": [ - round(math.degrees(value), 6) - for value in cycle_travels - ], - } - ) - if ( - len(cycle_axes) == self.repetitions - and not all( - source == "upstream_constraint" - for source in cycle_axis_sources - ) - ): - maximum_difference = 0.0 - for left in cycle_axes: - for right in cycle_axes: - maximum_difference = max( - maximum_difference, - math.acos( - abs(float(np.clip(left @ right, -1.0, 1.0))) - ), - ) - if ( - maximum_difference - > self.zero_maximum_axis_cycle_difference_rad - and joint_spec.zero_kind - == "axis_cross_view_validation" - ): - # As above, the side alias' independently reconstructed - # pose axis is not the axis written to the URDF. The - # published front direction and the side translation - # circle retain their own strict cross-round gates. - append_jsonl( - self.raw_path, - { - "kind": "validation_only_quality_diagnostic", - "joint": joint_name, - "metric": "axis_cycle_difference_deg", - "actual": round( - math.degrees(maximum_difference), 6 - ), - "reference_limit": round( - math.degrees( - self.zero_maximum_axis_cycle_difference_rad - ), - 6, - ), - "decision": "diagnostic_only", - }, - ) - elif ( - maximum_difference - > self.zero_maximum_axis_cycle_difference_rad - ): - outliers = _isolated_axis_cycle_outliers( - cycle_axes, - self.zero_maximum_axis_cycle_difference_rad, - ) - failure = { - "joint": joint_name, - "metric": "axis_cycle_difference_deg", - "actual": round(math.degrees(maximum_difference), 6), - "limit": round( - math.degrees( - self.zero_maximum_axis_cycle_difference_rad - ), - 6, - ), - "comparison": "maximum", - } - if outliers: - outlier = next(iter(outliers)) - failure["cycle"] = outlier + 1 - failure["inlier_cycles"] = [ - index + 1 - for index in range(len(cycle_axes)) - if index != outlier - ] - failures.append(failure) - if profile.cross_view_roll_curve: - for primary_name, validation_name in ( - profile.axis_validation_sources or {} - ).items(): - if not {primary_name, validation_name}.issubset(spec.joints): - continue - try: - cycle_fits = [ - ( - self._fit_joint_records( - primary_name, - [ - record - for record in self.records_by_joint[ - primary_name - ] - if int(record.get("cycle", -1)) == cycle - ], - ), - self._fit_joint_records( - validation_name, - [ - record - for record in self.records_by_joint[ - validation_name - ] - if int(record.get("cycle", -1)) == cycle - ], - ), - ) - for cycle in range(self.repetitions) - ] - training_cycles = set(range(max(1, self.repetitions - 1))) - scopes = ( - ( - "training", - [ - record - for record in self.records_by_joint[primary_name] - if int(record.get("cycle", -1)) - in training_cycles - ], - [ - record - for record in self.records_by_joint[ - validation_name - ] - if int(record.get("cycle", -1)) - in training_cycles - ], - ), - ( - "all_cycles", - self.records_by_joint[primary_name], - self.records_by_joint[validation_name], - ), - ) - for scope, primary_records, validation_records in scopes: - failure = _cross_view_curve_failure( - primary_name, - validation_name, - self._fit_joint_records( - primary_name, primary_records - ), - self._fit_joint_records( - validation_name, validation_records - ), - scope=scope, - maximum_rms_difference_rad=( - self.cross_view_roll_maximum_shape_rms_rad - ), - maximum_branch_gap_difference_rad=( - self.cross_view_roll_maximum_branch_gap_difference_rad - ), - allow_projection_scale=True, - maximum_projection_scale_ratio=( - self.cross_view_roll_maximum_projection_scale_ratio - ), - cycle_fits=cycle_fits, - ) - if failure is not None: - append_jsonl( - self.raw_path, - { - "kind": ( - "validation_only_quality_diagnostic" - ), - **failure, - "decision": "diagnostic_only", - }, - ) - # One structured result contains all per-cycle - # evidence; avoid duplicate training/final errors. - break - except Exception as error: - append_jsonl( - self.raw_path, - { - "kind": "validation_only_quality_diagnostic", - "joint": primary_name, - "metric": "cross_view_roll_curve", - "reason": str(error), - "quality_source_joints": [validation_name], - "decision": "diagnostic_only", - }, - ) - return failures - - def _cross_view_roll_diagnostic_role( - self, spec: SweepSpec - ) -> str | None: - finger = str( - getattr(self, "cross_view_roll_diagnostic_finger", "") - ) - if not finger: - return None - if spec.key == f"{finger}_roll_multiview": - return "multiview" - return None - - @staticmethod - def _only_baseline_hysteresis_failures( - failures: Sequence[Mapping[str, Any]], - ) -> bool: - return bool(failures) and all( - str(item.get("metric", "")) - in { - "baseline_hysteresis_deg", - "baseline_directional_gap_deg", - "baseline_directional_gap_range_deg", - } - for item in failures - ) - - def _roll_baseline_hysteresis_degrees( - self, joint_name: str - ) -> list[float]: - records = self.records_by_joint[joint_name] - fit = self._fit_joint_records(joint_name, records) - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - spec = profile.record_specs[joint_name] - values = baseline_hysteresis_by_cycle_rad( - G20ThreeCameraCalibrationNode._baseline_hysteresis_records( - self, joint_name, records - ), - zero_command_u8=int( - self.baseline_command[spec.motor_index] - ), - axis_xyz=fit.circle["axis_xyz"], - ) - return [round(math.degrees(value), 6) for value in values] - - def _complete_cross_view_roll_diagnostic( - self, - side_spec: SweepSpec, - side_quality_failures: Sequence[Mapping[str, Any]] = (), - ) -> None: - finger = self.cross_view_roll_diagnostic_finger - front_joint = f"{finger}_mcp_roll" - side_joint = f"{front_joint}_side" - front_values = self._roll_baseline_hysteresis_degrees(front_joint) - side_values = self._roll_baseline_hysteresis_degrees(side_joint) - limit_deg = math.degrees(self.baseline_maximum_hysteresis_rad) - interpretation = _classify_cross_view_roll_hysteresis( - front_values, side_values, limit_deg=limit_deg - ) - result = { - "kind": "cross_view_roll_diagnostic", - "finger": finger, - "motor_index": side_spec.motor_index, - "front_joint": front_joint, - "side_joint": side_joint, - "front_by_cycle_deg": front_values, - "side_by_cycle_deg": side_values, - "front_maximum_deg": max(front_values), - "side_maximum_deg": max(side_values), - "formal_limit_deg": limit_deg, - "interpretation": interpretation, - "side_quality_failures": [ - dict(item) for item in side_quality_failures - ], - "publication_locked": True, - } - self.cross_view_roll_diagnostic_result = result - self.fit_failure = result - append_jsonl(self.raw_path, result) - self._pause("cross_view_roll_diagnostic_complete") - - def _pause_for_provisional_fit_failure( - self, - spec: SweepSpec, - failures: list[dict[str, Any]], - *, - allow_warning: bool = False, - ) -> bool: - diagnostic_role = ( - G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( - self, spec - ) - ) - if ( - diagnostic_role == "front" - and G20ThreeCameraCalibrationNode._only_baseline_hysteresis_failures( - failures - ) - ): - append_jsonl( - self.raw_path, - { - "kind": "cross_view_roll_front_failure_deferred", - "finger": self.cross_view_roll_diagnostic_finger, - "view": spec.view, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "failures": failures, - "publication_locked": True, - }, - ) - self.reason = "cross_view_roll_front_failure_deferred" - return False - - def is_warning(item: Mapping[str, Any]) -> bool: - if "actual" not in item or "limit" not in item: - return False - actual = float(item["actual"]) - limit = float(item["limit"]) - ratio = float(getattr(self, "provisional_warning_ratio", 1.0)) - if limit <= 0.0: - return False - if item.get("comparison") == "minimum": - return actual >= limit / ratio - return actual <= limit * ratio - - if allow_warning and failures and all(is_warning(item) for item in failures): - band_attempt = int( - self.sweep_attempts.get(_sweep_storage_key(spec), 1) - ) - retry_limit = int( - getattr(self, "automatic_fit_retry_limit", 0) - ) - # The final fit re-applies the unmodified hard thresholds to the - # same records. Letting even a tiny overrun continue would make - # the final fit recall this task after every later joint has been - # collected. Consume every remaining retry here; when the budget - # is exhausted, the common failure path below pauses in place. - append_jsonl( - self.raw_path, - { - "kind": ( - "provisional_fit_warning_rescan" - if band_attempt <= retry_limit - else "provisional_fit_warning_retry_exhausted" - ), - "view": spec.view, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "attempt": band_attempt, - "attempt_limit": retry_limit + 1, - "warning_ratio": float( - getattr( - self, "provisional_warning_ratio", 1.0 - ), - ), - "failures": failures, - }, - ) - - attempt = self.sweep_attempts.get(_sweep_storage_key(spec), 1) - self.retry_resume_index = self.sweep_index - self.retry_sweep_spec = spec - self.retry_joint_names = _fit_retry_joint_names( - _node_profile(self), spec, failures - ) - source_task_keys = ( - tuple(getattr(self, "retry_source_task_keys", ())) - if getattr(self, "retry_source_failure_task_key", None) == spec.key - else () - ) - source_key_set = set(source_task_keys) - source_specs = [ - candidate - for candidate in _node_profile(self).sweep_specs - if candidate.key in source_key_set - ] - if source_specs: - self.retry_joint_names = { - joint_name - for candidate in source_specs - for joint_name in candidate.joints - } - localized_cycles = { - int(item["cycle"]) - 1 - for item in failures - if item.get("cycle") is not None - } - cycle_override = set( - getattr(self, "retry_cycle_override", set()) - ) - if source_specs and cycle_override: - # These source tasks jointly define one PnP-relative zero. Mixing - # cycles acquired before and after a tracker/reference reset can - # make a clean branch change look like a physical zero change. - # Reacquire the whole source holdout set as one generation. - self.retry_cycles = cycle_override - elif failures and all( - item.get("cycle") is not None - or str(item.get("metric", "")).startswith("third_cycle_") - for item in failures - ): - if any( - str(item.get("metric", "")).startswith("third_cycle_") - for item in failures - ): - localized_cycles.add(self.repetitions - 1) - self.retry_cycles = localized_cycles - else: - self.retry_cycles = set(range(self.repetitions)) - history_by_task = getattr( - self, "fit_failure_history_by_task", None - ) - if history_by_task is None: - self.fit_failure_history_by_task = {} - history_by_task = self.fit_failure_history_by_task - task_history = history_by_task.setdefault(spec.key, []) - repeated_branch_clusters = bool( - task_history - and _fit_failure_repeats_branch_clusters( - task_history[-1], failures - ) - ) - task_history.append([dict(item) for item in failures]) - systematic = bool( - _fit_failure_is_systematic(failures, self.repetitions) - or repeated_branch_clusters - ) - fit_retry_limit = int( - getattr(self, "automatic_fit_retry_limit", 0) - ) - self.fit_failure = { - "kind": "fit_failure", - "view": spec.view, - "task_name": spec.key, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "joints_to_rescan": sorted(self.retry_joint_names), - **( - {"source_task_names": list(source_task_keys)} - if source_task_keys - else {} - ), - "attempt": attempt, - "fit_attempt": attempt, - "fit_attempt_limit": fit_retry_limit + 1, - "automatic_retry_count": max(0, attempt - 1), - "automatic_retry_limit": fit_retry_limit, - "cycles_to_rescan": [ - cycle + 1 for cycle in sorted(self.retry_cycles) - ], - "directions_to_rescan": ( - 0 - if systematic - else 2 - * len(self.retry_cycles) - * max(1, len(source_specs)) - ), - "recoverable_by_rescan": not systematic, - "repeated_branch_clusters": repeated_branch_clusters, - "failures": failures, - } - append_jsonl( - self.raw_path, - { - "kind": "fit_failure", - **self.fit_failure, - }, - ) - if systematic: - self._pause( - "joint_fit_repeated_branch_failure" - if repeated_branch_clusters - else "joint_fit_systematic_failure" - ) - return True - if attempt <= fit_retry_limit: - self.paused_reason = "joint_fit_check_failed" - self.reason = "automatic_retry_joint_fit_check_failed" - self._prepare_failed_sweep_retry() - self._begin_return_baseline("resume_sweep") - return True - self._pause("joint_fit_check_failed") - return True - - def _pause_for_zero_model_failure( - self, zero_result: ZeroSolveResult - ) -> None: - """Stop once for a model/zero failure; repeated motion cannot fix it.""" - line_errors_m = dict( - getattr(zero_result, "validation_line_error_by_joint_m", {}) - ) - axis_line_rms_m = float(getattr(zero_result, "axis_line_rms_m", 0.0)) - axis_line_limit_m = float( - getattr(self, "axis_maximum_pose_line_rms_m", 0.0) - ) - observability_rank = int(getattr(zero_result, "observability_rank", 0)) - observability_parameter_count = int( - getattr(zero_result, "observability_parameter_count", 0) - ) - observability_condition_number = float( - getattr(zero_result, "observability_condition_number", 0.0) - ) - failures: list[dict[str, Any]] = [] - for name, reason in zero_result.failure_reasons.items(): - failure: dict[str, Any] = { - "joint": name, - "metric": "zero_guard", - "reason": reason, - } - if reason == "zero_offset_exceeds_configured_limit": - configured_limit = self.zero_joint_maximum_offsets_rad.get( - name, - ( - self.zero_finger_maximum_offset_rad - if name.startswith( - ("index_", "middle_", "ring_", "pinky_") - ) - else self.zero_maximum_offset_rad - ), - ) - failure["actual_deg"] = round( - math.degrees(zero_result.direct_offsets_rad[name]), 6 - ) - failure["limit_deg"] = round( - math.degrees(configured_limit), 6 - ) - failures.append(failure) - failures.extend( - { - "joint": observer, - "metric": "third_cycle_axis_holdout_deg", - "actual": round(math.degrees(error), 6), - "limit": math.degrees(self.maximum_validation_p95_rad), - "comparison": "maximum", - } - for observer, error in ( - zero_result.validation_error_by_joint_rad.items() - ) - if error > self.maximum_validation_p95_rad - ) - failures.extend( - { - "joint": observer, - "metric": "validation_axis_line_residual_mm", - "actual": round(1000.0 * float(error), 6), - "limit": round( - 1000.0 * axis_line_limit_m, 6 - ), - "comparison": "maximum", - } - for observer, error in ( - line_errors_m.items() - ) - if error > axis_line_limit_m - ) - direct_joint = next( - iter(zero_result.failure_reasons), - next( - iter(zero_result.validation_error_by_joint_rad), - "thumb_cmc_pitch", - ), - ) - spec = next( - ( - item - for item in self.profile.sweep_specs - if direct_joint in item.joints - ), - self.profile.sweep_specs[0], - ) - self.retry_sweep_spec = None - self.retry_resume_index = None - self.retry_cycles = set() - self.retry_joint_names = set() - self.fit_failure = { - "kind": "zero_model_failure", - "view": spec.view, - "motor_index": spec.motor_index, - "joints": list(spec.joints), - "directions_to_rescan": 0, - "recoverable_by_rescan": False, - "failures": failures, - "zero_geometry": { - "axis_line_rms_mm": round( - 1000.0 * axis_line_rms_m, 6 - ), - "axis_line_rms_limit_mm": round( - 1000.0 * axis_line_limit_m, 6 - ), - "validation_line_error_by_joint_mm": { - name: round(1000.0 * float(value), 6) - for name, value in ( - line_errors_m.items() - ) - }, - "observability_rank": observability_rank, - "observability_parameter_count": observability_parameter_count, - "observability_condition_number": observability_condition_number, - }, - } - append_jsonl( - self.raw_path, - {"kind": "zero_model_failure", **self.fit_failure}, - ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - atomic_write_json( - self.session_dir / "g20_right_19_failed_diagnostics.json", - { - "schema_version": 5, - "layout_id": G20_RIGHT_19_LAYOUT, - "passed": False, - "reason": "zero_model_validation_failed", - "failure_reasons": dict(zero_result.failure_reasons), - "axis_line_rms_m": axis_line_rms_m, - "axis_line_rms_limit_m": axis_line_limit_m, - "validation_line_error_by_joint_m": dict( - line_errors_m - ), - "validation_error_by_joint_rad": dict( - zero_result.validation_error_by_joint_rad - ), - "direct_offsets_rad": dict(zero_result.direct_offsets_rad), - "offset_confidence_95_half_width_rad": dict( - zero_result.offset_confidence_half_width_rad - ), - "observability": { - "rank": observability_rank, - "parameter_count": observability_parameter_count, - "condition_number": observability_condition_number, - }, - "raw_samples": str(self.raw_path), - "source_urdf": str(self.source_urdf_path), - "camera_extrinsics": str( - self.camera_extrinsics_file - ), - "urdf_published": False, - }, - ) - self._pause("zero_model_validation_failed") - - def _retry_active_sweep_or_pause(self, reason: str) -> None: - """Retry a recoverable acquisition failure without weakening gates.""" - if self.active_sweep is None: - self._pause(reason) - return - item = self.active_sweep - key = ( - _sweep_storage_key(item.spec), item.cycle, item.direction - ) - retries = self.sweep_retry_counts.get(key, 0) - retry_limit = getattr(self, "automatic_sweep_retry_limit", 0) - if retries >= retry_limit: - self._pause(reason) - return - retries += 1 - self.sweep_retry_counts[key] = retries - observer = next( - ( - candidate - for candidate in _node_profile(self).palm_axis_observers - if candidate.task_name == item.spec.key - ), - None, - ) - retry_views = set(_sweep_views(_node_profile(self), item.spec)) - if observer is not None: - retry_views.add(observer.view) - for view in retry_views: - runtime = getattr(self, "views", {}).get(view) - if runtime is None: - continue - self._reset_view_trackers(runtime) - runtime.pnp_reset_count += 1 - speed_scales = getattr( - self, "retry_speed_scales", (0.8, 0.6, 0.5) - ) - endpoint_holds = getattr( - self, "retry_endpoint_hold_seconds", (0.75, 1.0, 1.25) - ) - # Write the invalidation before mutating the authoritative in-memory - # stores. Restart replays this event as a tombstone, so a crash can - # neither resurrect the rejected sweep nor merge it with its - # replacement acquisition. - append_jsonl( - self.raw_path, - { - "kind": "automatic_sweep_retry", - "task_name": item.spec.key, - "attempt": int( - getattr(self, "sweep_attempts", {}).get( - _sweep_storage_key(item.spec), 1 - ) - ), - "view": item.spec.view, - "motor_index": item.spec.motor_index, - "joints": list(item.spec.joints), - "cycle": item.cycle, - "direction": item.direction, - "retry": retries, - "retry_limit": retry_limit, - "reason": str(reason), - "speed_scale": float( - speed_scales[ - min(retries, len(speed_scales)) - 1 - ] - ), - "endpoint_hold_seconds": float( - endpoint_holds[ - min(retries, len(endpoint_holds)) - 1 - ] - ), - }, - ) - command_records = getattr(self, "command_records_by_joint", {}) - for joint_name in item.spec.joints: - if joint_name in command_records: - command_records[joint_name] = [ - record - for record in command_records[joint_name] - if not ( - str(record.get("task_name")) == item.spec.key - and int(record.get("cycle", -999)) == item.cycle - and str(record.get("direction")) == item.direction - ) - ] - if observer is not None: - records = self.palm_axis_records_by_source.get( - observer.source_name, [] - ) - records[:] = [ - record - for record in records - if not ( - int(record.get("cycle", -999)) == item.cycle - and str(record.get("direction", "")) == item.direction - ) - ] - getattr(self, "sweep_frames", []).clear() - getattr(self, "sweep_start_frames", []).clear() - self.reason = f"automatic_retry_{reason}" - self._begin_return_baseline("retry_sweep") - - def _record_dedicated_baseline_hold(self, item: SweepItem) -> None: - """Persist one settled baseline observation for one approach path.""" - if not G20ThreeCameraCalibrationNode._requires_mid_sweep_baseline_hold( - self, item - ): - return - frames = list(getattr(self, "sweep_baseline_frames", [])) - minimum_frames = int(getattr(self, "minimum_baseline_hold_frames", 10)) - motor = item.spec.motor_index - command = int(self.baseline_command[motor]) - attempt = self.sweep_attempts.get(_sweep_storage_key(item.spec), 1) - record_joint_names = tuple(item.spec.joints) - if getattr(self, "active_sweep_is_fit_retry", False): - scoped = set(getattr(self, "retry_joint_names", set())) - if scoped: - record_joint_names = tuple( - name for name in item.spec.joints if name in scoped - ) - for joint_name in record_joint_names: - selected = _frames_for_joint(frames, joint_name) - if len(selected) < minimum_frames: - raise RuntimeError( - "dedicated baseline hold for " - f"{joint_name} has {len(selected)} valid frames; " - f"at least {minimum_frames} required" - ) - state = np.median( - np.asarray( - [frame.state_u8 for frame in selected], dtype=float - ), - axis=0, - ) - quaternion = robust_rotation_summary( - [ - frame.joint_quaternions_xyzw[joint_name] - for frame in selected - ] - )[0] - relative_translation = np.median( - np.asarray( - [ - frame.joint_vectors_xyz_m[joint_name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - image_relative = np.median( - np.asarray( - [ - frame.image_vectors_xy_px[joint_name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - durable = canonical_sample_record({ - "kind": "baseline_hold_sample", - "attempt": attempt, - "task_name": item.spec.key, - "view": self.profile.record_specs[joint_name].view, - "joint": joint_name, - "motor_index": motor, - "cycle": item.cycle, - "direction": item.direction, - "requested_command_u8": command, - "feedback_u8": round(float(state[motor]), 6), - "image_stamp_ns": int( - np.median([frame.stamp_ns for frame in selected]) - ), - "relative_quaternion_xyzw": [ - float(value) for value in quaternion - ], - "relative_translation_xyz_m": [ - float(value) for value in relative_translation - ], - "image_relative_xy_px": [ - float(value) for value in image_relative - ], - "parent_pose_common": _robust_pose_payload( - [ - frame.parent_poses_common[joint_name] - for frame in selected - ] - ), - "child_pose_common": _robust_pose_payload( - [ - frame.child_poses_common[joint_name] - for frame in selected - ] - ), - "state_u8": [float(value) for value in state], - "state_image_sync_error_ms": round( - float( - np.percentile( - [ - abs(frame.state_sync_error_ns) - for frame in selected - ], - 95.0, - ) - / 1_000_000.0 - ), - 6, - ), - "pnp_reprojection_error_px": round( - float( - np.percentile( - [ - frame.joint_reprojection_error_px[joint_name] - for frame in selected - ], - 95.0, - ) - ), - 6, - ), - "valid_frames": len(selected), - "hold_seconds": float(self.baseline_hold_seconds), - }) - record = fitting_sample_record(durable) - self.baseline_records_by_joint[joint_name].append(record) - if item.cycle == 0: - self.command_records_by_joint[joint_name].append(record) - append_jsonl(self.raw_path, durable) - - def _baseline_hysteresis_records( - self, - joint_name: str, - trajectory_records: Sequence[Mapping[str, Any]], - ) -> Sequence[Mapping[str, Any]]: - """Use dedicated settled records for non-endpoint baseline joints.""" - profile = getattr(self, "profile", LEFT_HAND_PROFILE) - spec = profile.record_specs[joint_name] - baseline = int(self.baseline_command[spec.motor_index]) - if ( - profile.layout_id == G20_RIGHT_19_LAYOUT - and baseline not in {0, 255} - ): - return self.baseline_records_by_joint.get(joint_name, []) - return trajectory_records - - def _finish_active_sweep(self) -> None: - assert self.active_sweep is not None - item = self.active_sweep - motor = item.spec.motor_index - start_tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, item.spec, item.start_u8 - ) - ) - target_tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, item.spec, item.target_u8 - ) - ) - record_joint_names = tuple(item.spec.joints) - if getattr(self, "active_sweep_is_fit_retry", False): - scoped = set(getattr(self, "retry_joint_names", set())) - if scoped: - record_joint_names = tuple( - name for name in item.spec.joints if name in scoped - ) - - def observation_bins( - joint_name: str, - ) -> dict[int, list[FrameObservation]]: - bins: dict[int, list[FrameObservation]] = {} - frames = _frames_for_joint(self.sweep_frames, joint_name) - for frame in frames: - state = float(frame.state_u8[motor]) - if abs(state - item.start_u8) <= start_tolerance: - command = item.start_u8 - elif abs(state - item.target_u8) <= target_tolerance: - command = item.target_u8 - else: - command = int(np.clip(np.rint(state), 0, 255)) - bins.setdefault(command, []).append(frame) - return bins - - joint_bins: dict[str, dict[int, list[FrameObservation]]] = {} - for joint_name in record_joint_names: - bins = observation_bins(joint_name) - commands = sorted(bins) - if not commands or commands[0] != 0 or commands[-1] != 255: - self._retry_active_sweep_or_pause( - _joint_failure_reason( - "sweep_missing_endpoint_bin", item.spec, joint_name - ) - ) - return - joint_bins[joint_name] = bins - if item.precheck: - # This low-speed pass is a visibility/safety check at 0, 127 and - # 255, not a source for the command-angle curve. Its full-sweep - # detection rate is counted directly in the camera callback. A - # raw-frame rate below the nominal threshold is not itself a data - # defect when synchronised valid poses still cover the complete - # trajectory densely: detector flicker then changes only how many - # duplicate observations exist, not what motion is observable. - for joint_name, bins in joint_bins.items(): - commands = sorted(bins) - if min(abs(command - 127) for command in commands) > 2: - self._retry_active_sweep_or_pause( - _joint_failure_reason( - "task_precheck_missing_command_127", - item.spec, - joint_name, - ) - ) - return - detection_rates: dict[str, float] = {} - total_by_view = getattr( - self, "sweep_detection_total_by_view", {} - ) - valid_by_view = getattr( - self, "sweep_detection_valid_by_view", {} - ) - capture_views = _sweep_views(_node_profile(self), item.spec) - profile = _node_profile(self) - trajectory_coverage_by_view: dict[str, bool] = {} - for view in capture_views: - view_joint_bins = [ - joint_bins[name] - for name in _sweep_joints_for_view( - profile, item.spec, view - ) - if name in joint_bins - ] - trajectory_coverage_by_view[view] = bool(view_joint_bins) and all( - len(bins) >= self.minimum_sweep_bins - and max(np.diff(sorted(bins)), default=0) - <= self.maximum_bin_gap - for bins in view_joint_bins - ) - detection_rate_below_threshold_views: list[str] = [] - trajectory_coverage_override_views: list[str] = [] - for view in capture_views: - if view in total_by_view: - total = int(total_by_view.get(view, 0)) - valid = int(valid_by_view.get(view, 0)) - elif len(capture_views) == 1: - # Compatibility for legacy sessions and focused unit - # harnesses created before per-camera counters existed. - total = int( - getattr(self, "sweep_detection_total_frames", 0) - ) - valid = int( - getattr(self, "sweep_detection_valid_frames", 0) - ) - else: - total = 0 - valid = 0 - rate = 0.0 if total <= 0 else valid / total - detection_rates[view] = rate - if rate < self.minimum_detection_rate: - detection_rate_below_threshold_views.append(view) - if trajectory_coverage_by_view.get(view, False): - trajectory_coverage_override_views.append(view) - else: - self._retry_active_sweep_or_pause( - f"task_precheck_detection_rate_too_low:{view}" - ) - return - minimum_bin_count = min( - len(bins) for bins in joint_bins.values() - ) - precheck_maximum_bin_gap = max( - int(max(np.diff(sorted(bins)), default=0)) - for bins in joint_bins.values() - ) - minimum_valid_frames = min( - sum(len(frames) for frames in bins.values()) - for bins in joint_bins.values() - ) - G20ThreeCameraCalibrationNode._record_precheck_speed_metric( - self, - item, - bin_count=minimum_bin_count, - maximum_bin_gap=precheck_maximum_bin_gap, - valid_frames=minimum_valid_frames, - ) - append_jsonl( - self.raw_path, - { - "kind": "task_visibility_precheck", - "task_name": item.spec.key, - "view": item.spec.view, - "capture_views": list( - _sweep_views(_node_profile(self), item.spec) - ), - "motor_index": motor, - "direction": item.direction, - "checked_commands_u8": [0, 127, 255], - "bin_count": minimum_bin_count, - "maximum_bin_gap": precheck_maximum_bin_gap, - "detection_rate": round(min(detection_rates.values()), 6), - "detection_rate_by_view": { - view: round(rate, 6) - for view, rate in detection_rates.items() - }, - "minimum_detection_rate": round( - float(self.minimum_detection_rate), 6 - ), - "detection_rate_below_threshold_views": ( - detection_rate_below_threshold_views - ), - "trajectory_coverage_by_view": ( - trajectory_coverage_by_view - ), - "trajectory_coverage_override_views": ( - trajectory_coverage_override_views - ), - "passed": True, - }, - ) - if not getattr(self, "active_sweep_is_fit_retry", False): - self.sweep_index += 1 - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps( - self - ) - self.active_sweep = None - self.active_sweep_is_fit_retry = False - if self.sweep_index >= len(self.sweep_items): - self._begin_return_baseline("fit") - elif self.sweep_items[self.sweep_index].spec == item.spec: - following = self.sweep_items[self.sweep_index] - transition = _sweep_plan_transition( - self.profile, item, following - ) - if transition == "cycle_reset": - self._begin_return_baseline("next_cycle") - else: - G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( - self, - item, - following, - ) - self._start_next_sweep() - else: - self._begin_return_baseline( - G20ThreeCameraCalibrationNode._transition_after_completed_spec( - self, item.spec - ) - ) - return - - for joint_name, bins in joint_bins.items(): - commands = sorted(bins) - if len(commands) < self.minimum_sweep_bins: - self._retry_active_sweep_or_pause( - _joint_failure_reason( - "sweep_bins_too_few", item.spec, joint_name - ) - ) - return - if max(np.diff(commands), default=0) > self.maximum_bin_gap: - self._retry_active_sweep_or_pause( - _joint_failure_reason( - "sweep_bin_gap_too_large", item.spec, joint_name - ) - ) - return - - stored_joint_names = tuple(joint_bins) - - def synchronised_frame_payload( - frame: FrameObservation, - ) -> dict[str, Any]: - names = [ - name - for name in stored_joint_names - if name in frame.joint_quaternions_xyzw - ] - return { - "kind": "synchronised_frame", - "attempt": self.sweep_attempts.get( - _sweep_storage_key(item.spec), 1 - ), - "task_name": item.spec.key, - "view": frame.view, - "motor_index": motor, - "joints": names, - "cycle": item.cycle, - "direction": item.direction, - "requested_command_u8": int( - self.sweep_checkpoint_target_u8 - if self.sweep_checkpoint_target_u8 is not None - else item.target_u8 - ), - "feedback_u8": round(float(frame.state_u8[motor]), 6), - "image_stamp_ns": int(frame.stamp_ns), - "actual_state_u8": [ - float(value) for value in frame.state_u8 - ], - "state_image_sync_error_ms": round( - abs(frame.state_sync_error_ns) / 1_000_000.0, 6 - ), - "relative_quaternion_xyzw": { - name: list(frame.joint_quaternions_xyzw[name]) - for name in names - }, - "parent_pose_common": { - name: frame.parent_poses_common[name] - for name in names - }, - "child_pose_common": { - name: frame.child_poses_common[name] - for name in names - }, - "pnp_reprojection_error_px": { - name: frame.joint_reprojection_error_px[name] - for name in names - }, - } - append_jsonl_many( - self.raw_path, - ( - synchronised_frame_payload(frame) - for frame in self.sweep_frames - if any( - name in frame.joint_quaternions_xyzw - for name in stored_joint_names - ) - ), - ) - - for joint_name in stored_joint_names: - bins = joint_bins[joint_name] - commands = sorted(bins) - for command in commands: - frames = bins[command] - vector = np.median( - np.asarray( - [frame.joint_vectors_xyz_m[joint_name] for frame in frames] - ), - axis=0, - ) - image_vector = np.median( - np.asarray( - [frame.image_vectors_xy_px[joint_name] for frame in frames] - ), - axis=0, - ) - relative_quaternion = robust_rotation_summary( - [ - frame.joint_quaternions_xyzw[joint_name] - for frame in frames - ] - )[0] - parent_pose = _robust_pose_payload( - [ - frame.parent_poses_common[joint_name] - for frame in frames - ] - ) - child_pose = _robust_pose_payload( - [ - frame.child_poses_common[joint_name] - for frame in frames - ] - ) - state = np.median( - np.asarray([frame.state_u8 for frame in frames], dtype=float), - axis=0, - ) - durable = canonical_sample_record({ - "kind": "sample", - "attempt": self.sweep_attempts.get( - _sweep_storage_key(item.spec), 1 - ), - "task_name": item.spec.key, - "view": self.profile.record_specs[joint_name].view, - "joint": joint_name, - "motor_index": motor, - "cycle": item.cycle, - "direction": item.direction, - "requested_command_u8": int( - self.sweep_checkpoint_target_u8 - if self.sweep_checkpoint_target_u8 is not None - else item.target_u8 - ), - "feedback_u8": int(command), - "image_stamp_ns": int( - np.median([frame.stamp_ns for frame in frames]) - ), - "relative_translation_xyz_m": [ - float(value) for value in vector - ], - "image_relative_xy_px": [ - float(value) for value in image_vector - ], - "relative_quaternion_xyzw": [ - float(value) for value in relative_quaternion - ], - "parent_pose_common": parent_pose, - "child_pose_common": child_pose, - "state_u8": [float(value) for value in state], - "state_image_sync_error_ms": round( - float( - np.percentile( - [ - abs(frame.state_sync_error_ns) - for frame in frames - ], - 95.0, - ) - / 1_000_000.0 - ), - 6, - ), - "pnp_reprojection_error_px": round( - float( - np.percentile( - [ - frame.joint_reprojection_error_px[joint_name] - for frame in frames - ], - 95.0, - ) - ), - 6, - ), - "valid_frames": len(frames), - }) - record = fitting_sample_record(durable) - self.records_by_joint[joint_name].append(record) - append_jsonl(self.raw_path, durable) - - G20ThreeCameraCalibrationNode._persist_palm_axis_samples(self, item) - G20ThreeCameraCalibrationNode._record_dedicated_baseline_hold( - self, item - ) - previous_spec = item.spec - was_fit_retry = bool( - getattr(self, "active_sweep_is_fit_retry", False) - ) - if not was_fit_retry: - self.sweep_index += 1 - G20ThreeCameraCalibrationNode._advance_past_resumed_sweeps(self) - self.active_sweep = None - self.active_sweep_is_fit_retry = False - if was_fit_retry: - if self.retry_sweep_items: - following = self.retry_sweep_items[0] - transition = _sweep_plan_transition( - self.profile, item, following - ) - if transition == "cycle_reset": - self._begin_return_baseline("next_cycle") - else: - G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( - self, - item, - following, - ) - self._start_next_sweep() - return - # All physical source items for a dependent retry are complete. - # Clear the routing before checking the final source task itself, - # so an ordinary source-task failure retries only that task. - self.retry_source_failure_task_key = None - self.retry_source_task_keys = () - self.retry_cycle_override.clear() - self.retry_preserve_pnp_continuity = False - failures = self._provisional_fit_failures(previous_spec) - diagnostic_role = ( - G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( - self, previous_spec - ) - ) - if diagnostic_role in {"side", "multiview"}: - G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( - self, previous_spec, failures - ) - return - if failures and self._pause_for_provisional_fit_failure( - previous_spec, failures, allow_warning=True - ): - return - self.retry_sweep_spec = None - self.retry_resume_index = None - self.retry_cycles = set() - self.retry_joint_names = set() - self.fit_failure = {} - if self.sweep_index >= len(self.sweep_items): - self._begin_return_baseline("fit") - else: - self._begin_return_baseline( - G20ThreeCameraCalibrationNode._transition_after_completed_spec( - self, previous_spec - ) - ) - return - spec_complete = bool( - self.sweep_index >= len(self.sweep_items) - or self.sweep_items[self.sweep_index].spec != previous_spec - ) - if spec_complete: - failures = self._provisional_fit_failures(previous_spec) - diagnostic_role = ( - G20ThreeCameraCalibrationNode._cross_view_roll_diagnostic_role( - self, previous_spec - ) - ) - if diagnostic_role in {"side", "multiview"}: - G20ThreeCameraCalibrationNode._complete_cross_view_roll_diagnostic( - self, previous_spec, failures - ) - return - if failures and self._pause_for_provisional_fit_failure( - previous_spec, failures, allow_warning=True - ): - return - if self.retry_sweep_spec == previous_spec: - resume_index = self.retry_resume_index - self.retry_sweep_spec = None - self.retry_resume_index = None - self.retry_cycles = set() - self.retry_joint_names = set() - if resume_index is not None: - self.sweep_index = resume_index - self.fit_failure = {} - if self.sweep_index >= len(self.sweep_items): - self._begin_return_baseline("fit") - elif self.sweep_items[self.sweep_index].spec != previous_spec: - self._begin_return_baseline( - G20ThreeCameraCalibrationNode._transition_after_completed_spec( - self, previous_spec - ) - ) - elif _sweep_plan_transition( - self.profile, - item, - self.sweep_items[self.sweep_index], - ) == "cycle_reset": - # Keep the task's complete avoidance pose between its three - # repetitions. Only the active joint returns to its standard-side - # baseline (roll: 255->127); global unfolding is deferred until - # the task really changes. - self._begin_return_baseline("next_cycle") - else: - G20ThreeCameraCalibrationNode._stage_immediate_reverse_start_frames( - self, - item, - self.sweep_items[self.sweep_index], - ) - self._start_next_sweep() - - def _fit_all_curves(self) -> None: - G20ThreeCameraCalibrationNode._invalidate_fitted_calibration_state(self) - self.state = STATE_FITTING - self.reason = "fitting_3d_axes_and_urdf_zero_offsets" - # Fitting is deliberately synchronous so no capture callback can - # mutate the observation set midway through a model fit. Announce - # the state before entering the CPU-bound section: the outer product - # runner then applies its fitting-specific watchdog instead of - # mistaking a blocked ROS timer for a motion communication failure. - fitting_started_at = time.monotonic() - self.last_status_publish = fitting_started_at - self._publish_status(fitting_started_at) - if int(getattr(self, "sample_schema_version", 0)) >= 1: - for store_name, store in ( - ("trajectory", self.records_by_joint), - ("baseline", self.baseline_records_by_joint), - ("steady", self.command_records_by_joint), - ("palm_axis", self.palm_axis_records_by_source), - ): - try: - validate_sample_records( - record - for records in store.values() - for record in records - ) - except SampleDataContractError as error: - raise SampleDataContractError( - f"{error};store={store_name}" - ) from error - # Provisional checks have a warning band so collection can continue, - # but the final fit always re-applies the unmodified hard thresholds. - fresh_task_keys = set( - getattr(self, "recalibration_task_keys", ()) - ) - for spec in self.profile.sweep_specs: - if fresh_task_keys and spec.key not in fresh_task_keys: - continue - failures = self._provisional_fit_failures(spec) - if failures: - self._pause_for_provisional_fit_failure( - spec, failures, allow_warning=False - ) - return - training_fits: dict[str, JointCurveFit] = {} - holdout_by_joint: dict[str, tuple[float, ...]] = {} - measured: dict[str, JointCurveFit] = {} - validation_cycle = self.repetitions - 1 - training_cycles = tuple(range(validation_cycle)) - training_cycle_set = set(training_cycles) - quality_joint_names = set( - recalibration_quality_joints( - self.profile, - getattr(self, "recalibration_scope", "full"), - ) - ) - standalone_thumb_calibration = bool( - getattr(self, "standalone_thumb_calibration", False) - ) - fit_joint_names = tuple( - name - for name in self.profile.measured_joints - if ( - not standalone_thumb_calibration - or name in quality_joint_names - ) - ) - for name in fit_joint_names: - training_records = [ - record - for record in self.records_by_joint[name] - if int(record["cycle"]) in training_cycle_set - ] - holdout_records = [ - record - for record in self.records_by_joint[name] - if int(record["cycle"]) == validation_cycle - ] - holdout_model_records = ( - G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, name, holdout_records - ) - ) - zero_command = int( - self.baseline_command[ - self.profile.joint_specs[name].motor_index - ] - ) - training_fits[name] = self._fit_joint_records( - name, training_records - ) - holdout_by_joint[name] = joint_curve_holdout_errors( - training_fits[name], - holdout_model_records, - zero_command_u8=zero_command, - ) - measured[name] = self._fit_joint_records( - name, self.records_by_joint[name] - ) - command_fits: dict[str, JointCurveFit] = dict(training_fits) - command_feedback_fits: dict[str, JointCurveFit] = dict( - training_fits - ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - command_fits = {} - command_feedback_fits = {} - for name in fit_joint_names: - if name in G20_REFERENCE_THUMB_CMC_JOINTS: - # a609d521's dense feedback-indexed relative-rotation - # curve is the validated transfer function for the three - # CMC axes. Do not replace it with nine planar PnP - # checkpoints; those points were the source of the later - # thumb regression and combination-pose false failures. - command_fits[name] = measured[name] - continue - command_records = [ - dict(record) - for record in self.command_records_by_joint[name] - if int(record["cycle"]) == 0 - ] - if len(command_records) < 18: - failed_spec = next( - spec - for spec in self.profile.sweep_specs - if name in spec.joints - ) - self._pause_for_provisional_fit_failure( - failed_spec, - [ - { - "joint": name, - "metric": "steady_command_checkpoints", - "actual": len(command_records), - "limit": 18, - "comparison": "minimum", - } - ], - ) - return - zero_command = int( - self.baseline_command[ - self.profile.joint_specs[name].motor_index - ] - ) - if name in RIGHT_19_END_ON_IMAGE_CURVE_JOINTS: - command_fits[name] = fit_joint_image_curve( - command_records, - maximum_radial_rms_px=( - self.image_trajectory_maximum_radial_rms_px - ), - maximum_radial_p95_px=( - self.image_trajectory_maximum_radial_p95_px - ), - minimum_radius_px=( - self.image_trajectory_minimum_radius_px - ), - minimum_arc_rad=self.trajectory_minimum_arc_rad, - ) - else: - command_fits[name] = fit_rotation_joint_curve( - command_records, - zero_command_u8=zero_command, - canonical_zero_direction=canonical_zero_direction( - self.profile, name - ), - ) - command_feedback_fits[name] = self._fit_joint_records( - name, - _steady_records_in_feedback_domain(command_records), - ) - self.cross_view_roll_metrics: dict[str, dict[str, float]] = {} - self.validation_only_fits: dict[str, JointCurveFit] = {} - for name, validation_name in ( - self.profile.axis_validation_sources or {} - ).items(): - if ( - name not in training_fits - or validation_name not in quality_joint_names - ): - continue - validation_training_records = [ - record - for record in self.records_by_joint[validation_name] - if int(record["cycle"]) in training_cycle_set - ] - validation_holdout_records = [ - record - for record in self.records_by_joint[validation_name] - if int(record["cycle"]) == validation_cycle - ] - validation_holdout_model_records = ( - G20ThreeCameraCalibrationNode._records_with_baseline_holds( - self, - validation_name, - validation_holdout_records, - ) - ) - validation_training_fit = self._fit_joint_records( - validation_name, validation_training_records - ) - validation_fit = self._fit_joint_records( - validation_name, self.records_by_joint[validation_name] - ) - # Any optional post-fit random validation must query the same - # frozen training model that passed the isolated holdout. - self.validation_only_fits[validation_name] = ( - validation_training_fit - ) - # The structured task-level check above records threshold - # overruns with per-cycle evidence. At publication time this - # secondary view remains diagnostic: the primary front curve and - # the side physical axis line retain their independent hard - # quality gates. Reuse the same policy as offline replay instead - # of reintroducing an unlocalized retry from a second code path. - training_metrics = cross_view_roll_diagnostic_metrics( - training_fits[name], validation_training_fit - ) - final_metrics = cross_view_roll_diagnostic_metrics( - measured[name], validation_fit - ) - validation_errors = joint_curve_holdout_errors( - validation_training_fit, - validation_holdout_model_records, - zero_command_u8=int( - self.baseline_command[ - self.profile.record_specs[validation_name].motor_index - ] - ), - ) - holdout_by_joint[validation_name] = validation_errors - self.cross_view_roll_metrics[name] = { - **{ - f"training_{key}": float(value) - for key, value in training_metrics.items() - }, - **{ - f"final_{key}": float(value) - for key, value in final_metrics.items() - }, - } - self.joint_dynamic_diagnostics: dict[str, dict[str, Any]] = {} - for name in fit_joint_names: - cycle_travel: list[float] = [] - for cycle in range(self.repetitions): - cycle_fit = self._fit_joint_records( - name, - [ - record - for record in self.records_by_joint[name] - if int(record["cycle"]) == cycle - ], - ) - cycle_travel.append( - abs( - float(cycle_fit.angle_rad[0]) - - float(cycle_fit.angle_rad[255]) - ) - ) - holdout = np.abs( - np.asarray(holdout_by_joint[name], dtype=float) - ) - baseline_hysteresis = baseline_hysteresis_by_cycle_rad( - G20ThreeCameraCalibrationNode._baseline_hysteresis_records( - self, name, self.records_by_joint[name] - ), - zero_command_u8=int( - self.baseline_command[ - self.profile.joint_specs[name].motor_index - ] - ), - axis_xyz=( - G20ThreeCameraCalibrationNode._hysteresis_axis_for_fit( - self, - name, - self.records_by_joint[name], - measured[name], - ) - ), - ) - self.joint_dynamic_diagnostics[name] = { - "cycle_travel_rad": [ - round(float(value), 8) for value in cycle_travel - ], - "cycle_travel_range_rad": round( - max(cycle_travel) - min(cycle_travel), 8 - ), - "baseline_hysteresis_by_cycle_rad": [ - round(float(value), 8) - for value in baseline_hysteresis - ], - "holdout_cycle": validation_cycle, - "holdout_cycle_mae_rad": round(float(np.mean(holdout)), 8), - "holdout_cycle_max_rad": round(float(np.max(holdout)), 8), - } - axes: list[JointAxisMeasurement] = [] - axis_profile = ( - get_right_19_thumb_zero_profile() - if ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and self.recalibration_scope == "thumb" - ) - else self.zero_profile - ) - for name in axis_profile.axis_joints: - group = [ - self._fit_axis_measurement(name, cycle) - for cycle in range(self.repetitions) - ] - axes.extend(self._refit_cross_view_axis_line_group(name, group)) - - try: - palm_orientation_measurements, palm_orientation_rejections = ( - fit_partial_palm_orientation_measurements( - sources=self.profile.palm_orientation_sources, - records_by_joint=self.palm_axis_records_by_source, - motor_by_source=self.profile.palm_axis_motor_by_source, - baseline_command_u8=self.baseline_command, - cycles=range(self.repetitions), - minimum_sources=int( - self.profile.minimum_palm_orientation_sources - ), - minimum_arc_rad=( - self.profile.palm_orientation_minimum_arc_rad - ), - maximum_rotation_orthogonal_rms_rad=( - self.active_maximum_rotation_orthogonal_rms_rad - ), - maximum_command_distance_u8=( - self.profile - .palm_orientation_maximum_command_distance_u8 - ), - ) - ) - except ValueError as error: - failure = str(error) - self.palm_orientation_rejections = {"coverage": failure} - self.fit_failure = { - "task": "palm_orientation_side_channel", - "failures": [ - { - "joint": "palm_orientation", - "metric": "palm_orientation_coverage", - "reason": failure, - } - ], - "directions_to_rescan": 0, - } - append_jsonl( - self.raw_path, - { - "kind": "palm_orientation_optional_source_diagnostic", - "rejections": {"coverage": failure}, - "decision": "reject_unobservable_palm_phase", - }, - ) - self._pause("palm_orientation_quality_failed") - return - if palm_orientation_rejections: - append_jsonl( - self.raw_path, - { - "kind": "palm_orientation_optional_source_diagnostic", - "rejections": dict(palm_orientation_rejections), - "decision": "use_qualified_redundant_sources", - }, - ) - - motor_by_joint = { - name: int(spec.motor_index) - for name, spec in self.profile.joint_specs.items() - } - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - if self.recalibration_scope == "full": - scoped_endpoint_joints = frozenset( - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - ) - else: - want_thumb_endpoints = self.recalibration_scope == "thumb" - scoped_endpoint_joints = frozenset( - name - for name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - if name.startswith("thumb_") == want_thumb_endpoints - ) - command_fits = anchor_right_19_mechanical_endpoint_curves( - command_fits, - { - name: ( - [ - record - for record in self.records_by_joint[name] - if int(record["cycle"]) in training_cycle_set - ] - if name == "thumb_cmc_roll" - else self.command_records_by_joint[name] - ) - for name in scoped_endpoint_joints - }, - maximum_direction_difference_rad=( - self.command_maximum_direction_gap_rad - ), - feedback_endpoint_joints=( - frozenset({"thumb_cmc_roll"}) - & scoped_endpoint_joints - ), - endpoint_joints=scoped_endpoint_joints, - ) - measured_endpoint_zero_offsets = ( - derive_right_19_mechanical_endpoint_offsets( - self.source_urdf_path, - command_fits, - maximum_offset_rad=( - self.mechanical_endpoint_maximum_offset_rad - ), - endpoint_joints=scoped_endpoint_joints, - ) - ) - frozen_offsets = dict( - getattr( - self, - "partial_scope_fixed_zero_offsets_rad", - {}, - ) - ) - # Endpoint-derived zeros also move URDF limits or mimic offsets. - # A partial run must preserve those coupled fields together with - # the certified non-target origin, not merely freeze the solver's - # scalar offset after endpoint derivation. - measured_endpoint_zero_offsets.update( - { - name: value - for name, value in frozen_offsets.items() - if name in RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - } - ) - endpoint_zero_offsets = { - name: value - for name, value in measured_endpoint_zero_offsets.items() - if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - } - post_solve_endpoint_offsets = { - name: value - for name, value in measured_endpoint_zero_offsets.items() - if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS - } - else: - endpoint_zero_offsets = {} - post_solve_endpoint_offsets = {} - zero_solve_arguments = dict( - source_urdf=self.source_urdf_path, - palm_orientation_measurements=palm_orientation_measurements, - curves=training_fits, - motor_by_joint=motor_by_joint, - maximum_offset_rad=self.zero_maximum_offset_rad, - joint_maximum_offset_rad=self.zero_joint_maximum_offsets_rad, - maximum_validation_mae_rad=self.maximum_validation_mae_rad, - maximum_validation_p95_rad=self.maximum_validation_p95_rad, - maximum_validation_error_rad=( - self.maximum_validation_error_rad - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else None - ), - maximum_confidence_half_width_rad=( - self.zero_maximum_confidence_half_width_rad - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else None - ), - maximum_axis_cone_mismatch_rad=( - self.zero_maximum_axis_cone_mismatch_rad - ), - maximum_systematic_axis_cone_bias_rad=( - self.cross_view_roll_maximum_axis_difference_rad - ), - maximum_observability_condition_number=( - self.zero_maximum_observability_condition_number - ), - maximum_pose_axis_line_rms_m=( - self.axis_maximum_pose_line_rms_m - ), - finger_maximum_offset_rad=self.zero_finger_maximum_offset_rad, - maximum_cycle_difference_rad=( - self.zero_maximum_axis_cycle_difference_rad - ), - hand_type=self.hand_type, - tag_layout=self.profile.layout_id, - training_cycles=training_cycles, - validation_cycle=validation_cycle, - ) - frozen_offsets = dict( - getattr(self, "partial_scope_fixed_zero_offsets_rad", {}) - ) - independent_thumb_scope = bool( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - and self.recalibration_scope in {"full", "thumb"} - ) - if independent_thumb_scope: - thumb_profile = get_right_19_thumb_zero_profile() - thumb_names = set(thumb_profile.direct_zero_joints) - thumb_result = solve_urdf_zero_offsets( - **zero_solve_arguments, - measurements=[ - item for item in axes if item.joint in thumb_profile.axis_joints - ], - fixed_direct_zero_offsets_rad={ - name: value - for name, value in endpoint_zero_offsets.items() - if name in thumb_names - }, - static_output_zero_offsets_rad={ - name: value - for name, value in post_solve_endpoint_offsets.items() - if name in thumb_names - }, - zero_profile=thumb_profile, - ) - if self.recalibration_scope == "thumb": - if self.standalone_thumb_calibration: - holdout_zero_result = ( - expand_right_19_thumb_zero_result_with_cad_fingers( - thumb_result - ) - ) - else: - holdout_zero_result = merge_right_19_thumb_zero_result( - thumb_result=thumb_result, - preserved_offsets_rad=frozen_offsets, - ) - else: - # The full-hand solve still determines the 12 finger zeros and - # palm nuisance pose, but all four thumb values are immutable. - # Consequently no finger observation can pull a thumb zero. - companion_result = solve_urdf_zero_offsets( - **zero_solve_arguments, - measurements=axes, - fixed_direct_zero_offsets_rad={ - **self.zero_profile.fixed_direct_zero_offsets_rad, - **endpoint_zero_offsets, - **thumb_result.direct_offsets_rad, - }, - static_output_zero_offsets_rad={ - **self.zero_profile.static_output_zero_offsets_rad, - **post_solve_endpoint_offsets, - }, - ) - holdout_zero_result = merge_right_19_thumb_zero_result( - thumb_result=thumb_result, - companion_result=companion_result, - ) - else: - holdout_zero_result = solve_urdf_zero_offsets( - **zero_solve_arguments, - measurements=axes, - fixed_direct_zero_offsets_rad={ - **self.zero_profile.fixed_direct_zero_offsets_rad, - **endpoint_zero_offsets, - **frozen_offsets, - }, - static_output_zero_offsets_rad={ - **self.zero_profile.static_output_zero_offsets_rad, - **post_solve_endpoint_offsets, - **{ - name: value - for name, value in frozen_offsets.items() - if name in RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS - }, - }, - ) - thumb_yaw_zero_failures = ( - _thumb_yaw_zero_repeatability_failures( - holdout_zero_result, - maximum_cycle_range_rad=( - self.thumb_yaw_maximum_zero_cycle_difference_rad - ), - maximum_confidence_half_width_rad=( - self.thumb_yaw_maximum_confidence_half_width_rad - ), - ) - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else [] - ) - append_jsonl( - self.raw_path, - { - "kind": "zero_holdout_diagnostics", - "hand_type": self.hand_type, - "passed": holdout_zero_result.passed, - "direct_offsets_rad": dict( - holdout_zero_result.direct_offsets_rad - ), - "partial_scope_fixed_zero_offsets_rad": dict( - getattr( - self, - "partial_scope_fixed_zero_offsets_rad", - {}, - ) - ), - "model_base_translation_xyz_m": list( - holdout_zero_result.base_translation_xyz_m - ), - "model_base_quaternion_xyzw": list( - holdout_zero_result.base_quaternion_xyzw - ), - "cycle_offsets_rad": { - name: list(values) - for name, values in ( - holdout_zero_result.cycle_offsets_rad.items() - ) - }, - "offset_uncertainty_rad": dict( - holdout_zero_result.offset_uncertainty_rad - ), - "validation_error_by_joint_rad": dict( - holdout_zero_result.validation_error_by_joint_rad - ), - "validation_original_error_by_joint_rad": dict( - holdout_zero_result.validation_original_error_by_joint_rad - ), - "validation_improvement_by_joint_rad": dict( - holdout_zero_result.validation_improvement_by_joint_rad - ), - "validation_improvement_confidence_lower_rad": dict( - holdout_zero_result - .validation_improvement_confidence_lower_rad - ), - "axis_line_rms_m": holdout_zero_result.axis_line_rms_m, - "validation_line_error_by_joint_m": dict( - holdout_zero_result.validation_line_error_by_joint_m - ), - "offset_confidence_95_half_width_rad": dict( - holdout_zero_result.offset_confidence_half_width_rad - ), - "observability_rank": holdout_zero_result.observability_rank, - "observability_parameter_count": ( - holdout_zero_result.observability_parameter_count - ), - "observability_condition_number": ( - holdout_zero_result.observability_condition_number - ), - "failure_reasons": dict( - holdout_zero_result.failure_reasons - ), - "thumb_yaw_repeatability_failures": ( - thumb_yaw_zero_failures - ), - "axis_cone_mismatch_by_joint_rad": dict( - holdout_zero_result.axis_cone_mismatch_by_joint_rad - ), - "axis_cone_bias_classification_by_joint": dict( - holdout_zero_result - .axis_cone_bias_classification_by_joint - ), - }, - ) - holdout_errors = np.abs( - np.asarray( - [ - value - for name, values in holdout_by_joint.items() - if name in quality_joint_names - for value in values - ], - dtype=float, - ) - ) - trajectory_holdout_passed = bool( - holdout_errors.size - and float(np.mean(holdout_errors)) - <= self.maximum_validation_mae_rad - and float(np.percentile(holdout_errors, 95.0)) - <= self.maximum_validation_p95_rad - and ( - self.profile.layout_id != G20_RIGHT_19_LAYOUT - or float(np.max(holdout_errors)) - <= self.maximum_validation_error_rad - ) - and ( - self.profile.layout_id != G20_RIGHT_19_LAYOUT - or all( - float(np.mean(np.abs(np.asarray(values, dtype=float)))) - <= self.maximum_validation_mae_rad - and float(np.max(np.abs(np.asarray(values, dtype=float)))) - <= self.maximum_validation_error_rad - for name, values in holdout_by_joint.items() - if name in quality_joint_names - ) - ) - ) - self.fit_quality_passed = all( - fit.maximum_monotonic_correction_rad - <= ( - self.maximum_monotonic_correction_rad - if self.profile.joint_specs[name].active - else self.passive_maximum_monotonic_correction_rad - ) - and ( - self.profile.layout_id == G20_RIGHT_19_LAYOUT - or fit.maximum_hysteresis_rad - <= ( - self.maximum_hysteresis_rad - if self.profile.joint_specs[name].active - else self.passive_maximum_hysteresis_rad - ) - ) - for name, fit in training_fits.items() - if name in quality_joint_names - ) and all( - fit.maximum_hysteresis_rad - <= self.command_maximum_direction_gap_rad - for name, fit in command_feedback_fits.items() - if name in quality_joint_names - ) - if not trajectory_holdout_passed: - trajectory_score = { - name: float(np.percentile(np.abs(values), 95.0)) - for name, values in holdout_by_joint.items() - if name in quality_joint_names - } - worst_joint = max(trajectory_score, key=trajectory_score.get) - failed_spec = next( - spec - for spec in self.profile.sweep_specs - if worst_joint in spec.joints - ) - failures: list[dict[str, Any]] = [ - { - "joint": name, - "metric": "third_cycle_trajectory_p95_deg", - "actual": round(math.degrees(error), 6), - "limit": math.degrees( - self.maximum_validation_p95_rad - ), - "comparison": "maximum", - } - for name, error in trajectory_score.items() - if error > self.maximum_validation_p95_rad - ] - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - for name, values in holdout_by_joint.items(): - if name not in quality_joint_names: - continue - absolute = np.abs(np.asarray(values, dtype=float)) - mean_error = float(np.mean(absolute)) - maximum_error = float(np.max(absolute)) - if mean_error > self.maximum_validation_mae_rad: - failures.append( - { - "joint": name, - "metric": "third_cycle_trajectory_mae_deg", - "actual": round( - math.degrees(mean_error), 6 - ), - "limit": math.degrees( - self.maximum_validation_mae_rad - ), - "comparison": "maximum", - } - ) - if maximum_error > self.maximum_validation_error_rad: - failures.append( - { - "joint": name, - "metric": "third_cycle_trajectory_max_deg", - "actual": round( - math.degrees(maximum_error), 6 - ), - "limit": math.degrees( - self.maximum_validation_error_rad - ), - "comparison": "maximum", - } - ) - self._pause_for_provisional_fit_failure(failed_spec, failures) - return - if thumb_yaw_zero_failures: - thumb_yaw_spec = next( - spec - for spec in self.profile.sweep_specs - if "thumb_cmc_yaw" in spec.joints - ) - yaw_pair = self.zero_profile.same_view_axis_pair_by_offset.get( - "thumb_cmc_yaw", () - ) - source_task_key_set = { - observer.task_name - for observer in self.profile.palm_axis_observers - if observer.model_joint in set(yaw_pair) - } - self.retry_source_failure_task_key = thumb_yaw_spec.key - self.retry_source_task_keys = tuple( - candidate.key - for candidate in self.profile.sweep_specs - if candidate.key in source_task_key_set - ) - # A localized scalar outlier is replaced in place while the live - # PnP tracker/reference remains continuous. Non-localized yaw - # failures still select all cycles in the generic retry policy, - # but no dependent retry is allowed to create a new yaw datum. - self.retry_cycle_override.clear() - self._pause_for_provisional_fit_failure( - thumb_yaw_spec, thumb_yaw_zero_failures - ) - return - if not holdout_zero_result.passed: - self._pause_for_zero_model_failure(holdout_zero_result) - return - # Publish the exact frozen training model that was evaluated on the - # isolated final cycle. Re-solving with the holdout would invalidate - # the acceptance result. - zero_result = holdout_zero_result - self.thumb_yaw_cross_session_diagnostic = {} - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - previous_yaw = _previous_passed_joint_zero_offset( - self.session_dir, - self.serial_number, - "thumb_cmc_yaw", - ) - current_yaw = zero_result.all_active_offsets_rad.get( - "thumb_cmc_yaw" - ) - if previous_yaw is not None and current_yaw is not None: - previous_session, previous_offset = previous_yaw - delta = float(current_yaw) - float(previous_offset) - self.thumb_yaw_cross_session_diagnostic = { - "kind": "thumb_yaw_cross_session_diagnostic", - "joint": "thumb_cmc_yaw", - "previous_session": str(previous_session), - "previous_offset_deg": round( - math.degrees(previous_offset), 6 - ), - "current_offset_deg": round( - math.degrees(float(current_yaw)), 6 - ), - "absolute_difference_deg": round( - abs(math.degrees(delta)), 6 - ), - "reference_limit_deg": round( - math.degrees( - self.thumb_yaw_cross_session_diagnostic_rad - ), - 6, - ), - "classification": ( - "review_position_or_tag_installation" - if abs(delta) - > self.thumb_yaw_cross_session_diagnostic_rad - else "within_reference_band" - ), - "decision": "diagnostic_only", - } - append_jsonl( - self.raw_path, - self.thumb_yaw_cross_session_diagnostic, - ) - append_jsonl( - self.raw_path, - { - "kind": "zero_final_diagnostics", - "hand_type": self.hand_type, - "direct_offsets_rad": dict(zero_result.direct_offsets_rad), - "partial_scope_fixed_zero_offsets_rad": dict( - getattr( - self, - "partial_scope_fixed_zero_offsets_rad", - {}, - ) - ), - "model_base_translation_xyz_m": list( - zero_result.base_translation_xyz_m - ), - "model_base_quaternion_xyzw": list( - zero_result.base_quaternion_xyzw - ), - "offset_uncertainty_rad": dict( - zero_result.offset_uncertainty_rad - ), - "offset_confidence_95_half_width_rad": dict( - zero_result.offset_confidence_half_width_rad - ), - "training_cycles": list(zero_result.training_cycles), - "validation_cycle": zero_result.validation_cycle, - "axis_line_rms_m": zero_result.axis_line_rms_m, - "validation_line_error_by_joint_m": dict( - zero_result.validation_line_error_by_joint_m - ), - "observability_rank": zero_result.observability_rank, - "observability_parameter_count": ( - zero_result.observability_parameter_count - ), - "observability_condition_number": ( - zero_result.observability_condition_number - ), - "offset_covariance_rad2": dict( - zero_result.offset_covariance_rad2 - ), - "axis_cone_mismatch_by_joint_rad": dict( - zero_result.axis_cone_mismatch_by_joint_rad - ), - "axis_cone_bias_classification_by_joint": dict( - zero_result.axis_cone_bias_classification_by_joint - ), - }, - ) - # Runtime/MuJoCo consumes requested commands, never feedback bins. - # Static geometry and holdout above intentionally continue to use the - # dense feedback-domain training fit. - if standalone_thumb_calibration: - self.measured_fits = dict(command_fits) - else: - self.measured_fits = clamp_runtime_fits_to_urdf_limits( - self.source_urdf_path, - derive_mimic_passive_fits( - self.source_urdf_path, - command_fits, - profile=self.profile, - ), - endpoint_anchored_offsets_rad=endpoint_zero_offsets, - ) - self.axis_measurements = axes - self.palm_orientation_measurements = list( - palm_orientation_measurements - ) - self.palm_orientation_rejections = dict( - palm_orientation_rejections - ) - self.zero_result = zero_result - self.validated_endpoint_zero_offsets_rad = dict(endpoint_zero_offsets) - self.validation_errors_rad = [ - float(value) - for name, values in holdout_by_joint.items() - if name in quality_joint_names - for value in values - ] - self.validation_errors_rad.extend( - float(value) - for value in holdout_zero_result.validation_errors_rad - ) - if self.combination_validation_enabled: - self._build_combination_validation_items() - self._start_next_combination_validation() - elif self.validation_enabled: - self._build_validation_items() - self._start_next_validation() - else: - self.validation_items.clear() - self.validation_index = 0 - self._begin_return_baseline("finalize") - - def _build_validation_items(self) -> None: - generator = random.Random(self.validation_seed) - self.validation_items = [] - for spec in self.profile.sweep_specs: - commands = generator.sample( - list(range(16, 240)), self.validation_command_count - ) - self.validation_items.extend( - ValidationItem(spec, command) for command in commands - ) - self.validation_index = 0 - - def _build_combination_validation_items(self) -> None: - self.combination_validation_items = list( - _combination_validation_items(self.baseline_command) - ) - self.combination_validation_index = 0 - self.combination_validation_completed = False - - def _start_next_combination_validation(self) -> None: - if self.combination_validation_index >= len( - self.combination_validation_items - ): - self.active_combination_validation = None - self.combination_validation_completed = True - self._begin_return_baseline("finalize") - return - item = self.combination_validation_items[ - self.combination_validation_index - ] - self.active_combination_validation = item - for frames in self.combination_validation_frames_buffer.values(): - frames.clear() - self.position_hold_since = None - self.validation_stage_started_at = time.monotonic() - self.motion_stage_started_at = self.validation_stage_started_at - self._reset_motion_progress( - self.validation_stage_started_at, - self._command_vector_error_u8(item.command_u8), - ) - self.state = STATE_VALIDATION_MOVE - self.reason = f"combination_validation_move_{item.name}" - self._publish_speed_profile(self._normal_speed_profile()) - self._publish_command(list(item.command_u8)) - - def _finish_combination_validation_capture(self) -> None: - item = self.active_combination_validation - if item is None: - raise RuntimeError("combination validation item is missing") - observations: dict[str, Any] = {} - all_frames = [ - frame - for frames in self.combination_validation_frames_buffer.values() - for frame in frames - ] - feedback = np.median( - np.asarray([frame.state_u8 for frame in all_frames], dtype=float), - axis=0, - ) - motor_directions = _combination_motor_directions( - self.combination_validation_items, - self.combination_validation_index, - self.baseline_command, - ) - angles = _combination_joint_angles( - self.profile, - self.measured_fits, - item.command_u8, - motor_directions, - ) - model = UrdfKinematicModel(self.source_urdf_path) - model_base_common = transform_matrix( - self.zero_result.base_translation_xyz_m, - self.zero_result.base_quaternion_xyzw, - ) - pose_position_errors: list[float] = [] - pose_orientation_errors: list[float] = [] - target_errors: list[dict[str, Any]] = [] - observed_target_keys: list[str] = [] - evaluated_target_keys: list[str] = [] - pending_mounts: dict[str, np.ndarray] = {} - for view, frames in self.combination_validation_frames_buffer.items(): - by_joint: dict[str, Any] = {} - joint_names = sorted( - set.intersection( - *(set(frame.joint_quaternions_xyzw) for frame in frames) - ) - ) - for name in joint_names: - by_joint[name] = { - "relative_quaternion_xyzw": [ - float(value) - for value in robust_rotation_summary( - [frame.joint_quaternions_xyzw[name] for frame in frames] - )[0] - ], - "parent_pose_common": _robust_pose_payload( - [frame.parent_poses_common[name] for frame in frames] - ), - "child_pose_common": _robust_pose_payload( - [frame.child_poses_common[name] for frame in frames] - ), - } - observations[view] = by_joint - base_role = COMBINATION_BASE_ROLE_BY_VIEW[view] - base_candidates = [ - name - for name in joint_names - if self.profile.record_specs[name].parent_role == base_role - ] - preferred_base = COMBINATION_BASE_OBSERVER_BY_VIEW[view] - if preferred_base in base_candidates: - base_name = preferred_base - elif base_candidates: - base_name = base_candidates[0] - else: - self._retry_combination_validation_or_pause( - f"combination_{view}_has_no_visible_base_target", - time.monotonic(), - ) - return - base_pose = _robust_pose_payload( - [frame.parent_poses_common[base_name] for frame in frames] - ) - base_matrix = transform_matrix( - base_pose["translation_xyz_m"], base_pose["quaternion_xyzw"] - ) - for observation_name, model_joint in COMBINATION_TAG_TARGETS_BY_VIEW[view]: - if observation_name not in by_joint: - continue - child_pose = by_joint[observation_name]["child_pose_common"] - child_matrix = transform_matrix( - child_pose["translation_xyz_m"], - child_pose["quaternion_xyzw"], - ) - observed = np.linalg.inv(base_matrix) @ child_matrix - # ``observed`` is expressed in this view's fixed palm-Tag - # frame, while UrdfKinematicModel returns hand_base->link. - # The zero solve already estimates hand_base->common; carry - # the model through common and into the observer base frame - # before solving or applying the moving Tag mount. - link = _model_link_in_observer_base( - base_matrix, - model_base_common, - model.link_transform( - model_joint, - zero_offsets=( - self.zero_result.all_active_offsets_rad - ), - joint_angles=angles, - independent_mimic_angles=True, - ), - ) - key = f"{view}:{observation_name}" - quality_gated = key in G20_COMBINATION_REQUIRED_TARGET_KEYS - observed_target_keys.append(key) - if key not in self.combination_tag_mounts: - pending_mounts[key] = np.linalg.inv(link) @ observed - continue - else: - mount = self.combination_tag_mounts[key] - predicted = link @ mount - if quality_gated: - evaluated_target_keys.append(key) - position_error = float( - np.linalg.norm(predicted[:3, 3] - observed[:3, 3]) - ) - orientation_error = float( - ( - Rotation.from_matrix(predicted[:3, :3]).inv() - * Rotation.from_matrix(observed[:3, :3]) - ).magnitude() - ) - if quality_gated: - pose_position_errors.append(position_error) - pose_orientation_errors.append(orientation_error) - target_errors.append( - { - "target": key, - "model_joint": model_joint, - "quality_gated": quality_gated, - "position_error_m": round(position_error, 8), - "orientation_error_rad": round( - orientation_error, 8 - ), - "orientation_error_deg": round( - math.degrees(orientation_error), 6 - ), - "observed_pose_in_base": matrix_payload(observed), - "predicted_pose_in_base": matrix_payload(predicted), - } - ) - if self.combination_validation_index > 0 and not pose_position_errors: - self._retry_combination_validation_or_pause( - "combination_pose_has_no_previously_observed_visible_target", - time.monotonic(), - ) - return - position_p95 = ( - 0.0 - if not pose_position_errors - else float(np.percentile(pose_position_errors, 95.0)) - ) - orientation_p95 = ( - 0.0 - if not pose_orientation_errors - else float(np.percentile(pose_orientation_errors, 95.0)) - ) - if ( - position_p95 > self.combination_maximum_position_p95_m - or orientation_p95 > self.combination_maximum_orientation_p95_rad - ): - retry_key = ("combination", self.combination_validation_index) - append_jsonl( - self.raw_path, - { - "kind": "combination_validation_failure", - "reason": "combination_pose_prediction_failed", - "pose_index": self.combination_validation_index, - "pose_name": item.name, - "label_zh": item.label_zh, - "capture_attempt": ( - self.validation_retry_counts.get(retry_key, 0) + 1 - ), - "requested_command_u8": list(item.command_u8), - "feedback_u8": [ - round(float(value), 6) for value in feedback - ], - "angle_branch_by_motor": list(motor_directions), - "evaluated_targets": sorted(set(evaluated_target_keys)), - "target_errors": sorted( - target_errors, key=lambda value: value["target"] - ), - "valid_frames_by_view": { - view: len(frames) - for view, frames in ( - self.combination_validation_frames_buffer.items() - ) - }, - "position_p95_m": round(position_p95, 8), - "maximum_position_p95_m": round( - self.combination_maximum_position_p95_m, 8 - ), - "orientation_p95_rad": round(orientation_p95, 8), - "orientation_p95_deg": round( - math.degrees(orientation_p95), 6 - ), - "maximum_orientation_p95_rad": round( - self.combination_maximum_orientation_p95_rad, 8 - ), - "maximum_orientation_p95_deg": round( - math.degrees( - self.combination_maximum_orientation_p95_rad - ), - 6, - ), - "passed": False, - }, - ) - self._retry_combination_validation_or_pause( - "combination_pose_prediction_failed", time.monotonic() - ) - return - self.combination_tag_mounts.update(pending_mounts) - for key in set(observed_target_keys): - self.combination_tag_observation_counts[key] = ( - self.combination_tag_observation_counts.get(key, 0) + 1 - ) - for key in set(evaluated_target_keys): - self.combination_tag_validation_counts[key] = ( - self.combination_tag_validation_counts.get(key, 0) + 1 - ) - is_final_pose = self.combination_validation_index + 1 >= len( - self.combination_validation_items - ) - if is_final_pose: - coverage = combination_target_coverage( - self.combination_tag_observation_counts, - self.combination_tag_validation_counts, - ) - if not coverage["coverage_passed"]: - append_jsonl( - self.raw_path, - { - "kind": "combination_target_coverage_failure", - "pose_name": item.name, - **coverage, - }, - ) - self._retry_combination_validation_or_pause( - "combination_target_coverage_incomplete", time.monotonic() - ) - return - self.combination_position_errors_m.extend(pose_position_errors) - self.combination_orientation_errors_rad.extend(pose_orientation_errors) - append_jsonl( - self.raw_path, - { - "kind": "combination_validation_sample", - "pose_index": self.combination_validation_index, - "pose_name": item.name, - "label_zh": item.label_zh, - "requested_command_u8": list(item.command_u8), - "feedback_u8": [round(float(value), 6) for value in feedback], - "image_stamp_ns": int( - np.median([frame.stamp_ns for frame in all_frames]) - ), - "views": observations, - "observed_targets": sorted(set(observed_target_keys)), - "evaluated_targets": sorted(set(evaluated_target_keys)), - "valid_frames_by_view": { - view: len(frames) - for view, frames in self.combination_validation_frames_buffer.items() - }, - "position_p95_m": round(position_p95, 8), - "orientation_p95_rad": round(orientation_p95, 8), - "passed": True, - }, - ) - self.combination_validation_index += 1 - self.active_combination_validation = None - if self.combination_validation_index >= len( - self.combination_validation_items - ): - self.combination_validation_completed = True - self._begin_return_baseline("finalize") - else: - self._begin_return_baseline("combination_next") - - def _retry_combination_validation_or_pause( - self, reason: str, now: float - ) -> None: - item = self.active_combination_validation - if item is None: - self._pause(reason) - return - key = ("combination", self.combination_validation_index) - retries = self.validation_retry_counts.get(key, 0) - if retries >= self.automatic_sweep_retry_limit: - self._pause(reason) - return - retries += 1 - self.validation_retry_counts[key] = retries - for view in self.combination_validation_frames_buffer: - runtime = self.views.get(view) - if runtime is None: - continue - self._reset_view_trackers(runtime) - runtime.pnp_invalid_since = None - runtime.pnp_reset_count += 1 - append_jsonl( - self.raw_path, - { - "kind": "automatic_combination_retry", - "pose_name": item.name, - "reason": reason, - "retry": retries, - "retry_limit": self.automatic_sweep_retry_limit, - }, - ) - for frames in self.combination_validation_frames_buffer.values(): - frames.clear() - self.validation_stage_started_at = now - self.motion_stage_started_at = now - self.position_hold_since = None - self.state = STATE_VALIDATION_MOVE - self.reason = f"automatic_retry_{reason}" - self._reset_motion_progress(now, self._command_vector_error_u8(item.command_u8)) - self._publish_command(list(item.command_u8)) - - def _start_next_validation(self) -> None: - if self.validation_index >= len(self.validation_items): - self.active_validation = None - self._begin_return_baseline("finalize") - return - self.active_validation = self.validation_items[self.validation_index] - validation_motor = self.active_validation.spec.motor_index - validation_target = float(self.active_validation.command_u8) - validation_current = ( - float(self.latest_state_u8[validation_motor]) - if len(self.latest_state_u8) == 20 - else validation_target - ) - self.active_validation_direction = ( - DIRECTION_INCREASING - if validation_target > validation_current + 0.5 - else DIRECTION_DECREASING - if validation_target < validation_current - 0.5 - else canonical_zero_direction( - self.profile, self.active_validation.spec.joints[0] - ) - ) - self.validation_frames_buffer.clear() - self.position_hold_since = None - self.validation_stage_started_at = time.monotonic() - self._reset_motion_progress( - self.validation_stage_started_at, - self._motion_command_error_u8( - self.active_validation.spec, - self.active_validation.command_u8, - ), - ) - self.state = STATE_VALIDATION_MOVE - self.reason = ( - f"validation_{self.active_validation.spec.view}_motor_" - f"{self.active_validation.spec.motor_index}_" - f"command_{self.active_validation.command_u8}" - ) - self._publish_speed_profile( - self._speed_profile_for_spec(self.active_validation.spec) - ) - self._publish_command( - build_calibration_motion_command( - self.active_validation.spec, - self.active_validation.command_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ) - - def _finish_validation_capture(self) -> None: - assert self.active_validation is not None - item = self.active_validation - command = item.command_u8 - for name in item.spec.joints: - selected = _frames_for_joint( - self.validation_frames_buffer, name - ) - if len(selected) < self.validation_frames: - raise RuntimeError( - f"random validation {name} has too few frames" - ) - fit = self.measured_fits.get(name) - if fit is None: - fit = self.validation_only_fits[name] - quaternion = robust_rotation_summary( - [ - frame.joint_quaternions_xyzw[name] - for frame in selected - ] - )[0] - image_relative_xy_px = np.median( - np.asarray( - [ - frame.image_vectors_xy_px[name] - for frame in selected - ], - dtype=float, - ), - axis=0, - ) - observed = measure_joint_curve_observation( - fit, - quaternion_xyzw=quaternion, - image_relative_xy_px=image_relative_xy_px, - ) - expected_curve = ( - fit.decreasing_rad - if self.active_validation_direction == DIRECTION_DECREASING - else fit.increasing_rad - if self.active_validation_direction == DIRECTION_INCREASING - else fit.angle_rad - ) - expected = float(expected_curve[command]) - self.validation_errors_rad.append(observed - expected) - previous_spec = item.spec - self.validation_index += 1 - self.active_validation = None - self.active_validation_direction = None - if self.validation_index >= len(self.validation_items): - self._begin_return_baseline("finalize") - elif self.validation_items[self.validation_index].spec != previous_spec: - self._begin_return_baseline("validation_next") - else: - self._start_next_validation() - - def _finalize(self) -> None: - if self.zero_result is None: - raise RuntimeError("URDF zero solution is missing") - errors = np.abs(np.asarray(self.validation_errors_rad, dtype=float)) - validation_passed = bool( - self.zero_result.passed - and ( - not self.combination_validation_enabled - or self.combination_validation_completed - ) - and ( - not self.validation_enabled - or ( - errors.size > 0 - and float(np.mean(errors)) - <= self.maximum_validation_mae_rad - and float(np.percentile(errors, 95.0)) - <= self.maximum_validation_p95_rad - and ( - self.profile.layout_id != G20_RIGHT_19_LAYOUT - or float(np.max(errors)) - <= self.maximum_validation_error_rad - ) - ) - ) - ) - passed = bool(self.fit_quality_passed and validation_passed) - if not passed: - raise RuntimeError("final_calibration_quality_failed") - endpoint_zero_offsets = ( - G20ThreeCameraCalibrationNode - ._endpoint_zero_offsets_for_publication(self) - ) - stamp = ( - self.session_dir.name - if re.fullmatch(r"\d{8}_\d{6}", self.session_dir.name) - else None - ) - # Zero calibration changes only joint-origin rotations. Measured - # motion curves belong in schema-v4 JSON and must never enlarge the - # original CAD/mechanical safety limits in the generated URDF. - # JSON schema v4 publishes eight decimal places. Generate the URDF - # from those exact same values so the pair is numerically inseparable - # instead of differing by harmless formatter quantisation. - published_zero_offsets = { - name: round(float(value), 8) - for name, value in self.zero_result.all_active_offsets_rad.items() - } - urdf_offsets = published_zero_offsets - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - urdf_offsets = { - name: published_zero_offsets[name] - for name in self.zero_profile.direct_zero_joints - } - correction_plan = None - typed_profile = getattr(self, "calibration_profile", None) - if typed_profile is not None: - frozen_names = typed_profile.scope.frozen_joints[ - self.recalibration_scope - ] - correction_plan = build_correction_plan( - typed_profile, - source_sha256=_file_sha256(self.source_urdf_path), - scope=self.recalibration_scope, - frozen_offsets_rad={ - name: urdf_offsets[name] for name in frozen_names - }, - ) - self.corrected_urdf_path = write_zero_corrected_urdf( - source_urdf=self.source_urdf_path, - output_directory=self.corrected_urdf_output_dir, - serial_number=self.serial_number, - offsets_rad=urdf_offsets, - endpoint_anchored_offsets_rad=endpoint_zero_offsets, - timestamp=stamp, - correction_plan=correction_plan, - ) - try: - if self.standalone_thumb_calibration: - payload = build_standalone_thumb_payload( - profile=self.profile, - serial_number=self.serial_number, - measured_fits=self.measured_fits, - thumb_offsets_rad={ - name: published_zero_offsets[name] - for name in ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - ) - }, - validation_errors_rad=self.validation_errors_rad, - baseline_command_u8=self.baseline_command, - source_urdf_sha256=_file_sha256( - self.source_urdf_path - ), - camera_extrinsics_sha256=_file_sha256( - self.camera_extrinsics_file - ), - corrected_urdf_sha256=_file_sha256( - self.corrected_urdf_path - ), - ) - else: - payload = build_compact_payload( - serial_number=self.serial_number, - measured_fits=self.measured_fits, - urdf_zero_offsets_rad=published_zero_offsets, - validation_errors_rad=self.validation_errors_rad, - passed=passed, - baseline=self.baseline_command, - side=self.hand_type, - layout_id=self.profile.layout_id, - zero_uncertainty_rad=( - self.zero_result.offset_confidence_half_width_rad - ), - zero_cycle_offsets_rad=self.zero_result.cycle_offsets_rad, - zero_observers=self.zero_profile.offset_observer_joint, - artifact_hashes=( - { - "source_urdf_sha256": _file_sha256( - self.source_urdf_path - ), - "camera_extrinsics_sha256": _file_sha256( - self.camera_extrinsics_file - ), - "corrected_urdf_sha256": _file_sha256( - self.corrected_urdf_path - ), - } - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else None - ), - cross_view_roll_metrics=getattr( - self, "cross_view_roll_metrics", {} - ), - joint_dynamic_diagnostics=getattr( - self, "joint_dynamic_diagnostics", {} - ), - zero_geometry_diagnostics=( - { - "training_cycles": self.zero_result.training_cycles, - "validation_cycle": self.zero_result.validation_cycle, - "axis_line_rms_m": self.zero_result.axis_line_rms_m, - "validation_line_error_by_joint_m": ( - self.zero_result.validation_line_error_by_joint_m - ), - "observability_rank": ( - self.zero_result.observability_rank - ), - "observability_parameter_count": ( - self.zero_result.observability_parameter_count - ), - "observability_condition_number": ( - self.zero_result.observability_condition_number - ), - "offset_covariance_rad2": ( - self.zero_result.offset_covariance_rad2 - ), - "palm_orientation_sources": [ - { - "source_joint": item.source_joint, - "model_joint": item.model_joint, - "cycle": item.cycle, - "observed_arc_rad": item.observed_arc_rad, - "rotation_orthogonal_rms_rad": ( - item.rotation_orthogonal_rms_rad - ), - "axis_estimator": item.axis_estimator, - "incremental_pair_count": ( - item.incremental_pair_count - ), - } - for item in getattr( - self, - "palm_orientation_measurements", - (), - ) - ], - "palm_orientation_rejections": dict( - getattr( - self, - "palm_orientation_rejections", - {}, - ) - ), - } - if self.profile.layout_id == G20_RIGHT_19_LAYOUT - else None - ), - ) - # The JSON is the commit marker for the URDF/JSON pair. It is - # atomically renamed only after the corrected URDF and all three - # artifact hashes have been produced and schema-validated. - atomic_write_json(self.final_path, payload) - except Exception: - if self.profile.layout_id == G20_RIGHT_19_LAYOUT: - rejected = self.corrected_urdf_path.with_suffix( - ".urdf.rejected" - ) - self.corrected_urdf_path.replace(rejected) - self.corrected_urdf_path = None - raise - self.completed_payload = payload - self.state = STATE_COMPLETE - self.reason = "calibration_passed" if passed else "quality_failed" - self.get_logger().info( - f"Three-camera calibration complete: passed={passed}; {self.final_path}" - ) - - def _pause(self, reason: str) -> None: - self._publish_hold_current() - self.paused_reason = str(reason) - self.state = STATE_PAUSED - self.reason = str(reason) - self.position_hold_since = None - - def _retry_motion_or_pause(self, reason: str, now: float) -> None: - retries = self.motion_retry_counts.get(reason, 0) - if retries >= self.automatic_motion_retry_limit: - self._pause(reason) - return - retries += 1 - self.motion_retry_counts[reason] = retries - reset_views: list[str] = [] - task_reference_preserved = False - if ( - reason == "sweep_start_tag_timeout" - and self.state == STATE_PREPARE_SWEEP - and self.active_sweep is not None - ): - # Re-publishing the same endpoint command cannot repair a PnP - # tracker stuck on a planar mirror branch. Give every active - # camera a genuinely fresh bounded initialization window while - # retaining the task-level endpoint reference established by the - # visibility precheck/earlier cycles. - profile = _node_profile(self) - task_reference_preserved = ( - _preserve_pnp_task_reference_for_sweep( - self.active_sweep, - is_fit_retry=bool( - getattr(self, "active_sweep_is_fit_retry", False) - ), - has_precheck_anchor=( - profile.layout_id == G20_RIGHT_19_LAYOUT - ), - ) - ) - reset_trackers = getattr(self, "_reset_view_trackers", None) - reset_diagnostics = getattr( - self, "_reset_view_pnp_diagnostics", None - ) - for view in _sweep_views(profile, self.active_sweep.spec): - runtime = getattr(self, "views", {}).get(view) - if runtime is None or not callable(reset_trackers): - continue - reset_trackers( - runtime, - preserve_task_reference=task_reference_preserved, - ) - if callable(reset_diagnostics): - reset_diagnostics(runtime) - runtime.pnp_invalid_since = None - runtime.pnp_reset_count += 1 - reset_views.append(view) - getattr(self, "sweep_start_frames", []).clear() - append_jsonl( - self.raw_path, - { - "kind": "automatic_motion_retry", - "state": self.state, - "reason": reason, - "retry": retries, - "retry_limit": self.automatic_motion_retry_limit, - "pnp_trackers_reset": reset_views, - "task_reference_preserved": task_reference_preserved, - }, - ) - self.motion_stage_started_at = now - self.position_hold_since = None - if self.state == STATE_RETURN_BASELINE: - self._reset_motion_progress(now, self._baseline_error_u8()) - return_command = ( - G20ThreeCameraCalibrationNode._current_return_command(self) - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, return_command - ) - ) - self._publish_command( - list(return_command) - ) - elif self.state == STATE_PREPARE_SWEEP and self.active_sweep is not None: - preparation_command = getattr( - self, - "preparation_command_u8", - tuple( - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.start_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ), - ) - self._reset_motion_progress( - now, - self._command_vector_error_u8(preparation_command), - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, preparation_command - ) - ) - self._publish_command(list(preparation_command)) - self.reason = f"automatic_retry_{reason}" - - def _retry_validation_or_pause(self, reason: str, now: float) -> None: - if self.active_validation is None: - self._pause(reason) - return - item = self.active_validation - key = (_sweep_storage_key(item.spec), item.command_u8) - retries = self.validation_retry_counts.get(key, 0) - retry_limit = self.automatic_sweep_retry_limit - if retries >= retry_limit: - self._pause(reason) - return - retries += 1 - self.validation_retry_counts[key] = retries - for view in _sweep_views(_node_profile(self), item.spec): - runtime = self.views[view] - self._reset_view_trackers(runtime) - runtime.pnp_reset_count += 1 - self.validation_frames_buffer.clear() - self.validation_stage_started_at = now - self._reset_motion_progress( - now, - self._motion_command_error_u8(item.spec, item.command_u8), - ) - self.position_hold_since = None - self.state = STATE_VALIDATION_MOVE - append_jsonl( - self.raw_path, - { - "kind": "automatic_validation_retry", - "view": item.spec.view, - "motor_index": item.spec.motor_index, - "requested_command_u8": item.command_u8, - "reason": reason, - "retry": retries, - "retry_limit": retry_limit, - }, - ) - self._publish_speed_profile(self._speed_profile_for_spec(item.spec)) - self._publish_command( - build_calibration_motion_command( - item.spec, - item.command_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ) - self.reason = f"automatic_retry_{reason}" - - def _timer_callback(self) -> None: - now = time.monotonic() - try: - self._advance(now) - except Exception as error: - self.get_logger().error( - f"Calibration paused: {error}\n{traceback.format_exc()}" - ) - self._pause(str(error)) - if now - self.last_status_publish >= 0.5: - self.last_status_publish = now - self._publish_status(now) - - def _advance(self, now: float) -> None: - if self.state == STATE_PREFLIGHT: - if not self.started: - if self._all_devices_ready(now): - self.state = STATE_WAIT_START - self.reason = "call_start_for_baseline_recovery" - 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(): - self._start_next_sweep() - else: - self.reason = "locking_fixed_base_references" - return - if self.state == STATE_WAIT_START: - if not self._all_devices_ready(now): - self.state = STATE_PREFLIGHT - self.reason = "device_preflight_lost" - return - if self.state == STATE_RETURN_BASELINE: - baseline_reached = self._baseline_reached() - baseline_details = self._baseline_error_details() - if ( - not baseline_reached - and self._pause_if_motion_stalled( - now=now, - error_u8=float(baseline_details["error_u8"]), - context="return_baseline", - details=baseline_details, - ) - ): - return - if now - self.motion_stage_started_at > self.position_timeout_seconds: - self._retry_motion_or_pause("return_baseline_timeout", now) - return - if baseline_reached: - return_waypoints = getattr( - self, "return_waypoints", deque() - ) - if return_waypoints: - # Intermediate avoidance waypoints need confirmed feedback, - # but only the final baseline needs the dedicated 0.5 s - # backlash hold. This removes several seconds of no-op - # waiting from every multi-finger return sequence. - self.return_command_u8 = return_waypoints.popleft() - self.position_hold_since = None - self.motion_stage_started_at = now - self._reset_motion_progress( - now, self._baseline_error_u8() - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, self.return_command_u8 - ) - ) - self._publish_command( - list(self.return_command_u8) - ) - return - if self.position_hold_since is None: - self.position_hold_since = now - elif now - self.position_hold_since >= self.baseline_hold_seconds: - after = self.baseline_after - self.motion_retry_counts.pop("return_baseline_timeout", None) - self.position_hold_since = None - if after in { - "next_sweep", - "next_cycle", - "next_task_same_finger", - "resume_sweep", - "retry_sweep", - }: - self._start_next_sweep() - elif after == "fit": - self._fit_all_curves() - elif after in {"validation_next", "resume_validation"}: - self._start_next_validation() - elif after == "combination_next": - self._start_next_combination_validation() - elif after == "finalize": - self._finalize() - elif after == "startup_tag_preflight": - # Discard observations and PnP branches from the - # interrupted pose. The normal preflight then requires - # a fresh window of all three fixed palm Tags while the - # hand is confirmed at baseline. - self._finish_startup_baseline_recovery() - elif after == "abort": - self.state = STATE_ABORTED - self.reason = ( - "safe_abort_complete:" - f"{self.abort_original_reason or 'operator_abort'}" - ) - else: - self.position_hold_since = None - return - if self.state == STATE_PREPARE_SWEEP: - assert self.active_sweep is not None - preparation_command = getattr( - self, - "preparation_command_u8", - tuple( - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.start_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ), - ) - reached = self._command_vector_reached(preparation_command) - if now - self.motion_stage_started_at > self.position_timeout_seconds: - self._retry_motion_or_pause( - ( - "sweep_start_tag_timeout" - if reached - else "sweep_start_position_timeout" - ), - now, - ) - return - prepare_context = ( - f"prepare_motor_{self.active_sweep.spec.motor_index}" - ) - prepare_details = self._command_vector_error_details( - preparation_command, prepare_context - ) - if ( - not reached - and self._pause_if_motion_stalled( - now=now, - error_u8=self._command_vector_error_u8( - preparation_command - ), - context=prepare_context, - details=prepare_details, - ) - ): - return - preparation_waypoints = getattr( - self, "preparation_waypoints", deque() - ) - if reached and preparation_waypoints: - self.preparation_command_u8 = ( - preparation_waypoints.popleft() - ) - self.motion_stage_started_at = now - self.position_hold_since = None - self._reset_motion_progress( - now, - self._command_vector_error_u8( - self.preparation_command_u8 - ), - ) - self._publish_speed_profile( - G20ThreeCameraCalibrationNode._transition_speed_profile( - self, self.preparation_command_u8 - ) - ) - self._publish_command(list(self.preparation_command_u8)) - return - if reached: - # Pose-entry waypoints use conservative joint-class speeds. - # Switch to the accepted task scan speed only after the full - # start pose has been reached, then honour the SDK settle time. - self._publish_speed_profile( - self._speed_profile_for_spec(self.active_sweep.spec) - ) - speed_ready = bool( - now - self.speed_commanded_at - >= self.speed_setting_settle_seconds - ) - # Do not leave the start endpoint until every contributing camera - # has enough timestamp-synchronised observations. The retained - # frames become the strict 0/255 endpoint bin and, in the first - # training round, the first steady command checkpoint. - minimum_start_frames = ( - 3 - if _steady_checkpoint_commands( - _node_profile(self), self.active_sweep - ) - else 1 - ) - start_frames_ready = _frames_cover_sweep_joints( - self.sweep_start_frames, - self.active_sweep.spec, - minimum_start_frames, - ) - if reached and speed_ready and not start_frames_ready: - self.reason = "waiting_for_task_tags_at_sweep_start" - if ( - reached - and speed_ready - and start_frames_ready - ): - if self.position_hold_since is None: - self.position_hold_since = now - elif now - self.position_hold_since >= self._active_endpoint_hold_seconds(): - self.motion_retry_counts.pop( - "sweep_start_position_timeout", None - ) - self.motion_retry_counts.pop( - "sweep_start_tag_timeout", None - ) - self.position_hold_since = None - self._begin_active_sweep(now) - else: - self.position_hold_since = None - return - if self.state == STATE_SWEEP: - assert self.active_sweep is not None - motor = self.active_sweep.spec.motor_index - if now - self.sweep_started_at > self._active_sweep_timeout_seconds(): - self._retry_active_sweep_or_pause("sweep_timeout") - return - stale_views = [ - view - for view in _sweep_views( - self.profile, self.active_sweep.spec - ) - if now - - getattr(self, "sweep_last_valid_at_by_view", {}).get( - view, self.sweep_started_at - ) - > self.invalid_timeout_seconds - ] - if stale_views: - self._retry_active_sweep_or_pause( - "synchronised_tag_state_timeout:" - + ",".join(stale_views) - ) - return - checkpoint_target_value = getattr( - self, "sweep_checkpoint_target_u8", None - ) - checkpoint_active = bool( - getattr(self, "sweep_checkpoint_mode", False) - and checkpoint_target_value is not None - and not getattr(self, "sweep_baseline_pending", False) - ) - motion_target_u8 = ( - int(self.baseline_command[motor]) - if getattr(self, "sweep_baseline_pending", False) - else int( - checkpoint_target_value - if checkpoint_target_value is not None - else self.active_sweep.target_u8 - ) - ) - target_reached = ( - self._steady_checkpoint_reached( - self.active_sweep.spec, motion_target_u8 - ) - if checkpoint_active - else self._motion_command_reached( - self.active_sweep.spec, motion_target_u8 - ) - ) - sweep_context = ( - f"sweep_baseline_motor_{motor}" - if getattr(self, "sweep_baseline_pending", False) - else f"sweep_motor_{motor}" - ) - sweep_details = self._motion_command_error_details( - self.active_sweep.spec, - motion_target_u8, - sweep_context, - ) - if ( - checkpoint_active - and int(sweep_details.get("motor_index", -1)) == motor - ): - sweep_details["tolerance_u8"] = max( - float( - self.steady_checkpoint_command_feedback_tolerance_u8 - ), - float(sweep_details["tolerance_u8"]), - ) - if ( - not target_reached - and self._pause_if_motion_stalled( - now=now, - error_u8=float(sweep_details["error_u8"]), - context=sweep_context, - details=sweep_details, - ) - ): - return - if getattr(self, "sweep_baseline_pending", False): - if target_reached: - if self.sweep_baseline_hold_since is None: - # Frames acquired during deceleration into 127 are not - # static observations. Wait baseline_hold_seconds - # after feedback first reaches 127, then collect a - # fresh multi-frame baseline sample. - self.sweep_baseline_frames.clear() - self.sweep_baseline_hold_since = now - elif ( - now - self.sweep_baseline_hold_since - >= self.baseline_hold_seconds - and _frames_cover_sweep_joints( - self.sweep_baseline_frames, - self.active_sweep.spec, - self.minimum_baseline_hold_frames, - ) - ): - self.sweep_baseline_pending = False - self.sweep_baseline_hold_since = None - self.sweep_endpoint_since = None - self.reason = ( - "collecting_timestamp_synchronised_tag_centres" - ) - if getattr(self, "sweep_checkpoint_mode", False): - self._publish_next_checkpoint(now) - else: - self._reset_motion_progress( - now, - self._motion_command_error_u8( - self.active_sweep.spec, - self.active_sweep.target_u8, - ), - ) - self._publish_command( - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.target_u8, - baseline=self.baseline_command, - profile=self.profile, - ) - ) - else: - self.sweep_baseline_hold_since = None - self.sweep_baseline_frames.clear() - return - if getattr(self, "sweep_checkpoint_mode", False): - checkpoint_target = self.sweep_checkpoint_target_u8 - if checkpoint_target is None: - if self.sweep_checkpoint_commands: - # Recover a missing in-flight target by publishing the - # next queued checkpoint instead of aborting a healthy - # hardware session. - self._publish_next_checkpoint(now) - return - self.sweep_checkpoint_mode = False - else: - if target_reached: - if self.sweep_checkpoint_hold_since is None: - self.sweep_checkpoint_frames.clear() - self.sweep_checkpoint_hold_since = now - elif ( - now - self.sweep_checkpoint_hold_since - >= self.endpoint_hold_seconds - and _frames_cover_sweep_joints( - self.sweep_checkpoint_frames, - self.active_sweep.spec, - 3, - ) - ): - if not self._steady_checkpoint_feedback_is_stable( - self.active_sweep.spec, - self.sweep_checkpoint_frames, - ): - # The command/feedback offset is acceptable, - # but feedback is still settling inside that - # band. Restart the hold window without - # issuing another command or weakening the - # mechanical-stall gate. - self.sweep_checkpoint_hold_since = now - self.sweep_checkpoint_frames.clear() - return - self._record_command_checkpoint( - self.active_sweep, - int(checkpoint_target), - list(self.sweep_checkpoint_frames), - ) - self.sweep_checkpoint_hold_since = None - self.sweep_checkpoint_frames.clear() - if self._publish_next_checkpoint(now): - return - self.sweep_endpoint_since = now - else: - self.sweep_checkpoint_hold_since = None - self.sweep_checkpoint_frames.clear() - if self.sweep_checkpoint_target_u8 is not None: - return - if target_reached: - if self.sweep_endpoint_since is None: - self.sweep_endpoint_since = now - else: - self.sweep_endpoint_since = None - if ( - self.sweep_endpoint_since is not None - and now - self.sweep_endpoint_since - >= self._active_endpoint_hold_seconds() - and _frames_cover_sweep_motion( - self.sweep_frames, - self.active_sweep.spec, - motor_index=motor, - minimum_per_joint=self.minimum_sweep_frames, - minimum_span_u8=self.minimum_state_span_u8, - ) - ): - self._finish_active_sweep() - return - if self.state == STATE_VALIDATION_MOVE: - if self.active_combination_validation is not None: - command = self.active_combination_validation.command_u8 - reached = self._command_vector_reached(command) - details = self._command_vector_error_details( - command, - f"combination_{self.active_combination_validation.name}", - ) - if ( - not reached - and self._pause_if_motion_stalled( - now=now, - error_u8=float(details["error_u8"]), - context=str(details["stage"]), - details=details, - ) - ): - return - if reached: - if self.position_hold_since is None: - self.position_hold_since = now - elif now - self.position_hold_since >= self.endpoint_hold_seconds: - self.position_hold_since = None - for frames in self.combination_validation_frames_buffer.values(): - frames.clear() - self.validation_stage_started_at = now - self.state = STATE_VALIDATION_CAPTURE - self.reason = ( - "combination_validation_capture_" - f"{self.active_combination_validation.name}" - ) - else: - self.position_hold_since = None - if now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._retry_combination_validation_or_pause( - "combination_validation_move_timeout", now - ) - return - assert self.active_validation is not None - reached = self._motion_command_reached( - self.active_validation.spec, - self.active_validation.command_u8, - ) - validation_context = ( - "validation_motor_" - f"{self.active_validation.spec.motor_index}" - ) - validation_details = self._motion_command_error_details( - self.active_validation.spec, - self.active_validation.command_u8, - validation_context, - ) - if ( - not reached - and self._pause_if_motion_stalled( - now=now, - error_u8=float(validation_details["error_u8"]), - context=validation_context, - details=validation_details, - ) - ): - return - speed_ready = bool( - now - self.speed_commanded_at - >= self.speed_setting_settle_seconds - ) - if reached and speed_ready: - if self.position_hold_since is None: - self.position_hold_since = now - elif now - self.position_hold_since >= self.endpoint_hold_seconds: - self.position_hold_since = None - self.validation_frames_buffer.clear() - self.validation_stage_started_at = now - self.state = STATE_VALIDATION_CAPTURE - self.reason = "capturing_random_validation_pose" - else: - self.position_hold_since = None - if now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._retry_validation_or_pause("validation_move_timeout", now) - return - if self.state == STATE_VALIDATION_CAPTURE: - if self.active_combination_validation is not None: - if all( - len(frames) >= self.combination_validation_frames - for frames in self.combination_validation_frames_buffer.values() - ): - self._finish_combination_validation_capture() - elif now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._retry_combination_validation_or_pause( - "combination_validation_capture_timeout", now - ) - return - if _frames_cover_sweep_joints( - self.validation_frames_buffer, - self.active_validation.spec, - self.validation_frames, - ): - self._finish_validation_capture() - elif now - self.validation_stage_started_at > self.validation_timeout_seconds: - self._retry_validation_or_pause( - "validation_capture_timeout", now - ) - - def _publish_status(self, now: float) -> None: - views: dict[str, dict[str, Any]] = {} - for name, runtime in self.views.items(): - required_roles = set(self._required_roles_for_view(name)) - live_required_roles = set( - self._live_required_roles_for_view( - name, tuple(required_roles) - ) - ) - locked_role = self._locked_base_role_for_active_capture(name) - locked_roles = set(() if locked_role is None else (locked_role,)) - views[name] = { - "ready": self._view_ready(runtime, now), - "stream_alive": bool( - runtime.last_message_at > 0.0 - and now - runtime.last_message_at <= 1.0 - ), - "camera_info_valid": runtime.camera_info_valid, - "camera_extrinsics_valid": runtime.extrinsics_valid, - "detection_hz": round(runtime.detection_hz, 2), - "valid_rate": round(runtime.valid_rate, 4), - "required_tag_ids": sorted( - runtime.view_tags[role] for role in required_roles - ), - "locked_reference_tag_ids": sorted( - runtime.view_tags[role] for role in locked_roles - ), - "locked_base_corner_drift_px": round( - runtime.latest_locked_base_corner_drift_px, 4 - ), - "locked_base_maximum_corner_drift_px": ( - self.fixed_base_maximum_corner_drift_px - ), - "locked_base_corner_drift_count": ( - runtime.locked_base_corner_drift_count - ), - "configured_tag_ids": sorted(runtime.view_tags.values()), - "visible_configured_tag_ids": sorted( - tag_id - for role, tag_id in runtime.view_tags.items() - if role in runtime.latest_tag_quality - and self._quality_valid( - runtime.latest_tag_quality[role], include_pnp=False - ) - ), - "detected_tag_ids": sorted( - tag_id - for tag_id, role in runtime.role_by_id.items() - if role in required_roles - and role in runtime.latest_tag_quality - ), - "missing_tag_ids": sorted( - tag_id - for tag_id, role in runtime.role_by_id.items() - if role in live_required_roles - and role not in runtime.latest_tag_quality - ), - "pnp_rejections": dict(runtime.latest_pnp_rejections), - "group_pnp_reason": runtime.latest_group_pnp_reason, - "group_missing_candidate_roles": list( - runtime.latest_group_missing_candidate_roles - ), - "pnp_candidate_diagnostics": { - role: { - "tag_id": runtime.view_tags[role], - **dict(diagnostics), - } - for role, diagnostics in ( - runtime.tracker.last_candidate_diagnostics_by_role.items() - ) - if role in required_roles and role not in locked_roles - }, - "pnp_pose_valid": runtime.latest_pnp_valid, - "pnp_rejection_counts": dict( - runtime.pnp_rejection_counts - ), - "group_pnp_rejection_counts": dict( - runtime.group_pnp_rejection_counts - ), - "pnp_initialization_progress": ( - None - if runtime.pnp_initialization_progress is None - else { - "accepted": runtime.pnp_initialization_progress[0], - "required": runtime.pnp_initialization_progress[1], - } - ), - "pnp_reset_count": runtime.pnp_reset_count, - "pnp_invalid_seconds": ( - 0.0 - if runtime.pnp_invalid_since is None - else round(max(0.0, now - runtime.pnp_invalid_since), 3) - ), - } - active: dict[str, Any] = {} - if self.motion_stall_details: - active = dict(self.motion_stall_details) - elif self.active_sweep is not None: - motor = self.active_sweep.spec.motor_index - start_tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, - self.active_sweep.spec, - self.active_sweep.start_u8, - ) - ) - target_tolerance = ( - G20ThreeCameraCalibrationNode._synchronised_endpoint_tolerance_for_spec( - self, - self.active_sweep.spec, - self.active_sweep.target_u8, - ) - ) - mechanical_start_tolerance = self._endpoint_tolerance_for_spec( - self.active_sweep.spec, self.active_sweep.start_u8 - ) - mechanical_target_tolerance = self._endpoint_tolerance_for_spec( - self.active_sweep.spec, self.active_sweep.target_u8 - ) - values = [ - float(frame.state_u8[motor]) for frame in self.sweep_frames - ] - bins = sorted( - { - ( - self.active_sweep.start_u8 - if abs(value - self.active_sweep.start_u8) - <= start_tolerance - else self.active_sweep.target_u8 - if abs(value - self.active_sweep.target_u8) - <= target_tolerance - else int(np.clip(np.rint(value), 0, 255)) - ) - for value in values - } - ) - adjacent_bin_gaps = [ - (right - left, left, right) - for left, right in zip(bins, bins[1:]) - ] - maximum_bin_gap, gap_start_u8, gap_end_u8 = max( - adjacent_bin_gaps, - default=(0, None, None), - ) - missing_endpoints = [ - endpoint for endpoint in (0, 255) if endpoint not in bins - ] - actual = ( - None - if len(self.latest_state_u8) != 20 - else float(self.latest_state_u8[motor]) - ) - motion_progress = None - if actual is not None: - motion_progress = float( - np.clip( - abs(actual - self.active_sweep.start_u8) / 255.0, - 0.0, - 1.0, - ) - ) - storage_key = _sweep_storage_key(self.active_sweep.spec) - frame_counts = { - name: len(_frames_for_joint(self.sweep_frames, name)) - for name in self.active_sweep.spec.joints - } - baseline_frame_counts = { - name: len( - _frames_for_joint(self.sweep_baseline_frames, name) - ) - for name in self.active_sweep.spec.joints - } - detection_rates_by_view = { - view: ( - 0.0 - if getattr( - self, "sweep_detection_total_by_view", {} - ).get(view, 0) <= 0 - else getattr( - self, "sweep_detection_valid_by_view", {} - ).get(view, 0) - / getattr( - self, "sweep_detection_total_by_view", {} - )[view] - ) - for view in _sweep_views( - self.profile, self.active_sweep.spec - ) - } - active = { - "kind": "sweep", - "view": self.active_sweep.spec.view, - "motor_index": motor, - "joints": list(self.active_sweep.spec.joints), - "cycle": self.active_sweep.cycle + 1, - "repetitions": self.repetitions, - "direction": self.active_sweep.direction, - "task_name": self.active_sweep.spec.key, - "fit_attempt": self.sweep_attempts.get( - storage_key, 1 - ), - "fit_attempt_limit": self.automatic_fit_retry_limit + 1, - "fit_retry_cycles": ( - [ - cycle + 1 - for cycle in sorted( - getattr(self, "retry_cycles", set()) - ) - ] - if getattr(self, "active_sweep_is_fit_retry", False) - else [] - ), - "automatic_retry_count": max( - self.sweep_retry_counts.get( - ( - storage_key, - self.active_sweep.cycle, - self.active_sweep.direction, - ), - 0, - ), - self.motion_retry_counts.get( - "sweep_start_tag_timeout", 0 - ), - self.motion_retry_counts.get( - "sweep_start_position_timeout", 0 - ), - ), - "automatic_retry_limit": self.automatic_sweep_retry_limit, - "retry_speed_scale": ( - 1.0 - if self.sweep_retry_counts.get( - ( - storage_key, - self.active_sweep.cycle, - self.active_sweep.direction, - ), - 0, - ) - == 0 - else self.retry_speed_scales[ - min( - self.sweep_retry_counts[ - ( - storage_key, - self.active_sweep.cycle, - self.active_sweep.direction, - ) - ], - len(self.retry_speed_scales), - ) - - 1 - ] - ), - "endpoint_hold_seconds": self._active_endpoint_hold_seconds(), - "sweep_timeout_seconds": self._active_sweep_timeout_seconds(), - "motor_stall_timeout_seconds": self.motor_stall_timeout_seconds, - "motor_stall_minimum_progress_u8": ( - self.motor_stall_minimum_progress_u8 - ), - "start_u8": self.active_sweep.start_u8, - "target_u8": self.active_sweep.target_u8, - "current_motion_target_u8": ( - int( - getattr( - self, - "preparation_command_u8", - build_calibration_motion_command( - self.active_sweep.spec, - self.active_sweep.start_u8, - baseline=self.baseline_command, - profile=self.profile, - ), - )[motor] - ) - if self.state == STATE_PREPARE_SWEEP - else int(self.baseline_command[motor]) - if getattr(self, "sweep_baseline_pending", False) - else int( - self.sweep_checkpoint_target_u8 - if getattr(self, "sweep_checkpoint_target_u8", None) - is not None - else self.active_sweep.target_u8 - ) - ), - "steady_checkpoint_mode": bool( - getattr(self, "sweep_checkpoint_mode", False) - ), - "remaining_steady_checkpoints": len( - getattr(self, "sweep_checkpoint_commands", ()) - ), - "baseline_hold_pending": bool( - getattr(self, "sweep_baseline_pending", False) - ), - "baseline_hold_valid_frames": min( - baseline_frame_counts.values(), default=0 - ), - "minimum_baseline_hold_frames": int( - getattr(self, "minimum_baseline_hold_frames", 10) - ), - "actual_u8": actual, - "motion_progress": motion_progress, - "valid_frames": min(frame_counts.values(), default=0), - "valid_frames_by_joint": frame_counts, - "detection_frames": int( - getattr(self, "sweep_detection_total_frames", 0) - ), - "detection_valid_frames": int( - getattr(self, "sweep_detection_valid_frames", 0) - ), - "detection_rate": min( - detection_rates_by_view.values(), default=0.0 - ), - "detection_rate_by_view": detection_rates_by_view, - "sample": { - "minimum_u8": None if not values else min(values), - "maximum_u8": None if not values else max(values), - "span_u8": 0.0 if not values else max(values) - min(values), - "bin_count": len(bins), - "minimum_bin_count": self.minimum_sweep_bins, - "maximum_bin_gap": int(maximum_bin_gap), - "maximum_bin_gap_start_u8": gap_start_u8, - "maximum_bin_gap_end_u8": gap_end_u8, - "allowed_maximum_bin_gap": self.maximum_bin_gap, - "missing_endpoint_u8": missing_endpoints, - "endpoint_tolerance_u8": max( - start_tolerance, target_tolerance - ), - "start_endpoint_tolerance_u8": start_tolerance, - "target_endpoint_tolerance_u8": target_tolerance, - "mechanical_endpoint_tolerance_u8": max( - mechanical_start_tolerance, - mechanical_target_tolerance, - ), - "synchronised_endpoint_tolerance_margin_u8": float( - getattr( - self, - "synchronised_endpoint_tolerance_margin_u8", - 1.0, - ) - ), - }, - "auxiliary_motors": [ - { - "motor_index": index, - "command_u8": command, - "actual_u8": ( - None - if len(self.latest_state_u8) != 20 - else float(self.latest_state_u8[index]) - ), - } - for index, command in calibration_auxiliary_commands( - self.active_sweep.spec, profile=self.profile - ).items() - ], - "speed": { - "commanded_finger_speed": self._speed_profile_for_spec( - self.active_sweep.spec - ), - "reported_finger_speed": self.latest_hand_info.get("speed"), - "normal_speed": self.normal_calibration_speed, - "index_roll_speed": self.index_roll_calibration_speed, - "index_flex_speed": self.index_flex_calibration_speed, - "adaptive_formal_speed_scale": float( - self.formal_speed_scales.get( - self.active_sweep.spec.key, 1.0 - ) - ), - }, - } - elif self.active_validation is not None: - motor = self.active_validation.spec.motor_index - active = { - "kind": "validation", - "view": self.active_validation.spec.view, - "motor_index": motor, - "command_u8": self.active_validation.command_u8, - "actual_u8": ( - None - if len(self.latest_state_u8) != 20 - else float(self.latest_state_u8[motor]) - ), - "valid_frames": len(self.validation_frames_buffer), - "auxiliary_motors": [ - { - "motor_index": index, - "command_u8": command, - "actual_u8": ( - None - if len(self.latest_state_u8) != 20 - else float(self.latest_state_u8[index]) - ), - } - for index, command in calibration_auxiliary_commands( - self.active_validation.spec, profile=self.profile - ).items() - ], - "speed": { - "commanded_finger_speed": self._speed_profile_for_spec( - self.active_validation.spec - ), - "reported_finger_speed": self.latest_hand_info.get("speed"), - "normal_speed": self.normal_calibration_speed, - "index_roll_speed": self.index_roll_calibration_speed, - "index_flex_speed": self.index_flex_calibration_speed, - "adaptive_formal_speed_scale": float( - self.formal_speed_scales.get( - self.active_validation.spec.key, 1.0 - ) - ), - }, - } - elif self.fit_failure: - active = dict(self.fit_failure) - group_pnp_reasons = { - name: runtime.latest_group_pnp_reason - for name, runtime in self.views.items() - if runtime.latest_group_pnp_reason - } - if group_pnp_reasons: - active = dict(active) - active["group_pnp_reasons"] = group_pnp_reasons - pnp_rejection_counts = { - name: dict(runtime.pnp_rejection_counts) - for name, runtime in self.views.items() - if runtime.pnp_rejection_counts - } - if pnp_rejection_counts: - active = dict(active) - active["pnp_rejection_counts"] = pnp_rejection_counts - group_pnp_rejection_counts = { - name: dict(runtime.group_pnp_rejection_counts) - for name, runtime in self.views.items() - if runtime.group_pnp_rejection_counts - } - if group_pnp_rejection_counts: - active = dict(active) - active["group_pnp_rejection_counts"] = ( - group_pnp_rejection_counts - ) - pnp_initialization_progress = { - name: { - "accepted": runtime.pnp_initialization_progress[0], - "required": runtime.pnp_initialization_progress[1], - } - for name, runtime in self.views.items() - if runtime.pnp_initialization_progress is not None - } - if pnp_initialization_progress: - active = dict(active) - active["pnp_initialization_progress"] = ( - pnp_initialization_progress - ) - resumed_tasks = set(self.resumed_task_keys) - completed_sweep_count = sum( - 1 - for index, item in enumerate(self.sweep_items) - if index < self.sweep_index or item.spec.key in resumed_tasks - ) - scan_progress = ( - 0.0 - if not self.sweep_items - else float( - np.clip( - completed_sweep_count / len(self.sweep_items), 0.0, 1.0 - ) - ) - ) - status_validation_index = ( - self.combination_validation_index - if self.combination_validation_items - else self.validation_index - ) - status_validation_total = ( - len(self.combination_validation_items) - if self.combination_validation_items - else len(self.validation_items) - ) - payload = { - "serial_number": self.serial_number, - "session_dir": str(self.session_dir), - "hand_type": self.hand_type, - "reference_finger": self.profile.reference_finger, - "state": self.state, - "reason": self.reason, - "preflight_mode": ( - "recovering_baseline" - if self.state == STATE_RETURN_BASELINE - and self.baseline_after == "startup_tag_preflight" - else ( - "device_only_before_baseline" - if self.state in {STATE_PREFLIGHT, STATE_WAIT_START} - and not self.started - else ( - "baseline_tags_after_recovery" - if self.state == STATE_PREFLIGHT - and self.started - and self.startup_baseline_recovered - else "" - ) - ) - ), - "progress": round( - _overall_progress( - self.state, - scan_progress=scan_progress, - validation_index=status_validation_index, - validation_total=status_validation_total, - ), - 4, - ), - "scan_progress": round(scan_progress, 4), - "completed_sweeps": self.sweep_index, - "total_sweeps": len(self.sweep_items), - "baseline_command_u8": list(self.baseline_command), - "return_command_u8": list( - G20ThreeCameraCalibrationNode._current_return_command(self) - ), - "hand_state_fresh": bool( - len(self.latest_state_u8) == 20 - and now - self.last_state_at <= 1.0 - ), - "feedback_hz": round( - ( - 0.0 - if len(self.state_receive_times) < 2 - or self.state_receive_times[-1] - <= self.state_receive_times[0] - else (len(self.state_receive_times) - 1) - / ( - self.state_receive_times[-1] - - self.state_receive_times[0] - ) - ), - 2, - ), - "views": views, - "active": active, - "result_path": ( - str(self.final_path) if self.completed_payload is not None else "" - ), - "corrected_urdf_path": ( - "" - if self.corrected_urdf_path is None - else str(self.corrected_urdf_path) - ), - "camera_extrinsics_error": self.extrinsics_error, - "resume": { - "used": bool(self.resumed_task_keys), - "source_session": self.resume_source_session, - "recalibration_scope": self.recalibration_scope, - "recalibration_task_keys": list( - self.recalibration_task_keys - ), - "completed_task_count": len(self.resumed_task_keys), - "total_task_count": len(self.profile.sweep_specs), - "completed_task_keys": list(self.resumed_task_keys), - }, - "base_import": dict( - getattr(self, "base_import_progress", {}) - ), - "quality": ( - {} if self.completed_payload is None else self.completed_payload["quality"] - ), - "thumb_yaw_cross_session": dict( - getattr(self, "thumb_yaw_cross_session_diagnostic", {}) - ), - "combination_validation": { - "enabled": bool(self.combination_validation_enabled), - "completed": bool(self.combination_validation_completed), - "completed_poses": int(self.combination_validation_index), - "total_poses": len(self.combination_validation_items), - "position_p95_m": ( - None - if not self.combination_position_errors_m - else round( - float( - np.percentile( - self.combination_position_errors_m, 95.0 - ) - ), - 8, - ) - ), - "orientation_p95_rad": ( - None - if not self.combination_orientation_errors_rad - else round( - float( - np.percentile( - self.combination_orientation_errors_rad, 95.0 - ) - ), - 8, - ) - ), - **combination_target_coverage( - self.combination_tag_observation_counts, - self.combination_tag_validation_counts, - ), - }, - } - status = String() - status.data = json.dumps(payload, ensure_ascii=False) - self.status_publisher.publish(status) - text = String() - text.data = render_three_camera_status_text_zh(payload) - self.status_text_publisher.publish(text) - - -def main(args: list[str] | None = None) -> None: - configure_fastdds_large_image_transport() - rclpy.init(args=args) - node: G20ThreeCameraCalibrationNode | None = None - try: - node = G20ThreeCameraCalibrationNode() - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - if node is not None: - node.destroy_node() - if rclpy.ok(): - rclpy.shutdown() - - -if __name__ == "__main__": - main() +sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py b/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py index e092521..edfb635 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py +++ b/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py @@ -1,4635 +1,7 @@ -"""Three-dimensional joint-axis fitting and URDF zero correction.""" +"""One-release module alias for the relocated model zero solver.""" -from __future__ import annotations +import sys -from dataclasses import dataclass, replace -from datetime import datetime -import math -import os -from pathlib import Path -import re -import shutil -from typing import Any, Mapping, Sequence -import xml.etree.ElementTree as ET +from .models.g20 import zero_solver as _implementation -import numpy as np -from scipy.optimize import least_squares -from scipy.spatial.transform import Rotation -from scipy.stats import t as student_t - -from .core import fit_rotation_axis, robust_rotation_summary -from .core.urdf import UrdfCorrectionPlan -from .full_hand import ( - G20_RIGHT_19_LAYOUT, - IMAGE_TRAJECTORY_JOINTS, - LEFT_HAND_PROFILE, - RIGHT_19_END_ON_IMAGE_CURVE_JOINTS, - RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS, - HandCalibrationProfile, - JointCurveFit, - get_hand_calibration_profile, -) -from .sample_schema import explicit_domain_value -from .trajectory import ( - _fit_circle_with_axis, - _fit_joint_curve, - _fit_plane_axis, - _plane_basis, -) - - -@dataclass(frozen=True) -class ZeroCalibrationProfile: - hand: HandCalibrationProfile - direct_zero_joints: tuple[str, ...] - axis_joints: tuple[str, ...] - # Dynamic command-angle curves may be shared by unobserved fingers. This - # mapping is intentionally *not* an authorization to copy an absolute - # encoder/URDF zero between independent motors. - inherited_zero_joints: Mapping[str, str] - # Absolute static-zero inheritance requires an independent mechanical - # guarantee. With the current 11-Tag layout only the reference finger is - # observed, so the conservative mapping is empty and unobserved active - # finger origins retain their source-CAD zero. - inherited_static_zero_joints: Mapping[str, str] - constrained_circle_joints: frozenset[str] - root_anchor_joints: frozenset[str] - axis_parent_joint: Mapping[str, str] - phase_parent_joint: Mapping[str, str] - offset_observer_joint: Mapping[str, str] - # Some serial offsets can be observed from the angle between two axes - # captured by the same camera. The mapping is geometric topology only; - # it never contains a model- or serial-specific zero value. - same_view_axis_pair_by_offset: Mapping[str, tuple[str, str]] - fixed_direct_zero_offsets_rad: Mapping[str, float] - static_output_zero_offsets_rad: Mapping[str, float] - # Product full-hand fitting uses the five root-axis line pattern. The - # thumb kernel instead uses only the serial thumb chain, so its nuisance - # palm pose cannot be influenced by finger observations. - base_pose_strategy: str = "full_hand" - - @property - def reference_finger(self) -> str: - return self.hand.reference_finger - - -def _build_zero_profile(hand: HandCalibrationProfile) -> ZeroCalibrationProfile: - if hand.layout_id == G20_RIGHT_19_LAYOUT: - return _build_right_19_zero_profile(hand) - reference = hand.reference_finger - reference_roll = f"{reference}_mcp_roll" - reference_pitch = f"{reference}_mcp_pitch" - reference_pip = f"{reference}_pip" - reference_dip = f"{reference}_dip" - direct = ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - reference_roll, - reference_pitch, - reference_pip, - ) - inherited = { - f"{finger}_{suffix}": f"{reference}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - if finger != reference - for suffix in ("mcp_roll", "mcp_pitch", "pip") - } - axis_parent = { - "thumb_cmc_yaw": "thumb_cmc_roll", - "thumb_cmc_pitch": "thumb_cmc_yaw", - reference_pitch: reference_roll, - } - phase_parent = { - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - reference_pip: reference_pitch, - reference_dip: reference_pip, - } - observer = { - "thumb_cmc_roll": "thumb_cmc_yaw", - "thumb_cmc_yaw": "thumb_cmc_pitch", - "thumb_cmc_pitch": "thumb_mcp", - "thumb_mcp": "thumb_ip", - reference_roll: reference_pitch, - reference_pitch: reference_pip, - reference_pip: reference_dip, - } - fixed_direct_zero_offsets = { - "thumb_mcp": 0.0, - reference_roll: 0.0, - reference_pitch: 0.0, - reference_pip: 0.0, - } - return ZeroCalibrationProfile( - hand=hand, - direct_zero_joints=direct, - axis_joints=( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - "thumb_ip", - reference_roll, - reference_pitch, - reference_pip, - reference_dip, - ), - inherited_zero_joints=inherited, - inherited_static_zero_joints={}, - constrained_circle_joints=( - hand.image_trajectory_joints | {reference_dip, "thumb_cmc_yaw"} - ), - root_anchor_joints=frozenset({"thumb_cmc_roll", reference_roll}), - axis_parent_joint=axis_parent, - phase_parent_joint=phase_parent, - offset_observer_joint=observer, - same_view_axis_pair_by_offset={}, - fixed_direct_zero_offsets_rad=fixed_direct_zero_offsets, - static_output_zero_offsets_rad={}, - ) - - -def _build_right_19_zero_profile( - hand: HandCalibrationProfile, -) -> ZeroCalibrationProfile: - """Return the 16-active-zero graph for the 19-Tag layout. - - Distal Tags recover all four PIP/DIP dynamic curves and axis quality, but - repeated motion with one unchanged camera/Tag installation cannot by - itself distinguish a fixed parallel-axis phase bias from an encoder zero. - The three coupled thumb-CMC offsets must therefore remain a visual - multi-axis solve; their electrical endpoints are not assumed to coincide - with source-CAD limits. Independently repeatable contact endpoints still - anchor thumb MCP, finger MCP-pitch and PIP from each session. DIP stays - passive and has no independently identifiable static zero. - """ - fingers = ("index", "middle", "ring", "pinky") - direct = ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - *( - f"{finger}_{suffix}" - for finger in fingers - for suffix in ("mcp_roll", "mcp_pitch", "pip") - ), - ) - axis_joints = ( - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - "thumb_ip", - *( - f"{finger}_{suffix}" - for finger in fingers - for suffix in ("mcp_roll", "mcp_pitch", "pip", "dip") - ), - ) - axis_parent = { - "thumb_cmc_yaw": "thumb_cmc_roll", - "thumb_cmc_pitch": "thumb_cmc_yaw", - **{ - f"{finger}_mcp_pitch": f"{finger}_mcp_roll" - for finger in fingers - }, - } - phase_parent = { - "thumb_mcp": "thumb_cmc_pitch", - "thumb_ip": "thumb_mcp", - **{ - f"{finger}_{child}": f"{finger}_{parent}" - for finger in fingers - for child, parent in ( - ("pip", "mcp_pitch"), - ("dip", "pip"), - ) - }, - } - observer = { - "thumb_cmc_roll": "thumb_cmc_yaw", - "thumb_cmc_yaw": "thumb_cmc_pitch", - "thumb_cmc_pitch": "thumb_mcp", - "thumb_mcp": "thumb_ip", - **{ - f"{finger}_{target}": f"{finger}_{observed}" - for finger in fingers - for target, observed in ( - ("mcp_roll", "mcp_pitch"), - ("mcp_pitch", "pip"), - ("pip", "dip"), - ) - }, - } - root_anchors = frozenset( - {"thumb_cmc_roll", *(f"{finger}_mcp_roll" for finger in fingers)} - ) - constrained = frozenset( - set(hand.image_trajectory_joints) - | {"thumb_cmc_yaw"} - # The side camera observes all four flexion axes nearly end-on. A - # 16 mm monocular planar Tag gives a very repeatable orientation axis, - # while its command-correlated depth bias can tilt the otherwise clean - # centre-trajectory plane by several degrees. Use the orientation - # axis to constrain that circle; PIP retains its independently fitted - # axis line and all final SE(3)/holdout quality gates. - | set(RIGHT_19_END_ON_IMAGE_CURVE_JOINTS) - # _fit_axis_measurement_raw constrains every passive DIP direction to - # its source-URDF-parallel upstream PIP. Declare the same fact here - # so validation does not compare that trusted direction with the - # depth-biased free plane of a distal Tag trajectory. - | set(RIGHT_19_VISUALLY_MEASURED_PASSIVE_DIPS) - ) - return ZeroCalibrationProfile( - hand=hand, - direct_zero_joints=tuple(direct), - axis_joints=tuple(axis_joints), - inherited_zero_joints={}, - inherited_static_zero_joints={}, - constrained_circle_joints=constrained, - root_anchor_joints=root_anchors, - axis_parent_joint=axis_parent, - phase_parent_joint=phase_parent, - offset_observer_joint=observer, - same_view_axis_pair_by_offset={ - "thumb_cmc_yaw": ("thumb_cmc_roll", "thumb_cmc_pitch") - }, - # Endpoint-observable offsets are supplied by the caller. CMC remains - # visually solved and no serial-specific zero is hidden in the shared - # profile. - fixed_direct_zero_offsets_rad={}, - static_output_zero_offsets_rad={}, - ) - - -def get_right_19_thumb_zero_profile() -> ZeroCalibrationProfile: - """Return the independent four-zero thumb observer graph. - - Roll and MCP are supplied as mechanical endpoint datums by the caller; - yaw is observed by the same-view roll/pitch axis pair and pitch by the - pitch/MCP serial phase. No finger axis is part of this profile. - """ - full = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) - thumb_direct = tuple( - name for name in full.direct_zero_joints if name.startswith("thumb_") - ) - thumb_axes = tuple( - name for name in full.axis_joints if name.startswith("thumb_") - ) - return replace( - full, - direct_zero_joints=thumb_direct, - axis_joints=thumb_axes, - constrained_circle_joints=frozenset( - name - for name in full.constrained_circle_joints - if name.startswith("thumb_") - ), - root_anchor_joints=frozenset({"thumb_cmc_roll"}), - axis_parent_joint={ - child: parent - for child, parent in full.axis_parent_joint.items() - if child.startswith("thumb_") and parent.startswith("thumb_") - }, - phase_parent_joint={ - child: parent - for child, parent in full.phase_parent_joint.items() - if child.startswith("thumb_") and parent.startswith("thumb_") - }, - offset_observer_joint={ - target: observer - for target, observer in full.offset_observer_joint.items() - if target.startswith("thumb_") and observer.startswith("thumb_") - }, - same_view_axis_pair_by_offset={ - target: pair - for target, pair in full.same_view_axis_pair_by_offset.items() - if target.startswith("thumb_") - }, - fixed_direct_zero_offsets_rad={}, - static_output_zero_offsets_rad={}, - base_pose_strategy="thumb_serial", - ) - - -RIGHT_19_MECHANICAL_ENDPOINT_JOINTS = frozenset( - { - "thumb_mcp", - *( - f"{finger}_{suffix}" - for finger in ("index", "middle", "ring", "pinky") - for suffix in ("mcp_pitch", "pip") - ), - } -) - -# Thumb CMC roll has a directly observed actuator-to-actuator travel, but its -# electrical stop is not used as a runtime URDF limit. Keep it out of -# RIGHT_19_MECHANICAL_ENDPOINT_JOINTS (whose offsets also move upper limits), -# and use its endpoint estimate only as the final static roll origin. This -# makes roll independent of the fitted palm-frame phase and of all yaw logic. -RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS = frozenset({"thumb_cmc_roll"}) -RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS = ( - RIGHT_19_MECHANICAL_ENDPOINT_JOINTS - | RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS -) -RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS = RIGHT_19_ROLL_ENDPOINT_ZERO_JOINTS - - -def anchor_right_19_mechanical_endpoint_curves( - curves: Mapping[str, JointCurveFit], - records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], - *, - maximum_direction_difference_rad: float = math.radians(1.0), - maximum_curve_correction_rad: float = math.radians(3.0), - feedback_endpoint_joints: frozenset[str] = frozenset(), - endpoint_joints: frozenset[str] | None = None, -) -> dict[str, JointCurveFit]: - """Set mechanical-endpoint curve scale from direct SO(3) travel. - - A fitted-axis projection is useful for a dense, signed command curve, but - a small view-dependent orthogonal PnP component can change that - projection's full-scale value. The rotation magnitude between the two - settled endpoint poses is the revolute-joint travel itself: conjugating - both poses by a rigid hand/camera transform or either fixed Tag mounting - rotation cannot change it. - - Use every supplied cycle and both sweep directions as independent endpoint - measurements, reject disagreement, and apply their robust median as one - scale correction to the already validated curve. Dense CMC-roll records - are selected in feedback coordinates; settled mechanical-contact records - remain selected in requested-command coordinates. - """ - if ( - not math.isfinite(maximum_direction_difference_rad) - or maximum_direction_difference_rad <= 0.0 - ): - raise ValueError("endpoint direction difference limit must be positive") - if ( - not math.isfinite(maximum_curve_correction_rad) - or maximum_curve_correction_rad <= 0.0 - ): - raise ValueError("endpoint curve correction limit must be positive") - - result = dict(curves) - selected_endpoint_joints = ( - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - if endpoint_joints is None - else frozenset(str(name) for name in endpoint_joints) - ) - unknown_endpoint_joints = sorted( - set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) - ) - if unknown_endpoint_joints: - raise ValueError( - "endpoint joints are not endpoint measurements: " - + ",".join(unknown_endpoint_joints) - ) - unknown_feedback_joints = sorted( - set(feedback_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) - ) - if unknown_feedback_joints: - raise ValueError( - "feedback endpoint joints are not endpoint measurements: " - + ",".join(unknown_feedback_joints) - ) - - for name in sorted(selected_endpoint_joints): - fit = curves.get(name) - records = list(records_by_joint.get(name, ())) - if fit is None: - raise ValueError(f"missing measured endpoint curve for {name}") - - direction_travel: dict[str, float] = {} - all_travel: list[float] = [] - for direction in ("decreasing", "increasing"): - cycles = sorted( - { - int(record.get("cycle", 0)) - for record in records - if str(record.get("direction")) == direction - } - ) - branch_travel: list[float] = [] - for cycle in cycles: - endpoint_rotations: dict[int, Rotation] = {} - for command in (0, 255): - quaternions = [] - for record in records: - if ( - str(record.get("direction")) != direction - or int(record.get("cycle", 0)) != cycle - ): - continue - endpoint_value = ( - explicit_domain_value(record, "feedback") - if name in feedback_endpoint_joints - else record.get( - "requested_command_u8", - record.get("command_u8", -1), - ) - ) - if endpoint_value is None: - continue - if int(round(float(endpoint_value))) == command: - quaternions.append(_relative_rotation(record)) - if not quaternions: - raise ValueError( - f"{name} {direction} cycle {cycle} is missing " - f"settled endpoint {command}" - ) - endpoint_rotations[command] = Rotation.from_quat( - robust_rotation_summary(quaternions)[0] - ) - branch_travel.append( - float( - ( - endpoint_rotations[255].inv() - * endpoint_rotations[0] - ).magnitude() - ) - ) - if not branch_travel: - raise ValueError(f"{name} is missing {direction} endpoint travel") - direction_travel[direction] = float(np.median(branch_travel)) - all_travel.extend(branch_travel) - - travel_values = np.asarray(list(direction_travel.values()), dtype=float) - direction_difference = float(np.ptp(travel_values)) - if direction_difference > maximum_direction_difference_rad: - raise ValueError( - f"{name} settled endpoint directions disagree: " - f"{math.degrees(direction_difference):.3f}deg" - ) - cycle_range = float(np.ptp(np.asarray(all_travel, dtype=float))) - if cycle_range > maximum_direction_difference_rad: - raise ValueError( - f"{name} settled endpoint cycles disagree: " - f"{math.degrees(cycle_range):.3f}deg" - ) - direct_travel = float(np.median(np.asarray(all_travel, dtype=float))) - curve = np.asarray(fit.angle_rad, dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"{name} endpoint curve must contain 256 finite bins") - projected_travel = float(curve[0] - curve[255]) - if direct_travel <= 0.0 or projected_travel <= 0.0: - raise ValueError(f"{name} endpoint travel must be positive") - correction = abs(projected_travel - direct_travel) - if correction > maximum_curve_correction_rad: - raise ValueError( - f"{name} fitted/direct endpoint travel differs by " - f"{math.degrees(correction):.3f}deg" - ) - scale = direct_travel / projected_travel - - def scaled(values: Sequence[float]) -> tuple[float, ...]: - return tuple(float(value) * scale for value in values) - - circle = dict(fit.circle) - circle.update( - { - "mechanical_endpoint_direct_travel_rad": direct_travel, - "mechanical_endpoint_direction_difference_rad": ( - direction_difference - ), - "mechanical_endpoint_cycle_range_rad": cycle_range, - "mechanical_endpoint_sample_count": len(all_travel), - "mechanical_endpoint_raw_curve_travel_rad": projected_travel, - "mechanical_endpoint_curve_scale": scale, - } - ) - quality = dict(fit.quality) - if "arc_rad" in quality: - quality["arc_rad"] = float(quality["arc_rad"]) * scale - result[name] = replace( - fit, - angle_rad=scaled(fit.angle_rad), - decreasing_rad=scaled(fit.decreasing_rad), - increasing_rad=scaled(fit.increasing_rad), - circle=circle, - maximum_monotonic_correction_rad=( - float(fit.maximum_monotonic_correction_rad) * scale - ), - maximum_hysteresis_rad=float(fit.maximum_hysteresis_rad) * scale, - quality=quality, - zero_offset_rad=float(fit.zero_offset_rad) * scale, - ) - return result - - -def derive_right_19_mechanical_endpoint_offsets( - source_urdf: str | Path, - curves: Mapping[str, JointCurveFit], - *, - maximum_offset_rad: float = math.radians(5.0), - endpoint_joints: frozenset[str] | None = None, -) -> dict[str, float]: - """Estimate encoder origins from repeatable measured mechanical endpoints. - - For the four fingers feedback 0 is the independently verified palm-contact - endpoint; thumb MCP uses its independently verified actuator endpoint. - The source URDF upper limit describes those same physical endpoints. - Therefore ``origin_offset + measured_travel == CAD_upper``. - - Thumb CMC pitch/yaw remain excluded because their endpoint-to-CAD contract - is not independently established. CMC roll is included as a post-solve - origin only: its measured travel determines roll without allowing the - palm common-direction fit to write that joint. - - The returned corrections are measured, generally non-zero encoder zeros; - they do not retain the CAD origin. Using full relative rotation travel - makes them invariant to rigid hand movement and arbitrary fixed Tag - mounting rotation, unlike a cross-camera absolute planar-PnP phase. - """ - if not math.isfinite(maximum_offset_rad) or maximum_offset_rad <= 0.0: - raise ValueError("mechanical endpoint maximum offset must be positive") - root = ET.parse(Path(source_urdf).expanduser().resolve()).getroot() - joints = {str(node.get("name")): node for node in root.findall("joint")} - result: dict[str, float] = {} - selected_endpoint_joints = ( - RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS - if endpoint_joints is None - else frozenset(str(name) for name in endpoint_joints) - ) - unknown_endpoint_joints = sorted( - set(selected_endpoint_joints) - set(RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS) - ) - if unknown_endpoint_joints: - raise ValueError( - "endpoint joints are not endpoint measurements: " - + ",".join(unknown_endpoint_joints) - ) - for name in sorted(selected_endpoint_joints): - fit = curves.get(name) - joint = joints.get(name) - limit = None if joint is None else joint.find("limit") - if fit is None: - raise ValueError(f"missing measured endpoint curve for {name}") - if limit is None or limit.get("upper") is None: - raise ValueError(f"source URDF joint {name} has no upper limit") - upper = float(limit.get("upper")) - curve = np.asarray(fit.angle_rad, dtype=float) - if curve.shape != (256,) or not np.all(np.isfinite(curve)): - raise ValueError(f"{name} endpoint curve must contain 256 finite bins") - measured_endpoint = float(curve[0]) - if measured_endpoint <= 0.0 or abs(float(curve[255])) > math.radians(0.05): - raise ValueError( - f"{name} endpoint curve does not use command 255 as its zero reference" - ) - if abs(measured_endpoint - float(np.max(curve))) > math.radians(0.05): - raise ValueError( - f"{name} command 0 is not the measured upper mechanical endpoint" - ) - offset = upper - measured_endpoint - if not math.isfinite(offset) or abs(offset) > maximum_offset_rad: - raise ValueError( - f"{name} endpoint-derived zero offset is outside the safe range: " - f"{math.degrees(offset):.3f}deg" - ) - result[name] = offset - return result - - -def get_zero_calibration_profile( - side: str, layout_id: str = "legacy_11" -) -> ZeroCalibrationProfile: - return _build_zero_profile(get_hand_calibration_profile(side, layout_id)) - - -LEFT_ZERO_PROFILE = _build_zero_profile(LEFT_HAND_PROFILE) - -# Backwards-compatible left-hand aliases. -CONSTRAINED_CIRCLE_JOINTS = LEFT_ZERO_PROFILE.constrained_circle_joints -DIRECT_ZERO_JOINTS = LEFT_ZERO_PROFILE.direct_zero_joints -FIXED_DIRECT_ZERO_OFFSETS_RAD = dict( - LEFT_ZERO_PROFILE.fixed_direct_zero_offsets_rad -) -OPTIMIZED_ZERO_JOINTS = tuple( - name for name in DIRECT_ZERO_JOINTS - if name not in FIXED_DIRECT_ZERO_OFFSETS_RAD -) -AXIS_JOINTS = LEFT_ZERO_PROFILE.axis_joints -INHERITED_ZERO_JOINTS = dict(LEFT_ZERO_PROFILE.inherited_zero_joints) - - -def circle_direction_is_constrained( - joint_name: str, constrained_circle_joints: frozenset[str] -) -> bool: - """Match the constraint used by both canonical and side aliases.""" - name = str(joint_name) - return name in constrained_circle_joints or name.endswith("_side") - - -def select_cross_view_roll_direction_source( - primary_cone_residuals_rad: Sequence[float], - secondary_cone_residuals_rad: Sequence[float], - maximum_cone_mismatch_rad: float, -) -> str: - """Choose one roll-axis view for the complete repeated-sweep group. - - The choice must not be made independently for every cycle. A residual - sitting just either side of the cone gate can otherwise alternate the - selected camera and turn a fixed cross-view bias into a false cycle-axis - spread. The secondary view wins only with a three-quarter consensus and - a median residual inside the unchanged geometry gate; ambiguous groups - retain the primary view. - """ - primary = np.asarray(primary_cone_residuals_rad, dtype=float) - secondary = np.asarray(secondary_cone_residuals_rad, dtype=float) - if ( - primary.ndim != 1 - or secondary.ndim != 1 - or len(primary) != len(secondary) - or len(primary) < 3 - or not np.all(np.isfinite(primary)) - or not np.all(np.isfinite(secondary)) - or float(maximum_cone_mismatch_rad) <= 0.0 - ): - return "primary" - limit = float(maximum_cone_mismatch_rad) - required_consensus = int(math.ceil(0.75 * len(primary))) - primary_passes = int(np.count_nonzero(primary <= limit)) - secondary_passes = int(np.count_nonzero(secondary <= limit)) - if ( - secondary_passes >= required_consensus - and primary_passes < required_consensus - and float(np.median(secondary)) <= limit - and float(np.median(primary)) > limit - ): - return "secondary" - return "primary" - - -ZERO_REFERENCE_MAXIMUM_DISTANCE_U8 = 16 - -# Fit an axis-line point from the complete relative SE(3) trajectory instead -# of using only the moving Tag centre's free 3-D circle. For an approximately -# end-on view, planar-PnP optical depth is deliberately projected out: image -# x/y still observes the radial motion that determines an axis line, while -# the independent Tag depths are the dominant source of false zero phase. -AXIS_POINT_MINIMUM_ROTATION_RAD = math.radians(3.0) -AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD = math.radians(45.0) -AXIS_POINT_RESIDUAL_SCALE_M = 0.001 - -# A joint's own screw axis is invariant to its own encoder-zero offset. Only -# the downstream axis selected by the kinematic chain can observe that zero. -# Feeding every axis-direction and axis-line residual into the optimizer lets -# fixed CAD/PnP geometry residuals push unrelated zero offsets to their bounds. -# Keep the two root lines as the palm-pose anchor, then use only the component -# that actually observes the preceding zero in each serial chain. -ZERO_AXIS_PARENT_JOINT = dict(LEFT_ZERO_PROFILE.axis_parent_joint) -ZERO_AXIS_OBSERVATION_JOINTS = frozenset( - set(LEFT_ZERO_PROFILE.root_anchor_joints) | set(ZERO_AXIS_PARENT_JOINT) -) -ZERO_LINE_OBSERVATION_JOINTS = frozenset( - set(LEFT_ZERO_PROFILE.root_anchor_joints) - | set(LEFT_ZERO_PROFILE.phase_parent_joint) -) -ZERO_ROOT_ANCHOR_JOINTS = LEFT_ZERO_PROFILE.root_anchor_joints -ZERO_MINIMUM_AXIS_CONE_RAD = math.radians(15.0) - - -def _zero_sensitive_axis_error_rad( - predicted_axis: Sequence[float], - observed_axis: Sequence[float], - parent_axis: Sequence[float], -) -> float: - """Return only the axis error that a parent-joint zero can change. - - Rotating a downstream axis about its parent preserves their mutual cone - angle. The component normal to their plane is therefore the observable - encoder-zero error; cone-angle mismatch belongs to fixed geometry/PnP and - must not push a zero offset or fail its holdout validation. - """ - predicted = _vector(predicted_axis, 3, name="predicted axis") - predicted /= np.linalg.norm(predicted) - observed = _vector(observed_axis, 3, name="observed axis") - observed /= np.linalg.norm(observed) - parent = _vector(parent_axis, 3, name="parent axis") - parent /= np.linalg.norm(parent) - if float(predicted @ observed) < 0.0: - observed = -observed - predicted_projected = predicted - parent * float(predicted @ parent) - observed_projected = observed - parent * float(observed @ parent) - predicted_norm = float(np.linalg.norm(predicted_projected)) - observed_norm = float(np.linalg.norm(observed_projected)) - minimum_projection = math.sin(ZERO_MINIMUM_AXIS_CONE_RAD) - if min(predicted_norm, observed_norm) < minimum_projection: - raise ValueError( - "parent and downstream axes have an unobservable cone angle" - ) - predicted_projected /= predicted_norm - observed_projected /= observed_norm - return math.atan2( - float(parent @ np.cross(predicted_projected, observed_projected)), - float( - np.clip(predicted_projected @ observed_projected, -1.0, 1.0) - ), - ) - - -def _axis_cone_mismatch_rad( - predicted_axis: Sequence[float], - observed_axis: Sequence[float], - parent_axis: Sequence[float], -) -> float: - """Return zero-invariant parent/downstream cone-angle disagreement.""" - predicted = _vector(predicted_axis, 3, name="predicted axis") - predicted /= np.linalg.norm(predicted) - observed = _vector(observed_axis, 3, name="observed axis") - observed /= np.linalg.norm(observed) - parent = _vector(parent_axis, 3, name="parent axis") - parent /= np.linalg.norm(parent) - predicted_cone = math.acos( - abs(float(np.clip(parent @ predicted, -1.0, 1.0))) - ) - observed_cone = math.acos( - abs(float(np.clip(parent @ observed, -1.0, 1.0))) - ) - return abs(predicted_cone - observed_cone) - - -def _vector(value: Sequence[float], size: int, *, name: str) -> np.ndarray: - result = np.asarray(value, dtype=float) - if result.shape != (size,) or not np.all(np.isfinite(result)): - raise ValueError(f"{name} must contain {size} finite values") - return result - - -def _pose_matrix(payload: Mapping[str, Any]) -> np.ndarray: - translation = _vector(payload["translation_xyz_m"], 3, name="translation") - quaternion = _vector(payload["quaternion_xyzw"], 4, name="quaternion") - quaternion /= np.linalg.norm(quaternion) - result = np.eye(4) - result[:3, :3] = Rotation.from_quat(quaternion).as_matrix() - result[:3, 3] = translation - return result - - -def _relative_rotation(record: Mapping[str, Any]) -> np.ndarray: - quaternion = _vector( - record["relative_quaternion_xyzw"], 4, name="relative quaternion" - ) - return quaternion / np.linalg.norm(quaternion) - - -def _reference_group_key(record: Mapping[str, Any]) -> tuple[Any, Any]: - return record.get("cycle"), record.get("direction") - - -def _canonical_reference_records( - records: Sequence[Mapping[str, Any]], - canonical_zero_direction: str | None, -) -> list[Mapping[str, Any]]: - """Select the sole physical-zero branch when one is configured.""" - if canonical_zero_direction is None: - return list(records) - if canonical_zero_direction not in {"decreasing", "increasing"}: - raise ValueError("canonical zero direction is invalid") - selected = [ - record - for record in records - if str(record.get("direction")) == canonical_zero_direction - ] - if not selected: - raise ValueError( - f"canonical {canonical_zero_direction} zero branch has no samples" - ) - return selected - - -def _interpolate_reference_rotation( - records: Sequence[Mapping[str, Any]], zero_command_u8: int -) -> Rotation | None: - by_command: dict[int, list[np.ndarray]] = {} - for record in records: - command = int(record["command_u8"]) - by_command.setdefault(command, []).append(_relative_rotation(record)) - if not by_command: - return None - - rotations = { - command: Rotation.from_quat(robust_rotation_summary(values)[0]) - for command, values in by_command.items() - } - zero = int(zero_command_u8) - if zero in rotations: - return rotations[zero] - - lower = [command for command in rotations if command < zero] - upper = [command for command in rotations if command > zero] - lower_command = max(lower) if lower else None - upper_command = min(upper) if upper else None - if lower_command is not None and upper_command is not None: - lower_distance = zero - lower_command - upper_distance = upper_command - zero - if max(lower_distance, upper_distance) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: - lower_rotation = rotations[lower_command] - upper_rotation = rotations[upper_command] - fraction = lower_distance / (upper_command - lower_command) - delta = (lower_rotation.inv() * upper_rotation).as_rotvec() - return lower_rotation * Rotation.from_rotvec(delta * fraction) - - nearest_command = min(rotations, key=lambda command: abs(command - zero)) - if abs(nearest_command - zero) <= ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: - return rotations[nearest_command] - return None - - -def _near_zero_records( - records: Sequence[Mapping[str, Any]], zero_command_u8: int -) -> list[Mapping[str, Any]]: - groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} - for record in records: - groups.setdefault(_reference_group_key(record), []).append(record) - selected: list[Mapping[str, Any]] = [] - zero = int(zero_command_u8) - for group in groups.values(): - distance = min(abs(int(record["command_u8"]) - zero) for record in group) - if distance > ZERO_REFERENCE_MAXIMUM_DISTANCE_U8: - continue - selected.extend( - record - for record in group - if abs(int(record["command_u8"]) - zero) == distance - ) - return selected - - -def _baseline_reference( - records: Sequence[Mapping[str, Any]], zero_command_u8: int -) -> tuple[float, float, float, float]: - groups: dict[tuple[Any, Any], list[Mapping[str, Any]]] = {} - for record in records: - groups.setdefault(_reference_group_key(record), []).append(record) - values = [ - rotation.as_quat() - for group in groups.values() - if ( - rotation := _interpolate_reference_rotation( - group, zero_command_u8 - ) - ) - is not None - ] - if not values: - raise ValueError( - "joint records have no samples within " - f"{ZERO_REFERENCE_MAXIMUM_DISTANCE_U8} commands of zero " - f"{zero_command_u8}" - ) - return robust_rotation_summary(values)[0] - - -def baseline_hysteresis_by_cycle_rad( - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, - axis_xyz: Sequence[float] | None = None, -) -> tuple[float, ...]: - """Return decreasing/increasing joint-angle disagreement per cycle. - - Full SO(3) disagreement includes planar-PnP tilt noise that is orthogonal - to the fitted revolute axis. When an axis is supplied, report only the - physically meaningful component about that axis; the orthogonal component - remains covered by the independent rotation-model residual gate. - """ - samples = [dict(record) for record in records] - axis: np.ndarray | None = None - if axis_xyz is not None: - axis = _vector(axis_xyz, 3, name="baseline hysteresis axis") - norm = float(np.linalg.norm(axis)) - if norm <= 0.0: - raise ValueError("baseline hysteresis axis must be non-zero") - axis /= norm - result: list[float] = [] - for cycle in sorted({int(record["cycle"]) for record in samples}): - rotations: dict[str, Rotation] = {} - for direction in ("decreasing", "increasing"): - selected = [ - record - for record in samples - if int(record["cycle"]) == cycle - and str(record["direction"]) == direction - ] - reference = _interpolate_reference_rotation( - selected, zero_command_u8 - ) - if reference is None: - raise ValueError( - f"cycle {cycle} {direction} is missing baseline samples" - ) - rotations[direction] = reference - delta = ( - rotations["decreasing"].inv() - * rotations["increasing"] - ) - result.append( - float( - delta.magnitude() - if axis is None - else abs(float(delta.as_rotvec() @ axis)) - ) - ) - if not result: - raise ValueError("baseline hysteresis requires at least one cycle") - return tuple(result) - - -def fit_rotation_joint_curve( - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, - canonical_zero_direction: str | None = None, -) -> JointCurveFit: - """Fit a direction-aware curve from parent-to-child Tag orientations. - - With ``canonical_zero_direction`` both branches share one physical - reference. Only the canonical branch is zero at ``zero_command_u8``; - the other branch retains its measured backlash/compliance offset. - """ - samples = [dict(record) for record in records] - if len(samples) < 12: - raise ValueError("rotation trajectory requires at least 12 samples") - vectors: list[np.ndarray] = [] - commands: list[int] = [] - values_by_record: list[float] = [] - references: dict[int, Rotation] = {} - for cycle in sorted({int(record["cycle"]) for record in samples}): - cycle_records = [ - record for record in samples if int(record["cycle"]) == cycle - ] - references[cycle] = Rotation.from_quat( - _baseline_reference( - _canonical_reference_records( - cycle_records, canonical_zero_direction - ), - zero_command_u8, - ) - ) - for record in samples: - observed = Rotation.from_quat(_relative_rotation(record)) - vector = ( - references[int(record["cycle"])].inv() * observed - ).as_rotvec() - vectors.append(vector) - commands.append(int(record["command_u8"])) - axis = fit_rotation_axis(vectors, commands) - values_by_record = [float(vector @ axis) for vector in vectors] - curves, correction, hysteresis = _fit_joint_curve( - samples, - values_by_record, - preserve_direction_offset=canonical_zero_direction is not None, - ) - if canonical_zero_direction is None: - for key in ("angle_rad", "decreasing_rad", "increasing_rad"): - values = np.asarray(curves[key], dtype=float) - values -= float(values[int(zero_command_u8)]) - curves[key] = [round(float(value), 8) for value in values] - else: - canonical_key = f"{canonical_zero_direction}_rad" - shared_zero = float(curves[canonical_key][int(zero_command_u8)]) - for key in ("decreasing_rad", "increasing_rad"): - values = np.asarray(curves[key], dtype=float) - shared_zero - curves[key] = [round(float(value), 8) for value in values] - # Before the bridge has observed motion direction it must use the - # same branch that defines the URDF physical zero, never an average - # pose that the mechanism may not be able to occupy. - curves["angle_rad"] = list(curves[canonical_key]) - orthogonal = [ - float(np.linalg.norm(vector - float(vector @ axis) * axis)) - for vector in vectors - ] - return JointCurveFit( - angle_rad=tuple(float(value) for value in curves["angle_rad"]), - decreasing_rad=tuple(float(value) for value in curves["decreasing_rad"]), - increasing_rad=tuple(float(value) for value in curves["increasing_rad"]), - circle={ - "space": "relative_rotation_3d", - "axis_xyz": [float(value) for value in axis], - "zero_command_u8": int(zero_command_u8), - "reference_quaternion_xyzw": [ - float(value) - for value in _baseline_reference( - _canonical_reference_records( - samples, canonical_zero_direction - ), - zero_command_u8, - ) - ], - "canonical_zero_direction": canonical_zero_direction, - }, - maximum_monotonic_correction_rad=float(correction), - maximum_hysteresis_rad=float(hysteresis), - quality={ - "rotation_orthogonal_rms_rad": float( - np.sqrt(np.mean(np.square(orthogonal))) - ), - "arc_rad": float( - max(curves["angle_rad"]) - min(curves["angle_rad"]) - ), - }, - ) - - -def measure_rotation_joint_observation( - fit: JointCurveFit, quaternion_xyzw: Sequence[float] -) -> float: - """Measure one parent-to-child orientation with a fitted 3-D curve.""" - if fit.circle.get("space") != "relative_rotation_3d": - raise ValueError("joint fit is not a relative-rotation curve") - reference = Rotation.from_quat( - _vector( - fit.circle["reference_quaternion_xyzw"], - 4, - name="reference quaternion", - ) - ) - observed = Rotation.from_quat( - _vector(quaternion_xyzw, 4, name="observed quaternion") - ) - axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") - axis /= np.linalg.norm(axis) - return float((reference.inv() * observed).as_rotvec() @ axis) - - -def measure_joint_curve_observation( - fit: JointCurveFit, - *, - quaternion_xyzw: Sequence[float] | None = None, - image_relative_xy_px: Sequence[float] | None = None, -) -> float: - """Measure one observation in the same space as its fitted curve.""" - if fit.circle.get("space") == "relative_rotation_3d": - if quaternion_xyzw is None: - raise ValueError("rotation observation quaternion is missing") - return measure_rotation_joint_observation(fit, quaternion_xyzw) - if fit.circle.get("space") != "image_2d": - raise ValueError("unsupported joint curve observation representation") - if image_relative_xy_px is None: - raise ValueError("image curve observation point is missing") - point = np.asarray(image_relative_xy_px, dtype=float) - centre = np.asarray(fit.circle["center_xy_px"], dtype=float) - reference = np.asarray(fit.circle["reference_xy_px"], dtype=float) - vector = point - centre - if ( - point.shape != (2,) - or not np.all(np.isfinite(point)) - or float(np.linalg.norm(vector)) < 1.0e-9 - ): - raise ValueError("image curve observation point is invalid") - return float(fit.circle["orientation_sign"]) * math.atan2( - float(reference[0] * vector[1] - reference[1] * vector[0]), - float(reference @ vector), - ) - - -def rotation_curve_holdout_errors( - fit: JointCurveFit, - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, -) -> tuple[float, ...]: - """Validate a fitted curve on an untouched scan cycle.""" - samples = [dict(record) for record in records] - if not samples: - raise ValueError("holdout records are empty") - canonical_zero_direction = fit.circle.get("canonical_zero_direction") - reference = Rotation.from_quat( - _baseline_reference( - _canonical_reference_records(samples, canonical_zero_direction), - zero_command_u8, - ) - ) - axis = _vector(fit.circle["axis_xyz"], 3, name="rotation axis") - axis /= np.linalg.norm(axis) - errors: list[float] = [] - for record in samples: - observed = Rotation.from_quat(_relative_rotation(record)) - angle = float((reference.inv() * observed).as_rotvec() @ axis) - command = int(record["command_u8"]) - direction = str(record["direction"]) - expected_curve = ( - fit.decreasing_rad - if direction == "decreasing" - else fit.increasing_rad - ) - errors.append(angle - float(expected_curve[command])) - return tuple(errors) - - -def joint_curve_holdout_errors( - fit: JointCurveFit, - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, -) -> tuple[float, ...]: - """Validate either a rotation or fixed-parent image-circle curve.""" - if fit.circle.get("space") == "relative_rotation_3d": - return rotation_curve_holdout_errors( - fit, records, zero_command_u8=zero_command_u8 - ) - if fit.circle.get("space") != "image_2d": - raise ValueError("unsupported joint curve holdout representation") - if not records: - raise ValueError("holdout records are empty") - errors: list[float] = [] - for record in records: - observed = measure_joint_curve_observation( - fit, - image_relative_xy_px=record["image_relative_xy_px"], - ) - command = int(record["command_u8"]) - direction = str(record["direction"]) - expected_curve = ( - fit.decreasing_rad - if direction == "decreasing" - else fit.increasing_rad - ) - errors.append(observed - float(expected_curve[command])) - return tuple(errors) - - -@dataclass(frozen=True) -class JointAxisMeasurement: - joint: str - cycle: int - axis_common_xyz: tuple[float, float, float] - point_common_xyz_m: tuple[float, float, float] - condition_state_u8: tuple[float, ...] - plane_rms_m: float - radial_rms_m: float - rotation_circle_axis_difference_rad: float - # Optical-axis direction expressed in the shared calibration frame. It - # lets the phase solver discard the least reliable monocular-PnP depth - # component when the joint axis is viewed approximately end-on. Older - # recordings and synthetic callers may omit it and retain the 3-D path. - view_normal_common_xyz: tuple[float, float, float] | None = None - # Logical commands that produced the condition. Firmware feedback may - # saturate a few u8 short of an endpoint (for example right motor 10 reads - # 250 for command 255); kinematics must use the command-indexed curve while - # retaining condition_state_u8 for diagnostics and safety. - condition_command_u8: tuple[float, ...] | None = None - axis_direction_source: str = "unspecified" - circle_axis_observability: float = 0.0 - axis_point_source: str = "circle_center" - pose_axis_line_rms_m: float = 0.0 - # Measurement record(s) that supplied pose_axis_line_rms_m. A combined - # cross-view axis may keep the front direction but take its physical line - # point and line-quality residual from the side alias. Retry logic must - # clear the actual quality source instead of blindly rescanning ``joint``. - pose_axis_line_source_joints: tuple[str, ...] = () - # Camera centre that observed ``point_common_xyz_m``. When populated, - # the zero solver can use only the ray from this centre to the fitted axis - # point. That ray is the depth-free interpretation-plane observation of - # the physical axis; translating a monocular planar-PnP solution along - # its optical ray therefore cannot rotate the recovered palm frame. - axis_point_camera_center_common_xyz_m: ( - tuple[float, float, float] | None - ) = None - # Normal of the source camera's interpretation plane for this axis line. - # Unlike a 3-D PnP line point, the plane is unchanged by optical-depth - # error. Several named parallel root lines jointly recover their common - # physical direction as the null direction of these plane normals. - axis_point_interpretation_plane_normal_common_xyz: ( - tuple[float, float, float] | None - ) = None - - -@dataclass(frozen=True) -class PalmOrientationMeasurement: - """Direction-only joint observation from a partially visible sweep. - - The moving Tag may disappear before the motor reaches its far endpoint. - Only the relative SO(3) trajectory is retained, so fixed Tag translation - and mounting rotation cannot define the palm phase. - """ - - source_joint: str - model_joint: str - cycle: int - axis_common_xyz: tuple[float, float, float] - condition_state_u8: tuple[float, ...] - observed_arc_rad: float - rotation_orthogonal_rms_rad: float - axis_estimator: str = "baseline_relative_so3" - incremental_pair_count: int = 0 - - -PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 = 24 -PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD = math.radians(0.3) -PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT = 24 -PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE = 75.0 - - -def _incremental_common_rotation_axis( - samples_by_bin: Mapping[ - tuple[str, int], Sequence[Mapping[str, Any]] - ], - *, - command_separation_u8: int = ( - PALM_AXIS_INCREMENT_COMMAND_SEPARATION_U8 - ), - minimum_rotation_rad: float = ( - PALM_AXIS_INCREMENT_MINIMUM_ROTATION_RAD - ), - minimum_pair_count: int = PALM_AXIS_INCREMENT_MINIMUM_PAIR_COUNT, - consensus_percentile: float = ( - PALM_AXIS_INCREMENT_CONSENSUS_PERCENTILE - ), -) -> tuple[np.ndarray, int]: - """Fit one physical axis from robust local Tag rotations. - - A single baseline-to-endpoint logarithm is sensitive to smooth planar-PnP - curvature: the fitted axis then changes when the same Tag trajectory is - translated to another part of the image. Local finite rotations are - expressed directly in the common camera frame, and their undirected - weighted consensus estimates the physical revolute axis. The worst - quartile is discarded once more after the initial consensus, which is - enough to reject endpoint branch curvature without learning a - serial-specific yaw value or an image-position correction table. - """ - separation = int(command_separation_u8) - if separation < 4 or separation > 64: - raise ValueError("palm axis command separation must be in [4, 64]") - if not 0.0 < float(minimum_rotation_rad) < math.pi: - raise ValueError("palm axis minimum increment must be in (0, pi)") - if int(minimum_pair_count) < 6: - raise ValueError("palm axis minimum pair count must be at least six") - percentile = float(consensus_percentile) - if not 50.0 <= percentile <= 90.0: - raise ValueError("palm axis consensus percentile must be in [50, 90]") - - binned_common_rotations: dict[tuple[str, int], Rotation] = {} - for key, group in samples_by_bin.items(): - child_quaternions = [] - for record in group: - pose = record.get("child_pose_common") - if not isinstance(pose, Mapping): - continue - quaternion = pose.get("quaternion_xyzw") - if quaternion is not None: - child_quaternions.append(quaternion) - if not child_quaternions: - continue - binned_common_rotations[(str(key[0]), int(key[1]))] = ( - Rotation.from_quat( - robust_rotation_summary(child_quaternions)[0] - ) - ) - - directions: list[np.ndarray] = [] - weights: list[float] = [] - for direction in ("decreasing", "increasing"): - commands = sorted( - command - for candidate_direction, command in binned_common_rotations - if candidate_direction == direction - ) - for start_command in commands: - candidates = [ - command - for command in commands - if separation <= command - start_command <= separation + 2 - ] - if not candidates: - continue - end_command = candidates[0] - start = binned_common_rotations[(direction, start_command)] - end = binned_common_rotations[(direction, end_command)] - local_rotvec = (start.inv() * end).as_rotvec() - common_rotvec = start.apply(local_rotvec) - increment = float(np.linalg.norm(common_rotvec)) - if increment < float(minimum_rotation_rad): - continue - directions.append(common_rotvec / increment) - weights.append(increment) - - if len(directions) < int(minimum_pair_count): - raise ValueError( - "palm axis has too few full-stroke local rotation pairs: " - f"{len(directions)}/{int(minimum_pair_count)}" - ) - vectors = np.asarray(directions, dtype=float) - increments = np.asarray(weights, dtype=float) - - def consensus(selected: np.ndarray) -> np.ndarray: - scatter = np.einsum( - "n,ni,nj->ij", - increments[selected], - vectors[selected], - vectors[selected], - ) - eigenvalues, eigenvectors = np.linalg.eigh(scatter) - if not np.all(np.isfinite(eigenvalues)): - raise ValueError("palm axis local-rotation consensus is invalid") - axis = eigenvectors[:, -1] - return axis / np.linalg.norm(axis) - - selected = np.ones(len(vectors), dtype=bool) - axis = consensus(selected) - # Two deterministic refinement rounds prevent the initial scatter from - # being pulled toward a dense endpoint-bias cluster. - for _ in range(2): - deviations = np.arccos( - np.clip(np.abs(vectors @ axis), -1.0, 1.0) - ) - limit = float(np.percentile(deviations, percentile)) - selected = deviations <= limit - if int(np.count_nonzero(selected)) < int(minimum_pair_count): - raise ValueError("palm axis robust consensus retained too few pairs") - axis = consensus(selected) - return axis, int(np.count_nonzero(selected)) - - -def fit_partial_palm_orientation_measurement( - source_joint: str, - model_joint: str, - records: Sequence[Mapping[str, Any]], - *, - cycle: int, - zero_command_u8: int, - minimum_arc_rad: float = math.radians(15.0), - maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), - maximum_command_distance_u8: int = 255, -) -> PalmOrientationMeasurement: - """Fit a physical axis within a configured window around encoder zero.""" - cycle_samples = [ - dict(record) - for record in records - if int(record.get("cycle", -1)) == int(cycle) - ] - if len(cycle_samples) < 12: - raise ValueError( - f"{source_joint} cycle {cycle + 1} has too few visible samples" - ) - if not 0.0 < float(minimum_arc_rad) < math.pi: - raise ValueError("palm orientation minimum arc must be in (0, pi)") - if float(maximum_rotation_orthogonal_rms_rad) <= 0.0: - raise ValueError( - "palm orientation rotation residual limit must be positive" - ) - command_distance = int(maximum_command_distance_u8) - if command_distance < 1 or command_distance > 255: - raise ValueError( - "palm orientation command distance must be in [1, 255]" - ) - zero = int(zero_command_u8) - samples = [ - record - for record in cycle_samples - if abs(int(record["command_u8"]) - zero) <= command_distance - ] - if len(samples) < 12: - raise ValueError( - f"{source_joint} cycle {cycle + 1} has too few zero-adjacent " - "visible samples" - ) - reference = Rotation.from_quat( - _baseline_reference(samples, int(zero_command_u8)) - ) - samples_by_bin: dict[tuple[str, int], list[Mapping[str, Any]]] = {} - for record in samples: - key = ( - str(record.get("direction", "")), - int(record["command_u8"]), - ) - samples_by_bin.setdefault(key, []).append(record) - if len(samples_by_bin) < 6: - raise ValueError( - f"{source_joint} cycle {cycle + 1} has too few visible command bins" - ) - binned_rotations = [ - Rotation.from_quat( - robust_rotation_summary( - [_relative_rotation(record) for record in group] - )[0] - ) - for group in samples_by_bin.values() - ] - vectors = [ - (reference.inv() * rotation).as_rotvec() - for rotation in binned_rotations - ] - axis_child = fit_rotation_axis( - vectors, [command for _direction, command in samples_by_bin] - ) - angles = np.asarray( - [float(vector @ axis_child) for vector in vectors], dtype=float - ) - observed_arc = float(np.ptp(angles)) - orthogonal_rms = float( - np.sqrt( - np.mean( - [ - np.linalg.norm( - vector - float(vector @ axis_child) * axis_child - ) - ** 2 - for vector in vectors - ] - ) - ) - ) - if observed_arc < float(minimum_arc_rad): - raise ValueError( - f"{source_joint} cycle {cycle + 1} visible rotation arc " - f"{math.degrees(observed_arc):.3f}deg is below " - f"{math.degrees(minimum_arc_rad):.3f}deg" - ) - if orthogonal_rms > float(maximum_rotation_orthogonal_rms_rad): - raise ValueError( - f"{source_joint} cycle {cycle + 1} zero-adjacent rotation " - f"residual {math.degrees(orthogonal_rms):.3f}deg exceeds " - f"{math.degrees(maximum_rotation_orthogonal_rms_rad):.3f}deg" - ) - zero_records = _near_zero_records(samples, int(zero_command_u8)) - if not zero_records: - raise ValueError( - f"{source_joint} cycle {cycle + 1} has no visible zero pose" - ) - parent_poses = np.asarray( - [_pose_matrix(record["parent_pose_common"]) for record in zero_records] - ) - parent_quaternion = robust_rotation_summary( - [ - Rotation.from_matrix(matrix[:3, :3]).as_quat() - for matrix in parent_poses - ] - )[0] - axis_parent = reference.apply(axis_child) - axis_common = Rotation.from_quat(parent_quaternion).apply(axis_parent) - axis_common /= np.linalg.norm(axis_common) - axis_estimator = "baseline_relative_so3" - incremental_pair_count = 0 - # Product recordings persist the selected moving-Tag pose in the common - # camera frame. Use the complete visible stroke only for a local-motion - # axis consensus; retain the zero-adjacent fit above as the unchanged arc - # and residual quality gate. Legacy/synthetic records without the common - # child pose keep their previous estimator exactly. - if any( - isinstance(record.get("child_pose_common"), Mapping) - for record in cycle_samples - ): - full_stroke_bins: dict[ - tuple[str, int], list[Mapping[str, Any]] - ] = {} - for record in cycle_samples: - key = ( - str(record.get("direction", "")), - int(record["command_u8"]), - ) - full_stroke_bins.setdefault(key, []).append(record) - incremental_axis, incremental_pair_count = ( - _incremental_common_rotation_axis(full_stroke_bins) - ) - if float(incremental_axis @ axis_common) < 0.0: - incremental_axis = -incremental_axis - axis_common = incremental_axis - axis_estimator = "robust_full_stroke_local_so3_v1" - state = np.median( - np.asarray( - [record["state_u8"] for record in zero_records], dtype=float - ), - axis=0, - ) - return PalmOrientationMeasurement( - source_joint=str(source_joint), - model_joint=str(model_joint), - cycle=int(cycle), - axis_common_xyz=tuple(float(value) for value in axis_common), - condition_state_u8=tuple(float(value) for value in state), - observed_arc_rad=observed_arc, - rotation_orthogonal_rms_rad=orthogonal_rms, - axis_estimator=axis_estimator, - incremental_pair_count=incremental_pair_count, - ) - - -def fit_partial_palm_orientation_measurements( - *, - sources: Mapping[str, str], - records_by_joint: Mapping[str, Sequence[Mapping[str, Any]]], - motor_by_source: Mapping[str, int], - baseline_command_u8: Sequence[int], - cycles: Sequence[int], - minimum_sources: int, - minimum_arc_rad: float = math.radians(15.0), - maximum_rotation_orthogonal_rms_rad: float = math.radians(2.5), - maximum_command_distance_u8: int = 255, -) -> tuple[tuple[PalmOrientationMeasurement, ...], Mapping[str, str]]: - """Fit every usable optional source and require configured coverage.""" - source_map = {str(name): str(model) for name, model in sources.items()} - minimum = int(minimum_sources) - if not source_map: - if minimum != 0: - raise ValueError( - "palm orientation minimum is non-zero without sources" - ) - return (), {} - if minimum < 2 or minimum > len(source_map): - raise ValueError( - "palm orientation minimum source count must be between two " - "and the configured source count" - ) - missing_motors = sorted(set(source_map) - set(motor_by_source)) - if missing_motors: - raise ValueError( - "palm orientation sources are missing motor mappings: " - + ",".join(missing_motors) - ) - fitted: list[PalmOrientationMeasurement] = [] - rejected: dict[str, str] = {} - for cycle in (int(value) for value in cycles): - cycle_fitted: list[PalmOrientationMeasurement] = [] - for source_joint, model_joint in source_map.items(): - motor = int(motor_by_source[source_joint]) - try: - measurement = fit_partial_palm_orientation_measurement( - source_joint, - model_joint, - records_by_joint.get(source_joint, ()), - cycle=cycle, - zero_command_u8=int(baseline_command_u8[motor]), - minimum_arc_rad=minimum_arc_rad, - maximum_rotation_orthogonal_rms_rad=( - maximum_rotation_orthogonal_rms_rad - ), - maximum_command_distance_u8=( - maximum_command_distance_u8 - ), - ) - except Exception as error: - rejected[f"{source_joint}:cycle{cycle + 1}"] = str(error) - continue - cycle_fitted.append(measurement) - if len(cycle_fitted) < minimum: - cycle_reasons = { - key: value - for key, value in rejected.items() - if key.endswith(f":cycle{cycle + 1}") - } - raise ValueError( - f"palm orientation cycle {cycle + 1} has " - f"{len(cycle_fitted)}/{minimum} usable sources: " - + "; ".join( - f"{key}={value}" for key, value in cycle_reasons.items() - ) - ) - fitted.extend(cycle_fitted) - return tuple(fitted), rejected - - -def with_depth_free_axis_projection( - measurement: JointAxisMeasurement, - camera_center_common_xyz_m: Sequence[float], -) -> JointAxisMeasurement: - """Attach the source-camera interpretation plane of an axis line. - - The plane is defined by the camera centre and the fitted 3-D line, but is - only a projective observation: moving either fitted line point along its - optical ray leaves the plane unchanged. A fixed Tag mount changes the - moving point trajectory, not its recovered physical screw axis. - """ - camera_center = _vector( - camera_center_common_xyz_m, 3, name="axis camera centre" - ) - point = np.asarray(measurement.point_common_xyz_m, dtype=float) - axis = np.asarray(measurement.axis_common_xyz, dtype=float) - axis /= np.linalg.norm(axis) - ray = point - camera_center - ray_norm = float(np.linalg.norm(ray)) - if ray_norm <= 1.0e-6: - raise ValueError("axis point coincides with its source camera") - plane_normal = np.cross(ray / ray_norm, axis) - plane_norm = float(np.linalg.norm(plane_normal)) - if plane_norm <= 1.0e-6: - raise ValueError("axis projection is degenerate in its source camera") - plane_normal /= plane_norm - return replace( - measurement, - axis_point_camera_center_common_xyz_m=tuple( - float(value) for value in camera_center - ), - axis_point_interpretation_plane_normal_common_xyz=tuple( - float(value) for value in plane_normal - ), - ) - - -def cross_view_side_line_source( - measurement: JointAxisMeasurement, -) -> str | None: - """Return the side alias that supplied a fallback axis-line point.""" - sources = tuple( - str(source) - for source in getattr( - measurement, "pose_axis_line_source_joints", () - ) - ) - if ( - getattr(measurement, "axis_point_source", "") - in { - "side_circle_cross_view", - "side_circle_shared_radius_cross_view", - "side_interpretation_plane_cross_view", - } - and len(sources) == 1 - and sources[0].endswith("_side") - ): - return sources[0] - return None - - -def axis_line_uses_depth_free_interpretation_plane( - measurement: JointAxisMeasurement, -) -> bool: - """Return whether only the source-camera bearing is geometrically used.""" - return bool( - measurement.axis_point_source - in { - "side_interpretation_plane_cross_view", - "front_interpretation_plane_cross_view_validated", - } - and measurement.axis_point_camera_center_common_xyz_m is not None - ) - - -def refit_axis_line_group_with_shared_radius( - measurements: Sequence[JointAxisMeasurement], - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, - canonical_zero_direction: str | None, -) -> tuple[JointAxisMeasurement, ...]: - """Refit repeated cross-view axis lines with one physical radius. - - A side-view roll alias observes the same child Tag and the same physical - lever arm in every cycle. Fitting an independent radius to each short, - near-edge-on arc leaves radius and circle centre strongly correlated; - sub-pixel PnP noise can then move the reported axis line by several - millimetres even though every trajectory has a low radial residual. - - Keep a separate centre (and therefore an independent line-repeatability - check) for every cycle, but solve one shared radius from all cycles. This - is a physical constraint rather than a relaxed quality gate: the returned - lines are still checked against the unchanged cycle RMS limit, and each - cycle's radial residual remains an independent hard check. - """ - group = tuple(measurements) - if len(group) < 2: - return group - cycles = [int(measurement.cycle) for measurement in group] - if len(set(cycles)) != len(cycles): - raise ValueError("shared-radius axis group contains duplicate cycles") - - entries: list[dict[str, Any]] = [] - initial_parameters: list[float] = [] - initial_radii: list[float] = [] - for measurement in group: - cycle_records = [ - dict(record) - for record in records - if int(record.get("cycle", -1)) == int(measurement.cycle) - ] - motion_records = [ - record - for record in cycle_records - if str(record.get("kind", "sample")) != "baseline_hold_sample" - ] - if len(motion_records) < 6: - motion_records = cycle_records - if len(motion_records) < 6: - raise ValueError( - f"cycle {measurement.cycle + 1} has too few shared-radius samples" - ) - - reference_records = _canonical_reference_records( - cycle_records, canonical_zero_direction - ) - zero_records = _near_zero_records( - reference_records, int(zero_command_u8) - ) - if not zero_records: - raise ValueError( - f"cycle {measurement.cycle + 1} has no shared-radius zero pose" - ) - parent_poses = np.asarray( - [_pose_matrix(record["parent_pose_common"]) for record in zero_records] - ) - parent_translation = np.median(parent_poses[:, :3, 3], axis=0) - parent_quaternion = robust_rotation_summary( - [ - Rotation.from_matrix(matrix[:3, :3]).as_quat() - for matrix in parent_poses - ] - )[0] - parent_rotation = Rotation.from_quat(parent_quaternion) - common_axis = _vector( - measurement.axis_common_xyz, 3, name="shared-radius common axis" - ) - common_axis /= np.linalg.norm(common_axis) - parent_axis = parent_rotation.inv().apply(common_axis) - parent_axis /= np.linalg.norm(parent_axis) - - points = np.asarray( - [record["relative_translation_xyz_m"] for record in motion_records], - dtype=float, - ) - basis_x, basis_y = _plane_basis(parent_axis) - origin = np.mean(points, axis=0) - local = points - origin - points_xy = np.column_stack((local @ basis_x, local @ basis_y)) - initial = _fit_circle_with_axis(points, parent_axis) - initial_center = _vector( - initial["center_xyz_m"], 3, name="initial shared-radius centre" - ) - initial_parameters.extend( - [ - float((initial_center - origin) @ basis_x), - float((initial_center - origin) @ basis_y), - ] - ) - initial_radii.append(float(initial["radius_m"])) - entries.append( - { - "measurement": measurement, - "points": points, - "points_xy": points_xy, - "origin": origin, - "basis_x": basis_x, - "basis_y": basis_y, - "parent_axis": parent_axis, - "parent_rotation": parent_rotation, - "parent_translation": parent_translation, - } - ) - - median_radius = float(np.median(initial_radii)) - if median_radius <= 1.0e-6 or not math.isfinite(median_radius): - raise ValueError("shared-radius axis group has an invalid initial radius") - initial_value = np.asarray( - [*initial_parameters, median_radius], dtype=float - ) - - def residual(parameters: np.ndarray) -> np.ndarray: - radius = float(parameters[-1]) - return np.concatenate( - [ - np.linalg.norm( - entry["points_xy"] - - parameters[2 * index : 2 * index + 2], - axis=1, - ) - - radius - for index, entry in enumerate(entries) - ] - ) / 0.0005 - - lower = np.full(initial_value.shape, -np.inf, dtype=float) - upper = np.full(initial_value.shape, np.inf, dtype=float) - lower[-1] = max(1.0e-6, 0.5 * median_radius) - upper[-1] = 2.0 * median_radius - solution = least_squares( - residual, - initial_value, - bounds=(lower, upper), - loss="soft_l1", - f_scale=1.0, - max_nfev=3000, - ) - if not solution.success: - raise ValueError( - "shared-radius axis optimization failed: " + solution.message - ) - - shared_radius = float(solution.x[-1]) - if not math.isfinite(shared_radius): - raise ValueError("shared-radius axis optimization is non-finite") - result: list[JointAxisMeasurement] = [] - for index, entry in enumerate(entries): - center_xy = solution.x[2 * index : 2 * index + 2] - center_parent = ( - entry["origin"] - + float(center_xy[0]) * entry["basis_x"] - + float(center_xy[1]) * entry["basis_y"] - ) - # The coordinate along an infinite axis is a gauge. Retain the - # robust centre of this cycle's observed axial coordinates. - axial_offsets = ( - entry["points"] - center_parent - ) @ entry["parent_axis"] - center_parent += float(np.median(axial_offsets)) * entry["parent_axis"] - center_common = ( - entry["parent_rotation"].apply(center_parent) - + entry["parent_translation"] - ) - radial_residual = ( - np.linalg.norm( - entry["points_xy"] - center_xy, - axis=1, - ) - - shared_radius - ) - radial_rms = float(np.sqrt(np.mean(np.square(radial_residual)))) - measurement = entry["measurement"] - result.append( - replace( - measurement, - point_common_xyz_m=tuple( - float(value) for value in center_common - ), - radial_rms_m=max(float(measurement.radial_rms_m), radial_rms), - axis_point_source="side_circle_shared_radius_cross_view", - ) - ) - return tuple(result) - - -def maximum_axis_line_cycle_spread_m( - measurements: Sequence[JointAxisMeasurement], -) -> float: - """Measure repeatability of independently fitted near-parallel lines. - - Axis-line points have an arbitrary coordinate along their own direction. - Compare only the perpendicular displacement, symmetrically against both - fitted directions, so that the result remains meaningful with the small - allowed cycle-to-cycle direction variation. - """ - maximum = 0.0 - for left_index, left in enumerate(measurements): - left_axis = np.asarray(left.axis_common_xyz, dtype=float) - left_axis /= np.linalg.norm(left_axis) - left_point = np.asarray(left.point_common_xyz_m, dtype=float) - for right in measurements[left_index + 1 :]: - right_axis = np.asarray(right.axis_common_xyz, dtype=float) - right_axis /= np.linalg.norm(right_axis) - delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point - maximum = max( - maximum, - float(np.linalg.norm(np.cross(delta, left_axis))), - float(np.linalg.norm(np.cross(delta, right_axis))), - ) - return maximum - - -def axis_line_cycle_rms_m( - measurements: Sequence[JointAxisMeasurement], -) -> float: - """Return RMS line-position scatter about the four-cycle consensus. - - For parallel lines, the sum of squared pairwise distances divided by - ``n**2`` equals the mean squared distance from their centroid. Averaging - each pair's distance against both near-parallel directions preserves that - identity while avoiding an arbitrary choice of one cycle's direction. - """ - count = len(measurements) - if count < 2: - return 0.0 - squared_pairwise_sum = 0.0 - for left_index, left in enumerate(measurements): - left_axis = np.asarray(left.axis_common_xyz, dtype=float) - left_axis /= np.linalg.norm(left_axis) - left_point = np.asarray(left.point_common_xyz_m, dtype=float) - for right in measurements[left_index + 1 :]: - right_axis = np.asarray(right.axis_common_xyz, dtype=float) - right_axis /= np.linalg.norm(right_axis) - delta = np.asarray(right.point_common_xyz_m, dtype=float) - left_point - left_distance = float(np.linalg.norm(np.cross(delta, left_axis))) - right_distance = float(np.linalg.norm(np.cross(delta, right_axis))) - squared_pairwise_sum += 0.5 * ( - left_distance**2 + right_distance**2 - ) - return float(math.sqrt(squared_pairwise_sum / (count**2))) - - -def _fit_axis_point_from_pose_trajectory( - records: Sequence[Mapping[str, Any]], - *, - zero_command_u8: int, - axis_parent_xyz: Sequence[float], - angle_axis_parent_xyz: Sequence[float], - phase_reference_point_parent_xyz: Sequence[float], - view_normal_common_xyz: Sequence[float] | None, - canonical_zero_direction: str | None = None, - allow_axial_translation: bool = False, -) -> tuple[np.ndarray, float, str]: - """Fit the closest point on a revolute axis from full relative poses. - - If ``T(q)`` maps the moving Tag into its parent Tag frame, then - ``T(q) @ inv(T(0))`` is a rotation about the physical joint axis and its - translation obeys ``(I - R(q)) p = t(q)``. Solving this equation over the - complete trajectory uses both pose orientation and translation and avoids - treating a monocular Tag-centre depth arc as ground-truth geometry. - """ - samples = [dict(record) for record in records] - reference_samples = _canonical_reference_records( - samples, canonical_zero_direction - ) - zero_records = _near_zero_records(reference_samples, zero_command_u8) - if not zero_records: - raise ValueError( - f"axis-point fit has no record near baseline {zero_command_u8}" - ) - - reference_rotation = Rotation.from_quat( - _baseline_reference(reference_samples, zero_command_u8) - ).as_matrix() - reference_translation = np.median( - np.asarray( - [record["relative_translation_xyz_m"] for record in zero_records], - dtype=float, - ), - axis=0, - ) - axis = _vector(axis_parent_xyz, 3, name="axis-point direction") - axis /= np.linalg.norm(axis) - angle_axis = _vector( - angle_axis_parent_xyz, 3, name="axis-point angle direction" - ) - angle_axis /= np.linalg.norm(angle_axis) - phase_reference_point = _vector( - phase_reference_point_parent_xyz, - 3, - name="axis-point phase reference", - ) - helper = ( - np.asarray([1.0, 0.0, 0.0]) - if abs(float(axis[0])) < 0.8 - else np.asarray([0.0, 1.0, 0.0]) - ) - basis_first = np.cross(axis, helper) - basis_first /= np.linalg.norm(basis_first) - basis = np.column_stack( - (basis_first, np.cross(axis, basis_first)) - ) - - view_normal_common = None - use_image_plane_projection = False - if view_normal_common_xyz is not None: - view_normal_common = _vector( - view_normal_common_xyz, 3, name="view normal" - ) - view_normal_common /= np.linalg.norm(view_normal_common) - reference_parent_quaternion = robust_rotation_summary( - [ - Rotation.from_matrix( - _pose_matrix(record["parent_pose_common"])[:3, :3] - ).as_quat() - for record in zero_records - ] - )[0] - reference_parent_rotation = Rotation.from_quat( - reference_parent_quaternion - ) - reference_view_normal_parent = reference_parent_rotation.inv().apply( - view_normal_common - ) - reference_view_normal_parent /= np.linalg.norm( - reference_view_normal_parent - ) - use_image_plane_projection = abs( - float(axis @ reference_view_normal_parent) - ) >= math.cos(AXIS_POINT_IMAGE_PLANE_MAXIMUM_OBLIQUITY_RAD) - - matrices: list[np.ndarray] = [] - translations: list[np.ndarray] = [] - used_image_plane_projection = False - raw_angles: list[float] = [] - translation_angles: list[float] = [] - reference_radial = reference_translation - phase_reference_point - reference_radial -= axis * float(reference_radial @ axis) - reference_radial_norm = float(np.linalg.norm(reference_radial)) - if reference_radial_norm < 1.0e-6: - raise ValueError("axis-point phase reference radius is degenerate") - reference_radial /= reference_radial_norm - for record in samples: - observed_rotation = Rotation.from_quat( - _relative_rotation(record) - ).as_matrix() - observed_delta_rotation = observed_rotation @ reference_rotation.T - raw_angle = float( - Rotation.from_matrix(observed_delta_rotation).as_rotvec() - @ angle_axis - ) - if abs(raw_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: - continue - observed_translation = _vector( - record["relative_translation_xyz_m"], - 3, - name="relative translation", - ) - observed_radial = observed_translation - phase_reference_point - observed_radial -= axis * float(observed_radial @ axis) - observed_radial_norm = float(np.linalg.norm(observed_radial)) - if observed_radial_norm <= 1.0e-6: - continue - observed_radial /= observed_radial_norm - raw_angles.append(raw_angle) - translation_angles.append( - math.atan2( - float(axis @ np.cross(reference_radial, observed_radial)), - float( - np.clip(reference_radial @ observed_radial, -1.0, 1.0) - ), - ) - ) - orientation_sign = 1.0 - if raw_angles and float( - np.sum(np.asarray(raw_angles) * np.asarray(translation_angles)) - ) < 0.0: - orientation_sign = -1.0 - - for record in samples: - observed_rotation = Rotation.from_quat( - _relative_rotation(record) - ).as_matrix() - observed_translation = _vector( - record["relative_translation_xyz_m"], - 3, - name="relative translation", - ) - observed_delta_rotation = observed_rotation @ reference_rotation.T - signed_angle = float( - Rotation.from_matrix(observed_delta_rotation).as_rotvec() - @ angle_axis - ) * orientation_sign - if abs(signed_angle) < AXIS_POINT_MINIMUM_ROTATION_RAD: - continue - # Direction and angle have deliberately separate sources. A trusted - # upstream/circle direction may replace a biased planar-PnP axis, while - # the relative-orientation trajectory still provides a repeatable - # scalar travel. Reconstruct the admissible revolute motion before - # solving its axis line instead of feeding a contradictory 3-D - # rotation into the translation equations. - # The rotation-trajectory axis is undirected. Align its scalar sign - # with the selected physical axis using the Tag-centre trajectory; - # this remains robust even when a trusted upstream direction replaces - # a badly biased planar-PnP orientation axis. - delta_rotation = Rotation.from_rotvec( - axis * signed_angle - ).as_matrix() - delta_translation = ( - observed_translation - - delta_rotation @ reference_translation - ) - # A screw-driven revolute joint may carry real translation along its - # axis. That component is in the null space of (I - R), contains no - # information about the perpendicular axis-line point, and must not - # inflate or bias the line fit as if the mechanism were pure rotary. - axis_projection = ( - np.eye(3) - np.outer(axis, axis) - if allow_axial_translation - else np.eye(3) - ) - projection = axis_projection - axis_point_matrix = (np.eye(3) - delta_rotation) @ basis - if use_image_plane_projection: - assert view_normal_common is not None - parent_rotation = Rotation.from_matrix( - _pose_matrix(record["parent_pose_common"])[:3, :3] - ) - view_normal_parent = parent_rotation.inv().apply( - view_normal_common - ) - view_normal_parent /= np.linalg.norm(view_normal_parent) - image_projection = np.eye(3) - np.outer( - view_normal_parent, view_normal_parent - ) - projection = image_projection @ axis_projection - # A single end-on camera cannot distinguish absolute depth from a - # shift along the revolute axis. Keep the observable image-plane - # equations only; adding a free baseline-depth variable makes the - # axis point rank-deficient and pretends that this gauge is - # observable. Parallel phase chains are later compared in the - # same view, where their common depth gauge cancels. - used_image_plane_projection = True - matrices.append(projection @ axis_point_matrix) - translations.append(projection @ delta_translation) - - if len(matrices) < 6: - raise ValueError("axis-point fit has too few observable pose samples") - matrix = np.vstack(matrices) - translation = np.concatenate(translations) - singular_values = np.linalg.svd(matrix, compute_uv=False) - if ( - singular_values.size < 2 - or singular_values[-1] <= 1.0e-9 - or singular_values[-1] / singular_values[0] < 1.0e-3 - ): - raise ValueError("axis-point pose trajectory is ill-conditioned") - - unknown_count = matrix.shape[1] - solution = least_squares( - lambda value: ( - matrix @ value - translation - ) / AXIS_POINT_RESIDUAL_SCALE_M, - np.zeros(unknown_count, dtype=float), - loss="soft_l1", - f_scale=1.0, - max_nfev=1000, - ) - if not solution.success: - raise ValueError( - "axis-point pose optimization failed: " + solution.message - ) - residual = matrix @ solution.x - translation - rms = float(np.sqrt(np.mean(np.square(residual)))) - source = ( - "pose_trajectory_image_plane" - if used_image_plane_projection - else "pose_trajectory_3d" - ) - return basis @ solution.x[:2], rms, source - - -def fit_joint_axis_measurement( - joint: str, - records: Sequence[Mapping[str, Any]], - *, - cycle: int, - zero_command_u8: int, - axis_common_constraint: Sequence[float] | None = None, - constrained_circle_joints: frozenset[str] = CONSTRAINED_CIRCLE_JOINTS, - view_normal_common_xyz: Sequence[float] | None = None, - canonical_zero_direction: str | None = None, -) -> JointAxisMeasurement: - """Fit one physical screw axis from one complete scan cycle.""" - samples = [ - dict(record) for record in records if int(record["cycle"]) == int(cycle) - ] - if len(samples) < 12: - raise ValueError(f"{joint} cycle {cycle + 1} has too few samples") - points = np.asarray( - [record["relative_translation_xyz_m"] for record in samples], dtype=float - ) - free_circle_axis, free_plane_rms = _fit_plane_axis([points]) - singular_values = np.linalg.svd( - points - np.mean(points, axis=0), compute_uv=False - ) - circle_axis_observability = ( - 0.0 - if singular_values.size < 2 or singular_values[0] <= 1.0e-12 - else float(singular_values[1] / singular_values[0]) - ) - - reference_samples = _canonical_reference_records( - samples, canonical_zero_direction - ) - reference = Rotation.from_quat( - _baseline_reference(reference_samples, zero_command_u8) - ) - rotation_vectors = [ - (reference.inv() * Rotation.from_quat(_relative_rotation(record))).as_rotvec() - for record in samples - ] - commands = [int(record["command_u8"]) for record in samples] - rotation_axis_child = fit_rotation_axis(rotation_vectors, commands) - # reference maps the child Tag frame at baseline into the parent Tag - # frame. The quaternion delta axis is expressed in that child frame, - # while the fitted centre circle is expressed in the parent frame. This - # conversion is what makes arbitrary Tag mounting rotations harmless. - rotation_axis = reference.apply(rotation_axis_child) - if float(rotation_axis @ free_circle_axis) < 0.0: - free_circle_axis = -free_circle_axis - disagreement = math.acos( - float(np.clip(rotation_axis @ free_circle_axis, -1.0, 1.0)) - ) - - zero_records = _near_zero_records(reference_samples, zero_command_u8) - if not zero_records: - raise ValueError( - f"{joint} cycle has no record near baseline {zero_command_u8}" - ) - parent_poses = np.asarray( - [_pose_matrix(record["parent_pose_common"]) for record in zero_records] - ) - parent_translation = np.median(parent_poses[:, :3, 3], axis=0) - parent_quaternion = robust_rotation_summary( - [ - Rotation.from_matrix(matrix[:3, :3]).as_quat() - for matrix in parent_poses - ] - )[0] - parent_rotation = Rotation.from_quat(parent_quaternion) - axis_direction_source = "rotation" - - if axis_common_constraint is not None: - common_axis = _vector( - axis_common_constraint, 3, name="common axis constraint" - ) - common_axis /= np.linalg.norm(common_axis) - fitted_axis = parent_rotation.inv().apply(common_axis) - if float(fitted_axis @ rotation_axis) < 0.0: - fitted_axis = -fitted_axis - circle = _fit_circle_with_axis(points, fitted_axis) - plane_rms = float(circle["plane_rms_m"]) - axis_direction_source = "upstream_constraint" - elif str(joint) in constrained_circle_joints: - # The side/front views are close to end-on for these axes. Monocular - # planar-PnP depth bias can therefore tilt even an apparently smooth, - # low-residual Tag-centre circle. The relative orientation trajectory - # observes the screw-axis direction directly and is invariant to an - # arbitrary fixed Tag mounting rotation; use it to constrain the 3-D - # circle and estimate only the axis line. Never switch the direction - # source based on a rotation-vs-circle threshold: that discontinuity - # makes otherwise identical repeat cycles choose different models. - fitted_axis = rotation_axis - circle = _fit_circle_with_axis(points, fitted_axis) - plane_rms = float(circle["plane_rms_m"]) - else: - # Oblique trajectories with an observable 3-D motion plane retain the - # independent rotation/centre cross-check and fuse both estimates. - free_circle = _fit_circle_with_axis(points, free_circle_axis) - fitted_axis = rotation_axis + free_circle_axis - if float(np.linalg.norm(fitted_axis)) < 1.0e-9: - raise ValueError(f"{joint} rotation and centre axes are opposed") - fitted_axis /= np.linalg.norm(fitted_axis) - circle = free_circle - plane_rms = max( - float(free_plane_rms), float(circle["plane_rms_m"]) - ) - axis_direction_source = "rotation_circle_fusion" - axis_common = parent_rotation.apply(fitted_axis) - point_parent, pose_axis_line_rms_m, axis_point_source = ( - _fit_axis_point_from_pose_trajectory( - samples, - zero_command_u8=zero_command_u8, - axis_parent_xyz=fitted_axis, - angle_axis_parent_xyz=rotation_axis, - phase_reference_point_parent_xyz=circle["center_xyz_m"], - view_normal_common_xyz=view_normal_common_xyz, - canonical_zero_direction=canonical_zero_direction, - allow_axial_translation=str(joint).endswith("_side"), - ) - ) - point_common = parent_rotation.apply(point_parent) + parent_translation - state = np.median( - np.asarray([record["state_u8"] for record in zero_records], dtype=float), - axis=0, - ) - return JointAxisMeasurement( - joint=str(joint), - cycle=int(cycle), - axis_common_xyz=tuple(float(value) for value in axis_common), - point_common_xyz_m=tuple(float(value) for value in point_common), - condition_state_u8=tuple(float(value) for value in state), - plane_rms_m=float(plane_rms), - radial_rms_m=float(circle["radial_rms_m"]), - rotation_circle_axis_difference_rad=float(disagreement), - view_normal_common_xyz=( - None - if view_normal_common_xyz is None - else tuple( - float(value) - for value in _vector( - view_normal_common_xyz, 3, name="view normal" - ) - ) - ), - axis_direction_source=axis_direction_source, - circle_axis_observability=float(circle_axis_observability), - axis_point_source=axis_point_source, - pose_axis_line_rms_m=pose_axis_line_rms_m, - pose_axis_line_source_joints=(str(joint),), - ) - - -@dataclass(frozen=True) -class _UrdfJoint: - name: str - parent: str - child: str - origin: np.ndarray - axis: np.ndarray - mimic_joint: str | None - mimic_multiplier: float - mimic_offset: float - - -class UrdfKinematicModel: - def __init__(self, source: str | Path) -> None: - self.source = Path(source).expanduser().resolve() - if not self.source.is_file(): - raise ValueError(f"source URDF does not exist: {self.source}") - root = ET.parse(self.source).getroot() - self.joints: dict[str, _UrdfJoint] = {} - self.parent_joint_by_child: dict[str, str] = {} - for element in root.findall("joint"): - if element.get("type") == "fixed": - pass - name = str(element.get("name")) - parent = element.find("parent") - child = element.find("child") - if parent is None or child is None: - continue - origin_node = element.find("origin") - xyz = _parse_triplet( - "0 0 0" if origin_node is None else origin_node.get("xyz", "0 0 0") - ) - rpy = _parse_triplet( - "0 0 0" if origin_node is None else origin_node.get("rpy", "0 0 0") - ) - origin = np.eye(4) - origin[:3, :3] = Rotation.from_euler("xyz", rpy).as_matrix() - origin[:3, 3] = xyz - axis_node = element.find("axis") - axis = _parse_triplet( - "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") - ) - axis /= np.linalg.norm(axis) - mimic = element.find("mimic") - model = _UrdfJoint( - name=name, - parent=str(parent.get("link")), - child=str(child.get("link")), - origin=origin, - axis=axis, - mimic_joint=None if mimic is None else str(mimic.get("joint")), - mimic_multiplier=( - 1.0 if mimic is None else float(mimic.get("multiplier", "1")) - ), - mimic_offset=( - 0.0 if mimic is None else float(mimic.get("offset", "0")) - ), - ) - self.joints[name] = model - self.parent_joint_by_child[model.child] = name - - def _chain(self, target_joint: str) -> list[_UrdfJoint]: - if target_joint not in self.joints: - raise ValueError(f"URDF is missing joint {target_joint}") - result: list[_UrdfJoint] = [] - current = self.joints[target_joint] - while True: - result.append(current) - parent_joint = self.parent_joint_by_child.get(current.parent) - if parent_joint is None: - break - current = self.joints[parent_joint] - result.reverse() - return result - - def link_transform( - self, - target_joint: str, - *, - zero_offsets: Mapping[str, float], - joint_angles: Mapping[str, float], - independent_mimic_angles: bool = False, - ) -> np.ndarray: - """Return base-to-child-link pose for calibration validation. - - ``independent_mimic_angles`` lets the product validator use the five - independently measured passive JSON curves while the emitted URDF's - original mimic XML remains untouched. - """ - transform = np.eye(4) - resolved = {str(name): float(value) for name, value in joint_angles.items()} - for joint in self._chain(target_joint): - transform = transform @ joint.origin @ _axis_rotation( - joint.axis, float(zero_offsets.get(joint.name, 0.0)) - ) - if independent_mimic_angles and joint.name in resolved: - angle = resolved[joint.name] - elif joint.mimic_joint is None: - angle = float(resolved.get(joint.name, 0.0)) - else: - angle = ( - joint.mimic_multiplier - * float(resolved.get(joint.mimic_joint, 0.0)) - + joint.mimic_offset - ) - resolved[joint.name] = angle - transform = transform @ _axis_rotation(joint.axis, angle) - return transform - - def axis_line( - self, - target_joint: str, - *, - zero_offsets: Mapping[str, float], - joint_angles: Mapping[str, float], - ) -> tuple[np.ndarray, np.ndarray]: - transform = np.eye(4) - resolved_angles = dict(joint_angles) - for joint in self._chain(target_joint): - corrected_origin = joint.origin @ _axis_rotation( - joint.axis, float(zero_offsets.get(joint.name, 0.0)) - ) - joint_frame = transform @ corrected_origin - if joint.name == target_joint: - return ( - joint_frame[:3, :3] @ joint.axis, - joint_frame[:3, 3].copy(), - ) - if joint.mimic_joint is None: - angle = float(resolved_angles.get(joint.name, 0.0)) - else: - angle = ( - joint.mimic_multiplier - * float(resolved_angles.get(joint.mimic_joint, 0.0)) - + joint.mimic_offset - ) - resolved_angles[joint.name] = angle - transform = joint_frame @ _axis_rotation(joint.axis, angle) - raise RuntimeError(f"could not resolve axis line for {target_joint}") - - -def _parse_triplet(value: str) -> np.ndarray: - result = np.asarray([float(item) for item in str(value).split()], dtype=float) - if result.shape != (3,) or not np.all(np.isfinite(result)): - raise ValueError(f"invalid URDF vector: {value}") - return result - - -def _axis_rotation(axis: Sequence[float], angle: float) -> np.ndarray: - result = np.eye(4) - result[:3, :3] = Rotation.from_rotvec( - _vector(axis, 3, name="joint axis") * float(angle) - ).as_matrix() - return result - - -@dataclass(frozen=True) -class ZeroSolveResult: - direct_offsets_rad: Mapping[str, float] - all_active_offsets_rad: Mapping[str, float] - base_translation_xyz_m: tuple[float, float, float] - base_quaternion_xyzw: tuple[float, float, float, float] - validation_errors_rad: tuple[float, ...] - validation_error_by_joint_rad: Mapping[str, float] - validation_line_error_by_joint_m: Mapping[str, float] - axis_line_rms_m: float - passed: bool - cycle_offsets_rad: Mapping[str, tuple[float, ...]] - offset_uncertainty_rad: Mapping[str, float] - offset_confidence_half_width_rad: Mapping[str, float] - training_cycles: tuple[int, ...] - validation_cycle: int - validation_original_error_by_joint_rad: Mapping[str, float] - validation_improvement_by_joint_rad: Mapping[str, float] - validation_improvement_confidence_lower_rad: Mapping[str, float] - observability_rank: int - observability_parameter_count: int - observability_condition_number: float - offset_covariance_rad2: Mapping[str, float] - axis_cone_mismatch_by_joint_rad: Mapping[str, float] - axis_cone_bias_classification_by_joint: Mapping[str, str] - failure_reasons: Mapping[str, str] - - -def merge_right_19_thumb_zero_result( - *, - thumb_result: ZeroSolveResult, - preserved_offsets_rad: Mapping[str, float] | None = None, - companion_result: ZeroSolveResult | None = None, -) -> ZeroSolveResult: - """Merge an independent thumb solve without allowing reverse coupling. - - ``companion_result`` is the finger/full solver result whose thumb values - were fixed to ``thumb_result``. For a thumb-only run, the 12 certified - finger values are supplied through ``preserved_offsets_rad`` instead. - """ - thumb_names = { - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - } - if set(thumb_result.direct_offsets_rad) != thumb_names: - raise ValueError("independent thumb result must contain exactly four zeros") - full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) - direct_names = set(full_profile.direct_zero_joints) - preserved = { - str(name): float(value) - for name, value in (preserved_offsets_rad or {}).items() - } - if set(preserved) - (direct_names - thumb_names): - raise ValueError("preserved offsets contain a thumb or unknown joint") - if any(not math.isfinite(value) for value in preserved.values()): - raise ValueError("preserved offsets must be finite") - - if companion_result is None: - if set(preserved) != direct_names - thumb_names: - raise ValueError("thumb-only merge requires all 12 certified finger zeros") - direct = dict(preserved) - all_active = {name: 0.0 for name in full_profile.hand.active_joints} - all_active.update(preserved) - base_result = thumb_result - else: - if set(companion_result.direct_offsets_rad) != direct_names: - raise ValueError("full companion result must contain all 16 zeros") - direct = dict(companion_result.direct_offsets_rad) - all_active = dict(companion_result.all_active_offsets_rad) - base_result = companion_result - direct.update(thumb_result.direct_offsets_rad) - all_active.update(thumb_result.direct_offsets_rad) - - def merged_map(name: str) -> dict[str, Any]: - values = ( - {} - if companion_result is None - else dict(getattr(companion_result, name)) - ) - values.update(dict(getattr(thumb_result, name))) - return values - - failure_reasons = merged_map("failure_reasons") - passed = bool( - thumb_result.passed - and (companion_result is None or companion_result.passed) - ) - return replace( - base_result, - direct_offsets_rad=direct, - all_active_offsets_rad=all_active, - validation_errors_rad=( - tuple(thumb_result.validation_errors_rad) - if companion_result is None - else ( - *companion_result.validation_errors_rad, - *thumb_result.validation_errors_rad, - ) - ), - validation_error_by_joint_rad=merged_map( - "validation_error_by_joint_rad" - ), - validation_line_error_by_joint_m=merged_map( - "validation_line_error_by_joint_m" - ), - axis_line_rms_m=( - thumb_result.axis_line_rms_m - if companion_result is None - else max( - companion_result.axis_line_rms_m, - thumb_result.axis_line_rms_m, - ) - ), - passed=passed, - cycle_offsets_rad=merged_map("cycle_offsets_rad"), - offset_uncertainty_rad=merged_map("offset_uncertainty_rad"), - offset_confidence_half_width_rad=merged_map( - "offset_confidence_half_width_rad" - ), - validation_original_error_by_joint_rad=merged_map( - "validation_original_error_by_joint_rad" - ), - validation_improvement_by_joint_rad=merged_map( - "validation_improvement_by_joint_rad" - ), - validation_improvement_confidence_lower_rad=merged_map( - "validation_improvement_confidence_lower_rad" - ), - offset_covariance_rad2=merged_map("offset_covariance_rad2"), - axis_cone_mismatch_by_joint_rad=merged_map( - "axis_cone_mismatch_by_joint_rad" - ), - axis_cone_bias_classification_by_joint=merged_map( - "axis_cone_bias_classification_by_joint" - ), - failure_reasons=failure_reasons, - ) - - -def expand_right_19_thumb_zero_result_with_cad_fingers( - thumb_result: ZeroSolveResult, -) -> ZeroSolveResult: - """Build a standalone thumb result with every finger left at source CAD. - - This is intentionally different from inheriting a prior finger - calibration: the twelve non-thumb values are explicit zeros and no finger - observation or artifact is required. - """ - full_profile = get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT) - thumb_names = { - "thumb_cmc_roll", - "thumb_cmc_yaw", - "thumb_cmc_pitch", - "thumb_mcp", - } - if set(thumb_result.direct_offsets_rad) != thumb_names: - raise ValueError("independent thumb result must contain exactly four zeros") - direct = {name: 0.0 for name in full_profile.direct_zero_joints} - direct.update(thumb_result.direct_offsets_rad) - all_active = {name: 0.0 for name in full_profile.hand.active_joints} - all_active.update(thumb_result.direct_offsets_rad) - return replace( - thumb_result, - direct_offsets_rad=direct, - all_active_offsets_rad=all_active, - ) - - -def _angles_from_state( - state_u8: Sequence[float], - *, - curves: Mapping[str, JointCurveFit], - motor_by_joint: Mapping[str, int], - inherited_zero_joints: Mapping[str, str] = INHERITED_ZERO_JOINTS, -) -> dict[str, float]: - state = np.asarray(state_u8, dtype=float) - result: dict[str, float] = {} - for joint, motor in motor_by_joint.items(): - source = inherited_zero_joints.get(joint, joint) - if source not in curves: - continue - command = int(np.clip(np.rint(state[int(motor)]), 0, 255)) - result[joint] = float(curves[source].angle_rad[command]) - return result - - -def solve_urdf_zero_offsets( - *, - source_urdf: str | Path, - measurements: Sequence[JointAxisMeasurement], - palm_orientation_measurements: Sequence[ - PalmOrientationMeasurement - ] = (), - curves: Mapping[str, JointCurveFit], - motor_by_joint: Mapping[str, int], - training_cycles: Sequence[int] = (0, 1), - validation_cycle: int = 2, - maximum_offset_rad: float = math.radians(20.0), - finger_maximum_offset_rad: float = math.radians(3.0), - joint_maximum_offset_rad: Mapping[str, float] | None = None, - maximum_cycle_difference_rad: float = math.radians(0.75), - minimum_applied_offset_rad: float = math.radians(0.3), - significance_sigma: float = 3.0, - maximum_validation_mae_rad: float = math.radians(1.0), - maximum_validation_p95_rad: float = math.radians(2.0), - maximum_validation_error_rad: float | None = None, - maximum_confidence_half_width_rad: float | None = None, - maximum_axis_cone_mismatch_rad: float = math.radians(5.0), - maximum_systematic_axis_cone_bias_rad: float | None = None, - maximum_pose_axis_line_rms_m: float = 0.001, - maximum_observability_condition_number: float = 1.0e10, - hand_type: str = "left", - tag_layout: str = "legacy_11", - fixed_direct_zero_offsets_rad: Mapping[str, float] | None = None, - static_output_zero_offsets_rad: Mapping[str, float] | None = None, - zero_profile: ZeroCalibrationProfile | None = None, -) -> ZeroSolveResult: - """Solve all directly observable zero offsets without fitting CAD errors. - - The product palm pose uses the robust common root-axis direction together - with independent partial-sweep MCP-pitch directions; root-line points - determine translation only. Legacy layouts retain their single observed - reference chain. Encoder zeros are then fitted from one angular residual - per joint. This prevents link length, along-axis Tag placement and - monocular depth bias from becoming encoder-zero corrections. - """ - profile = ( - get_zero_calibration_profile(hand_type, tag_layout) - if zero_profile is None - else zero_profile - ) - if profile.hand.side != str(hand_type).lower(): - raise ValueError("zero profile hand side does not match hand_type") - if profile.hand.layout_id != str(tag_layout): - raise ValueError("zero profile layout does not match tag_layout") - model = UrdfKinematicModel(source_urdf) - training = [m for m in measurements if m.cycle in set(training_cycles)] - validation = [m for m in measurements if m.cycle == int(validation_cycle)] - expected = set(profile.axis_joints) - if ( - {m.joint for m in training} != expected - or {m.joint for m in validation} != expected - ): - raise ValueError("axis measurements do not contain all required joints/cycles") - configured_palm_sources = { - str(name): str(model_joint) - for name, model_joint in ( - profile.hand.palm_orientation_sources or {} - ).items() - } - orientation_measurements = tuple(palm_orientation_measurements) - if configured_palm_sources: - minimum_orientation_sources = int( - profile.hand.minimum_palm_orientation_sources - ) - unknown_orientation_sources = sorted( - { - item.source_joint - for item in orientation_measurements - if item.source_joint not in configured_palm_sources - or configured_palm_sources[item.source_joint] - != item.model_joint - } - ) - if unknown_orientation_sources: - raise ValueError( - "palm orientation measurements do not match the product " - "profile: " - + ",".join(unknown_orientation_sources) - ) - required_orientation_cycles = { - *(int(value) for value in training_cycles), - int(validation_cycle), - } - for cycle in required_orientation_cycles: - cycle_sources = { - item.source_joint - for item in orientation_measurements - if int(item.cycle) == cycle - } - if len(cycle_sources) < minimum_orientation_sources: - raise ValueError( - f"palm orientation cycle {cycle + 1} contains " - f"{len(cycle_sources)}/{minimum_orientation_sources} " - "qualified sources" - ) - elif orientation_measurements: - raise ValueError( - "palm orientation measurements were supplied without a profile" - ) - orientation_training = tuple( - item - for item in orientation_measurements - if int(item.cycle) in set(int(value) for value in training_cycles) - ) - orientation_validation = tuple( - item - for item in orientation_measurements - if int(item.cycle) == int(validation_cycle) - ) - orientation_by_model_cycle: dict[ - tuple[str, int], PalmOrientationMeasurement - ] = {} - for item in orientation_measurements: - key = (str(item.model_joint), int(item.cycle)) - if key in orientation_by_model_cycle: - raise ValueError( - "palm orientation has multiple sources for " - f"{key[0]} cycle {key[1] + 1}" - ) - orientation_by_model_cycle[key] = item - if not 0.0 < finger_maximum_offset_rad <= maximum_offset_rad: - raise ValueError("finger maximum offset must be positive and no larger than thumb") - joint_limits = { - str(name): float(value) - for name, value in dict(joint_maximum_offset_rad or {}).items() - } - unknown_joint_limits = sorted(set(joint_limits) - set(profile.direct_zero_joints)) - if unknown_joint_limits: - raise ValueError( - "joint maximum offsets contain unknown direct joints: " - + ",".join(unknown_joint_limits) - ) - if any( - not math.isfinite(value) or not 0.0 < value <= math.radians(90.0) - for value in joint_limits.values() - ): - raise ValueError("joint maximum offsets must be finite and in (0, 90deg]") - if maximum_cycle_difference_rad <= 0.0: - raise ValueError("maximum cycle difference must be positive") - if minimum_applied_offset_rad < 0.0 or significance_sigma < 0.0: - raise ValueError("zero significance thresholds must be non-negative") - if maximum_axis_cone_mismatch_rad <= 0.0: - raise ValueError("maximum axis cone mismatch must be positive") - if ( - maximum_systematic_axis_cone_bias_rad is not None - and ( - maximum_systematic_axis_cone_bias_rad - <= maximum_axis_cone_mismatch_rad - or maximum_systematic_axis_cone_bias_rad > math.radians(90.0) - ) - ): - raise ValueError( - "maximum systematic axis cone bias must exceed the precision " - "limit and be at most 90deg" - ) - if maximum_pose_axis_line_rms_m <= 0.0: - raise ValueError("maximum pose axis-line RMS must be positive") - if maximum_observability_condition_number <= 1.0: - raise ValueError( - "maximum observability condition number must be greater than one" - ) - if ( - maximum_validation_error_rad is not None - and maximum_validation_error_rad <= 0.0 - ): - raise ValueError("maximum validation error must be positive") - if ( - maximum_confidence_half_width_rad is not None - and maximum_confidence_half_width_rad <= 0.0 - ): - raise ValueError("maximum confidence half width must be positive") - - def predicted_local( - measurement: JointAxisMeasurement, - offsets: Mapping[str, float], - ) -> tuple[np.ndarray, np.ndarray]: - angles = _angles_from_state( - ( - measurement.condition_state_u8 - if measurement.condition_command_u8 is None - else measurement.condition_command_u8 - ), - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - return model.axis_line( - measurement.joint, - zero_offsets=offsets, - joint_angles=angles, - ) - - def predicted_palm_orientation_local( - measurement: PalmOrientationMeasurement, - offsets: Mapping[str, float], - ) -> np.ndarray: - angles = _angles_from_state( - measurement.condition_state_u8, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - axis, _ = model.axis_line( - measurement.model_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - return axis / np.linalg.norm(axis) - - fixed_offsets = { - str(name): float(value) - for name, value in ( - profile.fixed_direct_zero_offsets_rad - if fixed_direct_zero_offsets_rad is None - else fixed_direct_zero_offsets_rad - ).items() - } - unknown_fixed_offsets = sorted( - set(fixed_offsets) - set(profile.direct_zero_joints) - ) - if unknown_fixed_offsets: - raise ValueError( - "fixed direct zero offsets contain unknown joints: " - + ",".join(unknown_fixed_offsets) - ) - if any( - not math.isfinite(value) or abs(value) > math.radians(90.0) - for value in fixed_offsets.values() - ): - raise ValueError( - "fixed direct zero offsets must be finite and within +/-90deg" - ) - output_offsets = { - str(name): float(value) - for name, value in ( - profile.static_output_zero_offsets_rad - if static_output_zero_offsets_rad is None - else static_output_zero_offsets_rad - ).items() - } - unknown_output_offsets = sorted( - set(output_offsets) - set(profile.direct_zero_joints) - ) - if unknown_output_offsets: - raise ValueError( - "static output zero offsets contain unknown joints: " - + ",".join(unknown_output_offsets) - ) - if any( - not math.isfinite(value) or abs(value) > math.radians(90.0) - for value in output_offsets.values() - ): - raise ValueError( - "static output zero offsets must be finite and within +/-90deg" - ) - zero_offsets = {name: 0.0 for name in profile.direct_zero_joints} - zero_offsets.update(fixed_offsets) - def fit_base_pose( - selected: Sequence[JointAxisMeasurement], - pose_offsets: Mapping[str, float] | None = None, - ) -> tuple[Rotation, np.ndarray]: - pose_zero_offsets = zero_offsets if pose_offsets is None else pose_offsets - anchors_by_joint = { - name: [item for item in selected if item.joint == name] - for name in profile.root_anchor_joints - } - if any(not items for items in anchors_by_joint.values()): - raise ValueError("zero solve is missing root axis anchors") - - def undirected_axis_average( - values: Sequence[Sequence[float]], - ) -> np.ndarray: - vectors = np.asarray(values, dtype=float) - reference = vectors[0] / np.linalg.norm(vectors[0]) - signs = np.where(vectors @ reference < 0.0, -1.0, 1.0) - result = np.sum(vectors * signs[:, None], axis=0) - norm = float(np.linalg.norm(result)) - if norm < 1.0e-9: - raise ValueError("root axis average is degenerate") - return result / norm - - root_names = tuple(sorted(profile.root_anchor_joints)) - predicted_axes: dict[str, np.ndarray] = {} - predicted_points: dict[str, np.ndarray] = {} - observed_axes: dict[str, np.ndarray] = {} - observed_points: dict[str, np.ndarray] = {} - for name in root_names: - items = anchors_by_joint[name] - predicted = [predicted_local(item, pose_zero_offsets) for item in items] - predicted_axes[name] = undirected_axis_average( - [axis for axis, _ in predicted] - ) - predicted_points[name] = np.median( - np.asarray([point for _, point in predicted]), axis=0 - ) - observed_axes[name] = undirected_axis_average( - [item.axis_common_xyz for item in items] - ) - observed_points[name] = np.median( - np.asarray( - [item.point_common_xyz_m for item in items], dtype=float - ), - axis=0, - ) - - if ( - profile.hand.layout_id == G20_RIGHT_19_LAYOUT - and profile.base_pose_strategy == "full_hand" - ): - # In the product layout none of the four finger-roll zeros is a - # mechanical prior. Using one finger's pitch axis to orient the - # palm would therefore absorb that finger's roll zero into the - # palm pose. The named positions of five parallel root lines do - # observe rotation about their common direction, independent of - # all five root-joint zeros. Fit that transverse line pattern and - # leave the unobservable translation along the common direction - # at zero; all zero-sensitive phase residuals use line-to-line - # differences and are invariant to that gauge. - predicted_common = undirected_axis_average( - [predicted_axes[name] for name in root_names] - ) - observed_common = undirected_axis_average( - [observed_axes[name] for name in root_names] - ) - predicted_center = np.mean( - np.asarray([predicted_points[name] for name in root_names]), - axis=0, - ) - observed_center = np.mean( - np.asarray([observed_points[name] for name in root_names]), - axis=0, - ) - predicted_pattern = { - name: ( - (delta := predicted_points[name] - predicted_center) - - predicted_common * float(delta @ predicted_common) - ) - for name in root_names - } - observed_pattern = { - name: ( - (delta := observed_points[name] - observed_center) - - observed_common * float(delta @ observed_common) - ) - for name in root_names - } - if max( - np.linalg.norm(value) for value in predicted_pattern.values() - ) < 0.003: - raise ValueError("root axis-line pattern is degenerate") - if max( - np.linalg.norm(value) for value in observed_pattern.values() - ) < 0.003: - raise ValueError("observed root axis-line pattern is degenerate") - - def align_axis(source: np.ndarray, target: np.ndarray) -> Rotation: - source = source / np.linalg.norm(source) - target = target / np.linalg.norm(target) - cross = np.cross(source, target) - cross_norm = float(np.linalg.norm(cross)) - dot = float(np.clip(source @ target, -1.0, 1.0)) - if cross_norm > 1.0e-10: - return Rotation.from_rotvec( - cross / cross_norm * math.atan2(cross_norm, dot) - ) - if dot > 0.0: - return Rotation.identity() - basis = np.eye(3)[int(np.argmin(np.abs(source)))] - axis = np.cross(source, basis) - axis /= np.linalg.norm(axis) - return Rotation.from_rotvec(axis * math.pi) - - candidates: list[tuple[float, Rotation, np.ndarray]] = [] - for sign in (1.0, -1.0): - signed_observed_axis = sign * observed_common - axis_rotation = align_axis( - predicted_common, signed_observed_axis - ) - mapped_pattern = { - name: axis_rotation.apply(predicted_pattern[name]) - for name in root_names - } - cosine = sum( - float(mapped_pattern[name] @ observed_pattern[name]) - for name in root_names - ) - sine = sum( - float( - signed_observed_axis - @ np.cross( - mapped_pattern[name], observed_pattern[name] - ) - ) - for name in root_names - ) - phase_rotation = Rotation.from_rotvec( - signed_observed_axis * math.atan2(sine, cosine) - ) - rotation = phase_rotation * axis_rotation - translation_samples = [] - for name in root_names: - delta = observed_points[name] - rotation.apply( - predicted_points[name] - ) - translation_samples.append( - delta - - signed_observed_axis - * float(delta @ signed_observed_axis) - ) - translation = np.median( - np.asarray(translation_samples), axis=0 - ) - - # Root-axis points recovered from planar Tags occasionally - # contain a large but repeatable depth/line-position outlier. - # A plain Procrustes sum lets one such point rotate the palm - # frame and then makes the same bias look like a common zero - # offset on all four finger-roll joints. Refine only the - # rigid palm pose with a millimetre-scale robust loss. At - # least three mutually consistent named root lines therefore - # determine the transverse pattern while an outlier remains - # visible in the line diagnostics below. - transverse_helper = np.eye(3)[ - int(np.argmin(np.abs(signed_observed_axis))) - ] - transverse_first = np.cross( - signed_observed_axis, transverse_helper - ) - transverse_first /= np.linalg.norm(transverse_first) - transverse_second = np.cross( - signed_observed_axis, transverse_first - ) - - def robust_root_pattern_residual( - value: np.ndarray, - ) -> np.ndarray: - candidate_rotation = ( - Rotation.from_rotvec( - signed_observed_axis * float(value[0]) - ) - * rotation - ) - candidate_translation = ( - translation - + transverse_first * float(value[1]) - + transverse_second * float(value[2]) - ) - residuals: list[float] = [] - for name in root_names: - delta = observed_points[name] - ( - candidate_rotation.apply(predicted_points[name]) - + candidate_translation - ) - residuals.extend( - ( - float(delta @ transverse_first) / 0.001, - float(delta @ transverse_second) / 0.001, - ) - ) - return np.asarray(residuals, dtype=float) - - root_pattern_solution = least_squares( - robust_root_pattern_residual, - np.zeros(3, dtype=float), - bounds=( - np.asarray([-math.pi, -0.25, -0.25]), - np.asarray([math.pi, 0.25, 0.25]), - ), - loss="soft_l1", - f_scale=1.0, - max_nfev=1000, - ) - if not root_pattern_solution.success: - raise ValueError("robust root axis-line pattern fit failed") - rotation = ( - Rotation.from_rotvec( - signed_observed_axis - * float(root_pattern_solution.x[0]) - ) - * rotation - ) - translation = ( - translation - + transverse_first * float(root_pattern_solution.x[1]) - + transverse_second * float(root_pattern_solution.x[2]) - ) - axis_errors: list[float] = [] - for item in selected: - predicted_axis, _ = predicted_local(item, pose_zero_offsets) - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - axis_errors.append( - math.acos( - abs( - float( - np.clip( - rotation.apply(predicted_axis) - @ observed_axis, - -1.0, - 1.0, - ) - ) - ) - ) - ) - line_errors = [] - for name in root_names: - predicted_point = ( - rotation.apply(predicted_points[name]) + translation - ) - delta = observed_points[name] - predicted_point - line_errors.append( - float( - np.linalg.norm( - delta - - signed_observed_axis - * float(delta @ signed_observed_axis) - ) - ) - ) - score = float(np.mean(np.square(axis_errors))) + float( - np.mean(np.square(np.asarray(line_errors) / 0.01)) - ) - candidates.append((score, rotation, translation)) - _, rotation, translation = min( - candidates, key=lambda item: item[0] - ) - non_root_items = [ - item - for item in selected - if item.joint not in profile.root_anchor_joints - ] - if not non_root_items: - raise ValueError( - "non-parallel axes are required for palm axial translation" - ) - - def axial_translation_residual(value: np.ndarray) -> np.ndarray: - candidate_translation = ( - translation + observed_common * float(value[0]) - ) - residuals: list[float] = [] - for item in non_root_items: - predicted_axis, predicted_point = predicted_local( - item, pose_zero_offsets - ) - predicted_axis = rotation.apply(predicted_axis) - predicted_point = ( - rotation.apply(predicted_point) - + candidate_translation - ) - observed_axis = np.asarray( - item.axis_common_xyz, dtype=float - ) - if float(predicted_axis @ observed_axis) < 0.0: - observed_axis = -observed_axis - delta = np.asarray( - item.point_common_xyz_m, dtype=float - ) - predicted_point - perpendicular = delta - observed_axis * float( - delta @ observed_axis - ) - residuals.extend( - float(component) / 0.001 - for component in perpendicular - ) - return np.asarray(residuals, dtype=float) - - axial_solution = least_squares( - axial_translation_residual, - np.asarray([0.0]), - bounds=(np.asarray([-1.0]), np.asarray([1.0])), - loss="soft_l1", - f_scale=1.0, - ) - if not axial_solution.success: - raise ValueError("palm axial translation fit failed") - translation = ( - translation - + observed_common * float(axial_solution.x[0]) - ) - return rotation, translation - - # Legacy layouts have only one observed finger chain. Their - # non-parallel reference-finger pitch axis remains the orientation - # anchor; the product profile above deliberately does not use it. - primary = max( - root_names, - key=lambda name: float( - np.ptp(np.asarray(curves[name].angle_rad)) - ), - ) - orientation_anchor = ( - "thumb_cmc_pitch" - if profile.base_pose_strategy == "thumb_serial" - else f"{profile.reference_finger}_mcp_pitch" - ) - orientation_items = [ - item for item in selected if item.joint == orientation_anchor - ] - if not orientation_items: - raise ValueError( - f"zero solve is missing palm orientation anchor " - f"{orientation_anchor}" - ) - predicted_orientation_axis = undirected_axis_average( - [ - predicted_local(item, pose_zero_offsets)[0] - for item in orientation_items - ] - ) - observed_orientation_axis = undirected_axis_average( - [item.axis_common_xyz for item in orientation_items] - ) - - def frame_from_two_axes( - primary_axis: np.ndarray, orientation_axis: np.ndarray - ) -> np.ndarray: - first = primary_axis / np.linalg.norm(primary_axis) - second = orientation_axis - first * float(orientation_axis @ first) - second_norm = float(np.linalg.norm(second)) - if second_norm < math.sin(ZERO_MINIMUM_AXIS_CONE_RAD): - raise ValueError("palm orientation axes are nearly parallel") - second /= second_norm - return np.column_stack((first, second, np.cross(first, second))) - - predicted_frame = frame_from_two_axes( - predicted_axes[primary], - predicted_orientation_axis, - ) - candidates: list[tuple[float, Rotation, np.ndarray]] = [] - for primary_sign in (1.0, -1.0): - for orientation_sign in (1.0, -1.0): - observed_frame = frame_from_two_axes( - primary_sign * observed_axes[primary], - orientation_sign * observed_orientation_axis, - ) - rotation = Rotation.from_matrix( - observed_frame @ predicted_frame.T - ) - translation = np.median( - np.asarray( - [ - observed_points[name] - - rotation.apply(predicted_points[name]) - for name in root_names - ] - ), - axis=0, - ) - - # Resolve both undirected-axis sign branches using every - # distinct measured joint. This selects a palm-frame branch; - # it does not fit link geometry or encoder offsets. - errors_by_joint: dict[str, list[float]] = {} - for item in selected: - predicted_axis, _ = predicted_local(item, pose_zero_offsets) - predicted_axis = rotation.apply(predicted_axis) - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - error = math.acos( - abs( - float( - np.clip( - predicted_axis @ observed_axis, -1.0, 1.0 - ) - ) - ) - ) - errors_by_joint.setdefault(item.joint, []).append(error) - branch_score = sum( - min(float(np.median(values)), math.radians(30.0)) ** 2 - for values in errors_by_joint.values() - ) - candidates.append((branch_score, rotation, translation)) - _, rotation, translation = min(candidates, key=lambda item: item[0]) - return rotation, translation - - def phase_error( - predicted_parent_axis: np.ndarray, - predicted_parent_point: np.ndarray, - predicted_child_point: np.ndarray, - observed_parent_axis: np.ndarray, - observed_parent_point: np.ndarray, - observed_child_point: np.ndarray, - view_normal_common: Sequence[float] | None = None, - ) -> float: - parent_axis = predicted_parent_axis / np.linalg.norm(predicted_parent_axis) - observed_axis = observed_parent_axis / np.linalg.norm(observed_parent_axis) - if float(parent_axis @ observed_axis) < 0.0: - observed_axis = -observed_axis - predicted_delta = predicted_child_point - predicted_parent_point - observed_delta = observed_child_point - observed_parent_point - predicted_radial = predicted_delta - parent_axis * float( - predicted_delta @ parent_axis - ) - observed_radial = observed_delta - observed_axis * float( - observed_delta @ observed_axis - ) - angle_axis = parent_axis - if view_normal_common is not None: - view_normal = np.asarray(view_normal_common, dtype=float) - normal_norm = float(np.linalg.norm(view_normal)) - if normal_norm > 1.0e-9: - view_normal /= normal_norm - # When looking approximately along the rotation axis, image - # x/y contains the complete radial phase while optical depth - # is both unnecessary and much noisier for a 16 mm planar - # Tag. For an edge-on axis the depth component is genuinely - # needed for observability, so retain the full 3-D residual. - if abs(float(parent_axis @ view_normal)) >= math.cos( - math.radians(45.0) - ): - # Orthographic image-plane displacement is invariant to - # independent optical-depth bias on the two planar Tags. - # With an end-on axis, along-axis placement also projects - # to zero (or a small component for a mildly oblique view). - predicted_radial = predicted_delta - view_normal * float( - predicted_delta @ view_normal - ) - observed_radial = observed_delta - view_normal * float( - observed_delta @ view_normal - ) - angle_axis = view_normal - if float(angle_axis @ parent_axis) < 0.0: - angle_axis = -angle_axis - predicted_radius = float(np.linalg.norm(predicted_radial)) - observed_radius = float(np.linalg.norm(observed_radial)) - if min(predicted_radius, observed_radius) < 0.003: - raise ValueError("parallel-axis radial phase is not observable") - predicted_radial /= predicted_radius - observed_radial /= observed_radius - return math.atan2( - float(angle_axis @ np.cross(predicted_radial, observed_radial)), - float(np.clip(predicted_radial @ observed_radial, -1.0, 1.0)), - ) - - def robust_circular_location(values: Sequence[float]) -> float: - array = np.asarray(values, dtype=float) - if array.size == 0: - raise ValueError("circular residual set is empty") - centre = math.atan2( - float(np.mean(np.sin(array))), - float(np.mean(np.cos(array))), - ) - centred = np.arctan2(np.sin(array - centre), np.cos(array - centre)) - return float( - math.atan2( - math.sin(centre + float(np.median(centred))), - math.cos(centre + float(np.median(centred))), - ) - ) - - def same_view_axis_pair_errors( - offset_joint: str, - selected: Sequence[JointAxisMeasurement], - offsets: Mapping[str, float], - ) -> tuple[float, ...]: - """Return the camera-frame-invariant residual for a serial-axis pair. - - This observation intentionally has two mirror roots. The optimiser - disambiguates them with the original cross-view estimate, but the - pair residual alone determines the final numerical zero. - """ - pair = profile.same_view_axis_pair_by_offset.get(offset_joint) - if pair is None: - return () - anchor_joint, observer_joint = pair - - def angle(left: np.ndarray, right: np.ndarray) -> float: - cosine = float(left @ right) / float( - np.linalg.norm(left) * np.linalg.norm(right) - ) - return math.acos(abs(float(np.clip(cosine, -1.0, 1.0)))) - - errors: list[float] = [] - for cycle in sorted({int(item.cycle) for item in selected}): - anchor = orientation_by_model_cycle.get((anchor_joint, cycle)) - observer = orientation_by_model_cycle.get( - (observer_joint, cycle) - ) - if anchor is None or observer is None: - return () - angles = _angles_from_state( - observer.condition_state_u8, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - predicted_anchor, _ = model.axis_line( - anchor_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - predicted_observer, _ = model.axis_line( - observer_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - observed_anchor = np.asarray( - anchor.axis_common_xyz, dtype=float - ) - observed_observer = np.asarray( - observer.axis_common_xyz, dtype=float - ) - errors.append( - angle(predicted_anchor, predicted_observer) - - angle(observed_anchor, observed_observer) - ) - return tuple(errors) - - def angular_error_samples( - offsets: Mapping[str, float], - selected: Sequence[JointAxisMeasurement], - base_rotation: Rotation, - base_translation: np.ndarray, - ) -> dict[str, tuple[float, ...]]: - by_key = {(item.joint, item.cycle): item for item in selected} - errors: dict[str, list[float]] = { - name: [] for name in profile.direct_zero_joints - } - for offset_joint, observer_joint in profile.offset_observer_joint.items(): - pair_errors = same_view_axis_pair_errors( - offset_joint, selected, offsets - ) - if pair_errors: - errors[offset_joint].extend(pair_errors) - continue - observer_items = [ - item for item in selected if item.joint == observer_joint - ] - if not observer_items: - raise ValueError(f"zero observer is missing: {observer_joint}") - for item in observer_items: - observer_axis, observer_point = predicted_local(item, offsets) - observer_axis = base_rotation.apply(observer_axis) - observer_point = ( - base_rotation.apply(observer_point) + base_translation - ) - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - if observer_joint in profile.axis_parent_joint: - parent_joint = profile.axis_parent_joint[observer_joint] - angles = _angles_from_state( - ( - item.condition_state_u8 - if item.condition_command_u8 is None - else item.condition_command_u8 - ), - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - parent_axis, _ = model.axis_line( - parent_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - parent_axis = base_rotation.apply(parent_axis) - error = _zero_sensitive_axis_error_rad( - observer_axis, observed_axis, parent_axis - ) - else: - parent_joint = profile.phase_parent_joint[observer_joint] - observed_parent = by_key.get((parent_joint, item.cycle)) - if observed_parent is None: - raise ValueError( - f"phase parent is missing: {parent_joint} cycle {item.cycle}" - ) - angles = _angles_from_state( - ( - item.condition_state_u8 - if item.condition_command_u8 is None - else item.condition_command_u8 - ), - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=profile.inherited_zero_joints, - ) - parent_axis, parent_point = model.axis_line( - parent_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - parent_axis = base_rotation.apply(parent_axis) - parent_point = ( - base_rotation.apply(parent_point) + base_translation - ) - error = phase_error( - parent_axis, - parent_point, - observer_point, - np.asarray(observed_parent.axis_common_xyz, dtype=float), - np.asarray(observed_parent.point_common_xyz_m, dtype=float), - np.asarray(item.point_common_xyz_m, dtype=float), - item.view_normal_common_xyz, - ) - errors[offset_joint].append(float(error)) - return { - name: tuple(values) - for name, values in errors.items() - if values - } - - def angular_errors( - offsets: Mapping[str, float], - selected: Sequence[JointAxisMeasurement], - base_rotation: Rotation, - base_translation: np.ndarray, - ) -> dict[str, float]: - return { - name: robust_circular_location(values) - for name, values in angular_error_samples( - offsets, selected, base_rotation, base_translation - ).items() - } - - product_finger_rolls = tuple( - name - for name in profile.direct_zero_joints - if name.endswith("_mcp_roll") and not name.startswith("thumb_") - ) - offset_limits = np.asarray( - [ - joint_limits.get( - name, - ( - finger_maximum_offset_rad - if name.startswith(("index_", "middle_", "ring_", "pinky_")) - else maximum_offset_rad - ), - ) - for name in profile.direct_zero_joints - ], - dtype=float, - ) - # Do not make a configured safety limit the numerical optimizer's bound. - # Otherwise a genuine out-of-range estimate and a modelling failure both - # collapse to exactly +/-20 or +/-3 degrees, which hides the magnitude and - # encourages pointless rescans. Search farther for diagnostics, then keep - # the original configured limits as unchanged pass/fail gates below. - diagnostic_offset_limits = np.minimum( - math.radians(90.0), - np.maximum(3.0 * offset_limits, offset_limits + math.radians(5.0)), - ) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: - # The absolute rotation of the fitted palm frame about the four - # parallel roll axes is a shared gauge. Each physical finger zero is - # only its deviation from the four-finger median, but the raw scalar - # solves include that common mode. Give those four diagnostic solves - # enough range for the unchanged global common-mode limit plus the - # unchanged per-finger deviation limit; applying the per-finger bound - # before removing the gauge clips every solve to the same value and - # destroys the observable deviations. - for index, name in enumerate(profile.direct_zero_joints): - if name in product_finger_rolls: - diagnostic_offset_limits[index] = min( - math.radians(90.0), - maximum_offset_rad + offset_limits[index], - ) - - def optimise_offsets( - selected: Sequence[JointAxisMeasurement], - base_rotation: Rotation, - base_translation: np.ndarray, - initial: Mapping[str, float] | None = None, - ) -> dict[str, float]: - # The observer graph is a pair of serial chains. Solve it in that - # dependency order so a bad distal phase cannot pull an already - # observable proximal zero toward a bound. Each scalar residual keeps - # the individual cycle values; robust loss then downweights one noisy - # cycle instead of hiding it in a seven-variable compromise. - result = {name: 0.0 for name in profile.direct_zero_joints} - if initial is not None: - result.update( - { - name: float(initial[name]) - for name in profile.direct_zero_joints - } - ) - result.update(fixed_offsets) - for name, diagnostic_limit, configured_limit in zip( - profile.direct_zero_joints, - diagnostic_offset_limits, - offset_limits, - ): - if name in fixed_offsets: - continue - has_axis_pair = name in profile.same_view_axis_pair_by_offset - limit = float( - configured_limit if has_axis_pair else diagnostic_limit - ) - - def residual(value: np.ndarray) -> np.ndarray: - candidate = dict(result) - candidate[name] = float(value[0]) - samples = angular_error_samples( - candidate, selected, base_rotation, base_translation - )[name] - return np.asarray( - [ - math.atan2(math.sin(item), math.cos(item)) - / math.radians(1.0) - for item in samples - ], - dtype=float, - ) - - starts = [0.0, -0.5 * limit, 0.5 * limit] - if initial is not None: - starts.append(float(initial[name])) - solutions = [ - least_squares( - residual, - np.asarray( - [np.clip(start, -limit + 1.0e-9, limit - 1.0e-9)] - ), - bounds=(np.asarray([-limit]), np.asarray([limit])), - loss="soft_l1", - f_scale=1.0, - max_nfev=1000, - ) - for start in starts - ] - solution = min( - solutions, key=lambda item: float(np.sum(np.square(item.fun))) - ) - if not solution.success: - raise ValueError( - f"URDF zero optimization failed for {name}: " - f"{solution.message}" - ) - result[name] = float(solution.x[0]) - return result - - def solve_selected( - selected: Sequence[JointAxisMeasurement], - initial: Mapping[str, float] | None = None, - ) -> tuple[Rotation, np.ndarray, dict[str, float]]: - rotation, translation = fit_base_pose(selected) - offsets = optimise_offsets( - selected, rotation, translation, initial=initial - ) - if profile.base_pose_strategy == "thumb_serial": - # The first pose estimate only supplies a branch for the invariant - # same-view yaw solve. Refit after yaw is known so that the - # downstream pitch phase is evaluated in the corrected serial - # thumb frame, then freeze that pose for holdout validation. - rotation, translation = fit_base_pose(selected, offsets) - offsets = optimise_offsets( - selected, rotation, translation, initial=offsets - ) - return rotation, translation, offsets - - base_rotation, base_translation, training_offsets = solve_selected(training) - - def observability_residual(parameters: np.ndarray) -> np.ndarray: - rotation = Rotation.from_rotvec(parameters[:3]) - translation = parameters[3:6] * 0.05 - offsets = { - name: float(value) - for name, value in zip( - profile.direct_zero_joints, parameters[6:] - ) - } - residuals: list[float] = [] - for item in training: - predicted_axis, predicted_point = predicted_local(item, offsets) - predicted_axis = rotation.apply(predicted_axis) - predicted_axis /= np.linalg.norm(predicted_axis) - predicted_point = rotation.apply(predicted_point) + translation - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - observed_axis /= np.linalg.norm(observed_axis) - if float(predicted_axis @ observed_axis) < 0.0: - observed_axis = -observed_axis - observed_point = np.asarray(item.point_common_xyz_m, dtype=float) - predicted_moment = np.cross(predicted_point, predicted_axis) - observed_moment = np.cross(observed_point, observed_axis) - residuals.extend(float(value) for value in predicted_axis - observed_axis) - residuals.extend( - float(value) / 0.05 - for value in predicted_moment - observed_moment - ) - return np.asarray(residuals, dtype=float) - - observability_parameters = np.asarray( - [ - *base_rotation.as_rotvec(), - *(base_translation / 0.05), - *( - training_offsets[name] - for name in profile.direct_zero_joints - ), - ], - dtype=float, - ) - observability_base_residual = observability_residual( - observability_parameters - ) - observability_jacobian = np.empty( - ( - observability_base_residual.size, - observability_parameters.size, - ), - dtype=float, - ) - finite_difference_step = 1.0e-6 - for column in range(observability_parameters.size): - positive = observability_parameters.copy() - negative = observability_parameters.copy() - positive[column] += finite_difference_step - negative[column] -= finite_difference_step - observability_jacobian[:, column] = ( - observability_residual(positive) - - observability_residual(negative) - ) / (2.0 * finite_difference_step) - singular_values = np.linalg.svd( - observability_jacobian, compute_uv=False - ) - singular_threshold = ( - 0.0 - if singular_values.size == 0 - else float(singular_values[0]) * 1.0e-7 - ) - observability_rank = int( - np.count_nonzero(singular_values > singular_threshold) - ) - observability_parameter_count = int(observability_parameters.size) - observability_condition_number = ( - float("inf") - if observability_rank < observability_parameter_count - else float(singular_values[0] / singular_values[-1]) - ) - residual_dof = max( - 1, - observability_base_residual.size - observability_parameter_count, - ) - residual_variance = float( - observability_base_residual @ observability_base_residual - ) / residual_dof - covariance = residual_variance * np.linalg.pinv( - observability_jacobian.T @ observability_jacobian, - rcond=1.0e-12, - ) - offset_covariance = { - name: max(0.0, float(covariance[6 + index, 6 + index])) - for index, name in enumerate(profile.direct_zero_joints) - } - - axis_cone_mismatch_by_joint: dict[str, float] = {} - axis_cone_bias_classification_by_joint: dict[str, str] = {} - - def zero_observation_failure_reasons( - selected: Sequence[JointAxisMeasurement], - offsets: Mapping[str, float], - rotation: Rotation, - ) -> dict[str, str]: - """Reject repeatable but geometrically inadmissible zero observers.""" - by_key = {(item.joint, item.cycle): item for item in selected} - failures: dict[str, str] = {} - for offset_joint, observer_joint in profile.offset_observer_joint.items(): - if offset_joint in fixed_offsets: - continue - observer_items = [ - item for item in selected if item.joint == observer_joint - ] - if observer_joint in profile.axis_parent_joint: - parent_joint = profile.axis_parent_joint[observer_joint] - cone_mismatches: list[float] = [] - for item in observer_items: - state = ( - item.condition_state_u8 - if item.condition_command_u8 is None - else item.condition_command_u8 - ) - angles = _angles_from_state( - state, - curves=curves, - motor_by_joint=motor_by_joint, - inherited_zero_joints=( - profile.inherited_zero_joints - ), - ) - predicted_axis, _ = model.axis_line( - observer_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - parent_axis, _ = model.axis_line( - parent_joint, - zero_offsets=offsets, - joint_angles=angles, - ) - cone_mismatches.append( - _axis_cone_mismatch_rad( - rotation.apply(predicted_axis), - item.axis_common_xyz, - rotation.apply(parent_axis), - ) - ) - maximum_cone_mismatch = max(cone_mismatches) - axis_cone_mismatch_by_joint[offset_joint] = ( - maximum_cone_mismatch - ) - if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad: - cone_range = float(np.ptp(cone_mismatches)) - stable_product_bias = bool( - profile.hand.stable_cross_view_cone_bias - and maximum_systematic_axis_cone_bias_rad is not None - and len(cone_mismatches) >= 4 - and maximum_cone_mismatch - <= maximum_systematic_axis_cone_bias_rad - and cone_range <= maximum_cycle_difference_rad - ) - if stable_product_bias: - # A parent zero rotates the downstream direction - # around the parent axis and cannot change their cone - # angle. The zero-sensitive residual above already - # projects both directions onto the parent-normal - # plane, so a repeatable cross-camera/planar-PnP cone - # bias cannot corrupt the written encoder zero. Keep - # it visible in the result while retaining the gross - # gate for a wrong axis, loose Tag, or moved camera. - axis_cone_bias_classification_by_joint[offset_joint] = ( - "stable_cross_view_or_planar_pnp_bias" - ) - else: - failures[offset_joint] = ( - "zero_axis_cone_mismatch_too_large" - ) - else: - parent_joint = profile.phase_parent_joint[observer_joint] - phase_items: list[JointAxisMeasurement] = [] - propagated_angle_uncertainties: list[float] = [] - for item in observer_items: - phase_items.append(item) - parent_item = by_key.get((parent_joint, item.cycle)) - if parent_item is not None: - phase_items.append(parent_item) - parent_axis = np.asarray( - parent_item.axis_common_xyz, dtype=float - ) - parent_axis /= np.linalg.norm(parent_axis) - separation = ( - np.asarray(item.point_common_xyz_m, dtype=float) - - np.asarray( - parent_item.point_common_xyz_m, dtype=float - ) - ) - radial = separation - parent_axis * float( - separation @ parent_axis - ) - effective_distance = float(np.linalg.norm(radial)) - if effective_distance <= 1.0e-6: - propagated_angle_uncertainties.append(float("inf")) - else: - line_uncertainty = math.hypot( - item.pose_axis_line_rms_m, - parent_item.pose_axis_line_rms_m, - ) - propagated_angle_uncertainties.append( - math.atan2( - line_uncertainty, effective_distance - ) - ) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT: - angle_limit = ( - maximum_confidence_half_width_rad - if maximum_confidence_half_width_rad is not None - else math.radians(0.5) - ) - if ( - not propagated_angle_uncertainties - or max(propagated_angle_uncertainties) > angle_limit - ): - failures[offset_joint] = ( - "zero_phase_axis_line_angle_uncertainty_too_large" - ) - elif ( - profile.hand.layout_id != G20_RIGHT_19_LAYOUT - and any( - item.pose_axis_line_rms_m - > maximum_pose_axis_line_rms_m - for item in phase_items - ) - ): - failures[offset_joint] = ( - "zero_phase_axis_line_residual_too_large" - ) - return failures - - observation_failures = zero_observation_failure_reasons( - list(measurements), training_offsets, base_rotation - ) - cycle_values: dict[str, list[float]] = { - name: [] for name in profile.direct_zero_joints - } - cycle_models: dict[int, tuple[Rotation, np.ndarray]] = {} - training_cycle_ids = tuple(sorted({int(value) for value in training_cycles})) - for cycle in training_cycle_ids: - selected = [item for item in measurements if item.cycle == cycle] - # Keep one palm pose while comparing cycles. Refitting a base pose from - # only two nearly parallel root axes per cycle makes harmless root-line - # noise appear as a large encoder-zero change. - cycle_rotation, cycle_translation = base_rotation, base_translation - cycle_offsets = optimise_offsets( - selected, - cycle_rotation, - cycle_translation, - initial=training_offsets, - ) - cycle_models[cycle] = (cycle_rotation, cycle_translation) - for name, value in cycle_offsets.items(): - cycle_values[name].append(value) - uncertainties: dict[str, float] = {} - confidence_half_widths: dict[str, float] = {} - cycle_consistent = True - inconsistent_cycles: list[str] = [] - for name, values in cycle_values.items(): - array = np.asarray(values, dtype=float) - spread = float(np.max(array) - np.min(array)) - if spread > maximum_cycle_difference_rad: - cycle_consistent = False - inconsistent_cycles.append(name) - uncertainties[name] = ( - 0.0 - if array.size < 2 - else float(np.std(array, ddof=1) / math.sqrt(array.size)) - ) - confidence_half_widths[name] = ( - float("inf") - if array.size < 2 - else float( - student_t.ppf(0.975, df=array.size - 1) - * uncertainties[name] - ) - ) - - insignificant_large: list[str] = [] - applied_training: dict[str, float] = {} - for name, value in training_offsets.items(): - if name in fixed_offsets: - applied_training[name] = fixed_offsets[name] - continue - uncertainty = uncertainties[name] - confidence_half_width = confidence_half_widths[name] - if ( - abs(value) < minimum_applied_offset_rad - or abs(value) - <= max(significance_sigma * uncertainty, confidence_half_width) - ): - applied_training[name] = 0.0 - if abs(value) >= minimum_applied_offset_rad: - insignificant_large.append(name) - else: - applied_training[name] = value - - candidate_errors = angular_errors( - applied_training, validation, base_rotation, base_translation - ) - original_errors = angular_errors( - zero_offsets, validation, base_rotation, base_translation - ) - validation_error_by_joint: dict[str, float] = {} - original_error_by_joint: dict[str, float] = {} - improvement_by_joint: dict[str, float] = {} - improvement_confidence_lower: dict[str, float] = {} - improvement_passed = True - if profile.same_view_axis_pair_by_offset: - # Validate the same camera/Tag-mount-invariant axis-pair angle used by - # the configured offset. This keeps the side channel out of every - # other joint, including the already stable thumb CMC roll solve. - palm_orientation_validation_errors = tuple( - abs(float(error)) - for offset_joint in profile.same_view_axis_pair_by_offset - for error in same_view_axis_pair_errors( - offset_joint, validation, applied_training - ) - ) - else: - palm_orientation_validation_errors = tuple( - math.acos( - abs( - float( - np.clip( - base_rotation.apply( - predicted_palm_orientation_local( - item, zero_offsets - ) - ) - @ np.asarray(item.axis_common_xyz, dtype=float), - -1.0, - 1.0, - ) - ) - ) - ) - for item in orientation_validation - ) - palm_orientation_validation_limit = ( - maximum_validation_error_rad - if maximum_validation_error_rad is not None - else maximum_axis_cone_mismatch_rad - ) - palm_orientation_validation_passed = bool( - not configured_palm_sources - or ( - palm_orientation_validation_errors - and float(np.median(palm_orientation_validation_errors)) - <= palm_orientation_validation_limit - ) - ) - for offset_joint, observer_joint in profile.offset_observer_joint.items(): - if offset_joint in fixed_offsets: - improvement_by_joint[offset_joint] = 0.0 - improvement_confidence_lower[offset_joint] = 0.0 - continue - candidate = abs(candidate_errors[offset_joint]) - original = abs(original_errors[offset_joint]) - validation_error_by_joint[observer_joint] = candidate - original_error_by_joint[observer_joint] = original - improvement_by_joint[offset_joint] = original - candidate - if applied_training[offset_joint] != 0.0 and not candidate < original: - improvement_passed = False - for offset_joint in profile.direct_zero_joints: - if offset_joint in fixed_offsets: - improvement_confidence_lower[offset_joint] = 0.0 - continue - if applied_training[offset_joint] == 0.0: - improvement_confidence_lower[offset_joint] = 0.0 - continue - cycle_improvements: list[float] = [] - for cycle in training_cycle_ids: - selected = [item for item in measurements if item.cycle == cycle] - cycle_rotation, cycle_translation = cycle_models[cycle] - candidate = abs( - angular_errors( - applied_training, - selected, - cycle_rotation, - cycle_translation, - )[offset_joint] - ) - original = abs( - angular_errors( - zero_offsets, - selected, - cycle_rotation, - cycle_translation, - )[offset_joint] - ) - cycle_improvements.append(original - candidate) - improvement_array = np.asarray(cycle_improvements, dtype=float) - improvement_se = ( - float("inf") - if improvement_array.size < 2 - else float( - np.std(improvement_array, ddof=1) - / math.sqrt(improvement_array.size) - ) - ) - lower = ( - float("-inf") - if not math.isfinite(improvement_se) - else float( - np.mean(improvement_array) - - student_t.ppf( - 0.975, df=improvement_array.size - 1 - ) - * improvement_se - ) - ) - improvement_confidence_lower[offset_joint] = lower - if lower <= 0.0: - improvement_passed = False - - validation_errors = np.asarray( - list(validation_error_by_joint.values()), dtype=float - ) - validation_line_samples: dict[str, list[float]] = {} - for item in validation: - predicted_axis, predicted_point = predicted_local( - item, applied_training - ) - predicted_axis = base_rotation.apply(predicted_axis) - predicted_axis /= np.linalg.norm(predicted_axis) - predicted_point = ( - base_rotation.apply(predicted_point) + base_translation - ) - observed_axis = np.asarray(item.axis_common_xyz, dtype=float) - observed_axis /= np.linalg.norm(observed_axis) - observed_point = np.asarray(item.point_common_xyz_m, dtype=float) - cross = np.cross(predicted_axis, observed_axis) - cross_norm = float(np.linalg.norm(cross)) - separation = observed_point - predicted_point - line_error = ( - abs(float(separation @ cross)) / cross_norm - if cross_norm > 1.0e-6 - else float( - np.linalg.norm( - separation - - predicted_axis * float(separation @ predicted_axis) - ) - ) - ) - validation_line_samples.setdefault(item.joint, []).append(line_error) - validation_line_error_by_joint = { - name: float(np.sqrt(np.mean(np.square(values)))) - for name, values in validation_line_samples.items() - } - all_validation_line_errors = np.asarray( - [ - value - for values in validation_line_samples.values() - for value in values - ], - dtype=float, - ) - axis_line_rms = ( - float("inf") - if all_validation_line_errors.size == 0 - else float( - np.sqrt(np.mean(np.square(all_validation_line_errors))) - ) - ) - finger_roll_common_mode = ( - float( - np.median( - [training_offsets[name] for name in product_finger_rolls] - ) - ) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT - and product_finger_rolls - else 0.0 - ) - configured_limit_exceeded: list[str] = [] - for name, limit in zip(profile.direct_zero_joints, offset_limits): - if name in fixed_offsets: - continue - # A post-solve mechanical endpoint datum is the value that will be - # published for this joint. The visual root-axis scalar remains a - # nuisance gauge used for holdout geometry and must not be compared - # with the safety bound of a different, endpoint-anchored output. - checked_offset = output_offsets.get(name, training_offsets[name]) - if name in product_finger_rolls: - # The four roll motors share the same electrical centre and the - # absolute palm axial datum is recovered from the root-line - # pattern. Protect the independently assembled finger-to-finger - # deviations with the strict finger bound; protect their shared - # common mode with the unchanged global zero bound. Treating the - # same common datum as four independent failures is both - # over-counting and sensitive to the palm-frame gauge. - checked_offset -= finger_roll_common_mode - if abs(checked_offset) > limit + math.radians(0.01): - configured_limit_exceeded.append(name) - if ( - product_finger_rolls - and abs(finger_roll_common_mode) - > maximum_offset_rad + math.radians(0.01) - ): - configured_limit_exceeded.append("finger_mcp_roll_common_mode") - diagnostic_bound_hits: list[str] = [] - for name, limit in zip( - profile.direct_zero_joints, diagnostic_offset_limits - ): - if name in fixed_offsets: - continue - checked_offset = output_offsets.get(name, training_offsets[name]) - if name in product_finger_rolls: - # Match the configured-limit and publication convention above. - # The raw common roll is a fitted-palm-frame gauge; only the - # finger-to-finger deviation is a physical zero correction. - checked_offset -= finger_roll_common_mode - if abs(checked_offset) >= limit - math.radians(0.01): - diagnostic_bound_hits.append(name) - failure_reasons: dict[str, str] = {} - if not palm_orientation_validation_passed: - failure_reasons["palm_orientation"] = ( - "palm_orientation_holdout_too_large" - ) - requires_full_observability = bool( - profile.hand.layout_id == G20_RIGHT_19_LAYOUT - and profile.base_pose_strategy == "full_hand" - ) - if ( - requires_full_observability - and observability_rank < observability_parameter_count - ): - failure_reasons["palm_and_static_zero"] = ( - "zero_observation_jacobian_rank_deficient" - ) - elif ( - requires_full_observability - and observability_condition_number - > maximum_observability_condition_number - ): - failure_reasons["palm_and_static_zero"] = ( - "zero_observation_jacobian_ill_conditioned" - ) - for name in configured_limit_exceeded: - failure_reasons[name] = "zero_offset_exceeds_configured_limit" - for name in diagnostic_bound_hits: - failure_reasons[name] = "zero_offset_reached_diagnostic_bound" - for name in inconsistent_cycles: - failure_reasons[name] = "zero_offset_cycle_difference_too_large" - for name in insignificant_large: - failure_reasons[name] = "zero_offset_not_statistically_significant" - if maximum_confidence_half_width_rad is not None: - for name, half_width in confidence_half_widths.items(): - if half_width > maximum_confidence_half_width_rad: - failure_reasons[name] = ( - "zero_offset_confidence_interval_too_wide" - ) - # This solver publishes rotational encoder zeros only. A post-fit CAD to - # measured axis-line displacement is invariant to the joint's own zero - # and cannot be repaired by changing that rotational parameter. Keep the - # per-joint and aggregate values in ZeroSolveResult for geometry audit, - # but do not misclassify a fixed link-origin/Tag-depth discrepancy as a - # failed rotational holdout. Axis-point *fit* quality is still guarded - # above for every phase observation that actually uses line position. - if not improvement_passed: - for name, value in applied_training.items(): - if ( - name not in fixed_offsets - and value != 0.0 - and improvement_confidence_lower.get(name, 0.0) <= 0.0 - ): - failure_reasons[name] = "zero_offset_did_not_improve_with_95pct_confidence" - # Geometry failures are the root cause and must not be hidden by the - # downstream validation symptom produced by the same bad observation. - failure_reasons.update(observation_failures) - passed = bool( - validation_errors.size - == len(profile.direct_zero_joints) - len(fixed_offsets) - and float(np.mean(validation_errors)) <= maximum_validation_mae_rad - and float(np.percentile(validation_errors, 95.0)) - <= maximum_validation_p95_rad - and ( - maximum_validation_error_rad is None - or float(np.max(validation_errors)) - <= maximum_validation_error_rad - ) - and cycle_consistent - and not insignificant_large - and not configured_limit_exceeded - and not diagnostic_bound_hits - and not observation_failures - and ( - not requires_full_observability - or ( - observability_rank == observability_parameter_count - and observability_condition_number - <= maximum_observability_condition_number - ) - ) - and not any( - reason == "zero_offset_confidence_interval_too_wide" - for reason in failure_reasons.values() - ) - and improvement_passed - and palm_orientation_validation_passed - ) - - # Never refit a model that has passed its holdout with the validation - # cycle. The published offsets are exactly the frozen training result - # that produced ``validation_errors`` above. - final_offsets = dict(applied_training) - # Apply independently validated assembly datums only after trajectory - # fitting and holdout validation. A mechanical prior must define the - # written artifact without perturbing downstream yaw/pitch estimates. - final_offsets.update(output_offsets) - if profile.hand.layout_id == G20_RIGHT_19_LAYOUT and product_finger_rolls: - # The camera solve observes the four roll axes in a fitted palm frame. - # Rotation of that frame about their shared datum is a gauge, not four - # independent finger assembly errors. The product command 127/CAD - # pose defines the common straight-ahead datum; publish only each - # finger's robust deviation from the four-finger median. Validation - # above remains in the observation gauge, so no measured residual is - # discarded. - for name in product_finger_rolls: - final_offsets[name] = ( - float(final_offsets[name]) - finger_roll_common_mode - ) - - # Every active joint must be present in the runtime payload/URDF writer, - # but absence of an absolute observation is not evidence for the - # reference finger's assembly offset. Preserve the source-CAD zero for - # those independent motors while continuing to share their dynamic curve. - all_offsets = { - name: 0.0 for name in profile.hand.active_joints - } - all_offsets.update(final_offsets) - for target, source in profile.inherited_static_zero_joints.items(): - all_offsets[target] = final_offsets[source] - return ZeroSolveResult( - direct_offsets_rad=final_offsets, - all_active_offsets_rad=all_offsets, - base_translation_xyz_m=tuple(float(value) for value in base_translation), - base_quaternion_xyzw=tuple(float(value) for value in base_rotation.as_quat()), - validation_errors_rad=tuple(float(value) for value in validation_errors), - validation_error_by_joint_rad=validation_error_by_joint, - validation_line_error_by_joint_m=validation_line_error_by_joint, - axis_line_rms_m=axis_line_rms, - passed=passed, - cycle_offsets_rad={ - name: tuple(float(value) for value in values) - for name, values in cycle_values.items() - }, - offset_uncertainty_rad=uncertainties, - offset_confidence_half_width_rad=confidence_half_widths, - training_cycles=training_cycle_ids, - validation_cycle=int(validation_cycle), - validation_original_error_by_joint_rad=original_error_by_joint, - validation_improvement_by_joint_rad=improvement_by_joint, - validation_improvement_confidence_lower_rad=( - improvement_confidence_lower - ), - observability_rank=observability_rank, - observability_parameter_count=observability_parameter_count, - observability_condition_number=observability_condition_number, - offset_covariance_rad2=offset_covariance, - axis_cone_mismatch_by_joint_rad=axis_cone_mismatch_by_joint, - axis_cone_bias_classification_by_joint=( - axis_cone_bias_classification_by_joint - ), - failure_reasons=failure_reasons, - ) - - -def _files_have_identical_contents(left: Path, right: Path) -> bool: - if left.stat().st_size != right.stat().st_size: - return False - with left.open("rb") as left_stream, right.open("rb") as right_stream: - while True: - left_chunk = left_stream.read(1024 * 1024) - right_chunk = right_stream.read(1024 * 1024) - if left_chunk != right_chunk: - return False - if not left_chunk: - return True - - -def _materialize_relative_mesh_assets( - *, source: Path, output: Path, urdf_root: ET.Element -) -> tuple[Path, ...]: - """Copy relative mesh resources so a session-local URDF remains loadable.""" - filenames = sorted( - { - str(mesh.get("filename", "")).strip() - for mesh in urdf_root.findall(".//mesh") - if str(mesh.get("filename", "")).strip() - } - ) - materialized: list[Path] = [] - for filename in filenames: - # URI-backed resources are resolved by the URDF consumer. Only local - # relative resources need to follow a URDF copied to a session folder. - if "://" in filename or filename.startswith("package:"): - continue - relative = Path(filename) - if relative.is_absolute() or ".." in relative.parts: - raise ValueError( - f"URDF mesh path must be a safe relative path or URI: {filename}" - ) - source_asset = (source.parent / relative).resolve() - if not source_asset.is_file(): - raise ValueError(f"URDF mesh resource does not exist: {source_asset}") - destination_asset = (output / relative).resolve() - try: - destination_asset.relative_to(output) - except ValueError as error: - raise ValueError( - f"URDF mesh destination escapes output directory: {filename}" - ) from error - if destination_asset == source_asset: - materialized.append(destination_asset) - continue - destination_asset.parent.mkdir(parents=True, exist_ok=True) - if destination_asset.exists(): - if not destination_asset.is_file() or not _files_have_identical_contents( - source_asset, destination_asset - ): - raise ValueError( - f"refusing to overwrite a different mesh resource: " - f"{destination_asset}" - ) - materialized.append(destination_asset) - continue - temporary_asset = destination_asset.with_name( - f".{destination_asset.name}.{os.getpid()}.tmp" - ) - if temporary_asset.exists(): - raise ValueError(f"temporary mesh path is occupied: {temporary_asset}") - try: - shutil.copy2(source_asset, temporary_asset) - os.replace(temporary_asset, destination_asset) - finally: - if temporary_asset.exists(): - temporary_asset.unlink() - materialized.append(destination_asset) - return tuple(materialized) - - -def write_zero_corrected_urdf( - *, - source_urdf: str | Path, - output_directory: str | Path, - serial_number: str, - offsets_rad: Mapping[str, float], - endpoint_anchored_offsets_rad: Mapping[str, float] | None = None, - timestamp: str | None = None, - correction_plan: UrdfCorrectionPlan | None = None, -) -> Path: - source = Path(source_urdf).expanduser().resolve() - output = Path(output_directory).expanduser().resolve() - if not source.is_file(): - raise ValueError(f"source URDF does not exist: {source}") - if ( - "zero_calibrated" in source.stem.lower() - or re.search(r"calibrated_20\d{6}", source.stem.lower()) - ): - raise ValueError( - "source_urdf must be the original CAD URDF, not a calibrated URDF" - ) - if not offsets_rad: - raise ValueError("offsets_rad must contain at least one joint") - offsets = {str(name): float(value) for name, value in offsets_rad.items()} - endpoint_offsets = { - str(name): float(value) - for name, value in dict(endpoint_anchored_offsets_rad or {}).items() - } - if not set(endpoint_offsets) <= set(offsets): - raise ValueError("endpoint-anchored offsets must be URDF zero targets") - if correction_plan is not None: - correction_plan.verify_source(source) - correction_plan.authorize_offsets(offsets) - if not set(endpoint_offsets).issubset( - correction_plan.endpoint_limit_joints - ): - raise ValueError( - "endpoint offsets are not authorized by the correction plan" - ) - if any( - not math.isfinite(value) or abs(value) > math.radians(90.0) - for value in offsets.values() - ): - raise ValueError("URDF zero offsets must be finite and within +/-90deg") - output.mkdir(parents=True, exist_ok=True) - stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") - if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: - raise ValueError("URDF zero timestamp must use YYYYMMDD_HHMMSS") - safe_serial = "".join( - character if character.isalnum() or character in "_.-" else "_" - for character in str(serial_number) - ) - if not safe_serial: - raise ValueError("serial_number must not be empty") - destination = output / f"{source.stem}_zero_calibrated_{safe_serial}_{stamp}.urdf" - if destination == source or destination.exists(): - raise ValueError(f"refusing to overwrite URDF: {destination}") - tree = ET.parse(source) - root = tree.getroot() - original_text = source.read_text(encoding="utf-8") - replacement_rpy: dict[str, str] = {} - replacement_upper: dict[str, str] = {} - replacement_mimic_offset: dict[str, str] = {} - found: set[str] = set() - for joint in root.findall("joint"): - name = str(joint.get("name")) - if name not in offsets: - mimic = joint.find("mimic") - source_joint = None if mimic is None else str(mimic.get("joint")) - if source_joint in endpoint_offsets: - multiplier = float(mimic.get("multiplier", "1")) - old_offset = float(mimic.get("offset", "0")) - calibrated_mimic_offset = ( - old_offset + multiplier * endpoint_offsets[source_joint] - ) - replacement_mimic_offset[name] = ( - f"{calibrated_mimic_offset:.15g}" - ) - continue - found.add(name) - if name in endpoint_offsets: - limit = joint.find("limit") - if limit is None or limit.get("upper") is None: - raise ValueError(f"joint {name} has no upper limit") - replacement_upper[name] = f"{float(limit.get('upper')) - endpoint_offsets[name]:.15g}" - # A statistically insignificant correction is represented as exact - # zero. Preserve that joint's source text byte-for-byte instead of - # serialising an equivalent Euler triplet. - if abs(offsets[name]) <= 1.0e-15: - continue - if joint.get("type") not in {"revolute", "continuous"}: - raise ValueError(f"joint {name} is not revolute") - axis_node = joint.find("axis") - axis = _parse_triplet( - "1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0") - ) - axis_norm = float(np.linalg.norm(axis)) - if axis_norm <= 1.0e-12: - raise ValueError(f"joint {name} has a degenerate axis") - axis /= axis_norm - origin_node = joint.find("origin") - if origin_node is None: - raise ValueError( - f"joint {name} has no origin; refusing a non-minimal rewrite" - ) - rpy = _parse_triplet(origin_node.get("rpy", "0 0 0")) - original = Rotation.from_euler("xyz", rpy).as_matrix() - corrected = original @ Rotation.from_rotvec( - axis * offsets[name] - ).as_matrix() - corrected_rpy = Rotation.from_matrix(corrected).as_euler("xyz") - replacement_rpy[name] = " ".join( - f"{float(value):.15g}" for value in corrected_rpy - ) - missing = sorted(set(offsets) - found) - if missing: - raise ValueError("source URDF is missing target joints: " + ",".join(missing)) - joint_pattern = re.compile( - r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>" - r".*?", - re.DOTALL, - ) - edits: list[tuple[int, int, str]] = [] - for match in joint_pattern.finditer(original_text): - name = match.group("name") - if name not in replacement_rpy: - if name not in replacement_upper and name not in replacement_mimic_offset: - continue - block = match.group(0) - if name in replacement_rpy: - origin_match = re.search( - r"]*\brpy\s*=\s*([\"'])(?P[^\"']*)\1", - block, - re.DOTALL, - ) - if origin_match is None: - raise ValueError(f"joint {name} origin has no rpy attribute") - edits.append(( - match.start() + origin_match.start("rpy"), - match.start() + origin_match.end("rpy"), - replacement_rpy[name], - )) - if name in replacement_upper: - limit_match = re.search( - r"]*\bupper\s*=\s*([\"'])(?P[^\"']*)\1", - block, - re.DOTALL, - ) - if limit_match is None: - raise ValueError(f"joint {name} limit has no upper attribute") - edits.append(( - match.start() + limit_match.start("upper"), - match.start() + limit_match.end("upper"), - replacement_upper[name], - )) - if name in replacement_mimic_offset: - mimic_match = re.search( - r"]*\boffset\s*=\s*([\"'])(?P[^\"']*)\1", - block, - re.DOTALL, - ) - if mimic_match is None: - raise ValueError(f"joint {name} mimic has no offset attribute") - edits.append(( - match.start() + mimic_match.start("offset"), - match.start() + mimic_match.end("offset"), - replacement_mimic_offset[name], - )) - expected_edit_count = ( - len(replacement_rpy) - + len(replacement_upper) - + len(replacement_mimic_offset) - ) - if len(edits) != expected_edit_count: - raise ValueError("could not locate every target joint field in source URDF text") - corrected_text = original_text - for start, end, value in reversed(edits): - corrected_text = corrected_text[:start] + value + corrected_text[end:] - _materialize_relative_mesh_assets(source=source, output=output, urdf_root=root) - temporary = destination.with_suffix(".urdf.tmp") - with temporary.open("w", encoding="utf-8") as stream: - stream.write(corrected_text) - stream.flush() - os.fsync(stream.fileno()) - os.replace(temporary, destination) - return destination +sys.modules[__name__] = _implementation diff --git a/src/linkerhand_calibration/setup.py b/src/linkerhand_calibration/setup.py index 83b6b88..837b3ec 100644 --- a/src/linkerhand_calibration/setup.py +++ b/src/linkerhand_calibration/setup.py @@ -26,7 +26,7 @@ setup( zip_safe=True, maintainer="lxp", maintainer_email="support@linker-robotics.com", - description="Profile-driven three-camera AprilTag hand calibration", + description="Profile-driven hand calibration and validated URDF correction", license="MIT", entry_points={ "console_scripts": [ @@ -45,7 +45,7 @@ setup( ), ( "three_camera_calibration_node = " - "linkerhand_calibration.three_camera_node:main" + "linkerhand_calibration.runtime.nodes.calibration:main" ), ( "three_camera_extrinsics_node = " @@ -55,6 +55,10 @@ setup( "offline_replay = " "linkerhand_calibration.offline_replay:main" ), + ( + "validate_g20_goldens = " + "linkerhand_calibration.models.g20.golden_regression:main" + ), ( "camera_alignment_view = " "linkerhand_calibration.alignment_view:main" diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py index 5d264b0..032c573 100644 --- a/src/linkerhand_calibration/test/test_architecture.py +++ b/src/linkerhand_calibration/test/test_architecture.py @@ -25,13 +25,18 @@ from linkerhand_calibration.core.solver import ( TaskEvaluation, ) from linkerhand_calibration.core.urdf import UrdfCorrectionPlan +from linkerhand_calibration.extrinsics import ( + validate_camera_extrinsics_payload, +) from linkerhand_calibration.models import ( EngineBindings, ProfileRegistry, RegisteredProfile, get_default_registry, ) +from linkerhand_calibration.product import get_product_calibration_contract from linkerhand_calibration.runtime import SessionController, SessionState +from linkerhand_calibration.runtime.nodes import calibration as calibration_node PACKAGE = Path(__file__).resolve().parents[1] / "linkerhand_calibration" @@ -122,6 +127,7 @@ def _bindings() -> EngineBindings: preparation_waypoints=no_waypoints, return_waypoints=no_waypoints, cli_main=lambda args=None: None, + node_main=lambda args=None: None, ) @@ -157,6 +163,81 @@ def test_registry_accepts_different_view_and_command_counts() -> None: assert selected.vision.view_names == ("oblique", "wrist") +def test_generic_extrinsics_validator_uses_profile_view_names() -> None: + identity = { + "translation_xyz_m": [0.0, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + } + payload = { + "schema_version": 1, + "reference_view": "oblique", + "cameras": { + "oblique": { + "serial_number": "A", + "width": 640, + "height": 480, + "intrinsics_sha256": "a" * 64, + }, + "wrist": { + "serial_number": "B", + "width": 800, + "height": 600, + "intrinsics_sha256": "b" * 64, + }, + }, + "oblique_from_view": { + "oblique": identity, + "wrist": { + "translation_xyz_m": [0.1, 0.0, 0.0], + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + }, + }, + "quality": {"passed": True, "rms": 0.2, "pair_captures": 4}, + } + + validate_camera_extrinsics_payload( + payload, + required_views=("oblique", "wrist"), + reference_view="oblique", + quality_limits={"rms": 0.5}, + minimum_capture_counts={"pair_captures": 3}, + ) + + +def test_ros_node_dispatcher_uses_profile_registry_without_model_branch( + monkeypatch, +) -> None: + profile = _small_profile() + forwarded: list[list[str] | None] = [] + bindings = _bindings() + bindings = EngineBindings( + hand_profile=bindings.hand_profile, + zero_profile=bindings.zero_profile, + motion_command=bindings.motion_command, + preparation_waypoints=bindings.preparation_waypoints, + return_waypoints=bindings.return_waypoints, + cli_main=bindings.cli_main, + node_main=forwarded.append, + ) + registry = ProfileRegistry() + registry.register(RegisteredProfile(profile, bindings)) + monkeypatch.setattr( + calibration_node, "get_default_registry", lambda: registry + ) + + calibration_node.main( + [ + "--profile-id", + profile.key.profile_id, + "--ros-args", + "-p", + "sample_rate:=30", + ] + ) + + assert forwarded == [["--ros-args", "-p", "sample_rate:=30"]] + + def test_schema_v2_selects_profile_only_by_typed_profile_id() -> None: key = product_profile_key( { @@ -167,6 +248,13 @@ def test_schema_v2_selects_profile_only_by_typed_profile_id() -> None: assert key == ProfileKey("TEST", "right", "two_view_three_command", 3) +def test_retired_layout_alias_is_confined_to_compatibility_resolution() -> None: + contract = get_product_calibration_contract( + "G20", "right", "g20_right_15" + ) + assert contract.typed_profile.key.layout == "g20_right_19" + + def test_controller_uses_one_evaluator_and_solver_contract() -> None: profile = _small_profile() diff --git a/src/linkerhand_calibration/test/test_g20_right_product.py b/src/linkerhand_calibration/test/test_g20_right_product.py index 42b2a1b..7d0c46b 100644 --- a/src/linkerhand_calibration/test/test_g20_right_product.py +++ b/src/linkerhand_calibration/test/test_g20_right_product.py @@ -9,6 +9,7 @@ from xml.etree import ElementTree as ET import numpy as np import pytest from scipy.spatial.transform import Rotation +import yaml from linkerhand_calibration.core import ( DIRECTION_DECREASING, @@ -190,13 +191,13 @@ def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None: G20_RIGHT_19_LAYOUT, ) assert config.calibration_contract.profile.command_count == 20 - assert config.calibration_contract.profile.supports( - "urdf_zero_publication" - ) - assert config.calibration_contract.profile.supports( - "stable_cross_view_cone_bias" + assert config.calibration_contract.typed_profile.artifacts.publish_corrected_urdf + assert ( + config.calibration_contract.typed_profile.measurement + .stable_cross_view_cone_bias ) assert config.serial_number == "G20_RIGHT_001" + assert config.namespace == "/g20_calibration" assert config.required_independent_passes == 1 assert set(config.cameras) == {"front", "side", "top"} assert len({camera["serial_number"] for camera in config.cameras.values()}) == 3 @@ -214,6 +215,26 @@ def test_product_config_locks_three_cameras_tags_and_artifact_hashes() -> None: assert "src/g20_thumb_apriltag_calibration/config/" in product_text +def test_schema_v2_loads_the_same_registered_product(tmp_path) -> None: + payload = yaml.safe_load(PRODUCT.read_text(encoding="utf-8")) + payload["schema_version"] = 2 + payload["profile_id"] = "G20/right/g20_right_19/v1" + payload.pop("model") + payload.pop("side") + payload.pop("tag_layout") + config_path = tmp_path / "product_v2.yaml" + config_path.write_text( + yaml.safe_dump(payload, allow_unicode=True, sort_keys=False), + encoding="utf-8", + ) + + config = load_product_config(config_path, workspace=REPO, check_can=False) + + assert config.schema_version == 2 + assert config.profile_key.profile_id == "G20/right/g20_right_19/v1" + assert config.calibration_contract.typed_profile.key == config.profile_key + + def test_first_round_uses_nine_bidirectional_steady_commands() -> None: profile = get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT) spec = profile.sweep_specs[0] diff --git a/src/linkerhand_calibration/test/test_golden_regression.py b/src/linkerhand_calibration/test/test_golden_regression.py new file mode 100644 index 0000000..f5b76ff --- /dev/null +++ b/src/linkerhand_calibration/test/test_golden_regression.py @@ -0,0 +1,72 @@ +import hashlib +import json +from pathlib import Path + +from linkerhand_calibration.models.g20.golden_regression import ( + validate_golden_sessions, +) + + +def _write(path: Path, value: str) -> str: + path.write_text(value, encoding="utf-8") + return hashlib.sha256(value.encode("utf-8")).hexdigest() + + +def test_static_golden_validation_covers_pass_fail_and_partial_scopes( + tmp_path: Path, +) -> None: + passed = { + "20260830_181154": ("full", {}), + "20260831_141123": ( + "thumb", + {"preserved_certified_zero_joints": [f"joint_{i}" for i in range(12)]}, + ), + "20260831_163843": ("thumb", {"standalone": True}), + } + for session_id, (scope, extras) in passed.items(): + session = tmp_path / session_id + session.mkdir() + payload = ( + {"artifact_type": "g20_right_standalone_thumb_calibration"} + if extras.pop("standalone", False) + else {"artifact_type": "full"} + ) + json_text = json.dumps(payload) + "\n" + json_hash = _write(session / "calibration.json", json_text) + urdf_hash = _write(session / "corrected.urdf", f"<{session_id}/>\n") + summary = { + "result": "PASS", + "calibration_scope": scope, + "artifacts": {"json": "calibration.json", "urdf": "corrected.urdf"}, + "hashes": { + "calibration_json_sha256": json_hash, + "corrected_urdf_sha256": urdf_hash, + }, + **extras, + } + (session / "calibration_summary_zh.json").write_text( + json.dumps(summary), encoding="utf-8" + ) + + failures = { + "20260831_111837": ("FIT-MODEL-401", "joint_fit_check_failed"), + "20260831_142322": ( + "VAL-QUALITY-501", + "zero_model_validation_failed", + ), + } + for session_id, (code, reason) in failures.items(): + session = tmp_path / session_id + session.mkdir() + (session / "calibration_summary_zh.json").write_text( + json.dumps( + {"result": "FAIL", "error_code": code, "reason": reason} + ), + encoding="utf-8", + ) + + (tmp_path / "latest_passed").symlink_to("20260830_181154") + result = validate_golden_sessions(tmp_path, replay_full_session=False) + + assert result["20260830_181154"]["result"] == "PASS" + assert result["20260831_111837"]["error_code"] == "FIT-MODEL-401" diff --git a/src/linkerhand_calibration/test/test_offline_replay.py b/src/linkerhand_calibration/test/test_offline_replay.py index 8bc0f98..6bff284 100644 --- a/src/linkerhand_calibration/test/test_offline_replay.py +++ b/src/linkerhand_calibration/test/test_offline_replay.py @@ -1,5 +1,8 @@ import pytest +from linkerhand_calibration.compat.legacy import ( + uses_coupled_full_hand_zero_solver, +) from linkerhand_calibration.offline_replay import ( _latest_attempt_records, _latest_palm_axis_records, @@ -67,3 +70,17 @@ def test_output_suffix_is_safe_and_explicit() -> None: for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65): with pytest.raises(ValueError, match="output tag"): _output_suffix(invalid) + + +def test_v1_session_solver_selection_is_confined_to_compatibility() -> None: + legacy = { + "sample_schema_version": 1, + "capabilities": ["palm_axis_side_channel_v2"], + } + current = { + "sample_schema_version": 1, + "capabilities": ["palm_axis_relative_motion_v3"], + } + + assert uses_coupled_full_hand_zero_solver(legacy) + assert not uses_coupled_full_hand_zero_solver(current) diff --git a/src/linkerhand_calibration/test/test_package_rename.py b/src/linkerhand_calibration/test/test_package_rename.py index 2c9c551..1ff600f 100644 --- a/src/linkerhand_calibration/test/test_package_rename.py +++ b/src/linkerhand_calibration/test/test_package_rename.py @@ -1,13 +1,21 @@ import importlib from pathlib import Path import re +import sys import pytest def test_legacy_python_package_forwards_only_public_config_api() -> None: with pytest.warns(DeprecationWarning, match="linkerhand_calibration"): - legacy = importlib.import_module("g20_thumb_apriltag_calibration") + if "g20_thumb_apriltag_calibration" in sys.modules: + legacy = importlib.reload( + sys.modules["g20_thumb_apriltag_calibration"] + ) + else: + legacy = importlib.import_module( + "g20_thumb_apriltag_calibration" + ) current = importlib.import_module("linkerhand_calibration.product") assert legacy.ProductConfig is current.ProductConfig @@ -24,3 +32,7 @@ def test_new_and_legacy_executable_names_share_one_implementation() -> None: r'"linkerhand_calibration\.runtime\.runner:main"', setup_text, ) + assert ( + "linkerhand_calibration.runtime.nodes.calibration:main" + in setup_text + )