From ba9f1b25e853f451ed5b4df3745a8b4d5771947b Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Mon, 31 Aug 2026 18:33:31 +0800 Subject: [PATCH] refactor: establish reusable calibration architecture --- src/linkerhand_calibration/README.md | 23 + .../linkerhand_calibration/__init__.py | 6 +- .../linkerhand_calibration/acquisition.py | 3 +- .../linkerhand_calibration/compat/__init__.py | 8 +- .../compat/config_v1.py | 30 + .../linkerhand_calibration/compat/defaults.py | 10 + .../compat/legacy/__init__.py | 1 + .../{core.py => compat/legacy/thumb_core.py} | 0 .../linkerhand_calibration/core/__init__.py | 76 + .../core/artifacts/__init__.py | 5 + .../core/artifacts/release.py | 27 + .../core/artifacts/storage.py | 105 + .../core/domain/__init__.py | 41 + .../core/domain/profile.py | 289 +++ .../core/domain/sample.py | 27 + .../core/domain/sample_schema.py | 192 ++ .../core/domain/task.py | 19 + .../core/fitting/__init__.py | 5 + .../core/fitting/curve.py | 70 + .../core/geometry/__init__.py | 25 + .../core/geometry/pnp.py | 1751 ++++++++++++++++ .../core/geometry/rotation.py | 157 ++ .../core/solver/__init__.py | 15 + .../core/solver/interfaces.py | 42 + .../core/urdf/__init__.py | 5 + .../linkerhand_calibration/core/urdf/plan.py | 104 + .../linkerhand_calibration/full_hand.py | 14 +- .../linkerhand_calibration/models/__init__.py | 15 + .../models/g20/__init__.py | 15 + .../models/g20/_adapter.py | 200 ++ .../models/g20/artifacts.py | 27 + .../models/g20/command_layout.py | 26 + .../models/g20/legacy_11.py | 35 + .../models/g20/motion.py | 15 + .../models/g20/reporting_zh.py | 893 +++++++++ .../models/g20/right_19.py | 27 + .../models/g20/runner.py | 731 +++++++ .../models/g20/zero_policy.py | 17 + .../models/l6/__init__.py | 1 + .../linkerhand_calibration/models/registry.py | 83 + .../linkerhand_calibration/node.py | 2 +- .../linkerhand_calibration/one_command.py | 724 +------ .../linkerhand_calibration/pnp.py | 1752 +---------------- .../linkerhand_calibration/product.py | 178 +- .../linkerhand_calibration/publication.py | 24 + .../runtime/__init__.py | 11 + .../runtime/adapters/__init__.py | 1 + .../runtime/controller.py | 160 ++ .../runtime/nodes/__init__.py | 1 + .../runtime/reporting/__init__.py | 3 + .../runtime/reporting/codes.py | 14 + .../linkerhand_calibration/runtime/runner.py | 41 + .../linkerhand_calibration/sample_schema.py | 193 +- .../linkerhand_calibration/storage.py | 106 +- .../three_camera_diagnostics.py | 895 +-------- .../three_camera_node.py | 67 +- .../linkerhand_calibration/tools/__init__.py | 1 + .../linkerhand_calibration/urdf_zero.py | 13 +- .../linkerhand_calibration/zero_node.py | 6 +- src/linkerhand_calibration/setup.py | 4 +- .../test/test_acquisition.py | 2 +- .../test/test_architecture.py | 218 ++ src/linkerhand_calibration/test/test_core.py | 2 +- .../test/test_g20_right_product.py | 2 +- .../test/test_package_rename.py | 2 +- .../test/test_trajectory.py | 6 +- 66 files changed, 5771 insertions(+), 3792 deletions(-) create mode 100644 src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py rename src/linkerhand_calibration/linkerhand_calibration/{core.py => compat/legacy/thumb_core.py} (100%) create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/domain/sample_schema.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/domain/task.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/fitting/curve.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/command_layout.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/reporting_zh.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/models/registry.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/controller.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/__init__.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/codes.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py create mode 100644 src/linkerhand_calibration/linkerhand_calibration/tools/__init__.py create mode 100644 src/linkerhand_calibration/test/test_architecture.py diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index 34eb5e0..75ce488 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -11,6 +11,29 @@ ros2 run linkerhand_calibration calibrate_hand 旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名； 旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。 +构建和正式调用统一为： + +```bash +colcon build --packages-select linkerhand_calibration +ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml> +``` + +## 代码边界 + +- `core/`：无 ROS、无具体型号，包含领域类型、PnP/旋转数学、拟合接口、统一样本 + 契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。 +- `runtime/`：通用会话状态机与注册 Profile 分发；ROS 消息和硬件适配只能位于 + `runtime/nodes`、`runtime/adapters`。 +- `models/g20/`：G20 right-19、legacy-11、运动、零位、产物和中文诊断策略。 + 后续型号或左右手作为新的独立 Profile 加入 `models/`，不在通用层增加分支。 +- `compat/`：v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留 + 一版最小转发 shim，不包含算法副本。 + +产品配置在启动硬件前通过本地 `ProfileRegistry` 完成命令索引、任务、视角、 +Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改；v2 配置使用 +`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明，通用层 +不要求 `front/side/top`，也不假设固定 20 个命令。 + 完全独立地只标定大拇指4项任务时，使用： ```bash diff --git a/src/linkerhand_calibration/linkerhand_calibration/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/__init__.py index b7d1225..23257e1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/__init__.py @@ -1,5 +1,5 @@ -"""Front-camera AprilTag calibration for the left LinkerHand G20 thumb.""" +"""Profile-driven LinkerHand calibration and validated URDF correction.""" -from .core import BASELINE_COMMAND, COMMAND_NAMES +from .core import CalibrationProfile, ProfileKey -__all__ = ["BASELINE_COMMAND", "COMMAND_NAMES"] +__all__ = ["CalibrationProfile", "ProfileKey"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/acquisition.py b/src/linkerhand_calibration/linkerhand_calibration/acquisition.py index d2e6b57..0e40a6c 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/acquisition.py +++ b/src/linkerhand_calibration/linkerhand_calibration/acquisition.py @@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence import numpy as np -from .core import PAIR_NAMES, robust_rotation_summary +from .core import robust_rotation_summary from .pnp import SquareTagPose @@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = { "t3_t4": ("t3", "t4"), "t4_t5": ("t4", "t5"), } +PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES) @dataclass(frozen=True) diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py index e4dd3a9..1065fa8 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/__init__.py @@ -1,5 +1,11 @@ """Compatibility adapters for one-release calibration migrations.""" +from .config_v1 import product_profile_key +from .defaults import default_product_config_path from .paths import resolve_renamed_package_path -__all__ = ["resolve_renamed_package_path"] +__all__ = [ + "default_product_config_path", + "product_profile_key", + "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 new file mode 100644 index 0000000..4de28f9 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/config_v1.py @@ -0,0 +1,30 @@ +"""Identity migration for deployed product configuration schemas.""" + +from __future__ import annotations + +from typing import Any, Mapping + +from ..core import ProfileKey + + +def product_profile_key(raw: Mapping[str, Any]) -> ProfileKey: + version = int(raw.get("schema_version", -1)) + if version == 2: + key = ProfileKey.parse(str(raw.get("profile_id", ""))) + for field, actual in ( + ("model", key.model), + ("side", key.side), + ("tag_layout", key.layout), + ): + configured = str(raw.get(field, "")).strip() + if configured and configured.lower() != actual.lower(): + raise ValueError(f"{field} differs from profile_id") + return key + if version != 1: + raise ValueError("product config schema_version must be 1 or 2") + model = str(raw.get("model", "")).strip().upper() + side = str(raw.get("side", "")).strip().lower() + layout = str(raw.get("tag_layout", "")).strip().lower() + if not layout and (model, side) == ("G20", "right"): + layout = "g20_right_19" + return ProfileKey(model, side, layout, 1) diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py b/src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py new file mode 100644 index 0000000..8349024 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/defaults.py @@ -0,0 +1,10 @@ +"""One-release default selection for invocations without ``--config``.""" + +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory + + +def default_product_config_path() -> Path: + share = Path(get_package_share_directory("linkerhand_calibration")) + return share / "config/g20_right_product.yaml" diff --git a/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py new file mode 100644 index 0000000..75ffb11 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/__init__.py @@ -0,0 +1 @@ +"""Legacy single-camera algorithms retained for one compatibility release.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core.py b/src/linkerhand_calibration/linkerhand_calibration/compat/legacy/thumb_core.py similarity index 100% rename from src/linkerhand_calibration/linkerhand_calibration/core.py rename to src/linkerhand_calibration/linkerhand_calibration/compat/legacy/thumb_core.py diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py new file mode 100644 index 0000000..5f99926 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/__init__.py @@ -0,0 +1,76 @@ +"""Hardware- and model-independent calibration kernel.""" + +from .domain import ( + ArtifactPolicy, + CalibrationProfile, + CommandLayout, + MeasurementPolicy, + MeasurementSpec, + MotionPolicy, + ProfileKey, + ProfileValidationError, + QualityPolicy, + SampleRecord, + ScopePolicy, + TagSpec, + TaskSpec, + ViewSpec, + VisionRigSpec, + ZeroSolvePolicy, + validate_profile, +) +from .domain.task import ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + DIRECTIONS, + PHASE_ROOT, + PHASE_TIP, +) +from .fitting import FitResult, isotonic_nonincreasing +from .geometry import ( + delta_rotation_vector, + fit_rotation_axis, + image_plane_tag_quaternion_xyzw, + normalize_quaternion_xyzw, + relative_quaternion_xyzw, + robust_rotation_summary, + rotation_inlier_fraction, + rotation_rms_rad, + rotation_spread_rad, +) + +__all__ = [ + "ArtifactPolicy", + "CalibrationProfile", + "CommandLayout", + "DIRECTION_DECREASING", + "DIRECTION_INCREASING", + "DIRECTIONS", + "FitResult", + "MeasurementPolicy", + "MeasurementSpec", + "MotionPolicy", + "PHASE_ROOT", + "PHASE_TIP", + "ProfileKey", + "ProfileValidationError", + "QualityPolicy", + "SampleRecord", + "ScopePolicy", + "TagSpec", + "TaskSpec", + "ViewSpec", + "VisionRigSpec", + "ZeroSolvePolicy", + "delta_rotation_vector", + "fit_rotation_axis", + "image_plane_tag_quaternion_xyzw", + "isotonic_nonincreasing", + "normalize_quaternion_xyzw", + "relative_quaternion_xyzw", + "robust_rotation_summary", + "rotation_inlier_fraction", + "rotation_rms_rad", + "rotation_spread_rad", + "validate_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py new file mode 100644 index 0000000..25cb69c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/__init__.py @@ -0,0 +1,5 @@ +"""Artifact schema and release validation contracts.""" + +from .release import ReleaseValidation, ReleaseValidator + +__all__ = ["ReleaseValidation", "ReleaseValidator"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py new file mode 100644 index 0000000..ce0bf4b --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/release.py @@ -0,0 +1,27 @@ +"""Release validation protocol used before atomic publication.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Mapping, Protocol + +from ..domain import CalibrationProfile +from ..urdf import UrdfCorrectionPlan + + +@dataclass(frozen=True) +class ReleaseValidation: + passed: bool + errors: tuple[str, ...] = () + verified_hashes: Mapping[str, str] | None = None + + +class ReleaseValidator(Protocol): + def validate_release( + self, + profile: CalibrationProfile, + plan: UrdfCorrectionPlan, + calibration_json: Path, + corrected_urdf: Path, + ) -> ReleaseValidation: ... diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py new file mode 100644 index 0000000..13f9a36 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/artifacts/storage.py @@ -0,0 +1,105 @@ +"""Crash-safe session storage for hardware calibration.""" + +from __future__ import annotations + +import json +import os +from pathlib import Path +from typing import Any, Iterable, Mapping + + +def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> None: + destination = Path(path) + destination.parent.mkdir(parents=True, exist_ok=True) + temporary = destination.with_suffix(destination.suffix + ".tmp") + with temporary.open("w", encoding="utf-8") as stream: + json.dump(payload, stream, ensure_ascii=False, indent=2) + stream.write("\n") + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, destination) + + +def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None: + destination = Path(path) + destination.parent.mkdir(parents=True, exist_ok=True) + line = json.dumps(payload, ensure_ascii=False, separators=(",", ":")) + with destination.open("a", encoding="utf-8") as stream: + stream.write(line + "\n") + stream.flush() + os.fsync(stream.fileno()) + + +def append_jsonl_many( + path: str | Path, payloads: Iterable[Mapping[str, Any]] +) -> None: + """Durably append a batch while paying the fsync cost only once.""" + destination = Path(path) + destination.parent.mkdir(parents=True, exist_ok=True) + lines = [ + json.dumps(payload, ensure_ascii=False, separators=(",", ":")) + for payload in payloads + ] + if not lines: + return + with destination.open("a", encoding="utf-8") as stream: + stream.write("\n".join(lines) + "\n") + stream.flush() + os.fsync(stream.fileno()) + + +def load_jsonl(path: str | Path) -> list[dict[str, Any]]: + source = Path(path) + if not source.exists(): + return [] + records: list[dict[str, Any]] = [] + with source.open("r", encoding="utf-8") as stream: + lines = stream.readlines() + nonempty_lines = [ + index for index, line in enumerate(lines, 1) if line.strip() + ] + last_nonempty_line = nonempty_lines[-1] if nonempty_lines else 0 + for line_number, line in enumerate(lines, 1): + if not line.strip(): + continue + try: + value = json.loads(line) + except json.JSONDecodeError as error: + if line_number == last_nonempty_line: + break + raise ValueError( + f"{source}:{line_number}: invalid JSONL record" + ) from error + if not isinstance(value, dict): + raise ValueError(f"{source}:{line_number}: record must be an object") + records.append(value) + return records + + +def load_json(path: str | Path) -> dict[str, Any] | None: + source = Path(path) + if not source.exists(): + return None + with source.open("r", encoding="utf-8") as stream: + value = json.load(stream) + if not isinstance(value, dict): + raise ValueError(f"{source} must contain a JSON object") + return value + + +def completed_scan_keys( + records: Iterable[Mapping[str, Any]], +) -> set[tuple[str, int, str, int]]: + keys: set[tuple[str, int, str, int]] = set() + for record in records: + if record.get("kind", "sample") != "sample": + continue + keys.add( + ( + str(record["phase"]), + int(record["cycle"]), + str(record["direction"]), + int(record["command_u8"]), + ) + ) + return keys diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py new file mode 100644 index 0000000..a085d23 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/__init__.py @@ -0,0 +1,41 @@ +"""Calibration domain types.""" + +from .profile import ( + ArtifactPolicy, + CalibrationProfile, + CommandLayout, + MeasurementPolicy, + MeasurementSpec, + MotionPolicy, + ProfileKey, + ProfileValidationError, + QualityPolicy, + ScopePolicy, + TagSpec, + TaskSpec, + ViewSpec, + VisionRigSpec, + ZeroSolvePolicy, + validate_profile, +) +from .sample import SampleRecord + +__all__ = [ + "ArtifactPolicy", + "CalibrationProfile", + "CommandLayout", + "MeasurementPolicy", + "MeasurementSpec", + "MotionPolicy", + "ProfileKey", + "ProfileValidationError", + "QualityPolicy", + "SampleRecord", + "ScopePolicy", + "TagSpec", + "TaskSpec", + "ViewSpec", + "VisionRigSpec", + "ZeroSolvePolicy", + "validate_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py new file mode 100644 index 0000000..de37899 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/profile.py @@ -0,0 +1,289 @@ +"""Typed, hardware-independent calibration profile contracts.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from pathlib import PurePath +from typing import Mapping + + +@dataclass(frozen=True, order=True) +class ProfileKey: + """Stable identity for one independently reviewed hand profile.""" + + model: str + side: str + layout: str + revision: int = 1 + + def __post_init__(self) -> None: + object.__setattr__(self, "model", str(self.model).strip().upper()) + object.__setattr__(self, "side", str(self.side).strip().lower()) + object.__setattr__(self, "layout", str(self.layout).strip().lower()) + 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.revision < 1: + raise ValueError("profile revision must be positive") + + @property + def profile_id(self) -> str: + return f"{self.model}/{self.side}/{self.layout}/v{self.revision}" + + @classmethod + def parse(cls, value: str) -> "ProfileKey": + parts = str(value).strip().split("/") + if len(parts) != 4 or not parts[3].startswith("v"): + raise ValueError( + "profile_id must be MODEL/side/layout/vREVISION" + ) + return cls(parts[0], parts[1], parts[2], int(parts[3][1:])) + + +@dataclass(frozen=True) +class CommandLayout: + """Command channels, joint bindings, and the reviewed baseline pose.""" + + names: tuple[str, ...] + baseline_u8: tuple[int, ...] + command_index_by_joint: Mapping[str, int] + disabled_indices: frozenset[int] = frozenset() + + @property + def command_count(self) -> int: + return len(self.names) + + +@dataclass(frozen=True) +class TagSpec: + role: str + tag_id: int + fixed_reference: bool = False + + +@dataclass(frozen=True) +class ViewSpec: + name: str + tags: tuple[TagSpec, ...] + + +@dataclass(frozen=True) +class VisionRigSpec: + """Any number of named views and their Tag roles.""" + + views: tuple[ViewSpec, ...] + common_frame: str + extrinsic_reference_view: str + + @property + def view_names(self) -> tuple[str, ...]: + return tuple(view.name for view in self.views) + + @property + def tag_ids(self) -> frozenset[int]: + return frozenset(tag.tag_id for view in self.views for tag in view.tags) + + +@dataclass(frozen=True) +class TaskSpec: + key: str + view: str + command_index: int + joints: tuple[str, ...] + auxiliary_commands: tuple[tuple[int, int], ...] = () + validation_only: bool = False + + +@dataclass(frozen=True) +class MotionPolicy: + """Reviewed motion tasks and optional safe waypoint sequences.""" + + tasks: tuple[TaskSpec, ...] + preparation_waypoints_u8: tuple[tuple[int, ...], ...] = () + safe_return_waypoints_u8: tuple[tuple[int, ...], ...] = () + speed_parameters: Mapping[str, float] = field(default_factory=dict) + precheck_sweeps: bool = False + steady_command_checkpoints: bool = False + + +@dataclass(frozen=True) +class MeasurementSpec: + joint: str + kind: str + view: str | None + parent_role: str | None + child_role: str | None + validation_source: str | None = None + + +@dataclass(frozen=True) +class MeasurementPolicy: + measurements: Mapping[str, MeasurementSpec] + cross_view_sources: Mapping[str, str] = field(default_factory=dict) + image_curve_joints: frozenset[str] = frozenset() + directional_zero: bool = False + cross_view_roll_curve: bool = False + stable_cross_view_cone_bias: bool = False + + +@dataclass(frozen=True) +class ZeroSolvePolicy: + active_joints: frozenset[str] + passive_joints: frozenset[str] + direct_zero_joints: tuple[str, ...] + axis_joints: tuple[str, ...] + mechanical_endpoint_joints: frozenset[str] + post_solve_endpoint_joints: frozenset[str] + mimic_source_by_joint: Mapping[str, str] + cad_frozen_joints: frozenset[str] + + +@dataclass(frozen=True) +class QualityPolicy: + training_cycles: tuple[int, ...] + holdout_cycle: int | None + hard_threshold_keys: frozenset[str] + retry_metric_scope: Mapping[str, str] = field(default_factory=dict) + isolated_holdout: bool = False + + +@dataclass(frozen=True) +class ScopePolicy: + calibrate_joints: Mapping[str, frozenset[str]] + frozen_joints: Mapping[str, frozenset[str]] + default_scope: str = "full" + + def selected_joints(self, scope: str) -> frozenset[str]: + try: + return self.calibrate_joints[str(scope)] + except KeyError as error: + raise ValueError(f"unsupported calibration scope: {scope}") from error + + +@dataclass(frozen=True) +class ArtifactPolicy: + output_schema_version: int + calibration_filename: str + corrected_urdf_filename: str + protected_input_fields: frozenset[str] + publication_pointer: str = "latest_passed" + session_compatibility_tokens: frozenset[str] = frozenset() + + +@dataclass(frozen=True) +class CalibrationProfile: + key: ProfileKey + namespace: str + command: CommandLayout + vision: VisionRigSpec + motion: MotionPolicy + measurement: MeasurementPolicy + zero: ZeroSolvePolicy + quality: QualityPolicy + scope: ScopePolicy + artifacts: ArtifactPolicy + + +class ProfileValidationError(ValueError): + """Raised before hardware startup when a profile is internally unsafe.""" + + +def validate_profile(profile: CalibrationProfile) -> None: + """Hard-check all cross-policy references before hardware is enabled.""" + errors: list[str] = [] + command = profile.command + if not command.names or len(command.names) != len(command.baseline_u8): + errors.append("command names and baseline must be non-empty and aligned") + if len(set(command.names)) != len(command.names): + errors.append("command names must be unique") + if any(value < 0 or value > 255 for value in command.baseline_u8): + errors.append("baseline command values must be in [0, 255]") + indices = set(range(command.command_count)) + if not set(command.disabled_indices).issubset(indices): + errors.append("disabled command index is out of range") + if any(index not in indices for index in command.command_index_by_joint.values()): + errors.append("joint command index is out of range") + + view_names = profile.vision.view_names + if not view_names or len(set(view_names)) != len(view_names): + errors.append("vision views must be non-empty and unique") + if profile.vision.extrinsic_reference_view not in view_names: + errors.append("extrinsic reference view is not declared") + tag_ids = [tag.tag_id for view in profile.vision.views for tag in view.tags] + tag_roles = [tag.role for view in profile.vision.views for tag in view.tags] + if len(set(tag_ids)) != len(tag_ids): + errors.append("Tag IDs must be unique across views") + if len(set(tag_roles)) != len(tag_roles): + errors.append("Tag roles must be unique across views") + if not any( + tag.fixed_reference for view in profile.vision.views for tag in view.tags + ): + errors.append("at least one fixed reference Tag is required") + + task_keys = [task.key for task in profile.motion.tasks] + if not task_keys or len(set(task_keys)) != len(task_keys): + errors.append("motion task keys must be non-empty and unique") + measurement_names = set(profile.measurement.measurements) + for task in profile.motion.tasks: + if task.view not in view_names: + errors.append(f"task {task.key} uses an unknown view") + if task.command_index not in indices: + errors.append(f"task {task.key} command index is out of range") + if not task.joints or not set(task.joints).issubset(measurement_names): + errors.append(f"task {task.key} references unknown measurements") + if any(index not in indices for index, _ in task.auxiliary_commands): + errors.append(f"task {task.key} auxiliary index is out of range") + for name, spec in profile.measurement.measurements.items(): + if name != spec.joint: + errors.append(f"measurement mapping key differs for {name}") + if spec.view is not None and spec.view not in view_names: + errors.append(f"measurement {name} uses an unknown view") + for primary, validation in profile.measurement.cross_view_sources.items(): + if primary not in measurement_names or validation not in measurement_names: + errors.append("cross-view measurement source is unknown") + + zero = profile.zero + if zero.active_joints & zero.passive_joints: + errors.append("active and passive joints must be disjoint") + all_joints = zero.active_joints | zero.passive_joints + if not zero.active_joints.issubset(command.command_index_by_joint): + errors.append("every active joint must bind to a command channel") + if not set(zero.direct_zero_joints).issubset(zero.active_joints): + errors.append("direct zero targets must be active joints") + if not set(zero.axis_joints).issubset(all_joints): + errors.append("axis targets must be known joints") + if not zero.mechanical_endpoint_joints.issubset(zero.active_joints): + errors.append("mechanical endpoint targets must be active joints") + if not zero.post_solve_endpoint_joints.issubset(zero.active_joints): + errors.append("post-solve endpoint targets must be active joints") + if not set(zero.mimic_source_by_joint).issubset(zero.passive_joints): + errors.append("mimic targets must be passive joints") + if not set(zero.mimic_source_by_joint.values()).issubset(all_joints): + errors.append("mimic sources must be known joints") + + scopes = set(profile.scope.calibrate_joints) + if profile.scope.default_scope not in scopes: + errors.append("default scope is not declared") + if scopes != set(profile.scope.frozen_joints): + errors.append("scope calibration and frozen mappings must align") + for name in scopes: + selected = profile.scope.calibrate_joints[name] + frozen = profile.scope.frozen_joints[name] + if selected & frozen or selected | frozen != zero.active_joints: + errors.append(f"scope {name} must partition all active joints") + + artifacts = profile.artifacts + if artifacts.output_schema_version < 1: + errors.append("artifact schema version must be positive") + for label, filename in ( + ("calibration", artifacts.calibration_filename), + ("corrected URDF", artifacts.corrected_urdf_filename), + ("publication pointer", artifacts.publication_pointer), + ): + if not filename or PurePath(filename).name != filename: + errors.append(f"{label} filename must not contain a directory") + if not profile.namespace.startswith("/"): + errors.append("runtime namespace must be absolute") + + if errors: + raise ProfileValidationError("; ".join(errors)) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py new file mode 100644 index 0000000..a1b7d04 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample.py @@ -0,0 +1,27 @@ +"""Normalized records shared by online evaluation and offline replay.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Mapping + + +@dataclass(frozen=True) +class SampleRecord: + task_key: str + measurement: str + view: str + cycle: int + direction: str + command_u8: int + timestamp_ns: int + values: Mapping[str, Any] + quality: Mapping[str, float] = field(default_factory=dict) + + def __post_init__(self) -> None: + if not self.task_key or not self.measurement or not self.view: + raise ValueError("sample task, measurement, and view are required") + if self.cycle < 0 or not 0 <= self.command_u8 <= 255: + raise ValueError("sample cycle or command is out of range") + if self.timestamp_ns < 0: + raise ValueError("sample timestamp must be non-negative") diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample_schema.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample_schema.py new file mode 100644 index 0000000..a39bddd --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/sample_schema.py @@ -0,0 +1,192 @@ +"""Canonical command/feedback schema for calibration observations. + +The hand command and its measured motor feedback are different physical +domains. Durable samples always retain both. Fitting code may still use the +historical ``command_u8`` key, but it is created only as an explicit projection +of a canonical record at the fitting boundary. +""" + +from __future__ import annotations + +import math +from typing import Any, Iterable, Literal, Mapping + +import numpy as np + + +SAMPLE_KINDS = frozenset( + { + 'sample', + 'baseline_hold_sample', + 'steady_command_sample', + 'palm_axis_sample', + } +) + +FitDomain = Literal['default', 'requested', 'feedback'] + + +class SampleDataContractError(ValueError): + """A calibration observation mixes or omits command domains.""" + + +def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float: + try: + number = float(value) + except (TypeError, ValueError) as error: + raise SampleDataContractError( + f'DATA-CONTRACT-701:{field} must be numeric' + ) from error + if not math.isfinite(number) or not 0.0 <= number <= 255.0: + raise SampleDataContractError( + f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]' + ) + if integral: + rounded = int(round(number)) + if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9): + raise SampleDataContractError( + f'DATA-CONTRACT-701:{field} must be an integer command' + ) + return rounded + return number + + +def explicit_domain_value( + source: Mapping[str, Any], domain: Literal['requested', 'feedback'] +) -> int | float: + """Read and validate one explicitly named domain from any observation.""" + field = ( + 'requested_command_u8' if domain == 'requested' else 'feedback_u8' + ) + if field not in source or source[field] is None: + raise SampleDataContractError( + f'DATA-CONTRACT-701:observation is missing explicit {field}' + ) + return _finite_u8(source[field], field, integral=domain == 'requested') + + +def canonical_sample_record( + source: Mapping[str, Any], + *, + allow_legacy_command: bool = False, +) -> dict[str, Any]: + """Return one durable, unambiguous calibration observation. + + ``allow_legacy_command`` is restricted to importing historical sessions + and unit fixtures. New online observations must provide both explicit + fields and therefore cannot silently reinterpret ``command_u8``. + """ + record = dict(source) + kind = str(record.get('kind', '')) + if not kind and allow_legacy_command: + # Old in-memory steady-curve fixtures predate durable sample kinds. + # This adapter is never enabled by the new online/import contract. + kind = 'steady_command_sample' + record['kind'] = kind + if kind not in SAMPLE_KINDS: + raise SampleDataContractError( + f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}' + ) + + requested = record.get('requested_command_u8') + feedback = record.get('feedback_u8') + legacy = record.get('command_u8') + if requested is None or feedback is None: + if not allow_legacy_command or legacy is None: + missing = [ + name + for name, value in ( + ('requested_command_u8', requested), + ('feedback_u8', feedback), + ) + if value is None + ] + raise SampleDataContractError( + 'DATA-CONTRACT-701:' + f'{kind} is missing explicit {",".join(missing)}' + ) + # Historical in-memory records used requested commands for settled + # checkpoints and feedback bins for dense/baseline/palm observations. + if requested is None: + requested = legacy + if feedback is None: + feedback = legacy + + record.pop('command_u8', None) + record['requested_command_u8'] = explicit_domain_value( + {'requested_command_u8': requested}, 'requested' + ) + record['feedback_u8'] = explicit_domain_value( + {'feedback_u8': feedback}, 'feedback' + ) + return record + + +def fitting_sample_record( + source: Mapping[str, Any], + *, + domain: FitDomain = 'default', + allow_legacy_command: bool = False, + snap_requested_endpoints: bool = False, +) -> dict[str, Any]: + """Project a canonical sample into the legacy curve-fitter interface.""" + record = canonical_sample_record( + source, allow_legacy_command=allow_legacy_command + ) + kind = str(record['kind']) + selected = domain + if selected == 'default': + selected = ( + 'requested' if kind == 'steady_command_sample' else 'feedback' + ) + if selected not in {'requested', 'feedback'}: + raise SampleDataContractError( + f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}' + ) + requested = int(record['requested_command_u8']) + if selected == 'requested' or ( + snap_requested_endpoints and requested in {0, 255} + ): + index = requested + else: + index = int( + np.clip(np.rint(float(record['feedback_u8'])), 0, 255) + ) + record['command_u8'] = index + return record + + +def fitting_sample_records( + records: Iterable[Mapping[str, Any]], + *, + domain: FitDomain = 'default', + allow_legacy_command: bool = False, + snap_requested_endpoints: bool = False, +) -> list[dict[str, Any]]: + """Project several canonical samples into one explicit fitting domain.""" + return [ + fitting_sample_record( + record, + domain=domain, + allow_legacy_command=allow_legacy_command, + snap_requested_endpoints=snap_requested_endpoints, + ) + for record in records + ] + + +def validate_sample_records( + records: Iterable[Mapping[str, Any]], + *, + allow_legacy_command: bool = False, +) -> None: + """Validate a collection without changing its representation.""" + for index, record in enumerate(records): + try: + canonical_sample_record( + record, allow_legacy_command=allow_legacy_command + ) + except SampleDataContractError as error: + raise SampleDataContractError( + f'{error};record_index={index}' + ) from error diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/domain/task.py b/src/linkerhand_calibration/linkerhand_calibration/core/domain/task.py new file mode 100644 index 0000000..87d4e16 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/domain/task.py @@ -0,0 +1,19 @@ +"""Shared task direction vocabulary.""" + +DIRECTION_DECREASING = "decreasing" +DIRECTION_INCREASING = "increasing" +DIRECTIONS: tuple[str, ...] = ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, +) + +PHASE_ROOT = "root" +PHASE_TIP = "tip" + +__all__ = [ + "DIRECTION_DECREASING", + "DIRECTION_INCREASING", + "DIRECTIONS", + "PHASE_ROOT", + "PHASE_TIP", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py new file mode 100644 index 0000000..cb2a4bb --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/__init__.py @@ -0,0 +1,5 @@ +"""Pure curve and axis fitting.""" + +from .curve import FitResult, isotonic_nonincreasing + +__all__ = ["FitResult", "isotonic_nonincreasing"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/fitting/curve.py b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/curve.py new file mode 100644 index 0000000..98d4f90 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/fitting/curve.py @@ -0,0 +1,70 @@ +"""Model-independent curve fitting result and monotonic projection.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Sequence + +import numpy as np + +from ..geometry import delta_rotation_vector + + +def isotonic_nonincreasing(values: Sequence[float]) -> np.ndarray: + """Unweighted PAVA projection onto non-increasing values.""" + original = np.asarray(values, dtype=float) + if original.ndim != 1 or not np.all(np.isfinite(original)): + raise ValueError("values must be a finite vector") + negated = -original + levels: list[float] = [] + weights: list[int] = [] + starts: list[int] = [] + for index, value in enumerate(negated): + levels.append(float(value)) + weights.append(1) + starts.append(index) + while len(levels) >= 2 and levels[-2] > levels[-1]: + total_weight = weights[-2] + weights[-1] + merged = ( + levels[-2] * weights[-2] + levels[-1] * weights[-1] + ) / total_weight + levels[-2:] = [merged] + weights[-2:] = [total_weight] + starts.pop() + projected = np.empty_like(original) + for block_index, (level, start) in enumerate(zip(levels, starts)): + end = ( + starts[block_index + 1] + if block_index + 1 < len(starts) + else len(original) + ) + projected[start:end] = -level + return projected + + +@dataclass(frozen=True) +class FitResult: + joints: dict[str, dict[str, Any]] + axes: dict[str, tuple[float, float, float]] + references: dict[str, tuple[float, float, float, float]] + ip_coupling: dict[str, float] + max_monotonic_correction_rad: float + max_hysteresis_rad: float + measurement_mode: str = "rotation" + trajectory_models: dict[str, Any] = field(default_factory=dict) + trajectory_quality: dict[str, Any] = field(default_factory=dict) + + def measure_from_reference( + self, + joint_name: str, + observed_quaternion_xyzw: Sequence[float], + reference_quaternion_xyzw: Sequence[float] | None = None, + ) -> float: + reference = ( + reference_quaternion_xyzw + if reference_quaternion_xyzw is not None + else self.references[joint_name] + ) + vector = delta_rotation_vector(reference, observed_quaternion_xyzw) + axis = np.asarray(self.axes[joint_name], dtype=float) + return float(vector @ axis) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py new file mode 100644 index 0000000..6691cb1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/__init__.py @@ -0,0 +1,25 @@ +"""Pure geometry used by online and offline calibration.""" + +from .rotation import ( + delta_rotation_vector, + fit_rotation_axis, + image_plane_tag_quaternion_xyzw, + normalize_quaternion_xyzw, + relative_quaternion_xyzw, + robust_rotation_summary, + rotation_inlier_fraction, + rotation_rms_rad, + rotation_spread_rad, +) + +__all__ = [ + "delta_rotation_vector", + "fit_rotation_axis", + "image_plane_tag_quaternion_xyzw", + "normalize_quaternion_xyzw", + "relative_quaternion_xyzw", + "robust_rotation_summary", + "rotation_inlier_fraction", + "rotation_rms_rad", + "rotation_spread_rad", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py new file mode 100644 index 0000000..9ee5c91 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/pnp.py @@ -0,0 +1,1751 @@ +"""Square AprilTag pose estimation with planar ambiguity tracking. + +The AprilTag detections contain accurately refined image corners. This module +uses OpenCV's IPPE square solver directly so the calibration node can inspect +both planar PnP solutions instead of accepting an occasionally flipped TF +pose. +""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +from itertools import product +import math +from typing import Mapping, Sequence + +import cv2 +import numpy as np +from scipy.spatial.transform import Rotation + + +@dataclass(frozen=True) +class SquareTagPose: + """One tag-to-camera pose candidate returned by IPPE.""" + + quaternion_xyzw: tuple[float, float, float, float] + translation_xyz_m: tuple[float, float, float] + reprojection_error_px: float + + +def _relative_pose( + parent: SquareTagPose, + child: SquareTagPose, +) -> tuple[Rotation, np.ndarray]: + parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) + child_rotation = Rotation.from_quat(child.quaternion_xyzw) + relative_rotation = parent_rotation.inv() * child_rotation + relative_translation = parent_rotation.inv().apply( + np.asarray(child.translation_xyz_m, dtype=float) + - np.asarray(parent.translation_xyz_m, dtype=float) + ) + return relative_rotation, relative_translation + + +def _normal_alignment_rad(first: SquareTagPose, second: SquareTagPose) -> float: + """Return the undirected angle between two observed Tag face normals.""" + first_normal = Rotation.from_quat(first.quaternion_xyzw).apply( + [0.0, 0.0, 1.0] + ) + second_normal = Rotation.from_quat(second.quaternion_xyzw).apply( + [0.0, 0.0, 1.0] + ) + return math.acos( + abs(float(np.clip(first_normal @ second_normal, -1.0, 1.0))) + ) + + +def select_rigid_group_trajectory( + frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], + *, + roles: Sequence[str], + fixed_pairs: Sequence[tuple[str, str]], + reprojection_scale_px: float, + rotation_scale_rad: float, + translation_scale_m: float, + pair_geometry: str = "pose", + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float | None = None, +) -> tuple[ + list[dict[str, SquareTagPose]], + dict[str, float | str], +]: + """Resolve planar branches using geometry that should stay rigid. + + Every possible branch combination in the first frame is treated as a + candidate rigid reference. For each such reference, every later frame + independently chooses the combination with the lowest reprojection plus + geometric-drift cost. ``pose`` compares relative rotation and translation; + ``distance`` compares only Euclidean centre distances and therefore does + not allow planar-PnP orientation jitter into centre-trajectory angles. + The globally cheapest reference and path win. + """ + role_names = tuple(str(role) for role in roles) + pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) + normal_pair_names = tuple( + (str(first), str(second)) + for first, second in normal_alignment_pairs + ) + if not frames: + raise ValueError("at least one PnP frame is required") + if len(set(role_names)) != len(role_names) or not role_names: + raise ValueError("roles must be non-empty and unique") + if any( + parent not in role_names or child not in role_names + for parent, child in (*pair_names, *normal_pair_names) + ): + raise ValueError("geometry pairs must reference roles") + reprojection_scale = float(reprojection_scale_px) + rotation_scale = float(rotation_scale_rad) + translation_scale = float(translation_scale_m) + geometry_mode = str(pair_geometry) + normal_scale = ( + rotation_scale + if normal_alignment_scale_rad is None + else float(normal_alignment_scale_rad) + ) + if min( + reprojection_scale, + rotation_scale, + translation_scale, + normal_scale, + ) <= 0.0: + raise ValueError("trajectory selection scales must be positive") + if geometry_mode not in {"pose", "distance"}: + raise ValueError("pair_geometry must be pose or distance") + + combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] + for frame in frames: + candidate_lists = [tuple(frame.get(role, ())) for role in role_names] + if any(not candidates for candidates in candidate_lists): + raise ValueError("every frame must contain every requested role") + combinations_by_frame.append( + [ + dict(zip(role_names, combination)) + for combination in product(*candidate_lists) + ] + ) + + best_total = float("inf") + best_path: list[dict[str, SquareTagPose]] | None = None + + def emission( + combination: Mapping[str, SquareTagPose], + reference_pairs: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray] + ], + reference_distances: Mapping[tuple[str, str], float], + ) -> tuple[float, float, float]: + reprojection_cost = sum( + pose.reprojection_error_px + for pose in combination.values() + ) / reprojection_scale + normal_alignment_cost = sum( + _normal_alignment_rad( + combination[first], combination[second] + ) + for first, second in normal_pair_names + ) / normal_scale + rotation_drifts: list[float] = [] + translation_drifts: list[float] = [] + distance_drifts: list[float] = [] + for pair, ( + reference_rotation, + reference_translation, + ) in reference_pairs.items(): + rotation, translation = _relative_pose( + combination[pair[0]], + combination[pair[1]], + ) + rotation_drifts.append( + float( + (reference_rotation.inv() * rotation).magnitude() + ) + ) + translation_drifts.append( + float( + np.linalg.norm( + translation - reference_translation + ) + ) + ) + current_distance = float( + np.linalg.norm( + np.asarray( + combination[pair[1]].translation_xyz_m, + dtype=float, + ) + - np.asarray( + combination[pair[0]].translation_xyz_m, + dtype=float, + ) + ) + ) + distance_drifts.append( + abs(current_distance - reference_distances[pair]) + ) + if geometry_mode == "distance": + geometry_cost = sum(distance_drifts) / translation_scale + else: + geometry_cost = ( + sum(rotation_drifts) / rotation_scale + + sum(translation_drifts) / translation_scale + ) + return ( + reprojection_cost + geometry_cost + normal_alignment_cost, + max(rotation_drifts, default=0.0), + ( + max(distance_drifts, default=0.0) + if geometry_mode == "distance" + else max(translation_drifts, default=0.0) + ), + ) + + def transition_cost( + previous: Mapping[str, SquareTagPose], + current: Mapping[str, SquareTagPose], + ) -> float: + rotation_motion = sum( + rotation_distance_rad( + previous[role].quaternion_xyzw, + current[role].quaternion_xyzw, + ) + for role in role_names + ) + translation_motion = sum( + float( + np.linalg.norm( + np.asarray(current[role].translation_xyz_m) + - np.asarray(previous[role].translation_xyz_m) + ) + ) + for role in role_names + ) + if geometry_mode == "distance": + return translation_motion / translation_scale + return ( + rotation_motion / rotation_scale + + translation_motion / translation_scale + ) + + for reference_index, reference_combination in enumerate( + combinations_by_frame[0] + ): + reference_pairs = { + pair: _relative_pose( + reference_combination[pair[0]], + reference_combination[pair[1]], + ) + for pair in pair_names + } + reference_distances = { + pair: float( + np.linalg.norm( + np.asarray( + reference_combination[pair[1]].translation_xyz_m, + dtype=float, + ) + - np.asarray( + reference_combination[pair[0]].translation_xyz_m, + dtype=float, + ) + ) + ) + for pair in pair_names + } + first_emission = emission( + reference_combination, + reference_pairs, + reference_distances, + ) + previous_costs = np.full( + len(combinations_by_frame[0]), + np.inf, + dtype=float, + ) + previous_costs[reference_index] = first_emission[0] + back_pointers: list[list[int]] = [] + for frame_index in range(1, len(combinations_by_frame)): + previous_combinations = combinations_by_frame[frame_index - 1] + combinations = combinations_by_frame[frame_index] + frame_emissions = [ + emission( + combination, + reference_pairs, + reference_distances, + ) + for combination in combinations + ] + current_costs = np.full(len(combinations), np.inf, dtype=float) + frame_back_pointers: list[int] = [] + for current_index, combination in enumerate(combinations): + transition_costs = [ + previous_costs[previous_index] + + transition_cost( + previous_combination, + combination, + ) + for previous_index, previous_combination in enumerate( + previous_combinations + ) + ] + best_previous = int(np.argmin(transition_costs)) + frame_back_pointers.append(best_previous) + current_costs[current_index] = ( + transition_costs[best_previous] + + frame_emissions[current_index][0] + ) + back_pointers.append(frame_back_pointers) + previous_costs = current_costs + + final_index = int(np.argmin(previous_costs)) + total = float(previous_costs[final_index]) + path_indices = [final_index] + for frame_back_pointers in reversed(back_pointers): + path_indices.append( + frame_back_pointers[path_indices[-1]] + ) + path_indices.reverse() + path = [ + combinations[index] + for combinations, index in zip( + combinations_by_frame, + path_indices, + ) + ] + if total < best_total: + best_total = total + best_path = path + + if best_path is None: + raise RuntimeError("trajectory branch selection produced no path") + + # The marker-to-marker mounting transforms are unknown, so the rigid + # reference must be estimated from the complete sweep. Using frame zero + # as both the optimisation seed and the reported quality reference made + # one noisy endpoint frame look like drift in every other frame. A + # rotation medoid and component-wise translation median are insensitive + # to that endpoint noise while still exposing a persistent mirror branch. + robust_reference_pairs: dict[ + tuple[str, str], tuple[Rotation, np.ndarray] + ] = {} + for pair in pair_names: + pair_poses = [ + _relative_pose(frame[pair[0]], frame[pair[1]]) + for frame in best_path + ] + pair_rotations = [pose[0] for pose in pair_poses] + angular_costs = np.asarray( + [ + sum( + float((candidate.inv() * other).magnitude()) + for other in pair_rotations + ) + for candidate in pair_rotations + ], + dtype=float, + ) + rotation_medoid = pair_rotations[int(np.argmin(angular_costs))] + translation_median = np.median( + np.asarray([pose[1] for pose in pair_poses], dtype=float), + axis=0, + ) + robust_reference_pairs[pair] = ( + rotation_medoid, + translation_median, + ) + + rotation_drifts_by_frame: list[float] = [] + translation_drifts_by_frame: list[float] = [] + pair_distances_by_pair = { + pair: np.asarray( + [ + np.linalg.norm( + np.asarray(frame[pair[1]].translation_xyz_m, dtype=float) + - np.asarray( + frame[pair[0]].translation_xyz_m, dtype=float + ) + ) + for frame in best_path + ], + dtype=float, + ) + for pair in pair_names + } + robust_pair_distances = { + pair: float(np.median(distances)) + for pair, distances in pair_distances_by_pair.items() + } + distance_drifts_by_frame: list[float] = [] + for frame in best_path: + frame_rotation_drifts: list[float] = [] + frame_translation_drifts: list[float] = [] + frame_distance_drifts: list[float] = [] + for pair, ( + reference_rotation, + reference_translation, + ) in robust_reference_pairs.items(): + rotation, translation = _relative_pose( + frame[pair[0]], frame[pair[1]] + ) + frame_rotation_drifts.append( + float((reference_rotation.inv() * rotation).magnitude()) + ) + frame_translation_drifts.append( + float(np.linalg.norm(translation - reference_translation)) + ) + distance = float( + np.linalg.norm( + np.asarray(frame[pair[1]].translation_xyz_m, dtype=float) + - np.asarray( + frame[pair[0]].translation_xyz_m, dtype=float + ) + ) + ) + frame_distance_drifts.append( + abs(distance - robust_pair_distances[pair]) + ) + rotation_drifts_by_frame.append( + max(frame_rotation_drifts, default=0.0) + ) + translation_drifts_by_frame.append( + max(frame_translation_drifts, default=0.0) + ) + distance_drifts_by_frame.append( + max(frame_distance_drifts, default=0.0) + ) + + rotation_drifts = np.asarray(rotation_drifts_by_frame, dtype=float) + translation_drifts = np.asarray( + translation_drifts_by_frame, dtype=float + ) + distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float) + normal_alignments = np.asarray( + [ + max( + ( + _normal_alignment_rad(frame[first], frame[second]) + for first, second in normal_pair_names + ), + default=0.0, + ) + for frame in best_path + ], + dtype=float, + ) + return best_path, { + "total_cost": float(best_total), + "pair_geometry": geometry_mode, + "maximum_normal_alignment_rad": float( + np.max(normal_alignments, initial=0.0) + ), + "maximum_pair_rotation_drift_rad": float( + np.max(rotation_drifts, initial=0.0) + ), + "p95_pair_rotation_drift_rad": float( + np.percentile(rotation_drifts, 95.0) + ), + "median_pair_rotation_drift_rad": float( + np.median(rotation_drifts) + ), + "maximum_pair_translation_drift_m": float( + np.max(translation_drifts, initial=0.0) + ), + "p95_pair_translation_drift_m": float( + np.percentile(translation_drifts, 95.0) + ), + "maximum_pair_distance_drift_m": float( + np.max(distance_drifts, initial=0.0) + ), + "p95_pair_distance_drift_m": float( + np.percentile(distance_drifts, 95.0) + ), + "median_pair_distance_drift_m": float( + np.median(distance_drifts) + ), + } + + +def select_static_rigid_group_initialization( + frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], + *, + roles: Sequence[str], + fixed_pairs: Sequence[tuple[str, str]], + reprojection_scale_px: float, + maximum_pose_jump_rad: float, + maximum_translation_jump_m: float, + relative_rotation_scale_rad: float, + relative_translation_scale_m: float, + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float = math.radians(5.0), + task_reference_pairs: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray] + ] | None = None, + task_reference_rotation_scale_rad: float = math.radians(1.0), + task_reference_translation_scale_m: float = 0.01, +) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]: + """Select a static multi-Tag IPPE branch path in bounded time. + + Group initialization is performed while the hand is held at an endpoint, + so every frame should describe the same camera and relative Tag poses. + Enumerating a full Viterbi transition matrix for every possible first-frame + branch is therefore unnecessary: with four two-branch Tags and eight + frames it performs more than one hundred thousand scipy rotations and can + block the live ROS callback for several seconds. + + Instead, treat every first-frame combination as a possible static + reference and independently select the closest combination in each later + frame. This preserves the multi-frame rigidity and normal-alignment + evidence while changing the search from O(F*C^2*C0) to O(F*C*C0). + """ + role_names = tuple(str(role) for role in roles) + pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) + normal_pair_names = tuple( + (str(first), str(second)) for first, second in normal_alignment_pairs + ) + task_references = dict(task_reference_pairs or {}) + if not frames: + raise ValueError("at least one PnP frame is required") + if not role_names or len(set(role_names)) != len(role_names): + raise ValueError("roles must be non-empty and unique") + if any( + parent not in role_names or child not in role_names + for parent, child in ( + *pair_names, + *normal_pair_names, + *task_references, + ) + ): + raise ValueError("geometry pairs must reference roles") + scales = ( + float(reprojection_scale_px), + float(maximum_pose_jump_rad), + float(maximum_translation_jump_m), + float(relative_rotation_scale_rad), + float(relative_translation_scale_m), + float(normal_alignment_scale_rad), + float(task_reference_rotation_scale_rad), + float(task_reference_translation_scale_m), + ) + if min(scales) <= 0.0: + raise ValueError("static initialization scales must be positive") + ( + reprojection_scale, + pose_scale, + translation_scale, + relative_rotation_scale, + relative_translation_scale, + normal_scale, + task_reference_rotation_scale, + task_reference_translation_scale, + ) = scales + + combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] + for frame in frames: + candidate_lists = [tuple(frame.get(role, ())) for role in role_names] + if any(not candidates for candidates in candidate_lists): + raise ValueError("every frame must contain every requested role") + combinations_by_frame.append( + [ + dict(zip(role_names, combination)) + for combination in product(*candidate_lists) + ] + ) + + def score_against_reference( + reference: Mapping[str, SquareTagPose], + reference_pairs: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray] + ], + combination: Mapping[str, SquareTagPose], + ) -> float: + score = sum( + pose.reprojection_error_px for pose in combination.values() + ) / reprojection_scale + score += sum( + _normal_alignment_rad(combination[first], combination[second]) + for first, second in normal_pair_names + ) / normal_scale + score += sum( + rotation_distance_rad( + reference[role].quaternion_xyzw, + combination[role].quaternion_xyzw, + ) + / pose_scale + + float( + np.linalg.norm( + np.asarray(combination[role].translation_xyz_m, dtype=float) + - np.asarray(reference[role].translation_xyz_m, dtype=float) + ) + ) + / translation_scale + for role in role_names + ) + for pair, (reference_rotation, reference_translation) in ( + reference_pairs.items() + ): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + score += ( + float((reference_rotation.inv() * rotation).magnitude()) + / relative_rotation_scale + + float(np.linalg.norm(translation - reference_translation)) + / relative_translation_scale + ) + for pair, (task_rotation, task_translation) in task_references.items(): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + score += ( + float((task_rotation.inv() * rotation).magnitude()) + / task_reference_rotation_scale + + float(np.linalg.norm(translation - task_translation)) + / task_reference_translation_scale + ) + return float(score) + + best_total = float("inf") + best_path: list[dict[str, SquareTagPose]] | None = None + for reference in combinations_by_frame[0]: + reference_pairs = { + pair: _relative_pose(reference[pair[0]], reference[pair[1]]) + for pair in pair_names + } + path = [reference] + total = score_against_reference(reference, reference_pairs, reference) + for combinations in combinations_by_frame[1:]: + scored = [ + ( + score_against_reference( + reference, reference_pairs, combination + ), + combination, + ) + for combination in combinations + ] + cost, selected = min(scored, key=lambda item: item[0]) + total += cost + path.append(selected) + if total < best_total: + best_total = total + best_path = path + + if best_path is None: + raise RuntimeError("static group initialization produced no path") + + # Reuse the complete quality calculation with exactly one chosen branch + # per role and frame. This retains all existing quality fields without + # reintroducing the combinatorial branch search. + reduced_frames = [ + {role: (frame[role],) for role in role_names} for frame in best_path + ] + selected_path, quality = select_rigid_group_trajectory( + reduced_frames, + roles=role_names, + fixed_pairs=pair_names, + reprojection_scale_px=reprojection_scale, + rotation_scale_rad=relative_rotation_scale, + translation_scale_m=relative_translation_scale, + normal_alignment_pairs=normal_pair_names, + normal_alignment_scale_rad=normal_scale, + ) + quality = dict(quality) + quality["total_cost"] = float(best_total) + quality["initialization_search"] = "static_reference" + quality["task_reference_used"] = ( + "true" if task_references else "false" + ) + return selected_path, quality + + +def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray: + matrix = np.asarray(camera_matrix, dtype=np.float64) + if matrix.shape != (3, 3): + raise ValueError("camera_matrix must have shape (3, 3)") + if not np.all(np.isfinite(matrix)): + raise ValueError("camera_matrix must be finite") + if matrix[0, 0] <= 0.0 or matrix[1, 1] <= 0.0: + raise ValueError("camera focal lengths must be positive") + return matrix + + +def square_object_points(tag_size_m: float) -> np.ndarray: + """Return IPPE-square points matching apriltag_msgs corner order. + + ``apriltag_ros`` reports bottom-left, bottom-right, top-right, top-left. + OpenCV's ``SOLVEPNP_IPPE_SQUARE`` requires the same physical corners in + the order below. + """ + size = float(tag_size_m) + if not math.isfinite(size) or size <= 0.0: + raise ValueError("tag_size_m must be finite and positive") + half = size / 2.0 + return np.asarray( + [ + [-half, half, 0.0], + [half, half, 0.0], + [half, -half, 0.0], + [-half, -half, 0.0], + ], + dtype=np.float64, + ) + + +def solve_square_tag_ippe( + corners_xy: Sequence[Sequence[float]], + *, + tag_size_m: float, + camera_matrix: Sequence[Sequence[float]], +) -> list[SquareTagPose]: + """Return every finite, positive-depth IPPE pose for one square tag.""" + image_points = np.asarray(corners_xy, dtype=np.float64) + if image_points.shape != (4, 2): + raise ValueError("corners_xy must have shape (4, 2)") + if not np.all(np.isfinite(image_points)): + raise ValueError("corners_xy must be finite") + intrinsic = _as_camera_matrix(camera_matrix) + object_points = square_object_points(tag_size_m) + distortion = np.zeros((4, 1), dtype=np.float64) + + solved, rotation_vectors, translations, _ = cv2.solvePnPGeneric( + object_points, + image_points, + intrinsic, + distortion, + flags=cv2.SOLVEPNP_IPPE_SQUARE, + ) + if not solved: + return [] + + candidates: list[SquareTagPose] = [] + for rotation_vector, translation in zip(rotation_vectors, translations): + rotation_matrix, _ = cv2.Rodrigues(rotation_vector) + translation_vector = np.asarray(translation, dtype=float).reshape(3) + camera_points = ( + rotation_matrix @ object_points.T + + translation_vector.reshape(3, 1) + ).T + if np.min(camera_points[:, 2]) <= 0.0: + continue + projected, _ = cv2.projectPoints( + object_points, + rotation_vector, + translation_vector, + intrinsic, + distortion, + ) + residual = projected.reshape(4, 2) - image_points + reprojection_error = float( + np.sqrt(np.mean(np.sum(residual * residual, axis=1))) + ) + quaternion = Rotation.from_matrix(rotation_matrix).as_quat() + if not ( + np.all(np.isfinite(quaternion)) + and np.all(np.isfinite(translation_vector)) + and math.isfinite(reprojection_error) + ): + continue + candidates.append( + SquareTagPose( + quaternion_xyzw=tuple(float(value) for value in quaternion), + translation_xyz_m=tuple( + float(value) for value in translation_vector + ), + reprojection_error_px=reprojection_error, + ) + ) + return candidates + + +def rotation_distance_rad( + first_xyzw: Sequence[float], + second_xyzw: Sequence[float], +) -> float: + first = np.asarray(first_xyzw, dtype=float) + second = np.asarray(second_xyzw, dtype=float) + if first.shape != (4,) or second.shape != (4,): + raise ValueError("quaternions must contain four values") + first_norm = float(np.linalg.norm(first)) + second_norm = float(np.linalg.norm(second)) + if ( + not np.all(np.isfinite(first)) + or not np.all(np.isfinite(second)) + or first_norm <= 0.0 + or second_norm <= 0.0 + ): + raise ValueError("quaternions must be finite and non-zero") + # q and -q represent the same rotation. The absolute dot-product gives + # the geodesic SO(3) distance without constructing two scipy Rotation + # objects for every branch comparison in the live tracker. + cosine_half_angle = abs( + float(np.dot(first / first_norm, second / second_norm)) + ) + return 2.0 * math.acos(float(np.clip(cosine_half_angle, 0.0, 1.0))) + + +def select_continuous_pose( + candidates: Sequence[SquareTagPose], + *, + previous: SquareTagPose | None, + maximum_reprojection_error_px: float, + reprojection_tie_px: float, + maximum_pose_jump_rad: float, + maximum_translation_jump_m: float, + maximum_tag_tilt_rad: float, +) -> tuple[SquareTagPose | None, str]: + """Select the best IPPE branch using image fit and temporal continuity.""" + maximum_error = float(maximum_reprojection_error_px) + tie_error = float(reprojection_tie_px) + maximum_rotation = float(maximum_pose_jump_rad) + maximum_translation = float(maximum_translation_jump_m) + maximum_tilt = float(maximum_tag_tilt_rad) + if min( + maximum_error, + maximum_rotation, + maximum_translation, + maximum_tilt, + ) <= 0.0: + raise ValueError("PnP selection thresholds must be positive") + if tie_error < 0.0: + raise ValueError("reprojection_tie_px must be non-negative") + + eligible: list[SquareTagPose] = [] + for candidate in candidates: + if candidate.reprojection_error_px > maximum_error: + continue + normal = Rotation.from_quat(candidate.quaternion_xyzw).as_matrix()[:, 2] + tilt = math.acos(float(np.clip(abs(normal[2]), 0.0, 1.0))) + if tilt > maximum_tilt: + continue + eligible.append(candidate) + if not eligible: + return None, "no_pose_within_reprojection_or_tilt_limit" + + eligible.sort(key=lambda item: item.reprojection_error_px) + best = eligible[0] + if previous is None: + return best, "" + + # Temporal continuity must only break a genuine planar-PnP tie. The old + # implementation normalised reprojection error by the permissive 1.5 px + # rejection limit, which allowed a stale mirror branch at 0.25 px to beat + # the true branch at e.g. 0.05 px merely because it was closer to the + # preceding (already wrong) pose. Once one IPPE solution has a meaningful + # image-fit advantage, trust it and allow the tracker to leave the stale + # branch even if that correction is a large pose jump. + competitive = [ + candidate + for candidate in eligible + if candidate.reprojection_error_px + <= best.reprojection_error_px + tie_error + ] + if len(competitive) == 1: + return best, "" + + previous_translation = np.asarray(previous.translation_xyz_m, dtype=float) + scored: list[tuple[float, SquareTagPose]] = [] + for candidate in competitive: + rotation_jump = rotation_distance_rad( + previous.quaternion_xyzw, + candidate.quaternion_xyzw, + ) + translation_jump = float( + np.linalg.norm( + np.asarray(candidate.translation_xyz_m, dtype=float) + - previous_translation + ) + ) + if ( + rotation_jump > maximum_rotation + or translation_jump > maximum_translation + ): + continue + score = ( + ( + candidate.reprojection_error_px + - best.reprojection_error_px + ) + / max(tie_error, np.finfo(float).eps) + + rotation_jump / maximum_rotation + + translation_jump / maximum_translation + ) + scored.append((float(score), candidate)) + if not scored: + return None, "pose_jump" + + selected = min(scored, key=lambda item: item[0])[1] + previous_quaternion = np.asarray(previous.quaternion_xyzw, dtype=float) + selected_quaternion = np.asarray(selected.quaternion_xyzw, dtype=float) + if float(np.dot(previous_quaternion, selected_quaternion)) < 0.0: + selected = replace( + selected, + quaternion_xyzw=tuple( + float(value) for value in -selected_quaternion + ), + ) + return selected, "" + + +class SquareTagPoseTracker: + """Maintain the selected planar-PnP branch independently for each tag.""" + + def __init__( + self, + *, + maximum_reprojection_error_px: float, + reprojection_tie_px: float, + maximum_pose_jump_rad: float, + maximum_translation_jump_m: float, + maximum_tag_tilt_rad: float, + reset_after_seconds: float, + ) -> None: + self.maximum_reprojection_error_px = float( + maximum_reprojection_error_px + ) + self.reprojection_tie_px = float(reprojection_tie_px) + self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) + self.maximum_translation_jump_m = float(maximum_translation_jump_m) + self.maximum_tag_tilt_rad = float(maximum_tag_tilt_rad) + self.reset_after_ns = int(float(reset_after_seconds) * 1_000_000_000) + if self.reset_after_ns <= 0: + raise ValueError("reset_after_seconds must be positive") + self._previous: dict[str, tuple[int, SquareTagPose]] = {} + self.last_candidates_by_role: dict[ + str, tuple[SquareTagPose, ...] + ] = {} + self.last_candidate_diagnostics_by_role: dict[ + str, dict[str, float | int] + ] = {} + self.branch_correction_counts: dict[str, int] = {} + + def reset(self) -> None: + self._previous.clear() + self.last_candidates_by_role.clear() + self.last_candidate_diagnostics_by_role.clear() + self.branch_correction_counts.clear() + + def estimate( + self, + role: str, + corners_xy: Sequence[Sequence[float]], + *, + tag_size_m: float, + camera_matrix: Sequence[Sequence[float]], + stamp_ns: int, + reprojection_tie_px: float | None = None, + ) -> tuple[SquareTagPose | None, str]: + try: + candidates = solve_square_tag_ippe( + corners_xy, + tag_size_m=tag_size_m, + camera_matrix=camera_matrix, + ) + except (ValueError, cv2.error): + self.last_candidates_by_role[str(role)] = () + self.last_candidate_diagnostics_by_role[str(role)] = { + "solved_candidate_count": 0, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } + return None, "pnp_solve_failed" + if not candidates: + self.last_candidates_by_role[str(role)] = () + self.last_candidate_diagnostics_by_role[str(role)] = { + "solved_candidate_count": 0, + "reprojection_candidate_count": 0, + "independent_tilt_candidate_count": 0, + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } + return None, "pnp_solve_failed" + reprojection_candidates = [ + candidate + for candidate in candidates + if candidate.reprojection_error_px + <= self.maximum_reprojection_error_px + ] + candidate_tilts_rad: list[float] = [] + independent_candidates: list[SquareTagPose] = [] + for candidate in reprojection_candidates: + normal = Rotation.from_quat( + candidate.quaternion_xyzw + ).as_matrix()[:, 2] + tilt = math.acos( + float(np.clip(abs(normal[2]), 0.0, 1.0)) + ) + candidate_tilts_rad.append(float(tilt)) + if tilt <= self.maximum_tag_tilt_rad: + independent_candidates.append(candidate) + + # Candidate generation and candidate selection have different + # contracts. The per-Tag tilt limit protects a pose used without any + # other geometry, but it must not erase a finite, low-reprojection + # IPPE solution before SquareTagGroupPoseTracker can evaluate it + # against the fixed palm reference, the articulated chain and the + # preceding group pose. At a strongly oblique view the planar + # ambiguity is usually smaller, and rejecting both branches at a + # fixed angle caused deterministic mid-sweep holes despite continuous + # image detections. Group tracking therefore receives every + # reprojection-valid candidate; independent tracking below retains the + # original tilt safety gate. + self.last_candidates_by_role[str(role)] = tuple( + reprojection_candidates + ) + diagnostics: dict[str, float | int] = { + "solved_candidate_count": len(candidates), + "reprojection_candidate_count": len(reprojection_candidates), + "independent_tilt_candidate_count": len(independent_candidates), + "minimum_reprojection_error_px": float( + min( + candidate.reprojection_error_px + for candidate in candidates + ) + ), + "maximum_reprojection_error_px": float( + self.maximum_reprojection_error_px + ), + "maximum_independent_tilt_deg": math.degrees( + self.maximum_tag_tilt_rad + ), + } + if candidate_tilts_rad: + diagnostics["minimum_candidate_tilt_deg"] = math.degrees( + min(candidate_tilts_rad) + ) + diagnostics["maximum_candidate_tilt_deg"] = math.degrees( + max(candidate_tilts_rad) + ) + self.last_candidate_diagnostics_by_role[str(role)] = diagnostics + if not independent_candidates: + return None, "no_pose_within_reprojection_or_tilt_limit" + + previous_record = self._previous.get(str(role)) + previous: SquareTagPose | None = None + if previous_record is not None: + previous_stamp, previous_pose = previous_record + elapsed = int(stamp_ns) - previous_stamp + if 0 <= elapsed <= self.reset_after_ns: + previous = previous_pose + + selected, reason = select_continuous_pose( + independent_candidates, + previous=previous, + maximum_reprojection_error_px=( + self.maximum_reprojection_error_px + ), + reprojection_tie_px=( + self.reprojection_tie_px + if reprojection_tie_px is None + else float(reprojection_tie_px) + ), + maximum_pose_jump_rad=self.maximum_pose_jump_rad, + maximum_translation_jump_m=self.maximum_translation_jump_m, + maximum_tag_tilt_rad=self.maximum_tag_tilt_rad, + ) + if selected is not None: + if previous is not None: + rotation_jump = rotation_distance_rad( + previous.quaternion_xyzw, + selected.quaternion_xyzw, + ) + translation_jump = float( + np.linalg.norm( + np.asarray(selected.translation_xyz_m, dtype=float) + - np.asarray( + previous.translation_xyz_m, + dtype=float, + ) + ) + ) + if ( + rotation_jump > self.maximum_pose_jump_rad + or translation_jump + > self.maximum_translation_jump_m + ): + key = str(role) + self.branch_correction_counts[key] = ( + self.branch_correction_counts.get(key, 0) + 1 + ) + self._previous[str(role)] = (int(stamp_ns), selected) + return selected, reason + + +class SquareTagGroupPoseTracker: + """Choose all tag branches together using thumb-chain continuity. + + A 30 px planar tag has two IPPE solutions whose reprojection errors can + exchange order from one frame to the next. Tracking each tag + independently can therefore choose an incompatible pair for a relative + joint such as T4->T5. This tracker enumerates the small Cartesian product + (at most 2**4 combinations) and favours the combination that keeps both + the camera poses and all adjacent relative poses continuous. + """ + + def __init__( + self, + *, + roles: Sequence[str], + adjacent_pairs: Sequence[tuple[str, str]], + maximum_pose_jump_rad: float, + maximum_translation_jump_m: float, + relative_rotation_scale_rad: float, + relative_translation_scale_m: float, + reprojection_scale_px: float, + reprojection_weight: float, + reset_after_seconds: float, + initialization_frames: int = 1, + normal_alignment_pairs: Sequence[tuple[str, str]] = (), + normal_alignment_scale_rad: float = math.radians(5.0), + maximum_normal_alignment_rad: float | None = None, + return_reference_rotation_scale_rad: float = math.radians(1.0), + return_reference_maximum_command_gap_u8: int = 8, + coupled_rotation_pairs: Sequence[ + tuple[str, str, str, str, float] + ] = (), + coupled_rotation_scale_rad: float = math.radians(3.0), + maximum_coupled_rotation_residual_rad: float | None = None, + ) -> None: + self.roles = tuple(str(role) for role in roles) + self.adjacent_pairs = tuple( + (str(parent), str(child)) + for parent, child in adjacent_pairs + ) + self.normal_alignment_pairs = tuple( + (str(first), str(second)) + for first, second in normal_alignment_pairs + ) + self.coupled_rotation_pairs = tuple( + ( + str(driver_parent), + str(driver_child), + str(follower_parent), + str(follower_child), + float(multiplier), + ) + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + multiplier, + ) in coupled_rotation_pairs + ) + if not self.roles or len(set(self.roles)) != len(self.roles): + raise ValueError("roles must be non-empty and unique") + if any( + parent not in self.roles or child not in self.roles + for parent, child in ( + *self.adjacent_pairs, + *self.normal_alignment_pairs, + ) + ): + raise ValueError("group geometry pairs must reference roles") + self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) + self.maximum_translation_jump_m = float( + maximum_translation_jump_m + ) + self.relative_rotation_scale_rad = float( + relative_rotation_scale_rad + ) + self.relative_translation_scale_m = float( + relative_translation_scale_m + ) + self.reprojection_scale_px = float(reprojection_scale_px) + self.reprojection_weight = float(reprojection_weight) + self.initialization_frames = int(initialization_frames) + self.normal_alignment_scale_rad = float( + normal_alignment_scale_rad + ) + self.maximum_normal_alignment_rad = ( + None + if maximum_normal_alignment_rad is None + else float(maximum_normal_alignment_rad) + ) + self.return_reference_rotation_scale_rad = float( + return_reference_rotation_scale_rad + ) + self.return_reference_maximum_command_gap_u8 = int( + return_reference_maximum_command_gap_u8 + ) + self.coupled_rotation_scale_rad = float( + coupled_rotation_scale_rad + ) + self.maximum_coupled_rotation_residual_rad = ( + None + if maximum_coupled_rotation_residual_rad is None + else float(maximum_coupled_rotation_residual_rad) + ) + reset_seconds = float(reset_after_seconds) + if min( + self.maximum_pose_jump_rad, + self.maximum_translation_jump_m, + self.relative_rotation_scale_rad, + self.relative_translation_scale_m, + self.reprojection_scale_px, + self.normal_alignment_scale_rad, + self.return_reference_rotation_scale_rad, + self.coupled_rotation_scale_rad, + reset_seconds, + ) <= 0.0: + raise ValueError("group tracking scales must be positive") + if self.reprojection_weight < 0.0: + raise ValueError("reprojection_weight must be non-negative") + if self.initialization_frames < 1: + raise ValueError("initialization_frames must be positive") + if self.return_reference_maximum_command_gap_u8 < 0: + raise ValueError( + "return reference maximum command gap must be non-negative" + ) + if ( + self.maximum_normal_alignment_rad is not None + and self.maximum_normal_alignment_rad <= 0.0 + ): + raise ValueError("maximum normal alignment must be positive") + if any( + role not in self.roles + for coupling in self.coupled_rotation_pairs + for role in coupling[:4] + ): + raise ValueError("coupled rotation pairs must reference roles") + if any( + multiplier <= 0.0 + for *_, multiplier in self.coupled_rotation_pairs + ): + raise ValueError("coupled rotation multipliers must be positive") + if ( + self.maximum_coupled_rotation_residual_rad is not None + and self.maximum_coupled_rotation_residual_rad <= 0.0 + ): + raise ValueError( + "maximum coupled rotation residual must be positive" + ) + self.reset_after_ns = int(reset_seconds * 1_000_000_000) + self._previous: dict[str, SquareTagPose] = {} + self._previous_stamp_ns: int | None = None + self._initial_candidates: list[ + dict[str, tuple[SquareTagPose, ...]] + ] = [] + self._initial_stamps_ns: list[int] = [] + self.last_initialization_quality: dict[str, float | str] = {} + self.branch_correction_counts: dict[str, int] = {} + self._decreasing_relative_rotations: dict[ + int, dict[tuple[str, str], Rotation] + ] = {} + self._coupled_reference_rotations: dict[ + tuple[str, str], Rotation + ] = {} + self._task_reference_relative_poses: dict[ + tuple[str, str], tuple[Rotation, np.ndarray] + ] = {} + self.last_missing_roles: tuple[str, ...] = () + + def reset(self, *, preserve_task_reference: bool = False) -> None: + self._previous.clear() + self._previous_stamp_ns = None + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self.last_initialization_quality.clear() + self.branch_correction_counts.clear() + self._decreasing_relative_rotations.clear() + self._coupled_reference_rotations.clear() + self.last_missing_roles = () + if not preserve_task_reference: + self._task_reference_relative_poses.clear() + + def _task_reference_cost( + self, combination: Mapping[str, SquareTagPose] + ) -> float: + residual = 0.0 + for pair, (expected_rotation, expected_translation) in ( + self._task_reference_relative_poses.items() + ): + rotation, translation = _relative_pose( + combination[pair[0]], combination[pair[1]] + ) + residual += ( + float((expected_rotation.inv() * rotation).magnitude()) + / self.return_reference_rotation_scale_rad + + float(np.linalg.norm(translation - expected_translation)) + / self.relative_translation_scale_m + ) + return residual + + def _coupled_rotation_residuals( + self, combination: Mapping[str, SquareTagPose] + ) -> tuple[float, ...]: + if not self.coupled_rotation_pairs: + return () + residuals: list[float] = [] + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + multiplier, + ) in self.coupled_rotation_pairs: + driver_pair = (driver_parent, driver_child) + follower_pair = (follower_parent, follower_child) + if ( + driver_pair not in self._coupled_reference_rotations + or follower_pair not in self._coupled_reference_rotations + ): + return () + driver_rotation = _relative_pose( + combination[driver_parent], combination[driver_child] + )[0] + follower_rotation = _relative_pose( + combination[follower_parent], combination[follower_child] + )[0] + driver_travel = ( + self._coupled_reference_rotations[driver_pair].inv() + * driver_rotation + ).magnitude() + follower_travel = ( + self._coupled_reference_rotations[follower_pair].inv() + * follower_rotation + ).magnitude() + residuals.append( + abs(float(follower_travel) - multiplier * float(driver_travel)) + ) + return tuple(residuals) + + def _informative_coupled_rotation_costs( + self, + combinations: Sequence[Mapping[str, SquareTagPose]], + ) -> tuple[float, ...]: + """Return branch costs only while the weak coupling prior is credible. + + The URDF mimic ratio is useful for distinguishing two planar-IPPE + branches, but it is not measurement truth for a passive joint. Once + every otherwise viable combination disagrees with that ratio, using + it would bias the measured curve (and previously rejected every + frame). In that case fall back to visual continuity for this frame. + """ + residuals = tuple( + self._coupled_rotation_residuals(combination) + for combination in combinations + ) + if not residuals or not any(residuals): + return tuple(0.0 for _ in combinations) + if ( + self.maximum_coupled_rotation_residual_rad is not None + and not any( + values + and max(values) + <= self.maximum_coupled_rotation_residual_rad + for values in residuals + ) + ): + return tuple(0.0 for _ in combinations) + return tuple( + sum(values) / self.coupled_rotation_scale_rad + for values in residuals + ) + + def _return_reference( + self, command_u8: int | None + ) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]: + if command_u8 is None or not self._decreasing_relative_rotations: + return {} + command = int(command_u8) + nearest = min( + self._decreasing_relative_rotations, + key=lambda candidate: abs(candidate - command), + ) + if ( + abs(nearest - command) + > self.return_reference_maximum_command_gap_u8 + ): + return {} + references = self._decreasing_relative_rotations[nearest] + commands = sorted(self._decreasing_relative_rotations) + axes: dict[tuple[str, str], np.ndarray | None] = {} + for pair in self.adjacent_pairs: + endpoint_delta = ( + self._decreasing_relative_rotations[commands[-1]][pair].inv() + * self._decreasing_relative_rotations[commands[0]][pair] + ).as_rotvec() + norm = float(np.linalg.norm(endpoint_delta)) + axes[pair] = ( + None + if norm < math.radians(5.0) + else endpoint_delta / norm + ) + return { + pair: (rotation, axes[pair]) + for pair, rotation in references.items() + } + + def _return_reference_cost( + self, + combination: Mapping[str, SquareTagPose], + reference: Mapping[ + tuple[str, str], tuple[Rotation, np.ndarray | None] + ], + ) -> float: + residual = 0.0 + for pair, (expected, motion_axis) in reference.items(): + vector = ( + expected.inv() + * _relative_pose( + combination[pair[0]], combination[pair[1]] + )[0] + ).as_rotvec() + if motion_axis is not None: + # The outbound trajectory identifies the physical one-DOF + # motion axis. Do not penalize return travel along that axis: + # it may contain real mechanical hysteresis that calibration + # must measure. A planar-IPPE mirror branch appears primarily + # as a large orthogonal tilt and is rejected by this residual. + vector = vector - motion_axis * float(vector @ motion_axis) + residual += float(np.linalg.norm(vector)) + return residual / self.return_reference_rotation_scale_rad + + def select( + self, + candidates_by_role: Mapping[str, Sequence[SquareTagPose]], + *, + stamp_ns: int, + trajectory_command_u8: int | None = None, + trajectory_direction: str | None = None, + ) -> tuple[dict[str, SquareTagPose] | None, str]: + """Return one mutually consistent pose for every configured role.""" + direction = ( + None + if trajectory_direction is None + else str(trajectory_direction) + ) + if direction not in {None, "decreasing", "increasing"}: + raise ValueError( + "trajectory_direction must be decreasing or increasing" + ) + return_reference = ( + self._return_reference(trajectory_command_u8) + if direction == "increasing" + else {} + ) + candidate_lists = [ + tuple(candidates_by_role.get(role, ())) + for role in self.roles + ] + self.last_missing_roles = tuple( + role + for role, candidates in zip(self.roles, candidate_lists) + if not candidates + ) + if self.last_missing_roles: + return None, "group_missing_pose_candidates" + self.last_missing_roles = () + + combinations = [ + dict(zip(self.roles, combination)) + for combination in product(*candidate_lists) + ] + minimum_errors = { + role: min( + candidate.reprojection_error_px + for candidate in candidates + ) + for role, candidates in zip(self.roles, candidate_lists) + } + stamp = int(stamp_ns) + previous_is_fresh = ( + self._previous_stamp_ns is not None + and 0 <= stamp - self._previous_stamp_ns + <= self.reset_after_ns + and set(self._previous) == set(self.roles) + ) + + if not previous_is_fresh: + if self._previous_stamp_ns is not None: + self._previous.clear() + self._previous_stamp_ns = None + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self.last_initialization_quality.clear() + if self.initialization_frames > 1: + if self._initial_stamps_ns and not ( + 0 <= stamp - self._initial_stamps_ns[-1] + <= self.reset_after_ns + ): + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + self._initial_candidates.append( + { + role: tuple(candidates_by_role.get(role, ())) + for role in self.roles + } + ) + self._initial_stamps_ns.append(stamp) + if len(self._initial_candidates) < self.initialization_frames: + return ( + None, + "group_initializing:" + f"{len(self._initial_candidates)}/" + f"{self.initialization_frames}", + ) + selected_path, initialization_quality = ( + select_static_rigid_group_initialization( + self._initial_candidates, + roles=self.roles, + fixed_pairs=self.adjacent_pairs, + reprojection_scale_px=self.reprojection_scale_px, + maximum_pose_jump_rad=self.maximum_pose_jump_rad, + maximum_translation_jump_m=( + self.maximum_translation_jump_m + ), + relative_rotation_scale_rad=( + self.relative_rotation_scale_rad + ), + relative_translation_scale_m=( + self.relative_translation_scale_m + ), + normal_alignment_pairs=( + self.normal_alignment_pairs + ), + normal_alignment_scale_rad=( + self.normal_alignment_scale_rad + ), + task_reference_pairs=( + self._task_reference_relative_poses + ), + task_reference_rotation_scale_rad=( + self.return_reference_rotation_scale_rad + ), + task_reference_translation_scale_m=( + self.relative_translation_scale_m + ), + ) + ) + selected = selected_path[-1] + stamp = self._initial_stamps_ns[-1] + self.last_initialization_quality = dict( + initialization_quality + ) + self._initial_candidates.clear() + self._initial_stamps_ns.clear() + if ( + self.maximum_normal_alignment_rad is not None + and float( + initialization_quality[ + "maximum_normal_alignment_rad" + ] + ) + > self.maximum_normal_alignment_rad + ): + return None, "group_normal_alignment" + else: + coupling_costs = self._informative_coupled_rotation_costs( + combinations + ) + selected = min( + zip(combinations, coupling_costs), + key=lambda item: ( + sum( + pose.reprojection_error_px + for pose in item[0].values() + ) + / self.reprojection_scale_px + + sum( + _normal_alignment_rad( + item[0][first], item[0][second] + ) + for first, second in self.normal_alignment_pairs + ) + / self.normal_alignment_scale_rad + + self._return_reference_cost( + item[0], return_reference + ) + + item[1] + + self._task_reference_cost(item[0]) + ), + )[0] + maximum_alignment = max( + ( + _normal_alignment_rad( + selected[first], selected[second] + ) + for first, second in self.normal_alignment_pairs + ), + default=0.0, + ) + self.last_initialization_quality = { + "maximum_normal_alignment_rad": maximum_alignment + } + if ( + self.maximum_normal_alignment_rad is not None + and maximum_alignment + > self.maximum_normal_alignment_rad + ): + return None, "group_normal_alignment" + else: + previous_relative = { + pair: _relative_pose( + self._previous[pair[0]], + self._previous[pair[1]], + ) + for pair in self.adjacent_pairs + } + base_scored: list[tuple[float, dict[str, SquareTagPose]]] = [] + for combination in combinations: + absolute_rotation_motion = 0.0 + absolute_translation_motion = 0.0 + rejected = False + for role in self.roles: + rotation_motion = rotation_distance_rad( + self._previous[role].quaternion_xyzw, + combination[role].quaternion_xyzw, + ) + translation_motion = float( + np.linalg.norm( + np.asarray( + combination[role].translation_xyz_m, + dtype=float, + ) + - np.asarray( + self._previous[role].translation_xyz_m, + dtype=float, + ) + ) + ) + if ( + rotation_motion > self.maximum_pose_jump_rad + or translation_motion + > self.maximum_translation_jump_m + ): + rejected = True + break + absolute_rotation_motion += rotation_motion + absolute_translation_motion += translation_motion + if rejected: + continue + + relative_rotation_motion = 0.0 + relative_translation_motion = 0.0 + for pair in self.adjacent_pairs: + rotation, translation = _relative_pose( + combination[pair[0]], + combination[pair[1]], + ) + old_rotation, old_translation = previous_relative[pair] + relative_rotation_motion += float( + (old_rotation.inv() * rotation).magnitude() + ) + relative_translation_motion += float( + np.linalg.norm(translation - old_translation) + ) + + reprojection_penalty = sum( + max( + 0.0, + combination[role].reprojection_error_px + - minimum_errors[role], + ) + for role in self.roles + ) / self.reprojection_scale_px + score = ( + absolute_rotation_motion + / self.maximum_pose_jump_rad + + absolute_translation_motion + / self.maximum_translation_jump_m + + relative_rotation_motion + / self.relative_rotation_scale_rad + + relative_translation_motion + / self.relative_translation_scale_m + + self.reprojection_weight * reprojection_penalty + + self._return_reference_cost( + combination, return_reference + ) + ) + base_scored.append((float(score), combination)) + + if not base_scored: + return None, "group_pose_jump" + coupling_costs = self._informative_coupled_rotation_costs( + [combination for _, combination in base_scored] + ) + scored = [ + (base_score + coupling_cost, combination) + for (base_score, combination), coupling_cost in zip( + base_scored, coupling_costs + ) + ] + selected = min(scored, key=lambda item: item[0])[1] + + aligned: dict[str, SquareTagPose] = {} + for role in self.roles: + pose = selected[role] + if previous_is_fresh: + old_quaternion = np.asarray( + self._previous[role].quaternion_xyzw, + dtype=float, + ) + quaternion = np.asarray( + pose.quaternion_xyzw, + dtype=float, + ) + if float(np.dot(old_quaternion, quaternion)) < 0.0: + pose = replace( + pose, + quaternion_xyzw=tuple( + float(value) for value in -quaternion + ), + ) + best_reprojection = min( + candidate_lists[self.roles.index(role)], + key=lambda candidate: candidate.reprojection_error_px, + ) + if pose != best_reprojection: + self.branch_correction_counts[role] = ( + self.branch_correction_counts.get(role, 0) + 1 + ) + aligned[role] = pose + + self._previous = aligned + self._previous_stamp_ns = stamp + if ( + direction == "decreasing" + and trajectory_command_u8 is not None + and not self._coupled_reference_rotations + ): + for ( + driver_parent, + driver_child, + follower_parent, + follower_child, + _multiplier, + ) in self.coupled_rotation_pairs: + for pair in ( + (driver_parent, driver_child), + (follower_parent, follower_child), + ): + self._coupled_reference_rotations[pair] = _relative_pose( + aligned[pair[0]], aligned[pair[1]] + )[0] + if direction == "decreasing" and trajectory_command_u8 is not None: + self._decreasing_relative_rotations[ + int(trajectory_command_u8) + ] = { + pair: _relative_pose( + aligned[pair[0]], aligned[pair[1]] + )[0] + for pair in self.adjacent_pairs + } + if not self._task_reference_relative_poses: + self._task_reference_relative_poses = { + pair: _relative_pose( + aligned[pair[0]], aligned[pair[1]] + ) + for pair in self.adjacent_pairs + } + return dict(aligned), "" diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py new file mode 100644 index 0000000..400cdd8 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/geometry/rotation.py @@ -0,0 +1,157 @@ +"""Pure quaternion summaries and rotation-axis fitting.""" + +from __future__ import annotations + +import math +from typing import Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + + +def normalize_quaternion_xyzw(values: Sequence[float]) -> np.ndarray: + quaternion = np.asarray(values, dtype=float) + if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)): + raise ValueError("quaternion must contain four finite xyzw values") + norm = float(np.linalg.norm(quaternion)) + if norm < 1e-12: + raise ValueError("quaternion norm is zero") + return quaternion / norm + + +def relative_quaternion_xyzw( + parent_camera_quaternion: Sequence[float], + child_camera_quaternion: Sequence[float], +) -> tuple[float, float, float, float]: + """Compute parent-to-child orientation from two camera-to-Tag rotations.""" + parent = Rotation.from_quat( + normalize_quaternion_xyzw(parent_camera_quaternion) + ) + child = Rotation.from_quat( + normalize_quaternion_xyzw(child_camera_quaternion) + ) + quaternion = (parent.inv() * child).as_quat() + return tuple(float(value) for value in quaternion) + + +def image_plane_tag_quaternion_xyzw( + corners_xy: Sequence[Sequence[float]], +) -> tuple[float, float, float, float]: + """Estimate Tag orientation about the optical axis from ordered corners.""" + corners = np.asarray(corners_xy, dtype=float) + if corners.shape != (4, 2) or not np.all(np.isfinite(corners)): + raise ValueError("corners_xy must contain four finite xy points") + x_axis = (corners[1] - corners[0]) + (corners[2] - corners[3]) + if float(np.linalg.norm(x_axis)) < 1e-9: + raise ValueError("tag x-axis is degenerate") + angle = -math.atan2(float(x_axis[1]), float(x_axis[0])) + quaternion = Rotation.from_rotvec([0.0, 0.0, angle]).as_quat() + return tuple(float(value) for value in quaternion) + + +def robust_rotation_summary( + quaternions_xyzw: Sequence[Sequence[float]], +) -> tuple[tuple[float, float, float, float], float]: + """Return a robust orientation and maximum angular residual in radians.""" + if not quaternions_xyzw: + raise ValueError("at least one quaternion is required") + rotations = Rotation.from_quat( + np.asarray( + [normalize_quaternion_xyzw(value) for value in quaternions_xyzw], + dtype=float, + ) + ) + reference = rotations[0] + delta_vectors = (reference.inv() * rotations).as_rotvec() + median_delta = np.median(delta_vectors, axis=0) + robust = reference * Rotation.from_rotvec(median_delta) + residuals = (robust.inv() * rotations).magnitude() + maximum = float(np.max(residuals)) if residuals.size else 0.0 + return tuple(float(value) for value in robust.as_quat()), maximum + + +def rotation_spread_rad( + quaternions_xyzw: Sequence[Sequence[float]], +) -> float: + """Return the maximum geodesic residual around a robust orientation.""" + _, spread = robust_rotation_summary(quaternions_xyzw) + return spread + + +def rotation_rms_rad( + quaternions_xyzw: Sequence[Sequence[float]], + *, + outlier_threshold_rad: float | None = None, +) -> float: + """Return RMS geodesic noise around a robust orientation.""" + robust, _ = robust_rotation_summary(quaternions_xyzw) + reference = Rotation.from_quat(robust) + rotations = Rotation.from_quat( + np.asarray( + [normalize_quaternion_xyzw(value) for value in quaternions_xyzw], + dtype=float, + ) + ) + residuals = (reference.inv() * rotations).magnitude() + if outlier_threshold_rad is not None: + threshold = float(outlier_threshold_rad) + if threshold <= 0.0: + raise ValueError("outlier_threshold_rad must be positive") + residuals = residuals[residuals <= threshold] + if residuals.size == 0: + return float("inf") + return float(np.sqrt(np.mean(np.square(residuals)))) + + +def rotation_inlier_fraction( + quaternions_xyzw: Sequence[Sequence[float]], + *, + outlier_threshold_rad: float, +) -> float: + """Return the fraction close to the robust orientation.""" + threshold = float(outlier_threshold_rad) + if threshold <= 0.0: + raise ValueError("outlier_threshold_rad must be positive") + robust, _ = robust_rotation_summary(quaternions_xyzw) + reference = Rotation.from_quat(robust) + rotations = Rotation.from_quat( + np.asarray( + [normalize_quaternion_xyzw(value) for value in quaternions_xyzw], + dtype=float, + ) + ) + residuals = (reference.inv() * rotations).magnitude() + return float(np.mean(residuals <= threshold)) + + +def delta_rotation_vector( + reference_xyzw: Sequence[float], + observed_xyzw: Sequence[float], +) -> np.ndarray: + reference = Rotation.from_quat(normalize_quaternion_xyzw(reference_xyzw)) + observed = Rotation.from_quat(normalize_quaternion_xyzw(observed_xyzw)) + return (reference.inv() * observed).as_rotvec() + + +def fit_rotation_axis( + vectors: Sequence[Sequence[float]], + commands: Sequence[int], +) -> np.ndarray: + """Fit and orient the single rotational axis used by one command sweep.""" + matrix = np.asarray(vectors, dtype=float) + command_values = np.asarray(commands, dtype=int) + if matrix.ndim != 2 or matrix.shape[1] != 3: + raise ValueError("vectors must have shape (N, 3)") + if command_values.shape != (matrix.shape[0],): + raise ValueError("commands must match vectors") + useful = np.linalg.norm(matrix, axis=1) > 1e-6 + if int(np.count_nonzero(useful)) < 3: + raise ValueError("insufficient non-zero rotations to fit an axis") + _, _, vh = np.linalg.svd(matrix[useful], full_matrices=False) + axis = vh[0] + projections = matrix @ axis + low = projections[command_values <= 16] + high = projections[command_values >= 239] + if low.size and high.size and float(np.median(low)) < float(np.median(high)): + axis = -axis + return axis / np.linalg.norm(axis) diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py new file mode 100644 index 0000000..ad28c51 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/solver/__init__.py @@ -0,0 +1,15 @@ +"""Task acceptance and final-session solver contracts.""" + +from .interfaces import ( + SessionSolution, + SessionSolver, + TaskEvaluation, + TaskEvaluator, +) + +__all__ = [ + "SessionSolution", + "SessionSolver", + "TaskEvaluation", + "TaskEvaluator", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py b/src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py new file mode 100644 index 0000000..023baeb --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/solver/interfaces.py @@ -0,0 +1,42 @@ +"""Shared evaluator and final-solver interfaces.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any, Mapping, Protocol, Sequence + +from ..domain import CalibrationProfile, SampleRecord, TaskSpec + + +@dataclass(frozen=True) +class TaskEvaluation: + accepted: bool + failures: tuple[Mapping[str, Any], ...] = () + rescan_measurements: frozenset[str] = frozenset() + rescan_cycles: frozenset[int] = frozenset() + + +@dataclass(frozen=True) +class SessionSolution: + passed: bool + calibration: Mapping[str, Any] + zero_offsets_rad: Mapping[str, float] + failures: tuple[Mapping[str, Any], ...] = () + diagnostics: Mapping[str, Any] = field(default_factory=dict) + + +class TaskEvaluator(Protocol): + def evaluate_task( + self, + profile: CalibrationProfile, + task: TaskSpec, + samples: Sequence[SampleRecord], + ) -> TaskEvaluation: ... + + +class SessionSolver(Protocol): + def solve_session( + self, + profile: CalibrationProfile, + samples: Sequence[SampleRecord], + ) -> SessionSolution: ... diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py new file mode 100644 index 0000000..93bf5f2 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py @@ -0,0 +1,5 @@ +"""URDF correction authorization and validation types.""" + +from .plan import UrdfCorrectionPlan, build_correction_plan + +__all__ = ["UrdfCorrectionPlan", "build_correction_plan"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py new file mode 100644 index 0000000..58eb643 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/plan.py @@ -0,0 +1,104 @@ +"""One authorization plan shared by URDF writers and validators.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import hashlib +from pathlib import Path +from typing import Mapping + +from ..domain import CalibrationProfile + + +@dataclass(frozen=True) +class UrdfCorrectionPlan: + source_sha256: str + allowed_active_joints: frozenset[str] + endpoint_limit_joints: frozenset[str] + mimic_source_by_joint: Mapping[str, str] + frozen_joints: frozenset[str] + frozen_offsets_rad: Mapping[str, float] = field(default_factory=dict) + forbid_calibrated_source: bool = True + forbid_overwrite: bool = True + preserve_passive_joints: bool = True + + def __post_init__(self) -> None: + if len(self.source_sha256) != 64 or any( + character not in "0123456789abcdef" + for character in self.source_sha256.lower() + ): + raise ValueError("source URDF SHA-256 is invalid") + if self.allowed_active_joints & self.frozen_joints: + raise ValueError("allowed and frozen URDF joints overlap") + applied = self.allowed_active_joints | set(self.frozen_offsets_rad) + if not set(self.frozen_offsets_rad).issubset(self.frozen_joints): + raise ValueError("frozen offsets must belong to frozen joints") + if not self.endpoint_limit_joints.issubset(applied): + raise ValueError("endpoint limit joint is not an applied active joint") + if set(self.mimic_source_by_joint) & self.allowed_active_joints: + raise ValueError("dependent mimic joints cannot be active edit targets") + + def authorize_offsets(self, offsets_rad: Mapping[str, float]) -> None: + required = self.allowed_active_joints | set(self.frozen_offsets_rad) + unexpected = set(offsets_rad) - required + if unexpected: + raise ValueError( + "URDF correction contains unauthorized joints: " + + ", ".join(sorted(unexpected)) + ) + missing = required - set(offsets_rad) + if missing: + raise ValueError( + "URDF correction is missing active joints: " + + ", ".join(sorted(missing)) + ) + changed_frozen = { + name + for name, expected in self.frozen_offsets_rad.items() + if abs(float(offsets_rad[name]) - float(expected)) > 1.0e-12 + } + if changed_frozen: + raise ValueError( + "URDF correction changed frozen offsets: " + + ", ".join(sorted(changed_frozen)) + ) + + def verify_source(self, source_urdf: str | Path) -> None: + digest = hashlib.sha256(Path(source_urdf).read_bytes()).hexdigest() + if digest != self.source_sha256.lower(): + raise ValueError("source URDF SHA-256 differs from correction plan") + + +def build_correction_plan( + profile: CalibrationProfile, + *, + source_sha256: str, + scope: str, + frozen_offsets_rad: Mapping[str, float] | None = None, +) -> UrdfCorrectionPlan: + """Build one scope-aware edit authorization from typed policies.""" + selected = profile.scope.selected_joints(scope) + frozen = profile.scope.frozen_joints[str(scope)] + expected_frozen = { + str(name): float(value) + for name, value in dict(frozen_offsets_rad or {}).items() + } + if set(expected_frozen) != set(frozen): + missing = set(frozen) - set(expected_frozen) + extra = set(expected_frozen) - set(frozen) + raise ValueError( + "frozen URDF offset state differs from scope policy: " + f"missing={','.join(sorted(missing)) or '-'};" + f"extra={','.join(sorted(extra)) or '-'}" + ) + applied = selected | frozen + return UrdfCorrectionPlan( + source_sha256=source_sha256, + allowed_active_joints=selected, + endpoint_limit_joints=( + profile.zero.mechanical_endpoint_joints & applied + ), + mimic_source_by_joint=profile.zero.mimic_source_by_joint, + frozen_joints=frozen | profile.zero.cad_frozen_joints, + frozen_offsets_rad=expected_frozen, + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/full_hand.py b/src/linkerhand_calibration/linkerhand_calibration/full_hand.py index 9c947c4..b808e7a 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/full_hand.py +++ b/src/linkerhand_calibration/linkerhand_calibration/full_hand.py @@ -17,7 +17,7 @@ import xml.etree.ElementTree as ET import numpy as np -from .core import COMMAND_NAMES +from .models.g20.command_layout import G20_COMMAND_NAMES as COMMAND_NAMES from .trajectory import ( _angle_for_circle, _fit_circle_with_axis, @@ -129,6 +129,12 @@ class HandCalibrationProfile: 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: @@ -703,6 +709,12 @@ def _build_right_19_profile() -> HandCalibrationProfile: "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, ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py new file mode 100644 index 0000000..1c6e914 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/__init__.py @@ -0,0 +1,15 @@ +"""Model- and side-specific calibration policies.""" + +from .registry import ( + EngineBindings, + ProfileRegistry, + RegisteredProfile, + get_default_registry, +) + +__all__ = [ + "EngineBindings", + "ProfileRegistry", + "RegisteredProfile", + "get_default_registry", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py new file mode 100644 index 0000000..5915a14 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/__init__.py @@ -0,0 +1,15 @@ +"""Registered profiles for this hand family.""" + +from ..registry import ProfileRegistry + + +def register_profiles(registry: ProfileRegistry) -> None: + from .legacy_11 import build_left_profile, build_right_profile + from .right_19 import build_profile + + registry.register(build_profile()) + registry.register(build_left_profile()) + registry.register(build_right_profile()) + + +__all__ = ["register_profiles"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py new file mode 100644 index 0000000..64c18b0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/_adapter.py @@ -0,0 +1,200 @@ +"""Adapt reviewed family profiles to the shared typed contract.""" + +from __future__ import annotations + +from ...core import ( + CalibrationProfile, + CommandLayout, + MeasurementPolicy, + MeasurementSpec, + MotionPolicy, + ProfileKey, + QualityPolicy, + ScopePolicy, + TagSpec, + TaskSpec, + ViewSpec, + VisionRigSpec, + ZeroSolvePolicy, +) +from ..registry import EngineBindings, RegisteredProfile +from .artifacts import build_artifact_policy +from .motion import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, +) +from .zero_policy import MIMIC_DERIVED_FINGER_DIPS + + +_HARD_THRESHOLD_KEYS = frozenset( + { + "minimum_detection_rate", + "maximum_reprojection_error_px", + "maximum_axis_cycle_difference_rad", + "maximum_pose_line_rms_m", + "maximum_hysteresis_rad", + "maximum_validation_error_rad", + } +) + + +def _run_cli(args: list[str] | None = None) -> None: + from .runner import main + + main(args) + + +def adapt_profile( + *, + key: ProfileKey, + namespace: str, + hand_profile, + zero_profile, + mechanical_endpoint_joints: frozenset[str] = frozenset(), + post_solve_endpoint_joints: frozenset[str] = frozenset(), +) -> RegisteredProfile: + fixed_by_view = { + view: frozenset(roles) + for view, roles in hand_profile.preflight_view_roles.items() + } + views = tuple( + ViewSpec( + name=view, + tags=tuple( + TagSpec( + role=role, + tag_id=int(tag_id), + fixed_reference=role in fixed_by_view.get(view, frozenset()), + ) + for role, tag_id in roles.items() + ), + ) + for view, roles in hand_profile.view_tags.items() + ) + record_specs = hand_profile.record_specs + command_index_by_joint = { + name: int(spec.motor_index) for name, spec in record_specs.items() + } + command_index_by_joint.update( + { + name: int(spec.motor_index) + for name, spec in hand_profile.joint_specs.items() + } + ) + measurements = { + name: MeasurementSpec( + joint=name, + kind=str(spec.zero_kind or "curve"), + view=spec.view, + parent_role=spec.parent_role, + child_role=spec.child_role, + validation_source=(hand_profile.axis_validation_sources or {}).get( + name + ), + ) + for name, spec in record_specs.items() + } + tasks = tuple( + TaskSpec( + key=spec.key, + view=spec.view, + command_index=int(spec.motor_index), + joints=tuple(spec.joints), + auxiliary_commands=tuple(spec.auxiliary_commands), + validation_only=bool(spec.validation_only), + ) + for spec in hand_profile.sweep_specs + ) + active = frozenset(hand_profile.active_joints) + passive = frozenset(hand_profile.passive_joints) + thumb = frozenset(name for name in active if name.startswith("thumb_")) + fingers = active - thumb + typed = CalibrationProfile( + key=key, + namespace=namespace, + command=CommandLayout( + names=tuple(hand_profile.command_names), + baseline_u8=tuple(int(value) for value in hand_profile.baseline_command), + command_index_by_joint=command_index_by_joint, + disabled_indices=frozenset( + index + for index, name in enumerate(hand_profile.command_names) + if name.startswith("reserved_") + ), + ), + vision=VisionRigSpec( + views=views, + common_frame="calibration_common", + extrinsic_reference_view=views[0].name, + ), + motion=MotionPolicy( + tasks=tasks, + precheck_sweeps=bool(hand_profile.precheck_sweeps), + steady_command_checkpoints=bool( + hand_profile.steady_command_checkpoints + ), + ), + measurement=MeasurementPolicy( + measurements=measurements, + cross_view_sources=dict( + hand_profile.axis_validation_sources or {} + ), + image_curve_joints=frozenset( + hand_profile.image_trajectory_joints + ), + directional_zero=bool(hand_profile.directional_zero), + cross_view_roll_curve=bool(hand_profile.cross_view_roll_curve), + stable_cross_view_cone_bias=bool( + hand_profile.stable_cross_view_cone_bias + ), + ), + zero=ZeroSolvePolicy( + active_joints=active, + passive_joints=passive, + direct_zero_joints=tuple(zero_profile.direct_zero_joints), + axis_joints=tuple(zero_profile.axis_joints), + mechanical_endpoint_joints=mechanical_endpoint_joints, + post_solve_endpoint_joints=post_solve_endpoint_joints, + mimic_source_by_joint={ + target: source + for target, source in MIMIC_DERIVED_FINGER_DIPS.items() + if target in passive and source in active + }, + cad_frozen_joints=frozenset( + passive - set(zero_profile.static_output_zero_offsets_rad) + ), + ), + quality=QualityPolicy( + training_cycles=(0, 1, 2), + holdout_cycle=3 if key.layout != "legacy_11" else None, + hard_threshold_keys=_HARD_THRESHOLD_KEYS, + isolated_holdout=bool(hand_profile.isolated_holdout), + ), + scope=ScopePolicy( + calibrate_joints={ + "full": active, + "thumb": thumb, + "fingers": fingers, + }, + frozen_joints={ + "full": frozenset(), + "thumb": fingers, + "fingers": thumb, + }, + ), + artifacts=build_artifact_policy( + frozenset(hand_profile.capabilities) + ), + ) + return RegisteredProfile( + profile=typed, + engine=EngineBindings( + hand_profile=hand_profile, + zero_profile=zero_profile, + motion_command=build_calibration_motion_command, + preparation_waypoints=build_calibration_preparation_waypoints, + return_waypoints=build_calibration_return_waypoints, + cli_main=_run_cli, + ), + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py new file mode 100644 index 0000000..932f36d --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/artifacts.py @@ -0,0 +1,27 @@ +"""Runtime JSON and corrected-URDF naming policy.""" + +from ...core import ArtifactPolicy + + +def build_artifact_policy( + compatibility_tokens: frozenset[str], +) -> ArtifactPolicy: + return ArtifactPolicy( + output_schema_version=4, + calibration_filename="g20_{side}_{serial_number}_calibration.json", + corrected_urdf_filename=( + "linkerhand_g20_{side}_{serial_number}_zero_calibrated.urdf" + ), + protected_input_fields=frozenset( + { + "source_urdf_sha256", + "camera_extrinsics_sha256", + "calibration_config_sha256", + "tag_config_sha256", + } + ), + session_compatibility_tokens=frozenset(compatibility_tokens), + ) + + +__all__ = ["build_artifact_policy"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/command_layout.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/command_layout.py new file mode 100644 index 0000000..1c47771 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/command_layout.py @@ -0,0 +1,26 @@ +"""Reviewed command-channel layouts for this hand family.""" + +G20_COMMAND_NAMES: tuple[str, ...] = ( + "thumb_cmc_pitch", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", + "thumb_cmc_roll", + "index_mcp_roll", + "middle_mcp_roll", + "ring_mcp_roll", + "pinky_mcp_roll", + "thumb_cmc_yaw", + "reserved_11", + "reserved_12", + "reserved_13", + "reserved_14", + "thumb_mcp", + "index_pip", + "middle_pip", + "ring_pip", + "pinky_pip", +) + +__all__ = ["G20_COMMAND_NAMES"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py new file mode 100644 index 0000000..004cf51 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/legacy_11.py @@ -0,0 +1,35 @@ +"""One-release typed wrappers for the legacy 11-Tag layouts.""" + +from ...core import ProfileKey +from ...full_hand import get_hand_calibration_profile +from ...urdf_zero import get_zero_calibration_profile +from ..registry import RegisteredProfile +from ._adapter import adapt_profile + + +LEFT_KEY = ProfileKey("G20", "left", "legacy_11", 1) +RIGHT_KEY = ProfileKey("G20", "right", "legacy_11", 1) + + +def build_left_profile() -> RegisteredProfile: + hand = get_hand_calibration_profile(LEFT_KEY.side, LEFT_KEY.layout) + return adapt_profile( + key=LEFT_KEY, + namespace="/g20_calibration", + hand_profile=hand, + zero_profile=get_zero_calibration_profile( + LEFT_KEY.side, LEFT_KEY.layout + ), + ) + + +def build_right_profile() -> RegisteredProfile: + hand = get_hand_calibration_profile(RIGHT_KEY.side, RIGHT_KEY.layout) + return adapt_profile( + key=RIGHT_KEY, + namespace="/g20_calibration", + hand_profile=hand, + zero_profile=get_zero_calibration_profile( + RIGHT_KEY.side, RIGHT_KEY.layout + ), + ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py new file mode 100644 index 0000000..0be31a0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/motion.py @@ -0,0 +1,15 @@ +"""Reviewed motion and safe-waypoint strategy exports.""" + +from ...full_hand import ( + build_calibration_motion_command, + build_calibration_preparation_waypoints, + build_calibration_return_waypoints, + build_calibration_speed_profile, +) + +__all__ = [ + "build_calibration_motion_command", + "build_calibration_preparation_waypoints", + "build_calibration_return_waypoints", + "build_calibration_speed_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/reporting_zh.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/reporting_zh.py new file mode 100644 index 0000000..aa91837 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/reporting_zh.py @@ -0,0 +1,893 @@ +"""Chinese, operator-facing diagnostics for three-camera calibration.""" + +from __future__ import annotations + +import re +from typing import Any, Mapping, Sequence + + +STATE_NAMES_ZH = { + "PREFLIGHT": "设备和标签预检", + "WAIT_START": "等待开始标定", + "IMPORTING_BASE": "正在读取基础标定会话", + "REVALIDATING_INHERITED": "正在复核继承的四指数据", + "RETURN_BASELINE": "正在恢复目标姿态", + "PREPARE_SWEEP": "正在到达扫描起点", + "SWEEP": "正在采集轨迹", + "FITTING": "正在拟合轨迹和零位", + "VALIDATION_MOVE": "正在移动到随机复测位置", + "VALIDATION_CAPTURE": "正在采集随机复测数据", + "PAUSED": "标定已暂停", + "ABORTED": "标定已终止", + "COMPLETE": "标定已完成", +} + +VIEW_NAMES_ZH = { + "front": "正面", + "side": "侧面", + "top": "上面", +} + +JOINT_NAMES_ZH = { + "thumb_cmc_pitch": "拇指CMC俯仰", + "thumb_cmc_roll": "拇指CMC滚转", + "thumb_mcp": "拇指MCP", + "thumb_ip": "拇指IP（被动）", + "index_mcp_roll": "食指MCP侧摆", + "index_mcp_pitch": "食指MCP屈伸", + "index_pip": "食指PIP", + "index_dip": "食指DIP（被动）", + "middle_mcp_roll": "中指MCP侧摆", + "middle_mcp_pitch": "中指MCP屈伸", + "middle_pip": "中指PIP", + "middle_dip": "中指DIP（被动）", + "ring_mcp_roll": "无名指MCP侧摆", + "ring_mcp_pitch": "无名指MCP屈伸", + "ring_pip": "无名指PIP", + "ring_dip": "无名指DIP（被动）", + "pinky_mcp_roll": "小指MCP侧摆", + "pinky_mcp_pitch": "小指MCP屈伸", + "pinky_pip": "小指PIP", + "pinky_dip": "小指DIP（被动）", + "thumb_cmc_yaw": "拇指CMC侧摆", + "index_mcp_roll_side": "食指MCP侧摆（侧面校验）", + "middle_mcp_roll_side": "中指MCP侧摆（侧面校验）", + "ring_mcp_roll_side": "无名指MCP侧摆（侧面校验）", + "pinky_mcp_roll_side": "小指MCP侧摆（侧面校验）", +} + + +def _format_u8(value: Any) -> str: + if value is None: + return "尚无反馈" + return f"{float(value):.1f}" + + +def _task_text(active: Mapping[str, Any]) -> str: + if not active: + return "尚无活动任务" + view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", ""))) + if active.get("kind") == "fit_failure": + joints = active.get("joints", []) + joint_text = "/".join( + JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints + ) + return ( + f"{view}机位，{joint_text}拟合检查失败，" + f"电机{active.get('motor_index')}，" + f"第{active.get('attempt', 1)}次尝试" + ) + if active.get("kind") == "zero_model_failure": + joints = active.get("joints", []) + joint_text = "/".join( + JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints + ) + return ( + f"{view}机位，{joint_text}零位/URDF验证失败，" + f"电机{active.get('motor_index')}，不会自动重扫" + ) + if active.get("kind") == "motion_stall": + return ( + f"电机{active.get('motor_index', '?')}运动停滞，目标" + f"{_format_u8(active.get('target_u8'))}、实际" + f"{_format_u8(active.get('actual_u8'))}" + ) + if active.get("kind") == "cross_view_roll_diagnostic": + return ( + f"{active.get('finger', '?')}侧摆跨机位诊断完成：" + f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°，" + f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°" + ) + if active.get("kind") == "validation": + return ( + f"{view}机位，随机复测，电机{active.get('motor_index')}，" + f"目标命令{active.get('command_u8')}" + ) + joints = active.get("joints", []) + joint_text = "/".join( + JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints + ) + start = active.get("start_u8") + target = active.get("target_u8") + cycle = active.get("cycle", "?") + repetitions = active.get("repetitions", "?") + task = ( + f"{view}机位，{joint_text}，电机{active.get('motor_index')}，" + f"第{cycle}/{repetitions}轮，{start}→{target}" + ) + fit_attempt = int(active.get("fit_attempt", 1)) + if fit_attempt > 1: + retry_cycles = active.get("fit_retry_cycles", []) + if retry_cycles: + task += "（补采异常轮" + "/".join( + str(cycle) for cycle in retry_cycles + ) + "）" + else: + task += f"（拟合补采第{fit_attempt}次）" + return task + + +def three_camera_reason_zh( + state: str, + reason: str, + active: Mapping[str, Any], +) -> tuple[str, str]: + """Translate a reason code and provide one concrete operator action.""" + reason = str(reason) + sample = active.get("sample", {}) if active else {} + missing = [int(value) for value in sample.get("missing_endpoint_u8", [])] + sample_range = ( + f"{_format_u8(sample.get('minimum_u8'))}～" + f"{_format_u8(sample.get('maximum_u8'))}" + ) + tolerance = sample.get("endpoint_tolerance_u8", "?") + + if reason.startswith("motor_state_stalled:"): + fields = reason.split(":") + context = fields[1] if len(fields) > 1 else "unknown" + error_match = re.search(r"error_u8=([0-9.]+)", reason) + error = error_match.group(1) if error_match else "未知" + timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason) + timeout_value = active.get("timeout_seconds") + if timeout_value is None and timeout_match is not None: + timeout_value = float(timeout_match.group(1)) + duration = ( + f"连续{float(timeout_value):g}秒" + if timeout_value is not None + else "在规定时间内" + ) + motor = active.get("motor_index") + if motor is not None: + return ( + f"电机{motor}反馈{duration}没有向目标推进；目标" + f"{_format_u8(active.get('target_u8'))}、实际" + f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，" + f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8" + f"（阶段={context}）。程序已保持当前位置。", + "若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后" + "重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。", + ) + return ( + f"电机反馈{duration}没有向目标推进；停止位置距目标{error}个u8" + f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。", + "检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的" + "正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。", + ) + + base_reason, separator, reason_detail = reason.partition(":") + if base_reason in { + "sweep_missing_endpoint_bin", + "sweep_bins_too_few", + "sweep_bin_gap_too_large", + "task_precheck_missing_command_127", + "task_precheck_detection_rate_too_low", + "synchronised_tag_state_timeout", + }: + reason = base_reason + detail_label = ( + JOINT_NAMES_ZH.get(reason_detail, reason_detail) + if separator and reason_detail + else "" + ) + detail_prefix = f"{detail_label}：" if detail_label else "" + + if "URDF zero offset reached the configured" in reason: + bound_match = re.search( + r"configured\s+([0-9.]+)\s+degree bound", reason + ) + bound = bound_match.group(1) if bound_match else "配置的" + hit_text = "" + if "bound:" in reason: + hit_text = reason.split("bound:", 1)[1].split( + "; all_offsets:", 1 + )[0] + for name, label in JOINT_NAMES_ZH.items(): + hit_text = hit_text.replace(name, label) + hit_suffix = f"；触边关节：{hit_text}" if hit_text else "" + return ( + f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的" + "零位结果，而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。", + "不要调用resume，也不要增大零位边界。先确认Tag有效黑框边长、三相机" + "内外参和原始CAD URDF；Tag尺寸修正后必须调用start重新采集，旧尺度" + "产生的轨迹不能直接生成修正URDF。", + ) + + if reason == "sweep_missing_endpoint_bin": + missing_text = "、".join(str(value) for value in missing) or "0或255" + return ( + f"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少" + f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为" + f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到" + "端点，但该时刻没有同时取得有效Tag图像和电机状态。", + "确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用" + "/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。", + ) + if reason == "sweep_bins_too_few": + return ( + f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少" + f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。", + "检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。", + ) + if reason == "sweep_bin_gap_too_large": + gap_start = sample.get("maximum_bin_gap_start_u8") + gap_end = sample.get("maximum_bin_gap_end_u8") + gap_range = ( + "" + if gap_start is None or gap_end is None + else f"（{gap_start}→{gap_end}）" + ) + return ( + f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，" + f"{gap_range}允许值不超过" + f"{sample.get('allowed_maximum_bin_gap', '?')}。", + "检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。", + ) + if reason == "synchronised_tag_state_timeout": + group_reasons = active.get("group_pnp_reasons", {}) + if isinstance(group_reasons, Mapping) and group_reasons: + tag_rejections = active.get("pnp_rejection_counts", {}) + group_rejections = active.get( + "group_pnp_rejection_counts", {} + ) + missing_roles = active.get( + "group_missing_candidate_roles", {} + ) + candidate_diagnostics = active.get( + "pnp_candidate_diagnostics", {} + ) + view_details: list[str] = [] + for view, value in group_reasons.items(): + view_name = str(view) + parts = [str(value)] + missing = ( + missing_roles.get(view_name, ()) + if isinstance(missing_roles, Mapping) + else () + ) + if isinstance(missing, Sequence) and not isinstance( + missing, (str, bytes) + ) and missing: + parts.append( + "缺候选=" + ",".join(str(role) for role in missing) + ) + counts: dict[str, int] = {} + for source in (tag_rejections, group_rejections): + values = ( + source.get(view_name) + if isinstance(source, Mapping) + else None + ) + if isinstance(values, Mapping): + for name, count in values.items(): + counts[str(name)] = counts.get(str(name), 0) + int( + count + ) + if counts: + common = sorted( + counts.items(), key=lambda pair: (-pair[1], pair[0]) + )[:3] + parts.append( + "累计拒绝=" + + ",".join( + f"{name}×{count}" for name, count in common + ) + ) + view_candidates = ( + candidate_diagnostics.get(view_name, {}) + if isinstance(candidate_diagnostics, Mapping) + else {} + ) + if isinstance(view_candidates, Mapping) and missing: + summaries: list[str] = [] + for role in missing: + diagnostic = view_candidates.get(str(role), {}) + if not isinstance(diagnostic, Mapping): + continue + summaries.append( + f"{role}(solve=" + f"{int(diagnostic.get('solved_candidate_count', 0))}," + "reproj=" + f"{int(diagnostic.get('reprojection_candidate_count', 0))}," + "tilt=" + f"{int(diagnostic.get('independent_tilt_candidate_count', 0))})" + ) + if summaries: + parts.append("候选统计=" + ",".join(summaries)) + view_details.append( + f"{VIEW_NAMES_ZH.get(view_name, view_name)}=" + + "；".join(parts) + ) + reason_text = "、".join(view_details) + return ( + f"{detail_prefix}已经取得部分有效轨迹，但Tag仍可见且反馈正常时，" + "后续连续图像帧" + "被整组PnP几何检查拒绝" + f"（{reason_text}），因此无法与电机状态形成有效轨迹帧。", + "不要调整或反复粘贴Tag；保留当前会话中的" + "group_pnp_candidate_event，" + "按缺失角色的候选统计检查PnP分支逻辑。", + ) + return ( + f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态" + "按时间戳配对”的图像帧。", + "查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，" + "程序会重扫当前方向。", + ) + if reason == "task_precheck_missing_command_127": + return ( + f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。", + "检查中位姿态的Tag遮挡和反光；程序只会重扫当前物理任务。", + ) + if reason == "task_precheck_detection_rate_too_low": + return ( + f"{detail_prefix}低速预检的有效Tag识别率低于门限。", + "检查该机位当前任务Tag的遮挡、反光和对焦；程序只会重扫当前物理任务。", + ) + if reason == "sweep_start_position_timeout": + return ( + f"电机{active.get('motor_index')}未在规定时间到达扫描起点" + f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。", + "检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。", + ) + if reason == "sweep_start_tag_timeout": + group_reasons = active.get("group_pnp_reasons", {}) + progress_by_view = active.get("pnp_initialization_progress", {}) + tag_rejections = active.get("pnp_rejection_counts", {}) + group_rejections = active.get("group_pnp_rejection_counts", {}) + if any( + isinstance(value, Mapping) and bool(value) + for value in ( + group_reasons, + progress_by_view, + tag_rejections, + group_rejections, + ) + ): + details: list[str] = [] + views = set() + for value in ( + group_reasons, + progress_by_view, + tag_rejections, + group_rejections, + ): + if isinstance(value, Mapping): + views.update(str(view) for view in value) + for view in sorted(views): + parts: list[str] = [] + progress = ( + progress_by_view.get(view) + if isinstance(progress_by_view, Mapping) + else None + ) + if isinstance(progress, Mapping): + parts.append( + "初始化" + f"{int(progress.get('accepted', 0))}/" + f"{int(progress.get('required', 0))}" + ) + counts: dict[str, int] = {} + for source in (tag_rejections, group_rejections): + values = ( + source.get(view) + if isinstance(source, Mapping) + else None + ) + if isinstance(values, Mapping): + for name, count in values.items(): + counts[str(name)] = ( + counts.get(str(name), 0) + int(count) + ) + if counts: + common = sorted( + counts.items(), key=lambda pair: (-pair[1], pair[0]) + )[:3] + parts.append( + "累计拒绝=" + + ",".join( + f"{name}×{count}" for name, count in common + ) + ) + latest = ( + group_reasons.get(view) + if isinstance(group_reasons, Mapping) + else None + ) + if latest and not str(latest).startswith( + "group_initializing:" + ): + parts.append(f"最后状态={latest}") + if parts: + details.append( + f"{VIEW_NAMES_ZH.get(view, view)}=" + "；".join(parts) + ) + reason_text = "、".join(details) or "未形成完整初始化窗口" + return ( + "被测电机已经到达扫描起点，所需Tag也可见，但三维PnP位姿初始化" + f"没有完成（{reason_text}），因此没有生成同步端点帧。", + "不要根据可见性重复粘贴Tag；保留累计拒绝原因并检查PnP候选选择。", + ) + return ( + "被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的" + "同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”，不会触发此错误。", + "只检查标记为✗的实时运动Tag、反光和外部遮挡；不要移动相机或手掌底座。", + ) + if reason == "sweep_timeout": + return ( + "当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。", + "检查电机实际值、Tag连续识别和标定速度，修正后调用resume。", + ) + if reason == "return_baseline_timeout": + return ( + "一个或多个标定电机未在规定时间返回基准命令。", + "检查机械手状态、CAN和机械卡阻，确认安全后调用resume。", + ) + if reason == "validation_move_timeout": + return ( + "随机复测时电机未在规定时间到达目标命令。", + "检查机械手状态和机械卡阻，确认安全后调用resume。", + ) + if reason == "validation_capture_timeout": + return ( + "随机复测位置没有采集到足够的同步有效Tag帧。", + "检查当前机位Tag可见性后调用resume。", + ) + if reason == "palm_orientation_quality_failed": + failures = active.get("failures", []) + detail = ( + str(failures[0].get("reason", "方向观测不足")) + if failures + else "方向观测不足" + ) + if "thumb_cmc_" in detail: + return ( + "拇指CMC yaw无法由顶部Tag 8/9的零位邻近短轨迹稳定确定：" + + detail, + "保持Tag安装不变；确保顶部Tag 8/9在拇指CMC pitch和roll" + "从零位开始的前1/4行程持续可见后重新标定。", + ) + return ( + "掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定：" + + detail, + "保持Tag安装不变；让正面Tag 10–13在对应MCP-pitch起始段" + "至少可见15°行程后重新标定。", + ) + if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}: + metric_names = { + "plane_rms_mm": "平面拟合RMS", + "radial_rms_mm": "圆半径拟合RMS", + "radius_mm": "拟合半径", + "image_radial_rms_px": "二维圆半径拟合RMS", + "image_radial_p95_px": "二维圆半径误差P95", + "image_radius_px": "二维拟合半径", + "arc_deg": "实测圆弧", + "monotonic_correction_deg": "最大单调修正", + "hysteresis_deg": "最大正反程差", + "baseline_hysteresis_deg": "baseline正反程关节角差", + "baseline_directional_gap_deg": "baseline方向分支间隙", + "baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差", + "cycle_travel_range_deg": "三轮行程差", + "rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS", + "axis_plane_rms_mm": "三维圆轴向RMS", + "axis_radial_rms_mm": "三维圆半径RMS", + "axis_pose_line_rms_mm": "姿态轨迹轴线RMS", + "axis_line_cycle_rms_mm": "四轮轴线位置RMS", + "rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角", + "axis_cycle_difference_deg": "各轮转轴方向极差", + "cross_view_roll_curve": "正面/侧面关节角曲线差异RMS", + "third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差", + "third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS", + "third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95", + "zero_cycle_offset_range_deg": "训练轮零位极差", + "zero_confidence_95_half_width_deg": "零位95%置信半宽", + "state_image_sync_p95_ms": "图像与电机状态同步误差P95", + "tag_valid_rate_percent": "所需Tag同时有效率", + } + metric_units = { + "plane_rms_mm": "mm", + "radial_rms_mm": "mm", + "radius_mm": "mm", + "image_radial_rms_px": "px", + "image_radial_p95_px": "px", + "image_radius_px": "px", + "arc_deg": "°", + "monotonic_correction_deg": "°", + "hysteresis_deg": "°", + "baseline_hysteresis_deg": "°", + "baseline_directional_gap_deg": "°", + "baseline_directional_gap_range_deg": "°", + "cycle_travel_range_deg": "°", + "rotation_orthogonal_rms_deg": "°", + "axis_plane_rms_mm": "mm", + "axis_radial_rms_mm": "mm", + "axis_pose_line_rms_mm": "mm", + "axis_line_cycle_rms_mm": "mm", + "rotation_circle_axis_difference_deg": "°", + "axis_cycle_difference_deg": "°", + "third_cycle_axis_holdout_deg": "°", + "third_cycle_axis_line_rms_mm": "mm", + "third_cycle_trajectory_p95_deg": "°", + "zero_cycle_offset_range_deg": "°", + "zero_confidence_95_half_width_deg": "°", + "state_image_sync_p95_ms": "ms", + "tag_valid_rate_percent": "%", + "cross_view_roll_curve": "°", + } + details: list[str] = [] + for failure in active.get("failures", []): + joint = JOINT_NAMES_ZH.get( + str(failure.get("joint")), str(failure.get("joint")) + ) + metric = str(failure.get("metric", "")) + if metric in metric_names: + comparison = str(failure.get("comparison", "")) + requirement = "不超过" if comparison == "maximum" else "至少" + unit = metric_units[metric] + detail = ( + f"{joint}的{metric_names[metric]}为" + f"{float(failure.get('actual', 0.0)):.2f}{unit}，" + f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}" + ) + cycle_travel = failure.get("cycle_travel_deg", []) + if cycle_travel: + detail += "（各轮=" + "/".join( + f"{float(value):.2f}°" for value in cycle_travel + ) + "）" + cycle_values = failure.get("cycle_values_deg", []) + if not cycle_values: + cycle_values = failure.get("cycle_offset_deg", []) + if cycle_values: + detail += "（各轮=" + "/".join( + f"{float(value):.2f}°" for value in cycle_values + ) + "）" + details.append(detail) + else: + cycle = failure.get("cycle") + cycle_text = "" if cycle is None else f"第{cycle}轮" + details.append( + f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败：" + f"{failure.get('reason', '未知原因')}" + ) + detail_text = "；".join(details) or "当前关节的轨迹拟合未通过" + directional_gap_failure = any( + str(failure.get("metric", "")).startswith( + "baseline_directional_gap" + ) + for failure in active.get("failures", []) + ) + if reason == "joint_fit_systematic_failure": + cross_view_systematic = any( + failure.get("classification") + in { + "stable_cross_view_installation_or_model_bias", + "stable_cross_view_direction_conflict", + } + for failure in active.get("failures", []) + ) + suggestion = ( + "四轮都出现稳定的正面/侧面差异，属于Tag安装外参或跨视角模型偏差，" + "继续重扫不会改善；检查Tag刚性安装与跨视角安装变换，不要放宽门限。" + if cross_view_systematic + else "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。" + "请直接复制诊断块给开发者，不要放宽门限。" + ) + else: + source_task_names = set(active.get("source_task_names", [])) + thumb_yaw_source_retry = source_task_names == { + "thumb_cmc_pitch_front", + "thumb_cmc_roll_front", + } + suggestion = ( + "方向分支已由软件保留，不要放宽门限；请检查传动回差或高支架刚度，" + "处理后重新执行一键标定命令，程序会从最近可靠断点继续。" + if directional_gap_failure + else ( + "保持顶部Tag 8/9无遮挡；程序只替换决定yaw零位的" + "CMC pitch/roll顶部轴观测并重扫" + f"{active.get('directions_to_rescan', 16)}个方向，" + "不会无效重扫yaw侧摆。" + if thumb_yaw_source_retry + else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令；" + "程序只清除当前失败关节的数据并重扫" + f"{active.get('directions_to_rescan', 6)}个方向，" + "不需要手工调用ROS服务。" + ) + ) + return ( + detail_text + "。程序已在当前关节结束后立即停止后续步骤。", + suggestion, + ) + if reason == "zero_model_validation_failed": + reason_names = { + "zero_offset_reached_configured_bound": "零位解触及安全边界", + "zero_offset_exceeds_configured_limit": "零位估计超过安全范围", + "zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界", + "zero_offset_cycle_difference_too_large": "三轮零位离散过大", + "zero_offset_not_statistically_significant": "零位偏移未达到统计显著性", + "zero_axis_cone_mismatch_too_large": ( + "父子轴夹角与原始URDF不一致，零位旋转无法解释" + ), + "zero_phase_axis_line_residual_too_large": ( + "整段SE(3)运动无法稳定确定平行轴线相位" + ), + "zero_offset_did_not_improve_with_95pct_confidence": ( + "第三轮留出验证未以95%置信度改善" + ), + } + details: list[str] = [] + for failure in active.get("failures", []): + joint = JOINT_NAMES_ZH.get( + str(failure.get("joint")), str(failure.get("joint")) + ) + if failure.get("metric") == "zero_guard": + reason_text = reason_names.get( + str(failure.get("reason")), str(failure.get("reason")) + ) + if "actual_deg" in failure and "limit_deg" in failure: + reason_text += ( + f"（估计{float(failure['actual_deg']):+.2f}°，" + f"允许±{float(failure['limit_deg']):.2f}°）" + ) + details.append(f"{joint}：{reason_text}") + return ( + "轨迹采集已完成，但零位/URDF几何验证失败" + + ("（" + "；".join(details) + "）" if details else "") + + "。程序没有生成正式JSON或修正URDF。", + "该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；" + "请检查Tag固定、相机外参和原始URDF后重新启动新标定。", + ) + if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}: + return ( + "正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪；此阶段不以Tag可见性阻止基准恢复。", + "保持机械手运动范围无障碍；设备就绪后系统会先安全恢复基准形态，再检查掌部Tag。", + ) + if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}: + return ( + "正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。", + "根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。", + ) + if reason == "waiting_for_baseline_tags_after_recovery": + return ( + "机械手已经稳定恢复到基准形态，正在用新采集的画面确认三台相机各自的固定掌部Tag。", + "若某个掌部Tag持续缺失，只调整遮挡手指或检查Tag固定情况，不要移动相机和手掌底座。", + ) + if reason == "locking_fixed_base_references": + return ( + "基准形态Tag预检已通过，正在把三台相机的固定掌部Tag稳健锁定为本会话参考。", + "无需操作；锁定完成后允许任务姿态遮挡固定掌部Tag。", + ) + if reason == "fixed_base_reference_moved": + return ( + "顶部Tag 8在本会话基准锁定后连续多帧发生角点位移；程序已立即保持机械手当前位置，" + "本会话中已采集数据不再用于发布。", + "Tag 8允许在下一次标定预检前重新摆放，但本次不能继续；固定Tag 8和顶部相机后" + "重新启动新会话。", + ) + if reason == "waiting_for_task_tags_at_sweep_start": + return ( + "电机已到扫描起点，正在等待当前任务的实时运动Tag；显示为“锁”的固定掌部Tag" + "允许被手指遮挡。", + "只检查标记为✗的实时运动Tag；若均为✓或锁，程序会自动开始运动。", + ) + if reason == "call_start_for_baseline_recovery": + return ( + "相机数据和机械手反馈已就绪，等待一键程序触发安全基准恢复。", + "保持机械手运动范围无障碍；程序会自动开始，无需手工调用ROS服务。", + ) + if reason == "call_start": + return ( + "三机位预检已经通过，等待操作员确认开始。", + "清空机械手运动范围后调用/g20_calibration/start。", + ) + if reason == "operator_pause": + return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。" + if reason == "operator_abort": + return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。" + if reason == "collecting_timestamp_synchronised_tag_centres": + return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。" + if reason == "collecting_dedicated_baseline_hold": + return ( + "正在从当前方向到达关节baseline并静止采集Tag与电机反馈；这批数据单独用于回差验收。", + "无需操作，保持相机、标签和底座不动。", + ) + if reason == "steady checkpoint target is missing": + return ( + "首轮稳态检查点已经到达最终端点，但采集状态没有及时切换到端点完成阶段。", + "程序已停止发布并保留已采样数据；这是软件状态切换问题，不需要调整相机、Tag或机械手。", + ) + if reason == "cross_view_roll_front_failure_deferred": + return ( + "正面侧摆回差不合格已保留，诊断模式将继续采集同一手指的侧面数据。", + "无需操作；该诊断会锁定URDF发布。", + ) + if reason == "cross_view_roll_diagnostic_complete": + interpretation = str(active.get("interpretation", "")) + explanations = { + "both_views_confirm_direction_dependent_pose": ( + "正面和侧面都确认了方向相关姿态，优先判断为roll输出机构或共同下游链的真实回差。" + ), + "front_only_difference_check_roll_tag_bracket_or_front_pnp": ( + "只有正面差异超限，优先检查roll Tag高支架刚度和正面PnP。" + ), + "side_only_difference_check_side_tag_chain_or_side_pnp": ( + "只有侧面差异超限，优先检查侧面Tag链和侧面PnP。" + ), + "both_views_within_formal_hysteresis_limit": ( + "两个机位的静止回差均满足正式门限。" + ), + } + return ( + explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。") + + " 本次为诊断会话，不会生成或发布URDF。", + "保存当前状态和raw_samples.jsonl；根据两机位结论处理后重新启动正式标定。", + ) + if reason == "capturing_random_validation_pose": + return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。" + if reason in {"calibration_passed", "calibration_complete"}: + return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。" + if reason == "quality_failed": + return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。" + if reason.startswith("validated_endpoint_zero_state"): + return ( + "轨迹和URDF零位验证已经通过，但发布前检测到端点零位状态缺失或与" + "已验证模型不一致；这是程序内部状态生命周期错误，结果未发布。", + "不要移动相机、Tag或机械手底座；保留当前会话并把原因码交给开发者。", + ) + if reason.startswith("PUB-ARTIFACT-601:"): + return ( + "标定节点已经生成通过质量门限的JSON和候选URDF，但一键程序在正式发布前" + "发现这对产物的坐标、限位、哈希或资源一致性检查失败；原始URDF未被覆盖。", + "不要重新标定相机或调整Tag；保留本会话产物和启动日志供开发者检查发布契约。", + ) + if reason == "combination_pose_prediction_failed": + return ( + "单关节、零位和URDF几何验证已通过，但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。", + "程序会在原姿态重新初始化PnP并自动复测；若最终仍失败，请把raw_samples.jsonl中的" + "combination_validation_failure记录交给开发者，不要重新采集16个单关节任务。", + ) + if reason.startswith("prepare_") or state == "PREPARE_SWEEP": + return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。" + if reason == "holding_same_finger_clearance_before_next_task": + return ( + "同一根手指的上一项已经完成；相邻手指继续保持当前避让姿态，只调整" + "被测关节以衔接下一项。", + "无需操作，不要手动展开正在避让的手指。", + ) + if state == "RETURN_BASELINE": + return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。" + if state == "FITTING": + return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。" + return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。" + + +def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str: + """Render the complete operator status; the JSON topic remains unchanged.""" + state = str(payload.get("state", "")) + active = payload.get("active", {}) + reason_zh, action_zh = three_camera_reason_zh( + state, str(payload.get("reason", "")), active + ) + progress = float(payload.get("progress", 0.0)) + completed = payload.get("completed_sweeps", 0) + total = payload.get("total_sweeps", 0) + scan_progress = float( + payload.get( + "scan_progress", + 0.0 if not total else float(completed) / float(total), + ) + ) + lines = [ + f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）", + f"原因：{reason_zh}", + f"建议：{action_zh}", + f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，" + f"扫描进度{scan_progress:.1%}）", + f"当前任务：{_task_text(active)}", + ] + if state == "RETURN_BASELINE": + baseline_command = payload.get("baseline_command_u8", []) + return_command = payload.get("return_command_u8", baseline_command) + label = "恢复姿态" if return_command != baseline_command else "基准姿态" + lines.append(f"正在确认{label}：{return_command}") + if active and active.get("kind") not in { + "fit_failure", + "zero_model_failure", + "motion_stall", + }: + retry_count = int(active.get("automatic_retry_count", 0)) + if retry_count: + lines.append( + "自动重试：当前方向已自动重扫" + f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，" + f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，" + f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s" + ) + sample = active.get("sample", {}) + motion_progress = active.get("motion_progress") + motion_text = ( + "未知" if motion_progress is None else f"{float(motion_progress):.1%}" + ) + lines.append( + "运动采样：" + f"目标{active.get('target_u8', active.get('command_u8', '?'))}，" + f"实际{_format_u8(active.get('actual_u8'))}，" + f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，" + f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～" + f"{_format_u8(sample.get('maximum_u8'))}" + ) + detection_frames = int(active.get("detection_frames", 0)) + if detection_frames: + lines.append( + "本方向Tag检出：" + f"{float(active.get('detection_rate', 0.0)):.1%}" + f"（{active.get('detection_valid_frames', 0)}/" + f"{detection_frames}帧）" + ) + auxiliary = active.get("auxiliary_motors", []) + if auxiliary: + lines.append( + "避挡姿态：" + + "，".join( + f"电机{item.get('motor_index')}目标" + f"{item.get('command_u8')}、实际" + f"{_format_u8(item.get('actual_u8'))}" + for item in auxiliary + ) + ) + speed = active.get("speed", {}) + if speed: + lines.append( + "阶段速度：五指目标" + f"{speed.get('commanded_finger_speed')}，SDK报告" + f"{speed.get('reported_finger_speed')}" + ) + if active.get("sweep_timeout_seconds") is not None: + lines.append( + "运动保护：扫描超时" + f"{float(active['sweep_timeout_seconds']):.1f}s，" + "连续" + f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s" + "进展不足" + f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}" + "则立即暂停" + ) + lines.append("机位：") + for name, view in payload.get("views", {}).items(): + missing = view.get("missing_tag_ids", []) + missing_text = "无" if not missing else ",".join(map(str, missing)) + lines.append( + f"- {VIEW_NAMES_ZH.get(str(name), str(name))}：" + f"{'就绪' if view.get('ready') else '等待'}，" + f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'}，" + f"{float(view.get('detection_hz', 0.0)):.1f}Hz，" + f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，" + f"当前缺失Tag={missing_text}" + ) + extrinsics_error = payload.get("camera_extrinsics_error") + if extrinsics_error: + lines.append(f"外参文件：{extrinsics_error}") + lines.append(f"JSON结果：{payload.get('result_path') or '尚未生成'}") + lines.append( + f"修正URDF：{payload.get('corrected_urdf_path') or '尚未生成'}" + ) + return "\n".join(lines) diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py new file mode 100644 index 0000000..074653f --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/right_19.py @@ -0,0 +1,27 @@ +"""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 .zero_policy import ( + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, + RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, + get_zero_calibration_profile, +) +from ..registry import RegisteredProfile +from ._adapter import adapt_profile + + +KEY = ProfileKey("G20", "right", G20_RIGHT_19_LAYOUT, 1) + + +def build_profile() -> RegisteredProfile: + hand = get_hand_calibration_profile(KEY.side, KEY.layout) + zero = get_zero_calibration_profile(KEY.side, KEY.layout) + return adapt_profile( + key=KEY, + namespace="/g20_calibration", + hand_profile=hand, + zero_profile=zero, + mechanical_endpoint_joints=RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, + post_solve_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 new file mode 100644 index 0000000..ebcf339 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/runner.py @@ -0,0 +1,731 @@ +"""One-command runner for a registered hand-calibration product.""" + +from __future__ import annotations + +import argparse +from datetime import datetime +import json +import os +from pathlib import Path +import signal +import subprocess +import sys +import time +import traceback +from typing import Any, Mapping + +from ament_index_python.packages import get_package_share_directory +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from ...hikrobot_camera import configure_fastdds_large_image_transport +from ...operator_report import ( + ProgressEstimator, + build_failure_report, + render_progress_zh, +) +from ...product import ProductConfig, load_product_config, sha256_file +from ...publication import atomic_session_pointer, finalize_session_artifacts +from ...storage import atomic_write_json + + +EXIT_PASS = 0 +EXIT_QUALITY = 2 +EXIT_SAFETY = 3 +STATUS_TIMEOUT_SECONDS = 90.0 +FITTING_STATUS_TIMEOUT_SECONDS = 600.0 + + +def _status_timeout_seconds(status: Mapping[str, Any]) -> float: + """Return the watchdog deadline for the node's current phase. + + Motion and acquisition are expected to publish twice a second and retain + the strict transport watchdog. The final 3-D fit is intentionally a + synchronous, CPU-bound operation, so its executor cannot service the + status timer until the fit returns. The node publishes an explicit + FITTING status immediately before entering that operation; allow that + known phase enough time without weakening motion safety checks. + """ + if str(status.get("state", "")) == "FITTING": + return FITTING_STATUS_TIMEOUT_SECONDS + return STATUS_TIMEOUT_SECONDS + + +def _calibration_node_exited_before_status(log_path: Path) -> bool: + """Detect a launch child crash while the parent launch is still alive.""" + try: + with log_path.open("rb") as stream: + stream.seek(0, os.SEEK_END) + size = stream.tell() + stream.seek(max(0, size - 65536), os.SEEK_SET) + tail = stream.read().decode("utf-8", errors="replace") + except OSError: + return False + return ( + "[three_camera_calibration_node-" in tail + and "]: process has died" in tail + ) + + +class CalibrationMonitor(Node): + def __init__(self) -> None: + super().__init__("g20_calibration_product_runner") + self.latest_status: dict[str, Any] = {} + self.last_status_at = time.monotonic() + self.start_requested = False + self.start_future: Any = None + self.abort_future: Any = None + self.create_subscription(String, "/g20_calibration/status", self._status, 10) + self.start_client = self.create_client(Trigger, "/g20_calibration/start") + self.abort_client = self.create_client(Trigger, "/g20_calibration/abort") + + def _status(self, message: String) -> None: + try: + payload = json.loads(message.data) + except (TypeError, json.JSONDecodeError): + return + if isinstance(payload, dict): + self.latest_status = payload + self.last_status_at = time.monotonic() + + def maybe_start(self) -> None: + if self.start_requested or self.latest_status.get("state") != "WAIT_START": + return + if not self.start_client.service_is_ready(): + self.start_client.wait_for_service(timeout_sec=0.05) + return + self.start_requested = True + self.start_future = self.start_client.call_async(Trigger.Request()) + + def abort(self) -> None: + if not self.abort_client.service_is_ready(): + self.abort_client.wait_for_service(timeout_sec=1.0) + if self.abort_client.service_is_ready(): + self.abort_future = self.abort_client.call_async(Trigger.Request()) + + +class ProgressConsole: + def __init__(self, serial_number: str) -> None: + self.serial_number = serial_number + self.estimator = ProgressEstimator.start() + self.last_text = "" + self.last_issue = "" + + def update(self, status: Mapping[str, Any]) -> None: + text = render_progress_zh(self.serial_number, status, self.estimator) + if text == self.last_text: + return + self.last_text = text + if sys.stdout.isatty(): + sys.stdout.write("\x1b[2J\x1b[H" + text + "\n") + sys.stdout.flush() + else: + print(text, flush=True) + reason = str(status.get("reason", "")) + if reason.startswith("automatic_retry_") and reason != self.last_issue: + self.last_issue = reason + active = status.get("active", {}) + print( + "\n".join( + [ + f"⚠ 当前任务出现问题：{reason.removeprefix('automatic_retry_')}", + f"系统处理：只重扫当前任务（第 {active.get('automatic_retry_count', 1)}/2 次）", + ] + ), + flush=True, + ) + + +def _default_product_config() -> Path: + try: + installed = Path( + get_package_share_directory("linkerhand_calibration") + ) / "config" / "g20_right_product.yaml" + if installed.is_file(): + return installed + except Exception: + pass + return ( + Path.cwd() + / "src/linkerhand_calibration/config/g20_right_product.yaml" + ).resolve() + + +def _launch_command( + config: ProductConfig, + session: Path, + *, + resume_from: Path | None = None, + recalibration_scope: str = "full", +) -> list[str]: + values = { + "model": config.model, + "hand_type": config.side, + "tag_layout": config.tag_layout, + "serial_number": config.serial_number, + "can_interface": config.can_interface, + "session_dir": str(session), + "output_root": str(config.output_root), + "camera_extrinsics_file": str(config.camera_extrinsics), + "source_urdf_path": str(config.source_urdf), + "source_urdf_expected_sha256": config.source_urdf_sha256, + "corrected_urdf_output_dir": str(session), + "calibration_config": str(config.calibration_config), + "tag_config": str(config.tag_config), + "commands_enabled": "true", + "start_cameras": "true", + "start_sdk": "true", + "record_bag": "false", + "validation_enabled": "false", + "recalibration_scope": recalibration_scope, + } + if resume_from is not None: + values["resume_raw_samples_path"] = str( + resume_from / "raw_samples.jsonl" + ) + for view, camera in config.cameras.items(): + values[f"{view}_camera_serial"] = camera["serial_number"] + values[f"{view}_camera_name"] = camera["camera_name"] + values[f"{view}_camera_info_url"] = camera["camera_info"] + return [ + "ros2", + "launch", + "linkerhand_calibration", + "three_camera_calibration.launch.py", + *(f"{name}:={value}" for name, value in values.items()), + ] + + +def _stop_stack(process: subprocess.Popen[Any]) -> None: + if process.poll() is not None: + return + try: + os.killpg(process.pid, signal.SIGINT) + except ProcessLookupError: + return + try: + process.wait(timeout=15.0) + except subprocess.TimeoutExpired: + try: + os.killpg(process.pid, signal.SIGTERM) + except ProcessLookupError: + return + try: + process.wait(timeout=5.0) + except subprocess.TimeoutExpired: + try: + os.killpg(process.pid, signal.SIGKILL) + except ProcessLookupError: + return + process.wait(timeout=5.0) + + +def _write_trace(log_path: Path, error: BaseException) -> None: + with log_path.open("a", encoding="utf-8") as stream: + stream.write("\n[one-command exception]\n") + traceback.print_exception(type(error), error, error.__traceback__, file=stream) + + +def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None: + monitor.abort() + deadline = time.monotonic() + float(timeout_seconds) + while time.monotonic() < deadline and rclpy.ok(): + rclpy.spin_once(monitor, timeout_sec=0.1) + if monitor.latest_status.get("state") == "ABORTED": + return + + +def _run_hardware_session( + config: ProductConfig, + session: Path, + *, + resume_from: Path | None = None, + recalibration_scope: str = "full", +) -> tuple[dict[str, Any], int]: + session.mkdir(parents=True, exist_ok=False) + (session / "raw_samples.jsonl").touch() + log_path = session / "calibration.log" + log_stream = log_path.open("a", encoding="utf-8", buffering=1) + atomic_session_pointer(config.session_root, "latest_attempt", session) + monitor = CalibrationMonitor() + console = ProgressConsole(config.serial_number) + process: subprocess.Popen[Any] | None = None + latest_status: dict[str, Any] = { + "state": "PREFLIGHT", + "reason": "starting_ros_stack", + "progress": 0.0, + "views": {}, + "feedback_hz": 0.0, + } + exit_code = EXIT_QUALITY + try: + process = subprocess.Popen( + _launch_command( + config, + session, + resume_from=resume_from, + recalibration_scope=recalibration_scope, + ), + cwd=config.workspace, + stdout=log_stream, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + launched_at = time.monotonic() + last_render = 0.0 + last_startup_log_check = 0.0 + while True: + rclpy.spin_once(monitor, timeout_sec=0.1) + if monitor.latest_status: + latest_status = monitor.latest_status + monitor.maybe_start() + now = time.monotonic() + if now - last_render >= 0.5: + console.update(latest_status) + last_render = now + if monitor.start_future is not None and monitor.start_future.done(): + response = monitor.start_future.result() + if response is None or not response.success: + message = "start service failed" if response is None else response.message + raise RuntimeError(f"CFG-START-008:{message}") + monitor.start_future = None + state = str(latest_status.get("state", "")) + if state == "COMPLETE": + exit_code = EXIT_PASS + break + if state in {"PAUSED", "ABORTED"}: + reason = str(latest_status.get("reason", "calibration_paused")) + exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY + if state == "PAUSED" and "stall" not in reason: + # Ordinary quality failures return to the reviewed baseline + # before the process tree is stopped. Mechanical stalls + # deliberately skip this path and keep the current pose. + failure_status = dict(latest_status) + _request_safe_abort(monitor) + latest_status = failure_status + break + if process.poll() is not None: + raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}") + if ( + not monitor.latest_status + and now - last_startup_log_check >= 0.5 + ): + last_startup_log_check = now + log_stream.flush() + if _calibration_node_exited_before_status(log_path): + raise RuntimeError( + "CAM-STATUS-202:calibration node exited before status" + ) + if ( + not monitor.latest_status + and now - launched_at > STATUS_TIMEOUT_SECONDS + ): + raise RuntimeError("CAM-STATUS-202:no calibration status received") + if ( + monitor.latest_status + and now - monitor.last_status_at + > _status_timeout_seconds(monitor.latest_status) + ): + raise RuntimeError("MOTION-COMM-303:calibration status stopped") + except KeyboardInterrupt as error: + latest_status["state"] = "ABORTED" + latest_status["reason"] = "operator_abort" + _request_safe_abort(monitor) + _write_trace(log_path, error) + exit_code = EXIT_SAFETY + except BaseException as error: + latest_status["state"] = "PAUSED" + latest_status["reason"] = str(error) + _write_trace(log_path, error) + exit_code = EXIT_QUALITY + finally: + if process is not None: + _stop_stack(process) + monitor.destroy_node() + log_stream.flush() + os.fsync(log_stream.fileno()) + log_stream.close() + + if exit_code != EXIT_PASS: + _, block = build_failure_report( + config, + session, + latest_status, + reason=str(latest_status.get("reason", "unknown_failure")), + ) + print(block, flush=True) + return latest_status, exit_code + + +def _startup_failure_block(path: Path, error: BaseException) -> str: + return "\n".join( + [ + "========== 请复制以下内容给开发者 ==========", + "结果：FAIL", + "错误代码：CFG-PRODUCT-001", + "失败阶段：启动静态预检", + f"问题：{error}", + f"产品配置：{path}", + "自动处理：未启动相机、SDK或机械手运动", + "建议：复制本诊断块给开发者，不要手工修改哈希绕过检查。", + "========== 复制结束 ==========", + ] + ) + + +def _automatic_resume_candidate(config: ProductConfig) -> Path | None: + """Return the newest compatible failed attempt, never a passed session. + + Do not trust only ``latest_attempt``. A process interrupted during the + device-only startup gate may have already moved that pointer while still + containing no ``session_start`` checkpoint. In that case walk backwards + to the preceding usable failed session instead of throwing away hours of + completed tasks. + """ + root = config.session_root + try: + resolved_root = root.resolve(strict=True) + except OSError: + return None + + candidates: list[Path] = [] + pointer = config.session_root / "latest_attempt" + if pointer.exists(): + try: + candidates.append(pointer.resolve(strict=True)) + except OSError: + pass + try: + candidates.extend( + sorted( + ( + path + for path in root.iterdir() + if path.is_dir() and not path.name.startswith("latest_") + ), + key=lambda path: path.name, + reverse=True, + ) + ) + except OSError: + return None + + passed_pointer = config.session_root / "latest_passed" + passed: Path | None = None + if passed_pointer.exists(): + try: + passed = passed_pointer.resolve(strict=True) + except OSError: + pass + + seen: set[Path] = set() + for unresolved in candidates: + try: + candidate = unresolved.resolve(strict=True) + except OSError: + continue + if candidate in seen: + continue + seen.add(candidate) + if candidate.parent != resolved_root or not candidate.is_dir(): + continue + # A failed attempt older than the current formal release is stale and + # must not seed a new independent calibration. + if passed is not None and candidate.name <= passed.name: + continue + raw_path = candidate / "raw_samples.jsonl" + if not raw_path.is_file(): + continue + summary_path = candidate / "calibration_summary_zh.json" + summary: dict[str, Any] | None = None + if summary_path.is_file(): + try: + loaded = json.loads(summary_path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError): + continue + if not isinstance(loaded, dict) or loaded.get("result") != "FAIL": + continue + summary = loaded + hashes = summary.get("hashes", {}) + if not isinstance(hashes, Mapping): + continue + if ( + str(hashes.get("source_urdf_sha256", "")) + != config.source_urdf_sha256 + or str(hashes.get("camera_extrinsics_sha256", "")) + != config.camera_extrinsics_sha256 + ): + continue + start: dict[str, Any] | None = None + try: + with raw_path.open("r", encoding="utf-8") as stream: + for line in stream: + if not line.strip(): + continue + value = json.loads(line) + if ( + isinstance(value, dict) + and value.get("kind") == "session_start" + ): + start = value + break + except (OSError, json.JSONDecodeError): + continue + if ( + start is None + or start.get("hand_type") != config.side + or start.get("tag_layout") != config.tag_layout + or start.get("source_urdf_sha256") + != config.source_urdf_sha256 + ): + continue + if summary is None: + # Ctrl+C can terminate the ROS launch tree before the wrapper gets + # a chance to create calibration_summary_zh.json. The immutable + # checkpoint itself is enough to resume only after independently + # proving that its external geometry still matches the product. + try: + checkpoint_extrinsics = Path( + str(start["camera_extrinsics_file"]) + ).expanduser().resolve(strict=True) + if sha256_file(checkpoint_extrinsics) != ( + config.camera_extrinsics_sha256 + ): + continue + except (KeyError, OSError, ValueError): + continue + return candidate + return None + + +def _resolve_partial_base_session( + config: ProductConfig, base_session: str | Path | None +) -> Path: + """Validate the complete passed session that donates non-target tasks.""" + if base_session is None or not str(base_session).strip(): + raise ValueError( + "partial scope requires --base-session pointing to a passed " + "complete G20 right session" + ) + candidate = Path(base_session).expanduser().resolve(strict=True) + root = config.session_root.resolve() + if candidate.parent != root or not candidate.is_dir(): + raise ValueError( + "base session must resolve to a direct session directory under " + f"{root}" + ) + raw_path = candidate / "raw_samples.jsonl" + summary_path = candidate / "calibration_summary_zh.json" + payload_path = ( + candidate + / f"g20_right_{config.serial_number}_calibration.json" + ) + for required in (raw_path, summary_path, payload_path): + if not required.is_file(): + raise ValueError(f"base session is missing required artifact: {required}") + try: + summary = json.loads(summary_path.read_text(encoding="utf-8")) + except json.JSONDecodeError as error: + raise ValueError("base session summary is invalid JSON") from error + if ( + not isinstance(summary, dict) + or summary.get("result") != "PASS" + or not bool(summary.get("quality", {}).get("passed")) + ): + raise ValueError("base session is not a formally passed session") + hashes = summary.get("hashes", {}) + expected_hashes = { + "source_urdf_sha256": config.source_urdf_sha256, + "camera_extrinsics_sha256": config.camera_extrinsics_sha256, + "calibration_config_sha256": config.calibration_config_sha256, + } + if not isinstance(hashes, Mapping) or any( + str(hashes.get(name, "")) != expected + for name, expected in expected_hashes.items() + ): + raise ValueError( + "base session source URDF, camera extrinsics or calibration " + "configuration differs from the current product" + ) + return candidate + + +def run( + config_path: str | Path, + *, + workspace: str | Path | None = None, + preflight_only: bool = False, + allow_resume: bool = True, + scope: str = "full", + base_session: str | Path | None = None, +) -> int: + path = Path(config_path).expanduser().resolve() + try: + # Resolve every file and camera identity before allowing a hardware + # process to start. A second load enables the real CAN existence gate. + config = load_product_config(path, workspace=workspace, check_can=False) + load_product_config(path, workspace=workspace, check_can=True) + selected_scope = str(scope).strip().lower() + if selected_scope not in {"full", "thumb", "fingers"}: + raise ValueError("scope must be one of: full, thumb, fingers") + if selected_scope == "full" and base_session is not None: + raise ValueError( + "--base-session is valid only with --scope thumb/fingers" + ) + partial_base = None + if selected_scope == "fingers" or base_session is not None: + partial_base = _resolve_partial_base_session(config, base_session) + except BaseException as error: + print(_startup_failure_block(path, error), flush=True) + return EXIT_QUALITY + if preflight_only: + print("PASS：产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。") + return EXIT_PASS + + config.session_root.mkdir(parents=True, exist_ok=True) + resume_candidate = ( + partial_base + if selected_scope != "full" + else (_automatic_resume_candidate(config) if allow_resume else None) + ) + if resume_candidate is not None: + if selected_scope == "thumb": + print( + "拇指专项标定：四指任务继承自已通过会话 " + f"{resume_candidate.name}；4项拇指任务将全部重新采集，" + "四指零位保持不变。", + flush=True, + ) + elif selected_scope == "fingers": + print( + "四指专项标定：拇指任务和4个拇指零位继承自已通过会话 " + f"{resume_candidate.name}；12项四指任务将全部重新采集。", + flush=True, + ) + else: + print( + "检测到兼容的失败会话，将恢复已完整通过的关节任务：" + f"{resume_candidate.name}。失败中的当前任务会从头重做。", + flush=True, + ) + elif selected_scope == "thumb": + print( + "独立拇指标定：不导入四指会话；仅采集4项拇指任务，" + "四指URDF零位保持原始CAD值。", + flush=True, + ) + maximum_sessions = config.required_independent_passes + for pass_index in range(maximum_sessions): + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + session = config.session_root / stamp + while session.exists(): + time.sleep(1.0) + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + session = config.session_root / stamp + if maximum_sessions > 1: + print(f"正式标定复验：第 {pass_index + 1}/{maximum_sessions} 次", flush=True) + status, code = _run_hardware_session( + config, + session, + resume_from=( + resume_candidate + if selected_scope != "full" or pass_index == 0 + else None + ), + recalibration_scope=selected_scope, + ) + if code != EXIT_PASS: + return code + # Keep the exact node-side completion contract durable before the + # independent publication layer starts. If publication itself fails, + # developers can re-run artifact checks without repeating motion or + # inventing lost combination-validation metrics. + atomic_write_json(session / "node_status.json", status) + try: + summary, release_ready = finalize_session_artifacts( + config, session, node_status=status + ) + except BaseException as error: + _write_trace(session / "calibration.log", error) + status = dict(status) + status["state"] = "PAUSED" + status["reason"] = f"PUB-ARTIFACT-601:{error}" + _, block = build_failure_report(config, session, status, reason=status["reason"]) + print(block, flush=True) + return EXIT_QUALITY + if release_ready: + result_pointer = ( + config.session_root / "latest_thumb_passed" + if selected_scope == "thumb" and partial_base is None + else config.session_root / "latest_passed" + ) + print( + "\n".join( + [ + f"PASS：{config.model} {config.side} 标定、URDF修正和复验全部通过。", + f"正式结果：{result_pointer}", + f"JSON：{session / summary['artifacts']['json']}", + f"URDF：{summary['artifacts']['urdf']}", + ] + ), + flush=True, + ) + return EXIT_PASS + print("本次会话质量PASS；正在自动执行第二次独立完整复验。", flush=True) + return EXIT_QUALITY + + +def main(args: list[str] | None = None) -> None: + parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定") + parser.add_argument("--config", default=str(_default_product_config())) + parser.add_argument("--workspace", default=None) + parser.add_argument("--preflight-only", action="store_true") + parser.add_argument( + "--scope", + choices=("full", "thumb", "fingers"), + default="full", + help=( + "full重新标定全手；thumb仅重采4项拇指任务；" + "fingers复用已认证拇指并仅重采12项四指任务" + ), + ) + parser.add_argument( + "--base-session", + default=None, + help=( + "可选：thumb模式将结果合并到该完整会话；" + "fingers模式必须提供该基础会话" + ), + ) + parser.add_argument( + "--no-resume", + action="store_true", + help="忽略失败会话，从第一个关节开始全新采集", + ) + arguments = parser.parse_args(args) + configure_fastdds_large_image_transport() + ros_log_dir = Path( + os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs") + ) + ros_log_dir.mkdir(parents=True, exist_ok=True) + rclpy.init() + try: + code = run( + arguments.config, + workspace=arguments.workspace, + preflight_only=arguments.preflight_only, + allow_resume=not arguments.no_resume, + scope=arguments.scope, + base_session=arguments.base_session, + ) + finally: + if rclpy.ok(): + rclpy.shutdown() + raise SystemExit(code) + + +if __name__ == "__main__": + main() diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py new file mode 100644 index 0000000..f2e6650 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_policy.py @@ -0,0 +1,17 @@ +"""Reviewed static-zero, endpoint, and mimic topology exports.""" + +from ...full_hand import MIMIC_DERIVED_FINGER_DIPS +from ...urdf_zero import ( + RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS, + RIGHT_19_MECHANICAL_ENDPOINT_JOINTS, + RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS, + get_zero_calibration_profile, +) + +__all__ = [ + "MIMIC_DERIVED_FINGER_DIPS", + "RIGHT_19_ENDPOINT_MEASUREMENT_JOINTS", + "RIGHT_19_MECHANICAL_ENDPOINT_JOINTS", + "RIGHT_19_POST_SOLVE_ENDPOINT_JOINTS", + "get_zero_calibration_profile", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py new file mode 100644 index 0000000..680495c --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/__init__.py @@ -0,0 +1 @@ +"""Future profiles live here; no publishable profile is registered yet.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/registry.py b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py new file mode 100644 index 0000000..2ec12a6 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/models/registry.py @@ -0,0 +1,83 @@ +"""Local, deterministic profile registry for this ROS package.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Callable, Iterator + +from ..core import CalibrationProfile, ProfileKey, validate_profile + + +@dataclass(frozen=True) +class EngineBindings: + """Temporary bridge from typed policies to the proven engine objects.""" + + hand_profile: Any + zero_profile: Any + motion_command: Callable[..., list[int]] + preparation_waypoints: Callable[..., tuple[tuple[int, ...], ...]] + return_waypoints: Callable[..., tuple[tuple[int, ...], ...]] + cli_main: Callable[[list[str] | None], None] + + +@dataclass(frozen=True) +class RegisteredProfile: + profile: CalibrationProfile + engine: EngineBindings + + +class ProfileRegistry: + """An in-package registry; no discovery plugins or string evaluation.""" + + def __init__(self) -> None: + self._profiles: dict[ProfileKey, RegisteredProfile] = {} + + def register(self, registered: RegisteredProfile) -> None: + validate_profile(registered.profile) + key = registered.profile.key + existing = self._profiles.get(key) + if existing is not None and existing != registered: + raise ValueError(f"profile is already registered: {key.profile_id}") + self._profiles[key] = registered + + def get(self, key: ProfileKey) -> RegisteredProfile: + try: + return self._profiles[key] + except KeyError as error: + supported = ", ".join( + item.profile_id for item in sorted(self._profiles) + ) + raise ValueError( + f"unsupported calibration profile {key.profile_id}; " + f"registered={supported}" + ) from error + + def resolve( + self, + model: str, + side: str, + layout: str, + revision: int = 1, + ) -> RegisteredProfile: + return self.get(ProfileKey(model, side, layout, revision)) + + def __iter__(self) -> Iterator[RegisteredProfile]: + for key in sorted(self._profiles): + yield self._profiles[key] + + def __len__(self) -> int: + return len(self._profiles) + + +_DEFAULT_REGISTRY: ProfileRegistry | None = None + + +def get_default_registry() -> ProfileRegistry: + global _DEFAULT_REGISTRY + if _DEFAULT_REGISTRY is None: + from .g20 import register_profiles + + registry = ProfileRegistry() + register_profiles(registry) + _DEFAULT_REGISTRY = registry + return _DEFAULT_REGISTRY diff --git a/src/linkerhand_calibration/linkerhand_calibration/node.py b/src/linkerhand_calibration/linkerhand_calibration/node.py index 5ecc605..07570a3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/node.py @@ -36,7 +36,7 @@ from .acquisition import ( interpolate_state_u8, tag_quality_is_valid, ) -from .core import ( +from .compat.legacy.thumb_core import ( BASELINE_COMMAND, COMMAND_NAMES, DIRECTION_DECREASING, diff --git a/src/linkerhand_calibration/linkerhand_calibration/one_command.py b/src/linkerhand_calibration/linkerhand_calibration/one_command.py index edd2bc4..215f599 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/one_command.py +++ b/src/linkerhand_calibration/linkerhand_calibration/one_command.py @@ -1,726 +1,8 @@ -"""One-command runner for a registered hand-calibration product.""" +"""Compatibility import for the generic registered-profile runner.""" -from __future__ import annotations +from .runtime.runner import main -import argparse -from datetime import datetime -import json -import os -from pathlib import Path -import signal -import subprocess -import sys -import time -import traceback -from typing import Any, Mapping - -from ament_index_python.packages import get_package_share_directory -import rclpy -from rclpy.node import Node -from std_msgs.msg import String -from std_srvs.srv import Trigger - -from .hikrobot_camera import configure_fastdds_large_image_transport -from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh -from .product import ProductConfig, load_product_config, sha256_file -from .publication import atomic_session_pointer, finalize_session_artifacts -from .storage import atomic_write_json - - -EXIT_PASS = 0 -EXIT_QUALITY = 2 -EXIT_SAFETY = 3 -STATUS_TIMEOUT_SECONDS = 90.0 -FITTING_STATUS_TIMEOUT_SECONDS = 600.0 - - -def _status_timeout_seconds(status: Mapping[str, Any]) -> float: - """Return the watchdog deadline for the node's current phase. - - Motion and acquisition are expected to publish twice a second and retain - the strict transport watchdog. The final 3-D fit is intentionally a - synchronous, CPU-bound operation, so its executor cannot service the - status timer until the fit returns. The node publishes an explicit - FITTING status immediately before entering that operation; allow that - known phase enough time without weakening motion safety checks. - """ - if str(status.get("state", "")) == "FITTING": - return FITTING_STATUS_TIMEOUT_SECONDS - return STATUS_TIMEOUT_SECONDS - - -def _calibration_node_exited_before_status(log_path: Path) -> bool: - """Detect a launch child crash while the parent launch is still alive.""" - try: - with log_path.open("rb") as stream: - stream.seek(0, os.SEEK_END) - size = stream.tell() - stream.seek(max(0, size - 65536), os.SEEK_SET) - tail = stream.read().decode("utf-8", errors="replace") - except OSError: - return False - return ( - "[three_camera_calibration_node-" in tail - and "]: process has died" in tail - ) - - -class CalibrationMonitor(Node): - def __init__(self) -> None: - super().__init__("g20_calibration_product_runner") - self.latest_status: dict[str, Any] = {} - self.last_status_at = time.monotonic() - self.start_requested = False - self.start_future: Any = None - self.abort_future: Any = None - self.create_subscription(String, "/g20_calibration/status", self._status, 10) - self.start_client = self.create_client(Trigger, "/g20_calibration/start") - self.abort_client = self.create_client(Trigger, "/g20_calibration/abort") - - def _status(self, message: String) -> None: - try: - payload = json.loads(message.data) - except (TypeError, json.JSONDecodeError): - return - if isinstance(payload, dict): - self.latest_status = payload - self.last_status_at = time.monotonic() - - def maybe_start(self) -> None: - if self.start_requested or self.latest_status.get("state") != "WAIT_START": - return - if not self.start_client.service_is_ready(): - self.start_client.wait_for_service(timeout_sec=0.05) - return - self.start_requested = True - self.start_future = self.start_client.call_async(Trigger.Request()) - - def abort(self) -> None: - if not self.abort_client.service_is_ready(): - self.abort_client.wait_for_service(timeout_sec=1.0) - if self.abort_client.service_is_ready(): - self.abort_future = self.abort_client.call_async(Trigger.Request()) - - -class ProgressConsole: - def __init__(self, serial_number: str) -> None: - self.serial_number = serial_number - self.estimator = ProgressEstimator.start() - self.last_text = "" - self.last_issue = "" - - def update(self, status: Mapping[str, Any]) -> None: - text = render_progress_zh(self.serial_number, status, self.estimator) - if text == self.last_text: - return - self.last_text = text - if sys.stdout.isatty(): - sys.stdout.write("\x1b[2J\x1b[H" + text + "\n") - sys.stdout.flush() - else: - print(text, flush=True) - reason = str(status.get("reason", "")) - if reason.startswith("automatic_retry_") and reason != self.last_issue: - self.last_issue = reason - active = status.get("active", {}) - print( - "\n".join( - [ - f"⚠ 当前任务出现问题：{reason.removeprefix('automatic_retry_')}", - f"系统处理：只重扫当前任务（第 {active.get('automatic_retry_count', 1)}/2 次）", - ] - ), - flush=True, - ) - - -def _default_product_config() -> Path: - try: - installed = Path( - get_package_share_directory("linkerhand_calibration") - ) / "config" / "g20_right_product.yaml" - if installed.is_file(): - return installed - except Exception: - pass - return ( - Path.cwd() - / "src/linkerhand_calibration/config/g20_right_product.yaml" - ).resolve() - - -def _launch_command( - config: ProductConfig, - session: Path, - *, - resume_from: Path | None = None, - recalibration_scope: str = "full", -) -> list[str]: - values = { - "model": config.model, - "hand_type": config.side, - "tag_layout": config.tag_layout, - "serial_number": config.serial_number, - "can_interface": config.can_interface, - "session_dir": str(session), - "output_root": str(config.output_root), - "camera_extrinsics_file": str(config.camera_extrinsics), - "source_urdf_path": str(config.source_urdf), - "source_urdf_expected_sha256": config.source_urdf_sha256, - "corrected_urdf_output_dir": str(session), - "calibration_config": str(config.calibration_config), - "tag_config": str(config.tag_config), - "commands_enabled": "true", - "start_cameras": "true", - "start_sdk": "true", - "record_bag": "false", - "validation_enabled": "false", - "recalibration_scope": recalibration_scope, - } - if resume_from is not None: - values["resume_raw_samples_path"] = str( - resume_from / "raw_samples.jsonl" - ) - for view, camera in config.cameras.items(): - values[f"{view}_camera_serial"] = camera["serial_number"] - values[f"{view}_camera_name"] = camera["camera_name"] - values[f"{view}_camera_info_url"] = camera["camera_info"] - return [ - "ros2", - "launch", - "linkerhand_calibration", - "three_camera_calibration.launch.py", - *(f"{name}:={value}" for name, value in values.items()), - ] - - -def _stop_stack(process: subprocess.Popen[Any]) -> None: - if process.poll() is not None: - return - try: - os.killpg(process.pid, signal.SIGINT) - except ProcessLookupError: - return - try: - process.wait(timeout=15.0) - except subprocess.TimeoutExpired: - try: - os.killpg(process.pid, signal.SIGTERM) - except ProcessLookupError: - return - try: - process.wait(timeout=5.0) - except subprocess.TimeoutExpired: - try: - os.killpg(process.pid, signal.SIGKILL) - except ProcessLookupError: - return - process.wait(timeout=5.0) - - -def _write_trace(log_path: Path, error: BaseException) -> None: - with log_path.open("a", encoding="utf-8") as stream: - stream.write("\n[one-command exception]\n") - traceback.print_exception(type(error), error, error.__traceback__, file=stream) - - -def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None: - monitor.abort() - deadline = time.monotonic() + float(timeout_seconds) - while time.monotonic() < deadline and rclpy.ok(): - rclpy.spin_once(monitor, timeout_sec=0.1) - if monitor.latest_status.get("state") == "ABORTED": - return - - -def _run_hardware_session( - config: ProductConfig, - session: Path, - *, - resume_from: Path | None = None, - recalibration_scope: str = "full", -) -> tuple[dict[str, Any], int]: - session.mkdir(parents=True, exist_ok=False) - (session / "raw_samples.jsonl").touch() - log_path = session / "calibration.log" - log_stream = log_path.open("a", encoding="utf-8", buffering=1) - atomic_session_pointer(config.session_root, "latest_attempt", session) - monitor = CalibrationMonitor() - console = ProgressConsole(config.serial_number) - process: subprocess.Popen[Any] | None = None - latest_status: dict[str, Any] = { - "state": "PREFLIGHT", - "reason": "starting_ros_stack", - "progress": 0.0, - "views": {}, - "feedback_hz": 0.0, - } - exit_code = EXIT_QUALITY - try: - process = subprocess.Popen( - _launch_command( - config, - session, - resume_from=resume_from, - recalibration_scope=recalibration_scope, - ), - cwd=config.workspace, - stdout=log_stream, - stderr=subprocess.STDOUT, - text=True, - start_new_session=True, - ) - launched_at = time.monotonic() - last_render = 0.0 - last_startup_log_check = 0.0 - while True: - rclpy.spin_once(monitor, timeout_sec=0.1) - if monitor.latest_status: - latest_status = monitor.latest_status - monitor.maybe_start() - now = time.monotonic() - if now - last_render >= 0.5: - console.update(latest_status) - last_render = now - if monitor.start_future is not None and monitor.start_future.done(): - response = monitor.start_future.result() - if response is None or not response.success: - message = "start service failed" if response is None else response.message - raise RuntimeError(f"CFG-START-008:{message}") - monitor.start_future = None - state = str(latest_status.get("state", "")) - if state == "COMPLETE": - exit_code = EXIT_PASS - break - if state in {"PAUSED", "ABORTED"}: - reason = str(latest_status.get("reason", "calibration_paused")) - exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY - if state == "PAUSED" and "stall" not in reason: - # Ordinary quality failures return to the reviewed baseline - # before the process tree is stopped. Mechanical stalls - # deliberately skip this path and keep the current pose. - failure_status = dict(latest_status) - _request_safe_abort(monitor) - latest_status = failure_status - break - if process.poll() is not None: - raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}") - if ( - not monitor.latest_status - and now - last_startup_log_check >= 0.5 - ): - last_startup_log_check = now - log_stream.flush() - if _calibration_node_exited_before_status(log_path): - raise RuntimeError( - "CAM-STATUS-202:calibration node exited before status" - ) - if ( - not monitor.latest_status - and now - launched_at > STATUS_TIMEOUT_SECONDS - ): - raise RuntimeError("CAM-STATUS-202:no calibration status received") - if ( - monitor.latest_status - and now - monitor.last_status_at - > _status_timeout_seconds(monitor.latest_status) - ): - raise RuntimeError("MOTION-COMM-303:calibration status stopped") - except KeyboardInterrupt as error: - latest_status["state"] = "ABORTED" - latest_status["reason"] = "operator_abort" - _request_safe_abort(monitor) - _write_trace(log_path, error) - exit_code = EXIT_SAFETY - except BaseException as error: - latest_status["state"] = "PAUSED" - latest_status["reason"] = str(error) - _write_trace(log_path, error) - exit_code = EXIT_QUALITY - finally: - if process is not None: - _stop_stack(process) - monitor.destroy_node() - log_stream.flush() - os.fsync(log_stream.fileno()) - log_stream.close() - - if exit_code != EXIT_PASS: - _, block = build_failure_report( - config, - session, - latest_status, - reason=str(latest_status.get("reason", "unknown_failure")), - ) - print(block, flush=True) - return latest_status, exit_code - - -def _startup_failure_block(path: Path, error: BaseException) -> str: - return "\n".join( - [ - "========== 请复制以下内容给开发者 ==========", - "结果：FAIL", - "错误代码：CFG-PRODUCT-001", - "失败阶段：启动静态预检", - f"问题：{error}", - f"产品配置：{path}", - "自动处理：未启动相机、SDK或机械手运动", - "建议：复制本诊断块给开发者，不要手工修改哈希绕过检查。", - "========== 复制结束 ==========", - ] - ) - - -def _automatic_resume_candidate(config: ProductConfig) -> Path | None: - """Return the newest compatible failed attempt, never a passed session. - - Do not trust only ``latest_attempt``. A process interrupted during the - device-only startup gate may have already moved that pointer while still - containing no ``session_start`` checkpoint. In that case walk backwards - to the preceding usable failed session instead of throwing away hours of - completed tasks. - """ - root = config.session_root - try: - resolved_root = root.resolve(strict=True) - except OSError: - return None - - candidates: list[Path] = [] - pointer = config.session_root / "latest_attempt" - if pointer.exists(): - try: - candidates.append(pointer.resolve(strict=True)) - except OSError: - pass - try: - candidates.extend( - sorted( - ( - path - for path in root.iterdir() - if path.is_dir() and not path.name.startswith("latest_") - ), - key=lambda path: path.name, - reverse=True, - ) - ) - except OSError: - return None - - passed_pointer = config.session_root / "latest_passed" - passed: Path | None = None - if passed_pointer.exists(): - try: - passed = passed_pointer.resolve(strict=True) - except OSError: - pass - - seen: set[Path] = set() - for unresolved in candidates: - try: - candidate = unresolved.resolve(strict=True) - except OSError: - continue - if candidate in seen: - continue - seen.add(candidate) - if candidate.parent != resolved_root or not candidate.is_dir(): - continue - # A failed attempt older than the current formal release is stale and - # must not seed a new independent calibration. - if passed is not None and candidate.name <= passed.name: - continue - raw_path = candidate / "raw_samples.jsonl" - if not raw_path.is_file(): - continue - summary_path = candidate / "calibration_summary_zh.json" - summary: dict[str, Any] | None = None - if summary_path.is_file(): - try: - loaded = json.loads(summary_path.read_text(encoding="utf-8")) - except (OSError, json.JSONDecodeError): - continue - if not isinstance(loaded, dict) or loaded.get("result") != "FAIL": - continue - summary = loaded - hashes = summary.get("hashes", {}) - if not isinstance(hashes, Mapping): - continue - if ( - str(hashes.get("source_urdf_sha256", "")) - != config.source_urdf_sha256 - or str(hashes.get("camera_extrinsics_sha256", "")) - != config.camera_extrinsics_sha256 - ): - continue - start: dict[str, Any] | None = None - try: - with raw_path.open("r", encoding="utf-8") as stream: - for line in stream: - if not line.strip(): - continue - value = json.loads(line) - if ( - isinstance(value, dict) - and value.get("kind") == "session_start" - ): - start = value - break - except (OSError, json.JSONDecodeError): - continue - if ( - start is None - or start.get("hand_type") != config.side - or start.get("tag_layout") != config.tag_layout - or start.get("source_urdf_sha256") - != config.source_urdf_sha256 - ): - continue - if summary is None: - # Ctrl+C can terminate the ROS launch tree before the wrapper gets - # a chance to create calibration_summary_zh.json. The immutable - # checkpoint itself is enough to resume only after independently - # proving that its external geometry still matches the product. - try: - checkpoint_extrinsics = Path( - str(start["camera_extrinsics_file"]) - ).expanduser().resolve(strict=True) - if sha256_file(checkpoint_extrinsics) != ( - config.camera_extrinsics_sha256 - ): - continue - except (KeyError, OSError, ValueError): - continue - return candidate - return None - - -def _resolve_partial_base_session( - config: ProductConfig, base_session: str | Path | None -) -> Path: - """Validate the complete passed session that donates non-target tasks.""" - if base_session is None or not str(base_session).strip(): - raise ValueError( - "partial scope requires --base-session pointing to a passed " - "complete G20 right session" - ) - candidate = Path(base_session).expanduser().resolve(strict=True) - root = config.session_root.resolve() - if candidate.parent != root or not candidate.is_dir(): - raise ValueError( - "base session must resolve to a direct session directory under " - f"{root}" - ) - raw_path = candidate / "raw_samples.jsonl" - summary_path = candidate / "calibration_summary_zh.json" - payload_path = ( - candidate - / f"g20_right_{config.serial_number}_calibration.json" - ) - for required in (raw_path, summary_path, payload_path): - if not required.is_file(): - raise ValueError(f"base session is missing required artifact: {required}") - try: - summary = json.loads(summary_path.read_text(encoding="utf-8")) - except json.JSONDecodeError as error: - raise ValueError("base session summary is invalid JSON") from error - if ( - not isinstance(summary, dict) - or summary.get("result") != "PASS" - or not bool(summary.get("quality", {}).get("passed")) - ): - raise ValueError("base session is not a formally passed session") - hashes = summary.get("hashes", {}) - expected_hashes = { - "source_urdf_sha256": config.source_urdf_sha256, - "camera_extrinsics_sha256": config.camera_extrinsics_sha256, - "calibration_config_sha256": config.calibration_config_sha256, - } - if not isinstance(hashes, Mapping) or any( - str(hashes.get(name, "")) != expected - for name, expected in expected_hashes.items() - ): - raise ValueError( - "base session source URDF, camera extrinsics or calibration " - "configuration differs from the current product" - ) - return candidate - - -def run( - config_path: str | Path, - *, - workspace: str | Path | None = None, - preflight_only: bool = False, - allow_resume: bool = True, - scope: str = "full", - base_session: str | Path | None = None, -) -> int: - path = Path(config_path).expanduser().resolve() - try: - # Resolve every file and camera identity before allowing a hardware - # process to start. A second load enables the real CAN existence gate. - config = load_product_config(path, workspace=workspace, check_can=False) - load_product_config(path, workspace=workspace, check_can=True) - selected_scope = str(scope).strip().lower() - if selected_scope not in {"full", "thumb", "fingers"}: - raise ValueError("scope must be one of: full, thumb, fingers") - if selected_scope == "full" and base_session is not None: - raise ValueError( - "--base-session is valid only with --scope thumb/fingers" - ) - partial_base = None - if selected_scope == "fingers" or base_session is not None: - partial_base = _resolve_partial_base_session(config, base_session) - except BaseException as error: - print(_startup_failure_block(path, error), flush=True) - return EXIT_QUALITY - if preflight_only: - print("PASS：产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。") - return EXIT_PASS - - config.session_root.mkdir(parents=True, exist_ok=True) - resume_candidate = ( - partial_base - if selected_scope != "full" - else (_automatic_resume_candidate(config) if allow_resume else None) - ) - if resume_candidate is not None: - if selected_scope == "thumb": - print( - "拇指专项标定：四指任务继承自已通过会话 " - f"{resume_candidate.name}；4项拇指任务将全部重新采集，" - "四指零位保持不变。", - flush=True, - ) - elif selected_scope == "fingers": - print( - "四指专项标定：拇指任务和4个拇指零位继承自已通过会话 " - f"{resume_candidate.name}；12项四指任务将全部重新采集。", - flush=True, - ) - else: - print( - "检测到兼容的失败会话，将恢复已完整通过的关节任务：" - f"{resume_candidate.name}。失败中的当前任务会从头重做。", - flush=True, - ) - elif selected_scope == "thumb": - print( - "独立拇指标定：不导入四指会话；仅采集4项拇指任务，" - "四指URDF零位保持原始CAD值。", - flush=True, - ) - maximum_sessions = config.required_independent_passes - for pass_index in range(maximum_sessions): - stamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session = config.session_root / stamp - while session.exists(): - time.sleep(1.0) - stamp = datetime.now().strftime("%Y%m%d_%H%M%S") - session = config.session_root / stamp - if maximum_sessions > 1: - print(f"正式标定复验：第 {pass_index + 1}/{maximum_sessions} 次", flush=True) - status, code = _run_hardware_session( - config, - session, - resume_from=( - resume_candidate - if selected_scope != "full" or pass_index == 0 - else None - ), - recalibration_scope=selected_scope, - ) - if code != EXIT_PASS: - return code - # Keep the exact node-side completion contract durable before the - # independent publication layer starts. If publication itself fails, - # developers can re-run artifact checks without repeating motion or - # inventing lost combination-validation metrics. - atomic_write_json(session / "node_status.json", status) - try: - summary, release_ready = finalize_session_artifacts( - config, session, node_status=status - ) - except BaseException as error: - _write_trace(session / "calibration.log", error) - status = dict(status) - status["state"] = "PAUSED" - status["reason"] = f"PUB-ARTIFACT-601:{error}" - _, block = build_failure_report(config, session, status, reason=status["reason"]) - print(block, flush=True) - return EXIT_QUALITY - if release_ready: - result_pointer = ( - config.session_root / "latest_thumb_passed" - if selected_scope == "thumb" and partial_base is None - else config.session_root / "latest_passed" - ) - print( - "\n".join( - [ - f"PASS：{config.model} {config.side} 标定、URDF修正和复验全部通过。", - f"正式结果：{result_pointer}", - f"JSON：{session / summary['artifacts']['json']}", - f"URDF：{summary['artifacts']['urdf']}", - ] - ), - flush=True, - ) - return EXIT_PASS - print("本次会话质量PASS；正在自动执行第二次独立完整复验。", flush=True) - return EXIT_QUALITY - - -def main(args: list[str] | None = None) -> None: - parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定") - parser.add_argument("--config", default=str(_default_product_config())) - parser.add_argument("--workspace", default=None) - parser.add_argument("--preflight-only", action="store_true") - parser.add_argument( - "--scope", - choices=("full", "thumb", "fingers"), - default="full", - help=( - "full重新标定全手；thumb仅重采4项拇指任务；" - "fingers复用已认证拇指并仅重采12项四指任务" - ), - ) - parser.add_argument( - "--base-session", - default=None, - help=( - "可选：thumb模式将结果合并到该完整会话；" - "fingers模式必须提供该基础会话" - ), - ) - parser.add_argument( - "--no-resume", - action="store_true", - help="忽略失败会话，从第一个关节开始全新采集", - ) - arguments = parser.parse_args(args) - configure_fastdds_large_image_transport() - ros_log_dir = Path( - os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs") - ) - ros_log_dir.mkdir(parents=True, exist_ok=True) - rclpy.init() - try: - code = run( - arguments.config, - workspace=arguments.workspace, - preflight_only=arguments.preflight_only, - allow_resume=not arguments.no_resume, - scope=arguments.scope, - base_session=arguments.base_session, - ) - finally: - if rclpy.ok(): - rclpy.shutdown() - raise SystemExit(code) +__all__ = ["main"] if __name__ == "__main__": diff --git a/src/linkerhand_calibration/linkerhand_calibration/pnp.py b/src/linkerhand_calibration/linkerhand_calibration/pnp.py index 9ee5c91..970bf40 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/pnp.py +++ b/src/linkerhand_calibration/linkerhand_calibration/pnp.py @@ -1,1751 +1,3 @@ -"""Square AprilTag pose estimation with planar ambiguity tracking. +"""Compatibility import for the model-independent PnP implementation.""" -The AprilTag detections contain accurately refined image corners. This module -uses OpenCV's IPPE square solver directly so the calibration node can inspect -both planar PnP solutions instead of accepting an occasionally flipped TF -pose. -""" - -from __future__ import annotations - -from dataclasses import dataclass, replace -from itertools import product -import math -from typing import Mapping, Sequence - -import cv2 -import numpy as np -from scipy.spatial.transform import Rotation - - -@dataclass(frozen=True) -class SquareTagPose: - """One tag-to-camera pose candidate returned by IPPE.""" - - quaternion_xyzw: tuple[float, float, float, float] - translation_xyz_m: tuple[float, float, float] - reprojection_error_px: float - - -def _relative_pose( - parent: SquareTagPose, - child: SquareTagPose, -) -> tuple[Rotation, np.ndarray]: - parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) - child_rotation = Rotation.from_quat(child.quaternion_xyzw) - relative_rotation = parent_rotation.inv() * child_rotation - relative_translation = parent_rotation.inv().apply( - np.asarray(child.translation_xyz_m, dtype=float) - - np.asarray(parent.translation_xyz_m, dtype=float) - ) - return relative_rotation, relative_translation - - -def _normal_alignment_rad(first: SquareTagPose, second: SquareTagPose) -> float: - """Return the undirected angle between two observed Tag face normals.""" - first_normal = Rotation.from_quat(first.quaternion_xyzw).apply( - [0.0, 0.0, 1.0] - ) - second_normal = Rotation.from_quat(second.quaternion_xyzw).apply( - [0.0, 0.0, 1.0] - ) - return math.acos( - abs(float(np.clip(first_normal @ second_normal, -1.0, 1.0))) - ) - - -def select_rigid_group_trajectory( - frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], - *, - roles: Sequence[str], - fixed_pairs: Sequence[tuple[str, str]], - reprojection_scale_px: float, - rotation_scale_rad: float, - translation_scale_m: float, - pair_geometry: str = "pose", - normal_alignment_pairs: Sequence[tuple[str, str]] = (), - normal_alignment_scale_rad: float | None = None, -) -> tuple[ - list[dict[str, SquareTagPose]], - dict[str, float | str], -]: - """Resolve planar branches using geometry that should stay rigid. - - Every possible branch combination in the first frame is treated as a - candidate rigid reference. For each such reference, every later frame - independently chooses the combination with the lowest reprojection plus - geometric-drift cost. ``pose`` compares relative rotation and translation; - ``distance`` compares only Euclidean centre distances and therefore does - not allow planar-PnP orientation jitter into centre-trajectory angles. - The globally cheapest reference and path win. - """ - role_names = tuple(str(role) for role in roles) - pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) - normal_pair_names = tuple( - (str(first), str(second)) - for first, second in normal_alignment_pairs - ) - if not frames: - raise ValueError("at least one PnP frame is required") - if len(set(role_names)) != len(role_names) or not role_names: - raise ValueError("roles must be non-empty and unique") - if any( - parent not in role_names or child not in role_names - for parent, child in (*pair_names, *normal_pair_names) - ): - raise ValueError("geometry pairs must reference roles") - reprojection_scale = float(reprojection_scale_px) - rotation_scale = float(rotation_scale_rad) - translation_scale = float(translation_scale_m) - geometry_mode = str(pair_geometry) - normal_scale = ( - rotation_scale - if normal_alignment_scale_rad is None - else float(normal_alignment_scale_rad) - ) - if min( - reprojection_scale, - rotation_scale, - translation_scale, - normal_scale, - ) <= 0.0: - raise ValueError("trajectory selection scales must be positive") - if geometry_mode not in {"pose", "distance"}: - raise ValueError("pair_geometry must be pose or distance") - - combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] - for frame in frames: - candidate_lists = [tuple(frame.get(role, ())) for role in role_names] - if any(not candidates for candidates in candidate_lists): - raise ValueError("every frame must contain every requested role") - combinations_by_frame.append( - [ - dict(zip(role_names, combination)) - for combination in product(*candidate_lists) - ] - ) - - best_total = float("inf") - best_path: list[dict[str, SquareTagPose]] | None = None - - def emission( - combination: Mapping[str, SquareTagPose], - reference_pairs: Mapping[ - tuple[str, str], tuple[Rotation, np.ndarray] - ], - reference_distances: Mapping[tuple[str, str], float], - ) -> tuple[float, float, float]: - reprojection_cost = sum( - pose.reprojection_error_px - for pose in combination.values() - ) / reprojection_scale - normal_alignment_cost = sum( - _normal_alignment_rad( - combination[first], combination[second] - ) - for first, second in normal_pair_names - ) / normal_scale - rotation_drifts: list[float] = [] - translation_drifts: list[float] = [] - distance_drifts: list[float] = [] - for pair, ( - reference_rotation, - reference_translation, - ) in reference_pairs.items(): - rotation, translation = _relative_pose( - combination[pair[0]], - combination[pair[1]], - ) - rotation_drifts.append( - float( - (reference_rotation.inv() * rotation).magnitude() - ) - ) - translation_drifts.append( - float( - np.linalg.norm( - translation - reference_translation - ) - ) - ) - current_distance = float( - np.linalg.norm( - np.asarray( - combination[pair[1]].translation_xyz_m, - dtype=float, - ) - - np.asarray( - combination[pair[0]].translation_xyz_m, - dtype=float, - ) - ) - ) - distance_drifts.append( - abs(current_distance - reference_distances[pair]) - ) - if geometry_mode == "distance": - geometry_cost = sum(distance_drifts) / translation_scale - else: - geometry_cost = ( - sum(rotation_drifts) / rotation_scale - + sum(translation_drifts) / translation_scale - ) - return ( - reprojection_cost + geometry_cost + normal_alignment_cost, - max(rotation_drifts, default=0.0), - ( - max(distance_drifts, default=0.0) - if geometry_mode == "distance" - else max(translation_drifts, default=0.0) - ), - ) - - def transition_cost( - previous: Mapping[str, SquareTagPose], - current: Mapping[str, SquareTagPose], - ) -> float: - rotation_motion = sum( - rotation_distance_rad( - previous[role].quaternion_xyzw, - current[role].quaternion_xyzw, - ) - for role in role_names - ) - translation_motion = sum( - float( - np.linalg.norm( - np.asarray(current[role].translation_xyz_m) - - np.asarray(previous[role].translation_xyz_m) - ) - ) - for role in role_names - ) - if geometry_mode == "distance": - return translation_motion / translation_scale - return ( - rotation_motion / rotation_scale - + translation_motion / translation_scale - ) - - for reference_index, reference_combination in enumerate( - combinations_by_frame[0] - ): - reference_pairs = { - pair: _relative_pose( - reference_combination[pair[0]], - reference_combination[pair[1]], - ) - for pair in pair_names - } - reference_distances = { - pair: float( - np.linalg.norm( - np.asarray( - reference_combination[pair[1]].translation_xyz_m, - dtype=float, - ) - - np.asarray( - reference_combination[pair[0]].translation_xyz_m, - dtype=float, - ) - ) - ) - for pair in pair_names - } - first_emission = emission( - reference_combination, - reference_pairs, - reference_distances, - ) - previous_costs = np.full( - len(combinations_by_frame[0]), - np.inf, - dtype=float, - ) - previous_costs[reference_index] = first_emission[0] - back_pointers: list[list[int]] = [] - for frame_index in range(1, len(combinations_by_frame)): - previous_combinations = combinations_by_frame[frame_index - 1] - combinations = combinations_by_frame[frame_index] - frame_emissions = [ - emission( - combination, - reference_pairs, - reference_distances, - ) - for combination in combinations - ] - current_costs = np.full(len(combinations), np.inf, dtype=float) - frame_back_pointers: list[int] = [] - for current_index, combination in enumerate(combinations): - transition_costs = [ - previous_costs[previous_index] - + transition_cost( - previous_combination, - combination, - ) - for previous_index, previous_combination in enumerate( - previous_combinations - ) - ] - best_previous = int(np.argmin(transition_costs)) - frame_back_pointers.append(best_previous) - current_costs[current_index] = ( - transition_costs[best_previous] - + frame_emissions[current_index][0] - ) - back_pointers.append(frame_back_pointers) - previous_costs = current_costs - - final_index = int(np.argmin(previous_costs)) - total = float(previous_costs[final_index]) - path_indices = [final_index] - for frame_back_pointers in reversed(back_pointers): - path_indices.append( - frame_back_pointers[path_indices[-1]] - ) - path_indices.reverse() - path = [ - combinations[index] - for combinations, index in zip( - combinations_by_frame, - path_indices, - ) - ] - if total < best_total: - best_total = total - best_path = path - - if best_path is None: - raise RuntimeError("trajectory branch selection produced no path") - - # The marker-to-marker mounting transforms are unknown, so the rigid - # reference must be estimated from the complete sweep. Using frame zero - # as both the optimisation seed and the reported quality reference made - # one noisy endpoint frame look like drift in every other frame. A - # rotation medoid and component-wise translation median are insensitive - # to that endpoint noise while still exposing a persistent mirror branch. - robust_reference_pairs: dict[ - tuple[str, str], tuple[Rotation, np.ndarray] - ] = {} - for pair in pair_names: - pair_poses = [ - _relative_pose(frame[pair[0]], frame[pair[1]]) - for frame in best_path - ] - pair_rotations = [pose[0] for pose in pair_poses] - angular_costs = np.asarray( - [ - sum( - float((candidate.inv() * other).magnitude()) - for other in pair_rotations - ) - for candidate in pair_rotations - ], - dtype=float, - ) - rotation_medoid = pair_rotations[int(np.argmin(angular_costs))] - translation_median = np.median( - np.asarray([pose[1] for pose in pair_poses], dtype=float), - axis=0, - ) - robust_reference_pairs[pair] = ( - rotation_medoid, - translation_median, - ) - - rotation_drifts_by_frame: list[float] = [] - translation_drifts_by_frame: list[float] = [] - pair_distances_by_pair = { - pair: np.asarray( - [ - np.linalg.norm( - np.asarray(frame[pair[1]].translation_xyz_m, dtype=float) - - np.asarray( - frame[pair[0]].translation_xyz_m, dtype=float - ) - ) - for frame in best_path - ], - dtype=float, - ) - for pair in pair_names - } - robust_pair_distances = { - pair: float(np.median(distances)) - for pair, distances in pair_distances_by_pair.items() - } - distance_drifts_by_frame: list[float] = [] - for frame in best_path: - frame_rotation_drifts: list[float] = [] - frame_translation_drifts: list[float] = [] - frame_distance_drifts: list[float] = [] - for pair, ( - reference_rotation, - reference_translation, - ) in robust_reference_pairs.items(): - rotation, translation = _relative_pose( - frame[pair[0]], frame[pair[1]] - ) - frame_rotation_drifts.append( - float((reference_rotation.inv() * rotation).magnitude()) - ) - frame_translation_drifts.append( - float(np.linalg.norm(translation - reference_translation)) - ) - distance = float( - np.linalg.norm( - np.asarray(frame[pair[1]].translation_xyz_m, dtype=float) - - np.asarray( - frame[pair[0]].translation_xyz_m, dtype=float - ) - ) - ) - frame_distance_drifts.append( - abs(distance - robust_pair_distances[pair]) - ) - rotation_drifts_by_frame.append( - max(frame_rotation_drifts, default=0.0) - ) - translation_drifts_by_frame.append( - max(frame_translation_drifts, default=0.0) - ) - distance_drifts_by_frame.append( - max(frame_distance_drifts, default=0.0) - ) - - rotation_drifts = np.asarray(rotation_drifts_by_frame, dtype=float) - translation_drifts = np.asarray( - translation_drifts_by_frame, dtype=float - ) - distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float) - normal_alignments = np.asarray( - [ - max( - ( - _normal_alignment_rad(frame[first], frame[second]) - for first, second in normal_pair_names - ), - default=0.0, - ) - for frame in best_path - ], - dtype=float, - ) - return best_path, { - "total_cost": float(best_total), - "pair_geometry": geometry_mode, - "maximum_normal_alignment_rad": float( - np.max(normal_alignments, initial=0.0) - ), - "maximum_pair_rotation_drift_rad": float( - np.max(rotation_drifts, initial=0.0) - ), - "p95_pair_rotation_drift_rad": float( - np.percentile(rotation_drifts, 95.0) - ), - "median_pair_rotation_drift_rad": float( - np.median(rotation_drifts) - ), - "maximum_pair_translation_drift_m": float( - np.max(translation_drifts, initial=0.0) - ), - "p95_pair_translation_drift_m": float( - np.percentile(translation_drifts, 95.0) - ), - "maximum_pair_distance_drift_m": float( - np.max(distance_drifts, initial=0.0) - ), - "p95_pair_distance_drift_m": float( - np.percentile(distance_drifts, 95.0) - ), - "median_pair_distance_drift_m": float( - np.median(distance_drifts) - ), - } - - -def select_static_rigid_group_initialization( - frames: Sequence[Mapping[str, Sequence[SquareTagPose]]], - *, - roles: Sequence[str], - fixed_pairs: Sequence[tuple[str, str]], - reprojection_scale_px: float, - maximum_pose_jump_rad: float, - maximum_translation_jump_m: float, - relative_rotation_scale_rad: float, - relative_translation_scale_m: float, - normal_alignment_pairs: Sequence[tuple[str, str]] = (), - normal_alignment_scale_rad: float = math.radians(5.0), - task_reference_pairs: Mapping[ - tuple[str, str], tuple[Rotation, np.ndarray] - ] | None = None, - task_reference_rotation_scale_rad: float = math.radians(1.0), - task_reference_translation_scale_m: float = 0.01, -) -> tuple[list[dict[str, SquareTagPose]], dict[str, float | str]]: - """Select a static multi-Tag IPPE branch path in bounded time. - - Group initialization is performed while the hand is held at an endpoint, - so every frame should describe the same camera and relative Tag poses. - Enumerating a full Viterbi transition matrix for every possible first-frame - branch is therefore unnecessary: with four two-branch Tags and eight - frames it performs more than one hundred thousand scipy rotations and can - block the live ROS callback for several seconds. - - Instead, treat every first-frame combination as a possible static - reference and independently select the closest combination in each later - frame. This preserves the multi-frame rigidity and normal-alignment - evidence while changing the search from O(F*C^2*C0) to O(F*C*C0). - """ - role_names = tuple(str(role) for role in roles) - pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs) - normal_pair_names = tuple( - (str(first), str(second)) for first, second in normal_alignment_pairs - ) - task_references = dict(task_reference_pairs or {}) - if not frames: - raise ValueError("at least one PnP frame is required") - if not role_names or len(set(role_names)) != len(role_names): - raise ValueError("roles must be non-empty and unique") - if any( - parent not in role_names or child not in role_names - for parent, child in ( - *pair_names, - *normal_pair_names, - *task_references, - ) - ): - raise ValueError("geometry pairs must reference roles") - scales = ( - float(reprojection_scale_px), - float(maximum_pose_jump_rad), - float(maximum_translation_jump_m), - float(relative_rotation_scale_rad), - float(relative_translation_scale_m), - float(normal_alignment_scale_rad), - float(task_reference_rotation_scale_rad), - float(task_reference_translation_scale_m), - ) - if min(scales) <= 0.0: - raise ValueError("static initialization scales must be positive") - ( - reprojection_scale, - pose_scale, - translation_scale, - relative_rotation_scale, - relative_translation_scale, - normal_scale, - task_reference_rotation_scale, - task_reference_translation_scale, - ) = scales - - combinations_by_frame: list[list[dict[str, SquareTagPose]]] = [] - for frame in frames: - candidate_lists = [tuple(frame.get(role, ())) for role in role_names] - if any(not candidates for candidates in candidate_lists): - raise ValueError("every frame must contain every requested role") - combinations_by_frame.append( - [ - dict(zip(role_names, combination)) - for combination in product(*candidate_lists) - ] - ) - - def score_against_reference( - reference: Mapping[str, SquareTagPose], - reference_pairs: Mapping[ - tuple[str, str], tuple[Rotation, np.ndarray] - ], - combination: Mapping[str, SquareTagPose], - ) -> float: - score = sum( - pose.reprojection_error_px for pose in combination.values() - ) / reprojection_scale - score += sum( - _normal_alignment_rad(combination[first], combination[second]) - for first, second in normal_pair_names - ) / normal_scale - score += sum( - rotation_distance_rad( - reference[role].quaternion_xyzw, - combination[role].quaternion_xyzw, - ) - / pose_scale - + float( - np.linalg.norm( - np.asarray(combination[role].translation_xyz_m, dtype=float) - - np.asarray(reference[role].translation_xyz_m, dtype=float) - ) - ) - / translation_scale - for role in role_names - ) - for pair, (reference_rotation, reference_translation) in ( - reference_pairs.items() - ): - rotation, translation = _relative_pose( - combination[pair[0]], combination[pair[1]] - ) - score += ( - float((reference_rotation.inv() * rotation).magnitude()) - / relative_rotation_scale - + float(np.linalg.norm(translation - reference_translation)) - / relative_translation_scale - ) - for pair, (task_rotation, task_translation) in task_references.items(): - rotation, translation = _relative_pose( - combination[pair[0]], combination[pair[1]] - ) - score += ( - float((task_rotation.inv() * rotation).magnitude()) - / task_reference_rotation_scale - + float(np.linalg.norm(translation - task_translation)) - / task_reference_translation_scale - ) - return float(score) - - best_total = float("inf") - best_path: list[dict[str, SquareTagPose]] | None = None - for reference in combinations_by_frame[0]: - reference_pairs = { - pair: _relative_pose(reference[pair[0]], reference[pair[1]]) - for pair in pair_names - } - path = [reference] - total = score_against_reference(reference, reference_pairs, reference) - for combinations in combinations_by_frame[1:]: - scored = [ - ( - score_against_reference( - reference, reference_pairs, combination - ), - combination, - ) - for combination in combinations - ] - cost, selected = min(scored, key=lambda item: item[0]) - total += cost - path.append(selected) - if total < best_total: - best_total = total - best_path = path - - if best_path is None: - raise RuntimeError("static group initialization produced no path") - - # Reuse the complete quality calculation with exactly one chosen branch - # per role and frame. This retains all existing quality fields without - # reintroducing the combinatorial branch search. - reduced_frames = [ - {role: (frame[role],) for role in role_names} for frame in best_path - ] - selected_path, quality = select_rigid_group_trajectory( - reduced_frames, - roles=role_names, - fixed_pairs=pair_names, - reprojection_scale_px=reprojection_scale, - rotation_scale_rad=relative_rotation_scale, - translation_scale_m=relative_translation_scale, - normal_alignment_pairs=normal_pair_names, - normal_alignment_scale_rad=normal_scale, - ) - quality = dict(quality) - quality["total_cost"] = float(best_total) - quality["initialization_search"] = "static_reference" - quality["task_reference_used"] = ( - "true" if task_references else "false" - ) - return selected_path, quality - - -def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray: - matrix = np.asarray(camera_matrix, dtype=np.float64) - if matrix.shape != (3, 3): - raise ValueError("camera_matrix must have shape (3, 3)") - if not np.all(np.isfinite(matrix)): - raise ValueError("camera_matrix must be finite") - if matrix[0, 0] <= 0.0 or matrix[1, 1] <= 0.0: - raise ValueError("camera focal lengths must be positive") - return matrix - - -def square_object_points(tag_size_m: float) -> np.ndarray: - """Return IPPE-square points matching apriltag_msgs corner order. - - ``apriltag_ros`` reports bottom-left, bottom-right, top-right, top-left. - OpenCV's ``SOLVEPNP_IPPE_SQUARE`` requires the same physical corners in - the order below. - """ - size = float(tag_size_m) - if not math.isfinite(size) or size <= 0.0: - raise ValueError("tag_size_m must be finite and positive") - half = size / 2.0 - return np.asarray( - [ - [-half, half, 0.0], - [half, half, 0.0], - [half, -half, 0.0], - [-half, -half, 0.0], - ], - dtype=np.float64, - ) - - -def solve_square_tag_ippe( - corners_xy: Sequence[Sequence[float]], - *, - tag_size_m: float, - camera_matrix: Sequence[Sequence[float]], -) -> list[SquareTagPose]: - """Return every finite, positive-depth IPPE pose for one square tag.""" - image_points = np.asarray(corners_xy, dtype=np.float64) - if image_points.shape != (4, 2): - raise ValueError("corners_xy must have shape (4, 2)") - if not np.all(np.isfinite(image_points)): - raise ValueError("corners_xy must be finite") - intrinsic = _as_camera_matrix(camera_matrix) - object_points = square_object_points(tag_size_m) - distortion = np.zeros((4, 1), dtype=np.float64) - - solved, rotation_vectors, translations, _ = cv2.solvePnPGeneric( - object_points, - image_points, - intrinsic, - distortion, - flags=cv2.SOLVEPNP_IPPE_SQUARE, - ) - if not solved: - return [] - - candidates: list[SquareTagPose] = [] - for rotation_vector, translation in zip(rotation_vectors, translations): - rotation_matrix, _ = cv2.Rodrigues(rotation_vector) - translation_vector = np.asarray(translation, dtype=float).reshape(3) - camera_points = ( - rotation_matrix @ object_points.T - + translation_vector.reshape(3, 1) - ).T - if np.min(camera_points[:, 2]) <= 0.0: - continue - projected, _ = cv2.projectPoints( - object_points, - rotation_vector, - translation_vector, - intrinsic, - distortion, - ) - residual = projected.reshape(4, 2) - image_points - reprojection_error = float( - np.sqrt(np.mean(np.sum(residual * residual, axis=1))) - ) - quaternion = Rotation.from_matrix(rotation_matrix).as_quat() - if not ( - np.all(np.isfinite(quaternion)) - and np.all(np.isfinite(translation_vector)) - and math.isfinite(reprojection_error) - ): - continue - candidates.append( - SquareTagPose( - quaternion_xyzw=tuple(float(value) for value in quaternion), - translation_xyz_m=tuple( - float(value) for value in translation_vector - ), - reprojection_error_px=reprojection_error, - ) - ) - return candidates - - -def rotation_distance_rad( - first_xyzw: Sequence[float], - second_xyzw: Sequence[float], -) -> float: - first = np.asarray(first_xyzw, dtype=float) - second = np.asarray(second_xyzw, dtype=float) - if first.shape != (4,) or second.shape != (4,): - raise ValueError("quaternions must contain four values") - first_norm = float(np.linalg.norm(first)) - second_norm = float(np.linalg.norm(second)) - if ( - not np.all(np.isfinite(first)) - or not np.all(np.isfinite(second)) - or first_norm <= 0.0 - or second_norm <= 0.0 - ): - raise ValueError("quaternions must be finite and non-zero") - # q and -q represent the same rotation. The absolute dot-product gives - # the geodesic SO(3) distance without constructing two scipy Rotation - # objects for every branch comparison in the live tracker. - cosine_half_angle = abs( - float(np.dot(first / first_norm, second / second_norm)) - ) - return 2.0 * math.acos(float(np.clip(cosine_half_angle, 0.0, 1.0))) - - -def select_continuous_pose( - candidates: Sequence[SquareTagPose], - *, - previous: SquareTagPose | None, - maximum_reprojection_error_px: float, - reprojection_tie_px: float, - maximum_pose_jump_rad: float, - maximum_translation_jump_m: float, - maximum_tag_tilt_rad: float, -) -> tuple[SquareTagPose | None, str]: - """Select the best IPPE branch using image fit and temporal continuity.""" - maximum_error = float(maximum_reprojection_error_px) - tie_error = float(reprojection_tie_px) - maximum_rotation = float(maximum_pose_jump_rad) - maximum_translation = float(maximum_translation_jump_m) - maximum_tilt = float(maximum_tag_tilt_rad) - if min( - maximum_error, - maximum_rotation, - maximum_translation, - maximum_tilt, - ) <= 0.0: - raise ValueError("PnP selection thresholds must be positive") - if tie_error < 0.0: - raise ValueError("reprojection_tie_px must be non-negative") - - eligible: list[SquareTagPose] = [] - for candidate in candidates: - if candidate.reprojection_error_px > maximum_error: - continue - normal = Rotation.from_quat(candidate.quaternion_xyzw).as_matrix()[:, 2] - tilt = math.acos(float(np.clip(abs(normal[2]), 0.0, 1.0))) - if tilt > maximum_tilt: - continue - eligible.append(candidate) - if not eligible: - return None, "no_pose_within_reprojection_or_tilt_limit" - - eligible.sort(key=lambda item: item.reprojection_error_px) - best = eligible[0] - if previous is None: - return best, "" - - # Temporal continuity must only break a genuine planar-PnP tie. The old - # implementation normalised reprojection error by the permissive 1.5 px - # rejection limit, which allowed a stale mirror branch at 0.25 px to beat - # the true branch at e.g. 0.05 px merely because it was closer to the - # preceding (already wrong) pose. Once one IPPE solution has a meaningful - # image-fit advantage, trust it and allow the tracker to leave the stale - # branch even if that correction is a large pose jump. - competitive = [ - candidate - for candidate in eligible - if candidate.reprojection_error_px - <= best.reprojection_error_px + tie_error - ] - if len(competitive) == 1: - return best, "" - - previous_translation = np.asarray(previous.translation_xyz_m, dtype=float) - scored: list[tuple[float, SquareTagPose]] = [] - for candidate in competitive: - rotation_jump = rotation_distance_rad( - previous.quaternion_xyzw, - candidate.quaternion_xyzw, - ) - translation_jump = float( - np.linalg.norm( - np.asarray(candidate.translation_xyz_m, dtype=float) - - previous_translation - ) - ) - if ( - rotation_jump > maximum_rotation - or translation_jump > maximum_translation - ): - continue - score = ( - ( - candidate.reprojection_error_px - - best.reprojection_error_px - ) - / max(tie_error, np.finfo(float).eps) - + rotation_jump / maximum_rotation - + translation_jump / maximum_translation - ) - scored.append((float(score), candidate)) - if not scored: - return None, "pose_jump" - - selected = min(scored, key=lambda item: item[0])[1] - previous_quaternion = np.asarray(previous.quaternion_xyzw, dtype=float) - selected_quaternion = np.asarray(selected.quaternion_xyzw, dtype=float) - if float(np.dot(previous_quaternion, selected_quaternion)) < 0.0: - selected = replace( - selected, - quaternion_xyzw=tuple( - float(value) for value in -selected_quaternion - ), - ) - return selected, "" - - -class SquareTagPoseTracker: - """Maintain the selected planar-PnP branch independently for each tag.""" - - def __init__( - self, - *, - maximum_reprojection_error_px: float, - reprojection_tie_px: float, - maximum_pose_jump_rad: float, - maximum_translation_jump_m: float, - maximum_tag_tilt_rad: float, - reset_after_seconds: float, - ) -> None: - self.maximum_reprojection_error_px = float( - maximum_reprojection_error_px - ) - self.reprojection_tie_px = float(reprojection_tie_px) - self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) - self.maximum_translation_jump_m = float(maximum_translation_jump_m) - self.maximum_tag_tilt_rad = float(maximum_tag_tilt_rad) - self.reset_after_ns = int(float(reset_after_seconds) * 1_000_000_000) - if self.reset_after_ns <= 0: - raise ValueError("reset_after_seconds must be positive") - self._previous: dict[str, tuple[int, SquareTagPose]] = {} - self.last_candidates_by_role: dict[ - str, tuple[SquareTagPose, ...] - ] = {} - self.last_candidate_diagnostics_by_role: dict[ - str, dict[str, float | int] - ] = {} - self.branch_correction_counts: dict[str, int] = {} - - def reset(self) -> None: - self._previous.clear() - self.last_candidates_by_role.clear() - self.last_candidate_diagnostics_by_role.clear() - self.branch_correction_counts.clear() - - def estimate( - self, - role: str, - corners_xy: Sequence[Sequence[float]], - *, - tag_size_m: float, - camera_matrix: Sequence[Sequence[float]], - stamp_ns: int, - reprojection_tie_px: float | None = None, - ) -> tuple[SquareTagPose | None, str]: - try: - candidates = solve_square_tag_ippe( - corners_xy, - tag_size_m=tag_size_m, - camera_matrix=camera_matrix, - ) - except (ValueError, cv2.error): - self.last_candidates_by_role[str(role)] = () - self.last_candidate_diagnostics_by_role[str(role)] = { - "solved_candidate_count": 0, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - "maximum_reprojection_error_px": float( - self.maximum_reprojection_error_px - ), - "maximum_independent_tilt_deg": math.degrees( - self.maximum_tag_tilt_rad - ), - } - return None, "pnp_solve_failed" - if not candidates: - self.last_candidates_by_role[str(role)] = () - self.last_candidate_diagnostics_by_role[str(role)] = { - "solved_candidate_count": 0, - "reprojection_candidate_count": 0, - "independent_tilt_candidate_count": 0, - "maximum_reprojection_error_px": float( - self.maximum_reprojection_error_px - ), - "maximum_independent_tilt_deg": math.degrees( - self.maximum_tag_tilt_rad - ), - } - return None, "pnp_solve_failed" - reprojection_candidates = [ - candidate - for candidate in candidates - if candidate.reprojection_error_px - <= self.maximum_reprojection_error_px - ] - candidate_tilts_rad: list[float] = [] - independent_candidates: list[SquareTagPose] = [] - for candidate in reprojection_candidates: - normal = Rotation.from_quat( - candidate.quaternion_xyzw - ).as_matrix()[:, 2] - tilt = math.acos( - float(np.clip(abs(normal[2]), 0.0, 1.0)) - ) - candidate_tilts_rad.append(float(tilt)) - if tilt <= self.maximum_tag_tilt_rad: - independent_candidates.append(candidate) - - # Candidate generation and candidate selection have different - # contracts. The per-Tag tilt limit protects a pose used without any - # other geometry, but it must not erase a finite, low-reprojection - # IPPE solution before SquareTagGroupPoseTracker can evaluate it - # against the fixed palm reference, the articulated chain and the - # preceding group pose. At a strongly oblique view the planar - # ambiguity is usually smaller, and rejecting both branches at a - # fixed angle caused deterministic mid-sweep holes despite continuous - # image detections. Group tracking therefore receives every - # reprojection-valid candidate; independent tracking below retains the - # original tilt safety gate. - self.last_candidates_by_role[str(role)] = tuple( - reprojection_candidates - ) - diagnostics: dict[str, float | int] = { - "solved_candidate_count": len(candidates), - "reprojection_candidate_count": len(reprojection_candidates), - "independent_tilt_candidate_count": len(independent_candidates), - "minimum_reprojection_error_px": float( - min( - candidate.reprojection_error_px - for candidate in candidates - ) - ), - "maximum_reprojection_error_px": float( - self.maximum_reprojection_error_px - ), - "maximum_independent_tilt_deg": math.degrees( - self.maximum_tag_tilt_rad - ), - } - if candidate_tilts_rad: - diagnostics["minimum_candidate_tilt_deg"] = math.degrees( - min(candidate_tilts_rad) - ) - diagnostics["maximum_candidate_tilt_deg"] = math.degrees( - max(candidate_tilts_rad) - ) - self.last_candidate_diagnostics_by_role[str(role)] = diagnostics - if not independent_candidates: - return None, "no_pose_within_reprojection_or_tilt_limit" - - previous_record = self._previous.get(str(role)) - previous: SquareTagPose | None = None - if previous_record is not None: - previous_stamp, previous_pose = previous_record - elapsed = int(stamp_ns) - previous_stamp - if 0 <= elapsed <= self.reset_after_ns: - previous = previous_pose - - selected, reason = select_continuous_pose( - independent_candidates, - previous=previous, - maximum_reprojection_error_px=( - self.maximum_reprojection_error_px - ), - reprojection_tie_px=( - self.reprojection_tie_px - if reprojection_tie_px is None - else float(reprojection_tie_px) - ), - maximum_pose_jump_rad=self.maximum_pose_jump_rad, - maximum_translation_jump_m=self.maximum_translation_jump_m, - maximum_tag_tilt_rad=self.maximum_tag_tilt_rad, - ) - if selected is not None: - if previous is not None: - rotation_jump = rotation_distance_rad( - previous.quaternion_xyzw, - selected.quaternion_xyzw, - ) - translation_jump = float( - np.linalg.norm( - np.asarray(selected.translation_xyz_m, dtype=float) - - np.asarray( - previous.translation_xyz_m, - dtype=float, - ) - ) - ) - if ( - rotation_jump > self.maximum_pose_jump_rad - or translation_jump - > self.maximum_translation_jump_m - ): - key = str(role) - self.branch_correction_counts[key] = ( - self.branch_correction_counts.get(key, 0) + 1 - ) - self._previous[str(role)] = (int(stamp_ns), selected) - return selected, reason - - -class SquareTagGroupPoseTracker: - """Choose all tag branches together using thumb-chain continuity. - - A 30 px planar tag has two IPPE solutions whose reprojection errors can - exchange order from one frame to the next. Tracking each tag - independently can therefore choose an incompatible pair for a relative - joint such as T4->T5. This tracker enumerates the small Cartesian product - (at most 2**4 combinations) and favours the combination that keeps both - the camera poses and all adjacent relative poses continuous. - """ - - def __init__( - self, - *, - roles: Sequence[str], - adjacent_pairs: Sequence[tuple[str, str]], - maximum_pose_jump_rad: float, - maximum_translation_jump_m: float, - relative_rotation_scale_rad: float, - relative_translation_scale_m: float, - reprojection_scale_px: float, - reprojection_weight: float, - reset_after_seconds: float, - initialization_frames: int = 1, - normal_alignment_pairs: Sequence[tuple[str, str]] = (), - normal_alignment_scale_rad: float = math.radians(5.0), - maximum_normal_alignment_rad: float | None = None, - return_reference_rotation_scale_rad: float = math.radians(1.0), - return_reference_maximum_command_gap_u8: int = 8, - coupled_rotation_pairs: Sequence[ - tuple[str, str, str, str, float] - ] = (), - coupled_rotation_scale_rad: float = math.radians(3.0), - maximum_coupled_rotation_residual_rad: float | None = None, - ) -> None: - self.roles = tuple(str(role) for role in roles) - self.adjacent_pairs = tuple( - (str(parent), str(child)) - for parent, child in adjacent_pairs - ) - self.normal_alignment_pairs = tuple( - (str(first), str(second)) - for first, second in normal_alignment_pairs - ) - self.coupled_rotation_pairs = tuple( - ( - str(driver_parent), - str(driver_child), - str(follower_parent), - str(follower_child), - float(multiplier), - ) - for ( - driver_parent, - driver_child, - follower_parent, - follower_child, - multiplier, - ) in coupled_rotation_pairs - ) - if not self.roles or len(set(self.roles)) != len(self.roles): - raise ValueError("roles must be non-empty and unique") - if any( - parent not in self.roles or child not in self.roles - for parent, child in ( - *self.adjacent_pairs, - *self.normal_alignment_pairs, - ) - ): - raise ValueError("group geometry pairs must reference roles") - self.maximum_pose_jump_rad = float(maximum_pose_jump_rad) - self.maximum_translation_jump_m = float( - maximum_translation_jump_m - ) - self.relative_rotation_scale_rad = float( - relative_rotation_scale_rad - ) - self.relative_translation_scale_m = float( - relative_translation_scale_m - ) - self.reprojection_scale_px = float(reprojection_scale_px) - self.reprojection_weight = float(reprojection_weight) - self.initialization_frames = int(initialization_frames) - self.normal_alignment_scale_rad = float( - normal_alignment_scale_rad - ) - self.maximum_normal_alignment_rad = ( - None - if maximum_normal_alignment_rad is None - else float(maximum_normal_alignment_rad) - ) - self.return_reference_rotation_scale_rad = float( - return_reference_rotation_scale_rad - ) - self.return_reference_maximum_command_gap_u8 = int( - return_reference_maximum_command_gap_u8 - ) - self.coupled_rotation_scale_rad = float( - coupled_rotation_scale_rad - ) - self.maximum_coupled_rotation_residual_rad = ( - None - if maximum_coupled_rotation_residual_rad is None - else float(maximum_coupled_rotation_residual_rad) - ) - reset_seconds = float(reset_after_seconds) - if min( - self.maximum_pose_jump_rad, - self.maximum_translation_jump_m, - self.relative_rotation_scale_rad, - self.relative_translation_scale_m, - self.reprojection_scale_px, - self.normal_alignment_scale_rad, - self.return_reference_rotation_scale_rad, - self.coupled_rotation_scale_rad, - reset_seconds, - ) <= 0.0: - raise ValueError("group tracking scales must be positive") - if self.reprojection_weight < 0.0: - raise ValueError("reprojection_weight must be non-negative") - if self.initialization_frames < 1: - raise ValueError("initialization_frames must be positive") - if self.return_reference_maximum_command_gap_u8 < 0: - raise ValueError( - "return reference maximum command gap must be non-negative" - ) - if ( - self.maximum_normal_alignment_rad is not None - and self.maximum_normal_alignment_rad <= 0.0 - ): - raise ValueError("maximum normal alignment must be positive") - if any( - role not in self.roles - for coupling in self.coupled_rotation_pairs - for role in coupling[:4] - ): - raise ValueError("coupled rotation pairs must reference roles") - if any( - multiplier <= 0.0 - for *_, multiplier in self.coupled_rotation_pairs - ): - raise ValueError("coupled rotation multipliers must be positive") - if ( - self.maximum_coupled_rotation_residual_rad is not None - and self.maximum_coupled_rotation_residual_rad <= 0.0 - ): - raise ValueError( - "maximum coupled rotation residual must be positive" - ) - self.reset_after_ns = int(reset_seconds * 1_000_000_000) - self._previous: dict[str, SquareTagPose] = {} - self._previous_stamp_ns: int | None = None - self._initial_candidates: list[ - dict[str, tuple[SquareTagPose, ...]] - ] = [] - self._initial_stamps_ns: list[int] = [] - self.last_initialization_quality: dict[str, float | str] = {} - self.branch_correction_counts: dict[str, int] = {} - self._decreasing_relative_rotations: dict[ - int, dict[tuple[str, str], Rotation] - ] = {} - self._coupled_reference_rotations: dict[ - tuple[str, str], Rotation - ] = {} - self._task_reference_relative_poses: dict[ - tuple[str, str], tuple[Rotation, np.ndarray] - ] = {} - self.last_missing_roles: tuple[str, ...] = () - - def reset(self, *, preserve_task_reference: bool = False) -> None: - self._previous.clear() - self._previous_stamp_ns = None - self._initial_candidates.clear() - self._initial_stamps_ns.clear() - self.last_initialization_quality.clear() - self.branch_correction_counts.clear() - self._decreasing_relative_rotations.clear() - self._coupled_reference_rotations.clear() - self.last_missing_roles = () - if not preserve_task_reference: - self._task_reference_relative_poses.clear() - - def _task_reference_cost( - self, combination: Mapping[str, SquareTagPose] - ) -> float: - residual = 0.0 - for pair, (expected_rotation, expected_translation) in ( - self._task_reference_relative_poses.items() - ): - rotation, translation = _relative_pose( - combination[pair[0]], combination[pair[1]] - ) - residual += ( - float((expected_rotation.inv() * rotation).magnitude()) - / self.return_reference_rotation_scale_rad - + float(np.linalg.norm(translation - expected_translation)) - / self.relative_translation_scale_m - ) - return residual - - def _coupled_rotation_residuals( - self, combination: Mapping[str, SquareTagPose] - ) -> tuple[float, ...]: - if not self.coupled_rotation_pairs: - return () - residuals: list[float] = [] - for ( - driver_parent, - driver_child, - follower_parent, - follower_child, - multiplier, - ) in self.coupled_rotation_pairs: - driver_pair = (driver_parent, driver_child) - follower_pair = (follower_parent, follower_child) - if ( - driver_pair not in self._coupled_reference_rotations - or follower_pair not in self._coupled_reference_rotations - ): - return () - driver_rotation = _relative_pose( - combination[driver_parent], combination[driver_child] - )[0] - follower_rotation = _relative_pose( - combination[follower_parent], combination[follower_child] - )[0] - driver_travel = ( - self._coupled_reference_rotations[driver_pair].inv() - * driver_rotation - ).magnitude() - follower_travel = ( - self._coupled_reference_rotations[follower_pair].inv() - * follower_rotation - ).magnitude() - residuals.append( - abs(float(follower_travel) - multiplier * float(driver_travel)) - ) - return tuple(residuals) - - def _informative_coupled_rotation_costs( - self, - combinations: Sequence[Mapping[str, SquareTagPose]], - ) -> tuple[float, ...]: - """Return branch costs only while the weak coupling prior is credible. - - The URDF mimic ratio is useful for distinguishing two planar-IPPE - branches, but it is not measurement truth for a passive joint. Once - every otherwise viable combination disagrees with that ratio, using - it would bias the measured curve (and previously rejected every - frame). In that case fall back to visual continuity for this frame. - """ - residuals = tuple( - self._coupled_rotation_residuals(combination) - for combination in combinations - ) - if not residuals or not any(residuals): - return tuple(0.0 for _ in combinations) - if ( - self.maximum_coupled_rotation_residual_rad is not None - and not any( - values - and max(values) - <= self.maximum_coupled_rotation_residual_rad - for values in residuals - ) - ): - return tuple(0.0 for _ in combinations) - return tuple( - sum(values) / self.coupled_rotation_scale_rad - for values in residuals - ) - - def _return_reference( - self, command_u8: int | None - ) -> dict[tuple[str, str], tuple[Rotation, np.ndarray | None]]: - if command_u8 is None or not self._decreasing_relative_rotations: - return {} - command = int(command_u8) - nearest = min( - self._decreasing_relative_rotations, - key=lambda candidate: abs(candidate - command), - ) - if ( - abs(nearest - command) - > self.return_reference_maximum_command_gap_u8 - ): - return {} - references = self._decreasing_relative_rotations[nearest] - commands = sorted(self._decreasing_relative_rotations) - axes: dict[tuple[str, str], np.ndarray | None] = {} - for pair in self.adjacent_pairs: - endpoint_delta = ( - self._decreasing_relative_rotations[commands[-1]][pair].inv() - * self._decreasing_relative_rotations[commands[0]][pair] - ).as_rotvec() - norm = float(np.linalg.norm(endpoint_delta)) - axes[pair] = ( - None - if norm < math.radians(5.0) - else endpoint_delta / norm - ) - return { - pair: (rotation, axes[pair]) - for pair, rotation in references.items() - } - - def _return_reference_cost( - self, - combination: Mapping[str, SquareTagPose], - reference: Mapping[ - tuple[str, str], tuple[Rotation, np.ndarray | None] - ], - ) -> float: - residual = 0.0 - for pair, (expected, motion_axis) in reference.items(): - vector = ( - expected.inv() - * _relative_pose( - combination[pair[0]], combination[pair[1]] - )[0] - ).as_rotvec() - if motion_axis is not None: - # The outbound trajectory identifies the physical one-DOF - # motion axis. Do not penalize return travel along that axis: - # it may contain real mechanical hysteresis that calibration - # must measure. A planar-IPPE mirror branch appears primarily - # as a large orthogonal tilt and is rejected by this residual. - vector = vector - motion_axis * float(vector @ motion_axis) - residual += float(np.linalg.norm(vector)) - return residual / self.return_reference_rotation_scale_rad - - def select( - self, - candidates_by_role: Mapping[str, Sequence[SquareTagPose]], - *, - stamp_ns: int, - trajectory_command_u8: int | None = None, - trajectory_direction: str | None = None, - ) -> tuple[dict[str, SquareTagPose] | None, str]: - """Return one mutually consistent pose for every configured role.""" - direction = ( - None - if trajectory_direction is None - else str(trajectory_direction) - ) - if direction not in {None, "decreasing", "increasing"}: - raise ValueError( - "trajectory_direction must be decreasing or increasing" - ) - return_reference = ( - self._return_reference(trajectory_command_u8) - if direction == "increasing" - else {} - ) - candidate_lists = [ - tuple(candidates_by_role.get(role, ())) - for role in self.roles - ] - self.last_missing_roles = tuple( - role - for role, candidates in zip(self.roles, candidate_lists) - if not candidates - ) - if self.last_missing_roles: - return None, "group_missing_pose_candidates" - self.last_missing_roles = () - - combinations = [ - dict(zip(self.roles, combination)) - for combination in product(*candidate_lists) - ] - minimum_errors = { - role: min( - candidate.reprojection_error_px - for candidate in candidates - ) - for role, candidates in zip(self.roles, candidate_lists) - } - stamp = int(stamp_ns) - previous_is_fresh = ( - self._previous_stamp_ns is not None - and 0 <= stamp - self._previous_stamp_ns - <= self.reset_after_ns - and set(self._previous) == set(self.roles) - ) - - if not previous_is_fresh: - if self._previous_stamp_ns is not None: - self._previous.clear() - self._previous_stamp_ns = None - self._initial_candidates.clear() - self._initial_stamps_ns.clear() - self.last_initialization_quality.clear() - if self.initialization_frames > 1: - if self._initial_stamps_ns and not ( - 0 <= stamp - self._initial_stamps_ns[-1] - <= self.reset_after_ns - ): - self._initial_candidates.clear() - self._initial_stamps_ns.clear() - self._initial_candidates.append( - { - role: tuple(candidates_by_role.get(role, ())) - for role in self.roles - } - ) - self._initial_stamps_ns.append(stamp) - if len(self._initial_candidates) < self.initialization_frames: - return ( - None, - "group_initializing:" - f"{len(self._initial_candidates)}/" - f"{self.initialization_frames}", - ) - selected_path, initialization_quality = ( - select_static_rigid_group_initialization( - self._initial_candidates, - roles=self.roles, - fixed_pairs=self.adjacent_pairs, - reprojection_scale_px=self.reprojection_scale_px, - maximum_pose_jump_rad=self.maximum_pose_jump_rad, - maximum_translation_jump_m=( - self.maximum_translation_jump_m - ), - relative_rotation_scale_rad=( - self.relative_rotation_scale_rad - ), - relative_translation_scale_m=( - self.relative_translation_scale_m - ), - normal_alignment_pairs=( - self.normal_alignment_pairs - ), - normal_alignment_scale_rad=( - self.normal_alignment_scale_rad - ), - task_reference_pairs=( - self._task_reference_relative_poses - ), - task_reference_rotation_scale_rad=( - self.return_reference_rotation_scale_rad - ), - task_reference_translation_scale_m=( - self.relative_translation_scale_m - ), - ) - ) - selected = selected_path[-1] - stamp = self._initial_stamps_ns[-1] - self.last_initialization_quality = dict( - initialization_quality - ) - self._initial_candidates.clear() - self._initial_stamps_ns.clear() - if ( - self.maximum_normal_alignment_rad is not None - and float( - initialization_quality[ - "maximum_normal_alignment_rad" - ] - ) - > self.maximum_normal_alignment_rad - ): - return None, "group_normal_alignment" - else: - coupling_costs = self._informative_coupled_rotation_costs( - combinations - ) - selected = min( - zip(combinations, coupling_costs), - key=lambda item: ( - sum( - pose.reprojection_error_px - for pose in item[0].values() - ) - / self.reprojection_scale_px - + sum( - _normal_alignment_rad( - item[0][first], item[0][second] - ) - for first, second in self.normal_alignment_pairs - ) - / self.normal_alignment_scale_rad - + self._return_reference_cost( - item[0], return_reference - ) - + item[1] - + self._task_reference_cost(item[0]) - ), - )[0] - maximum_alignment = max( - ( - _normal_alignment_rad( - selected[first], selected[second] - ) - for first, second in self.normal_alignment_pairs - ), - default=0.0, - ) - self.last_initialization_quality = { - "maximum_normal_alignment_rad": maximum_alignment - } - if ( - self.maximum_normal_alignment_rad is not None - and maximum_alignment - > self.maximum_normal_alignment_rad - ): - return None, "group_normal_alignment" - else: - previous_relative = { - pair: _relative_pose( - self._previous[pair[0]], - self._previous[pair[1]], - ) - for pair in self.adjacent_pairs - } - base_scored: list[tuple[float, dict[str, SquareTagPose]]] = [] - for combination in combinations: - absolute_rotation_motion = 0.0 - absolute_translation_motion = 0.0 - rejected = False - for role in self.roles: - rotation_motion = rotation_distance_rad( - self._previous[role].quaternion_xyzw, - combination[role].quaternion_xyzw, - ) - translation_motion = float( - np.linalg.norm( - np.asarray( - combination[role].translation_xyz_m, - dtype=float, - ) - - np.asarray( - self._previous[role].translation_xyz_m, - dtype=float, - ) - ) - ) - if ( - rotation_motion > self.maximum_pose_jump_rad - or translation_motion - > self.maximum_translation_jump_m - ): - rejected = True - break - absolute_rotation_motion += rotation_motion - absolute_translation_motion += translation_motion - if rejected: - continue - - relative_rotation_motion = 0.0 - relative_translation_motion = 0.0 - for pair in self.adjacent_pairs: - rotation, translation = _relative_pose( - combination[pair[0]], - combination[pair[1]], - ) - old_rotation, old_translation = previous_relative[pair] - relative_rotation_motion += float( - (old_rotation.inv() * rotation).magnitude() - ) - relative_translation_motion += float( - np.linalg.norm(translation - old_translation) - ) - - reprojection_penalty = sum( - max( - 0.0, - combination[role].reprojection_error_px - - minimum_errors[role], - ) - for role in self.roles - ) / self.reprojection_scale_px - score = ( - absolute_rotation_motion - / self.maximum_pose_jump_rad - + absolute_translation_motion - / self.maximum_translation_jump_m - + relative_rotation_motion - / self.relative_rotation_scale_rad - + relative_translation_motion - / self.relative_translation_scale_m - + self.reprojection_weight * reprojection_penalty - + self._return_reference_cost( - combination, return_reference - ) - ) - base_scored.append((float(score), combination)) - - if not base_scored: - return None, "group_pose_jump" - coupling_costs = self._informative_coupled_rotation_costs( - [combination for _, combination in base_scored] - ) - scored = [ - (base_score + coupling_cost, combination) - for (base_score, combination), coupling_cost in zip( - base_scored, coupling_costs - ) - ] - selected = min(scored, key=lambda item: item[0])[1] - - aligned: dict[str, SquareTagPose] = {} - for role in self.roles: - pose = selected[role] - if previous_is_fresh: - old_quaternion = np.asarray( - self._previous[role].quaternion_xyzw, - dtype=float, - ) - quaternion = np.asarray( - pose.quaternion_xyzw, - dtype=float, - ) - if float(np.dot(old_quaternion, quaternion)) < 0.0: - pose = replace( - pose, - quaternion_xyzw=tuple( - float(value) for value in -quaternion - ), - ) - best_reprojection = min( - candidate_lists[self.roles.index(role)], - key=lambda candidate: candidate.reprojection_error_px, - ) - if pose != best_reprojection: - self.branch_correction_counts[role] = ( - self.branch_correction_counts.get(role, 0) + 1 - ) - aligned[role] = pose - - self._previous = aligned - self._previous_stamp_ns = stamp - if ( - direction == "decreasing" - and trajectory_command_u8 is not None - and not self._coupled_reference_rotations - ): - for ( - driver_parent, - driver_child, - follower_parent, - follower_child, - _multiplier, - ) in self.coupled_rotation_pairs: - for pair in ( - (driver_parent, driver_child), - (follower_parent, follower_child), - ): - self._coupled_reference_rotations[pair] = _relative_pose( - aligned[pair[0]], aligned[pair[1]] - )[0] - if direction == "decreasing" and trajectory_command_u8 is not None: - self._decreasing_relative_rotations[ - int(trajectory_command_u8) - ] = { - pair: _relative_pose( - aligned[pair[0]], aligned[pair[1]] - )[0] - for pair in self.adjacent_pairs - } - if not self._task_reference_relative_poses: - self._task_reference_relative_poses = { - pair: _relative_pose( - aligned[pair[0]], aligned[pair[1]] - ) - for pair in self.adjacent_pairs - } - return dict(aligned), "" +from .core.geometry.pnp import * # noqa: F401,F403 diff --git a/src/linkerhand_calibration/linkerhand_calibration/product.py b/src/linkerhand_calibration/linkerhand_calibration/product.py index 3b4b057..72c56a5 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/product.py +++ b/src/linkerhand_calibration/linkerhand_calibration/product.py @@ -13,115 +13,60 @@ import hashlib from pathlib import Path import re from typing import Any, Mapping +import xml.etree.ElementTree as ET import yaml -from .compat import resolve_renamed_package_path +from .compat import product_profile_key, resolve_renamed_package_path +from .core import CalibrationProfile, ProfileKey from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics -from .full_hand import ( - G20_RIGHT_19_LAYOUT, - HandCalibrationProfile, - get_hand_calibration_profile, -) -from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile - - -VIEWS = ("front", "side", "top") +from .models import RegisteredProfile, get_default_registry @dataclass(frozen=True) class ProductCalibrationContract: - """Declarative boundary between one hand model and the shared engine.""" + """Compatibility facade over one typed, locally registered profile.""" - model: str - side: str - layout_id: str - profile: HandCalibrationProfile - zero_profile: ZeroCalibrationProfile + registered: RegisteredProfile + + @property + def model(self) -> str: + return self.registered.profile.key.model + + @property + def side(self) -> str: + return self.registered.profile.key.side + + @property + def layout_id(self) -> str: + return self.registered.profile.key.layout + + @property + def typed_profile(self) -> CalibrationProfile: + return self.registered.profile + + @property + def profile(self): + return self.registered.engine.hand_profile + + @property + def zero_profile(self): + return self.registered.engine.zero_profile @property def required_tag_ids(self) -> frozenset[int]: - return frozenset( - int(tag_id) - for tags in self.profile.view_tags.values() - for tag_id in tags.values() - ) + return self.typed_profile.vision.tag_ids @property def views(self) -> tuple[str, ...]: - return tuple(self.profile.view_tags) - - -_PRODUCT_CONTRACTS: dict[ - tuple[str, str, str], ProductCalibrationContract -] = {} - - -def register_product_calibration_contract( - contract: ProductCalibrationContract, -) -> None: - """Register one model/side/layout without modifying shared workflow code.""" - model = str(contract.model).strip().upper() - side = str(contract.side).strip().lower() - layout = str(contract.layout_id).strip().lower() - if not model or side not in {"left", "right"} or not layout: - raise ValueError("product calibration contract identity is invalid") - if contract.profile.side != side: - raise ValueError("product contract side differs from hand profile") - if contract.profile.layout_id.lower() != layout: - raise ValueError("product contract layout differs from hand profile") - if contract.zero_profile.hand != contract.profile: - raise ValueError("zero-calibration profile differs from hand profile") - if len(contract.profile.baseline_command) != contract.profile.command_count: - raise ValueError("profile baseline and command names differ in length") - if not contract.required_tag_ids: - raise ValueError("product contract must declare at least one Tag") - key = (model, side, layout) - existing = _PRODUCT_CONTRACTS.get(key) - if existing is not None and existing != contract: - raise ValueError(f"product calibration contract already registered: {key}") - _PRODUCT_CONTRACTS[key] = contract + return self.typed_profile.vision.view_names def get_product_calibration_contract( - model: str, side: str, layout_id: str + model: str, side: str, layout_id: str, revision: int = 1 ) -> ProductCalibrationContract: - key = ( - str(model).strip().upper(), - str(side).strip().lower(), - str(layout_id).strip().lower(), - ) - try: - return _PRODUCT_CONTRACTS[key] - except KeyError as error: - supported = ", ".join("/".join(item) for item in sorted(_PRODUCT_CONTRACTS)) - raise ValueError( - f"unsupported calibration product {key}; registered={supported}" - ) from error - - -register_product_calibration_contract( - ProductCalibrationContract( - model="G20", - side="right", - layout_id=G20_RIGHT_19_LAYOUT, - profile=get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT), - zero_profile=get_zero_calibration_profile( - "right", G20_RIGHT_19_LAYOUT - ), - ) -) -for _legacy_side in ("left", "right"): - register_product_calibration_contract( - ProductCalibrationContract( - model="G20", - side=_legacy_side, - layout_id="legacy_11", - profile=get_hand_calibration_profile(_legacy_side, "legacy_11"), - zero_profile=get_zero_calibration_profile( - _legacy_side, "legacy_11" - ), - ) + return ProductCalibrationContract( + get_default_registry().resolve(model, side, layout_id, revision) ) @@ -206,10 +151,36 @@ def _custom_pnp_tag_sizes_m_by_id( return result +def _validate_profile_urdf( + profile: CalibrationProfile, source_urdf: Path +) -> None: + """Verify every zero and mimic target before a hardware command is sent.""" + root = ET.parse(source_urdf).getroot() + urdf_joints = { + str(joint.get("name")) + for joint in root.findall("joint") + if joint.get("name") + } + required = ( + set(profile.zero.active_joints) + | set(profile.zero.passive_joints) + | set(profile.zero.mimic_source_by_joint) + | set(profile.zero.mimic_source_by_joint.values()) + ) + missing = required - urdf_joints + if missing: + raise ValueError( + "source URDF is missing profile joints: " + + ", ".join(sorted(missing)) + ) + + @dataclass(frozen=True) class ProductConfig: path: Path workspace: Path + schema_version: int + profile_key: ProfileKey model: str side: str tag_layout: str @@ -246,23 +217,17 @@ def load_product_config( raise ValueError(f"product config does not exist: {source}") with source.open("r", encoding="utf-8") as stream: raw = _mapping(yaml.safe_load(stream), str(source)) - if int(raw.get("schema_version", -1)) != 1: - raise ValueError("product config schema_version must be 1") + profile_key = product_profile_key(raw) root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve() serial = str(raw.get("serial_number", "")) if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None: raise ValueError("serial_number is invalid") - model = str(raw.get("model", "")).strip().upper() - side = str(raw.get("side", "")).strip().lower() - # Keep the deployed G20 schema compatible while making the layout an - # explicit product choice for all new configurations. - layout = str( - raw.get( - "tag_layout", - G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "", - ) - ).strip().lower() - contract = get_product_calibration_contract(model, side, layout) + model = profile_key.model + side = profile_key.side + layout = profile_key.layout + contract = get_product_calibration_contract( + model, side, layout, profile_key.revision + ) can_interface = str(raw.get("can_interface", "")).strip() if not can_interface: raise ValueError("can_interface is required") @@ -315,6 +280,7 @@ def load_product_config( actual = sha256_file(candidate) if actual != expected: raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}") + _validate_profile_urdf(contract.typed_profile, source_urdf) expected_tag_ids = set(contract.required_tag_ids) tag_sizes = _tag_sizes_m_by_id(tag_config) @@ -333,10 +299,6 @@ def load_product_config( camera_raw = _mapping(raw.get("cameras"), "cameras") required_views = tuple(contract.views) - if set(required_views) != set(VIEWS): - raise ValueError( - "the current shared engine requires front/side/top views" - ) if set(camera_raw) != set(required_views): raise ValueError(f"cameras must contain {'/'.join(required_views)}") loaded_extrinsics = load_three_camera_extrinsics(extrinsics) @@ -380,6 +342,8 @@ def load_product_config( return ProductConfig( path=source, workspace=root, + schema_version=int(raw["schema_version"]), + profile_key=profile_key, 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 cf8a7cf..9ce7085 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/publication.py +++ b/src/linkerhand_calibration/linkerhand_calibration/publication.py @@ -23,6 +23,7 @@ from .full_hand import ( ) 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, @@ -202,6 +203,7 @@ def verify_corrected_urdf( *, 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. @@ -210,6 +212,13 @@ def verify_corrected_urdf( """ 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): @@ -243,6 +252,12 @@ def verify_corrected_urdf( 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(): @@ -745,11 +760,20 @@ def finalize_session_artifacts( 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 = {} diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py new file mode 100644 index 0000000..7081990 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/__init__.py @@ -0,0 +1,11 @@ +"""Common orchestration; ROS integration is isolated below ``nodes``.""" + +from .controller import ControllerSnapshot, SessionController, SessionState +from .runner import build_session_controller + +__all__ = [ + "ControllerSnapshot", + "SessionController", + "SessionState", + "build_session_controller", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py new file mode 100644 index 0000000..7b286f8 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/adapters/__init__.py @@ -0,0 +1 @@ +"""Camera, detector, and hand-SDK runtime adapters.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/controller.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/controller.py new file mode 100644 index 0000000..7c7ddba --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/controller.py @@ -0,0 +1,160 @@ +"""ROS-free session state machine shared by live and replay runners.""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum +from typing import Sequence + +from ..core import CalibrationProfile, SampleRecord, validate_profile +from ..core.solver import ( + SessionSolution, + SessionSolver, + TaskEvaluation, + TaskEvaluator, +) + + +class SessionState(str, Enum): + CREATED = "CREATED" + PREFLIGHT = "PREFLIGHT" + PREPARING = "PREPARING" + CAPTURING = "CAPTURING" + EVALUATING = "EVALUATING" + RESCAN = "RESCAN" + SOLVING = "SOLVING" + VALIDATING = "VALIDATING" + PUBLISHING = "PUBLISHING" + COMPLETE = "COMPLETE" + FAILED = "FAILED" + + +@dataclass(frozen=True) +class ControllerSnapshot: + state: SessionState + completed_task_keys: tuple[str, ...] + current_task_key: str | None + failure_reason: str | None + + +class SessionController: + """Advance one typed task graph without depending on ROS clocks/messages.""" + + def __init__( + self, + profile: CalibrationProfile, + evaluator: TaskEvaluator, + solver: SessionSolver, + ) -> None: + validate_profile(profile) + self.profile = profile + self.evaluator = evaluator + self.solver = solver + self.state = SessionState.CREATED + self._task_index = 0 + self._samples: list[SampleRecord] = [] + self.last_evaluation: TaskEvaluation | None = None + self.solution: SessionSolution | None = None + self.failure_reason: str | None = None + + @property + def current_task(self): + if self._task_index >= len(self.profile.motion.tasks): + return None + return self.profile.motion.tasks[self._task_index] + + @property + def samples(self) -> tuple[SampleRecord, ...]: + return tuple(self._samples) + + def snapshot(self) -> ControllerSnapshot: + completed = tuple( + task.key for task in self.profile.motion.tasks[: self._task_index] + ) + task = self.current_task + return ControllerSnapshot( + state=self.state, + completed_task_keys=completed, + current_task_key=None if task is None else task.key, + failure_reason=self.failure_reason, + ) + + def start(self) -> None: + self._require(SessionState.CREATED) + self.state = SessionState.PREFLIGHT + + def finish_preflight(self, *, passed: bool, reason: str = "") -> None: + self._require(SessionState.PREFLIGHT) + if not passed: + self._fail(reason or "static preflight failed") + return + self.state = SessionState.PREPARING + + def task_pose_ready(self) -> None: + if self.state not in {SessionState.PREPARING, SessionState.RESCAN}: + self._raise_transition("task pose can only follow prepare or rescan") + if self.current_task is None: + self._raise_transition("there is no task left to capture") + self.state = SessionState.CAPTURING + + def submit_task_samples( + self, samples: Sequence[SampleRecord] + ) -> TaskEvaluation: + self._require(SessionState.CAPTURING) + task = self.current_task + if task is None: + self._raise_transition("there is no active task") + rows = tuple(samples) + if not rows or any(row.task_key != task.key for row in rows): + raise ValueError("captured samples do not belong to the active task") + self.state = SessionState.EVALUATING + evaluation = self.evaluator.evaluate_task(self.profile, task, rows) + self.last_evaluation = evaluation + if not evaluation.accepted: + if evaluation.rescan_measurements or evaluation.rescan_cycles: + self.state = SessionState.RESCAN + else: + self._fail("task evaluation failed without a safe rescan scope") + return evaluation + self._samples.extend(rows) + self._task_index += 1 + self.state = ( + SessionState.SOLVING + if self.current_task is None + else SessionState.PREPARING + ) + return evaluation + + def solve(self) -> SessionSolution: + self._require(SessionState.SOLVING) + solution = self.solver.solve_session(self.profile, self.samples) + self.solution = solution + if solution.passed: + self.state = SessionState.VALIDATING + else: + self._fail("session solver rejected the captured samples") + return solution + + def finish_release_validation( + self, *, passed: bool, reason: str = "" + ) -> None: + self._require(SessionState.VALIDATING) + if not passed: + self._fail(reason or "release validation failed") + return + self.state = SessionState.PUBLISHING + + def finish_publication(self) -> None: + self._require(SessionState.PUBLISHING) + self.state = SessionState.COMPLETE + + def _fail(self, reason: str) -> None: + self.failure_reason = str(reason) + self.state = SessionState.FAILED + + def _require(self, expected: SessionState) -> None: + if self.state != expected: + self._raise_transition(f"expected {expected.value}") + + def _raise_transition(self, detail: str) -> None: + raise RuntimeError(f"invalid session transition from {self.state.value}: {detail}") diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py new file mode 100644 index 0000000..5a82088 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/nodes/__init__.py @@ -0,0 +1 @@ +"""ROS message/service shells are migrated here after controller extraction.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/__init__.py new file mode 100644 index 0000000..c926162 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/__init__.py @@ -0,0 +1,3 @@ +from .codes import CalibrationErrorCode + +__all__ = ["CalibrationErrorCode"] diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/codes.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/codes.py new file mode 100644 index 0000000..09d16c1 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/codes.py @@ -0,0 +1,14 @@ +"""Model-independent runtime error categories.""" + +from enum import Enum + + +class CalibrationErrorCode(str, Enum): + PROFILE_INVALID = "profile_invalid" + PREFLIGHT_FAILED = "preflight_failed" + MOTION_FAILED = "motion_failed" + ACQUISITION_FAILED = "acquisition_failed" + TASK_QUALITY_FAILED = "task_quality_failed" + SESSION_SOLVE_FAILED = "session_solve_failed" + URDF_VALIDATION_FAILED = "urdf_validation_failed" + PUBLICATION_FAILED = "publication_failed" diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py new file mode 100644 index 0000000..774eb31 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -0,0 +1,41 @@ +"""Generic assembly of a registered profile and the session controller.""" + +from __future__ import annotations + +import argparse + +from ..compat import default_product_config_path +from ..core import ProfileKey +from ..core.solver import SessionSolver, TaskEvaluator +from ..models import ProfileRegistry, get_default_registry +from ..product import load_product_config +from .controller import SessionController + + +def build_session_controller( + profile_key: ProfileKey, + evaluator: TaskEvaluator, + solver: SessionSolver, + *, + registry: ProfileRegistry | None = None, +) -> SessionController: + selected_registry = registry or get_default_registry() + registered = selected_registry.get(profile_key) + return SessionController(registered.profile, evaluator, solver) + + +def main(args: list[str] | None = None) -> None: + """Select the profile first, then delegate to its reviewed CLI strategy.""" + selector = argparse.ArgumentParser(add_help=False) + selector.add_argument( + "--config", default=str(default_product_config_path()) + ) + selector.add_argument("--workspace", default=None) + selected, _ = selector.parse_known_args(args) + product = load_product_config( + selected.config, + workspace=selected.workspace, + check_can=False, + ) + registered = get_default_registry().get(product.profile_key) + registered.engine.cli_main(args) diff --git a/src/linkerhand_calibration/linkerhand_calibration/sample_schema.py b/src/linkerhand_calibration/linkerhand_calibration/sample_schema.py index a39bddd..3fdbbf9 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/sample_schema.py +++ b/src/linkerhand_calibration/linkerhand_calibration/sample_schema.py @@ -1,192 +1,3 @@ -"""Canonical command/feedback schema for calibration observations. +"""Compatibility import for the canonical sample data contract.""" -The hand command and its measured motor feedback are different physical -domains. Durable samples always retain both. Fitting code may still use the -historical ``command_u8`` key, but it is created only as an explicit projection -of a canonical record at the fitting boundary. -""" - -from __future__ import annotations - -import math -from typing import Any, Iterable, Literal, Mapping - -import numpy as np - - -SAMPLE_KINDS = frozenset( - { - 'sample', - 'baseline_hold_sample', - 'steady_command_sample', - 'palm_axis_sample', - } -) - -FitDomain = Literal['default', 'requested', 'feedback'] - - -class SampleDataContractError(ValueError): - """A calibration observation mixes or omits command domains.""" - - -def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float: - try: - number = float(value) - except (TypeError, ValueError) as error: - raise SampleDataContractError( - f'DATA-CONTRACT-701:{field} must be numeric' - ) from error - if not math.isfinite(number) or not 0.0 <= number <= 255.0: - raise SampleDataContractError( - f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]' - ) - if integral: - rounded = int(round(number)) - if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9): - raise SampleDataContractError( - f'DATA-CONTRACT-701:{field} must be an integer command' - ) - return rounded - return number - - -def explicit_domain_value( - source: Mapping[str, Any], domain: Literal['requested', 'feedback'] -) -> int | float: - """Read and validate one explicitly named domain from any observation.""" - field = ( - 'requested_command_u8' if domain == 'requested' else 'feedback_u8' - ) - if field not in source or source[field] is None: - raise SampleDataContractError( - f'DATA-CONTRACT-701:observation is missing explicit {field}' - ) - return _finite_u8(source[field], field, integral=domain == 'requested') - - -def canonical_sample_record( - source: Mapping[str, Any], - *, - allow_legacy_command: bool = False, -) -> dict[str, Any]: - """Return one durable, unambiguous calibration observation. - - ``allow_legacy_command`` is restricted to importing historical sessions - and unit fixtures. New online observations must provide both explicit - fields and therefore cannot silently reinterpret ``command_u8``. - """ - record = dict(source) - kind = str(record.get('kind', '')) - if not kind and allow_legacy_command: - # Old in-memory steady-curve fixtures predate durable sample kinds. - # This adapter is never enabled by the new online/import contract. - kind = 'steady_command_sample' - record['kind'] = kind - if kind not in SAMPLE_KINDS: - raise SampleDataContractError( - f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}' - ) - - requested = record.get('requested_command_u8') - feedback = record.get('feedback_u8') - legacy = record.get('command_u8') - if requested is None or feedback is None: - if not allow_legacy_command or legacy is None: - missing = [ - name - for name, value in ( - ('requested_command_u8', requested), - ('feedback_u8', feedback), - ) - if value is None - ] - raise SampleDataContractError( - 'DATA-CONTRACT-701:' - f'{kind} is missing explicit {",".join(missing)}' - ) - # Historical in-memory records used requested commands for settled - # checkpoints and feedback bins for dense/baseline/palm observations. - if requested is None: - requested = legacy - if feedback is None: - feedback = legacy - - record.pop('command_u8', None) - record['requested_command_u8'] = explicit_domain_value( - {'requested_command_u8': requested}, 'requested' - ) - record['feedback_u8'] = explicit_domain_value( - {'feedback_u8': feedback}, 'feedback' - ) - return record - - -def fitting_sample_record( - source: Mapping[str, Any], - *, - domain: FitDomain = 'default', - allow_legacy_command: bool = False, - snap_requested_endpoints: bool = False, -) -> dict[str, Any]: - """Project a canonical sample into the legacy curve-fitter interface.""" - record = canonical_sample_record( - source, allow_legacy_command=allow_legacy_command - ) - kind = str(record['kind']) - selected = domain - if selected == 'default': - selected = ( - 'requested' if kind == 'steady_command_sample' else 'feedback' - ) - if selected not in {'requested', 'feedback'}: - raise SampleDataContractError( - f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}' - ) - requested = int(record['requested_command_u8']) - if selected == 'requested' or ( - snap_requested_endpoints and requested in {0, 255} - ): - index = requested - else: - index = int( - np.clip(np.rint(float(record['feedback_u8'])), 0, 255) - ) - record['command_u8'] = index - return record - - -def fitting_sample_records( - records: Iterable[Mapping[str, Any]], - *, - domain: FitDomain = 'default', - allow_legacy_command: bool = False, - snap_requested_endpoints: bool = False, -) -> list[dict[str, Any]]: - """Project several canonical samples into one explicit fitting domain.""" - return [ - fitting_sample_record( - record, - domain=domain, - allow_legacy_command=allow_legacy_command, - snap_requested_endpoints=snap_requested_endpoints, - ) - for record in records - ] - - -def validate_sample_records( - records: Iterable[Mapping[str, Any]], - *, - allow_legacy_command: bool = False, -) -> None: - """Validate a collection without changing its representation.""" - for index, record in enumerate(records): - try: - canonical_sample_record( - record, allow_legacy_command=allow_legacy_command - ) - except SampleDataContractError as error: - raise SampleDataContractError( - f'{error};record_index={index}' - ) from error +from .core.domain.sample_schema import * # noqa: F401,F403 diff --git a/src/linkerhand_calibration/linkerhand_calibration/storage.py b/src/linkerhand_calibration/linkerhand_calibration/storage.py index 13f9a36..e403f72 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/storage.py +++ b/src/linkerhand_calibration/linkerhand_calibration/storage.py @@ -1,105 +1,3 @@ -"""Crash-safe session storage for hardware calibration.""" +"""Compatibility import for crash-safe artifact storage.""" -from __future__ import annotations - -import json -import os -from pathlib import Path -from typing import Any, Iterable, Mapping - - -def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> None: - destination = Path(path) - destination.parent.mkdir(parents=True, exist_ok=True) - temporary = destination.with_suffix(destination.suffix + ".tmp") - with temporary.open("w", encoding="utf-8") as stream: - json.dump(payload, stream, ensure_ascii=False, indent=2) - stream.write("\n") - stream.flush() - os.fsync(stream.fileno()) - os.replace(temporary, destination) - - -def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None: - destination = Path(path) - destination.parent.mkdir(parents=True, exist_ok=True) - line = json.dumps(payload, ensure_ascii=False, separators=(",", ":")) - with destination.open("a", encoding="utf-8") as stream: - stream.write(line + "\n") - stream.flush() - os.fsync(stream.fileno()) - - -def append_jsonl_many( - path: str | Path, payloads: Iterable[Mapping[str, Any]] -) -> None: - """Durably append a batch while paying the fsync cost only once.""" - destination = Path(path) - destination.parent.mkdir(parents=True, exist_ok=True) - lines = [ - json.dumps(payload, ensure_ascii=False, separators=(",", ":")) - for payload in payloads - ] - if not lines: - return - with destination.open("a", encoding="utf-8") as stream: - stream.write("\n".join(lines) + "\n") - stream.flush() - os.fsync(stream.fileno()) - - -def load_jsonl(path: str | Path) -> list[dict[str, Any]]: - source = Path(path) - if not source.exists(): - return [] - records: list[dict[str, Any]] = [] - with source.open("r", encoding="utf-8") as stream: - lines = stream.readlines() - nonempty_lines = [ - index for index, line in enumerate(lines, 1) if line.strip() - ] - last_nonempty_line = nonempty_lines[-1] if nonempty_lines else 0 - for line_number, line in enumerate(lines, 1): - if not line.strip(): - continue - try: - value = json.loads(line) - except json.JSONDecodeError as error: - if line_number == last_nonempty_line: - break - raise ValueError( - f"{source}:{line_number}: invalid JSONL record" - ) from error - if not isinstance(value, dict): - raise ValueError(f"{source}:{line_number}: record must be an object") - records.append(value) - return records - - -def load_json(path: str | Path) -> dict[str, Any] | None: - source = Path(path) - if not source.exists(): - return None - with source.open("r", encoding="utf-8") as stream: - value = json.load(stream) - if not isinstance(value, dict): - raise ValueError(f"{source} must contain a JSON object") - return value - - -def completed_scan_keys( - records: Iterable[Mapping[str, Any]], -) -> set[tuple[str, int, str, int]]: - keys: set[tuple[str, int, str, int]] = set() - for record in records: - if record.get("kind", "sample") != "sample": - continue - keys.add( - ( - str(record["phase"]), - int(record["cycle"]), - str(record["direction"]), - int(record["command_u8"]), - ) - ) - return keys +from .core.artifacts.storage import * # noqa: F401,F403 diff --git a/src/linkerhand_calibration/linkerhand_calibration/three_camera_diagnostics.py b/src/linkerhand_calibration/linkerhand_calibration/three_camera_diagnostics.py index aa91837..41eeb86 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/three_camera_diagnostics.py +++ b/src/linkerhand_calibration/linkerhand_calibration/three_camera_diagnostics.py @@ -1,893 +1,4 @@ -"""Chinese, operator-facing diagnostics for three-camera calibration.""" +"""Compatibility import for the installed Chinese model diagnostics.""" -from __future__ import annotations - -import re -from typing import Any, Mapping, Sequence - - -STATE_NAMES_ZH = { - "PREFLIGHT": "设备和标签预检", - "WAIT_START": "等待开始标定", - "IMPORTING_BASE": "正在读取基础标定会话", - "REVALIDATING_INHERITED": "正在复核继承的四指数据", - "RETURN_BASELINE": "正在恢复目标姿态", - "PREPARE_SWEEP": "正在到达扫描起点", - "SWEEP": "正在采集轨迹", - "FITTING": "正在拟合轨迹和零位", - "VALIDATION_MOVE": "正在移动到随机复测位置", - "VALIDATION_CAPTURE": "正在采集随机复测数据", - "PAUSED": "标定已暂停", - "ABORTED": "标定已终止", - "COMPLETE": "标定已完成", -} - -VIEW_NAMES_ZH = { - "front": "正面", - "side": "侧面", - "top": "上面", -} - -JOINT_NAMES_ZH = { - "thumb_cmc_pitch": "拇指CMC俯仰", - "thumb_cmc_roll": "拇指CMC滚转", - "thumb_mcp": "拇指MCP", - "thumb_ip": "拇指IP（被动）", - "index_mcp_roll": "食指MCP侧摆", - "index_mcp_pitch": "食指MCP屈伸", - "index_pip": "食指PIP", - "index_dip": "食指DIP（被动）", - "middle_mcp_roll": "中指MCP侧摆", - "middle_mcp_pitch": "中指MCP屈伸", - "middle_pip": "中指PIP", - "middle_dip": "中指DIP（被动）", - "ring_mcp_roll": "无名指MCP侧摆", - "ring_mcp_pitch": "无名指MCP屈伸", - "ring_pip": "无名指PIP", - "ring_dip": "无名指DIP（被动）", - "pinky_mcp_roll": "小指MCP侧摆", - "pinky_mcp_pitch": "小指MCP屈伸", - "pinky_pip": "小指PIP", - "pinky_dip": "小指DIP（被动）", - "thumb_cmc_yaw": "拇指CMC侧摆", - "index_mcp_roll_side": "食指MCP侧摆（侧面校验）", - "middle_mcp_roll_side": "中指MCP侧摆（侧面校验）", - "ring_mcp_roll_side": "无名指MCP侧摆（侧面校验）", - "pinky_mcp_roll_side": "小指MCP侧摆（侧面校验）", -} - - -def _format_u8(value: Any) -> str: - if value is None: - return "尚无反馈" - return f"{float(value):.1f}" - - -def _task_text(active: Mapping[str, Any]) -> str: - if not active: - return "尚无活动任务" - view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", ""))) - if active.get("kind") == "fit_failure": - joints = active.get("joints", []) - joint_text = "/".join( - JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints - ) - return ( - f"{view}机位，{joint_text}拟合检查失败，" - f"电机{active.get('motor_index')}，" - f"第{active.get('attempt', 1)}次尝试" - ) - if active.get("kind") == "zero_model_failure": - joints = active.get("joints", []) - joint_text = "/".join( - JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints - ) - return ( - f"{view}机位，{joint_text}零位/URDF验证失败，" - f"电机{active.get('motor_index')}，不会自动重扫" - ) - if active.get("kind") == "motion_stall": - return ( - f"电机{active.get('motor_index', '?')}运动停滞，目标" - f"{_format_u8(active.get('target_u8'))}、实际" - f"{_format_u8(active.get('actual_u8'))}" - ) - if active.get("kind") == "cross_view_roll_diagnostic": - return ( - f"{active.get('finger', '?')}侧摆跨机位诊断完成：" - f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°，" - f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°" - ) - if active.get("kind") == "validation": - return ( - f"{view}机位，随机复测，电机{active.get('motor_index')}，" - f"目标命令{active.get('command_u8')}" - ) - joints = active.get("joints", []) - joint_text = "/".join( - JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints - ) - start = active.get("start_u8") - target = active.get("target_u8") - cycle = active.get("cycle", "?") - repetitions = active.get("repetitions", "?") - task = ( - f"{view}机位，{joint_text}，电机{active.get('motor_index')}，" - f"第{cycle}/{repetitions}轮，{start}→{target}" - ) - fit_attempt = int(active.get("fit_attempt", 1)) - if fit_attempt > 1: - retry_cycles = active.get("fit_retry_cycles", []) - if retry_cycles: - task += "（补采异常轮" + "/".join( - str(cycle) for cycle in retry_cycles - ) + "）" - else: - task += f"（拟合补采第{fit_attempt}次）" - return task - - -def three_camera_reason_zh( - state: str, - reason: str, - active: Mapping[str, Any], -) -> tuple[str, str]: - """Translate a reason code and provide one concrete operator action.""" - reason = str(reason) - sample = active.get("sample", {}) if active else {} - missing = [int(value) for value in sample.get("missing_endpoint_u8", [])] - sample_range = ( - f"{_format_u8(sample.get('minimum_u8'))}～" - f"{_format_u8(sample.get('maximum_u8'))}" - ) - tolerance = sample.get("endpoint_tolerance_u8", "?") - - if reason.startswith("motor_state_stalled:"): - fields = reason.split(":") - context = fields[1] if len(fields) > 1 else "unknown" - error_match = re.search(r"error_u8=([0-9.]+)", reason) - error = error_match.group(1) if error_match else "未知" - timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason) - timeout_value = active.get("timeout_seconds") - if timeout_value is None and timeout_match is not None: - timeout_value = float(timeout_match.group(1)) - duration = ( - f"连续{float(timeout_value):g}秒" - if timeout_value is not None - else "在规定时间内" - ) - motor = active.get("motor_index") - if motor is not None: - return ( - f"电机{motor}反馈{duration}没有向目标推进；目标" - f"{_format_u8(active.get('target_u8'))}、实际" - f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，" - f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8" - f"（阶段={context}）。程序已保持当前位置。", - "若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后" - "重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。", - ) - return ( - f"电机反馈{duration}没有向目标推进；停止位置距目标{error}个u8" - f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。", - "检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的" - "正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。", - ) - - base_reason, separator, reason_detail = reason.partition(":") - if base_reason in { - "sweep_missing_endpoint_bin", - "sweep_bins_too_few", - "sweep_bin_gap_too_large", - "task_precheck_missing_command_127", - "task_precheck_detection_rate_too_low", - "synchronised_tag_state_timeout", - }: - reason = base_reason - detail_label = ( - JOINT_NAMES_ZH.get(reason_detail, reason_detail) - if separator and reason_detail - else "" - ) - detail_prefix = f"{detail_label}：" if detail_label else "" - - if "URDF zero offset reached the configured" in reason: - bound_match = re.search( - r"configured\s+([0-9.]+)\s+degree bound", reason - ) - bound = bound_match.group(1) if bound_match else "配置的" - hit_text = "" - if "bound:" in reason: - hit_text = reason.split("bound:", 1)[1].split( - "; all_offsets:", 1 - )[0] - for name, label in JOINT_NAMES_ZH.items(): - hit_text = hit_text.replace(name, label) - hit_suffix = f"；触边关节：{hit_text}" if hit_text else "" - return ( - f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的" - "零位结果，而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。", - "不要调用resume，也不要增大零位边界。先确认Tag有效黑框边长、三相机" - "内外参和原始CAD URDF；Tag尺寸修正后必须调用start重新采集，旧尺度" - "产生的轨迹不能直接生成修正URDF。", - ) - - if reason == "sweep_missing_endpoint_bin": - missing_text = "、".join(str(value) for value in missing) or "0或255" - return ( - f"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少" - f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为" - f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到" - "端点，但该时刻没有同时取得有效Tag图像和电机状态。", - "确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用" - "/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。", - ) - if reason == "sweep_bins_too_few": - return ( - f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少" - f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。", - "检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。", - ) - if reason == "sweep_bin_gap_too_large": - gap_start = sample.get("maximum_bin_gap_start_u8") - gap_end = sample.get("maximum_bin_gap_end_u8") - gap_range = ( - "" - if gap_start is None or gap_end is None - else f"（{gap_start}→{gap_end}）" - ) - return ( - f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，" - f"{gap_range}允许值不超过" - f"{sample.get('allowed_maximum_bin_gap', '?')}。", - "检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。", - ) - if reason == "synchronised_tag_state_timeout": - group_reasons = active.get("group_pnp_reasons", {}) - if isinstance(group_reasons, Mapping) and group_reasons: - tag_rejections = active.get("pnp_rejection_counts", {}) - group_rejections = active.get( - "group_pnp_rejection_counts", {} - ) - missing_roles = active.get( - "group_missing_candidate_roles", {} - ) - candidate_diagnostics = active.get( - "pnp_candidate_diagnostics", {} - ) - view_details: list[str] = [] - for view, value in group_reasons.items(): - view_name = str(view) - parts = [str(value)] - missing = ( - missing_roles.get(view_name, ()) - if isinstance(missing_roles, Mapping) - else () - ) - if isinstance(missing, Sequence) and not isinstance( - missing, (str, bytes) - ) and missing: - parts.append( - "缺候选=" + ",".join(str(role) for role in missing) - ) - counts: dict[str, int] = {} - for source in (tag_rejections, group_rejections): - values = ( - source.get(view_name) - if isinstance(source, Mapping) - else None - ) - if isinstance(values, Mapping): - for name, count in values.items(): - counts[str(name)] = counts.get(str(name), 0) + int( - count - ) - if counts: - common = sorted( - counts.items(), key=lambda pair: (-pair[1], pair[0]) - )[:3] - parts.append( - "累计拒绝=" - + ",".join( - f"{name}×{count}" for name, count in common - ) - ) - view_candidates = ( - candidate_diagnostics.get(view_name, {}) - if isinstance(candidate_diagnostics, Mapping) - else {} - ) - if isinstance(view_candidates, Mapping) and missing: - summaries: list[str] = [] - for role in missing: - diagnostic = view_candidates.get(str(role), {}) - if not isinstance(diagnostic, Mapping): - continue - summaries.append( - f"{role}(solve=" - f"{int(diagnostic.get('solved_candidate_count', 0))}," - "reproj=" - f"{int(diagnostic.get('reprojection_candidate_count', 0))}," - "tilt=" - f"{int(diagnostic.get('independent_tilt_candidate_count', 0))})" - ) - if summaries: - parts.append("候选统计=" + ",".join(summaries)) - view_details.append( - f"{VIEW_NAMES_ZH.get(view_name, view_name)}=" - + "；".join(parts) - ) - reason_text = "、".join(view_details) - return ( - f"{detail_prefix}已经取得部分有效轨迹，但Tag仍可见且反馈正常时，" - "后续连续图像帧" - "被整组PnP几何检查拒绝" - f"（{reason_text}），因此无法与电机状态形成有效轨迹帧。", - "不要调整或反复粘贴Tag；保留当前会话中的" - "group_pnp_candidate_event，" - "按缺失角色的候选统计检查PnP分支逻辑。", - ) - return ( - f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态" - "按时间戳配对”的图像帧。", - "查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，" - "程序会重扫当前方向。", - ) - if reason == "task_precheck_missing_command_127": - return ( - f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。", - "检查中位姿态的Tag遮挡和反光；程序只会重扫当前物理任务。", - ) - if reason == "task_precheck_detection_rate_too_low": - return ( - f"{detail_prefix}低速预检的有效Tag识别率低于门限。", - "检查该机位当前任务Tag的遮挡、反光和对焦；程序只会重扫当前物理任务。", - ) - if reason == "sweep_start_position_timeout": - return ( - f"电机{active.get('motor_index')}未在规定时间到达扫描起点" - f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。", - "检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。", - ) - if reason == "sweep_start_tag_timeout": - group_reasons = active.get("group_pnp_reasons", {}) - progress_by_view = active.get("pnp_initialization_progress", {}) - tag_rejections = active.get("pnp_rejection_counts", {}) - group_rejections = active.get("group_pnp_rejection_counts", {}) - if any( - isinstance(value, Mapping) and bool(value) - for value in ( - group_reasons, - progress_by_view, - tag_rejections, - group_rejections, - ) - ): - details: list[str] = [] - views = set() - for value in ( - group_reasons, - progress_by_view, - tag_rejections, - group_rejections, - ): - if isinstance(value, Mapping): - views.update(str(view) for view in value) - for view in sorted(views): - parts: list[str] = [] - progress = ( - progress_by_view.get(view) - if isinstance(progress_by_view, Mapping) - else None - ) - if isinstance(progress, Mapping): - parts.append( - "初始化" - f"{int(progress.get('accepted', 0))}/" - f"{int(progress.get('required', 0))}" - ) - counts: dict[str, int] = {} - for source in (tag_rejections, group_rejections): - values = ( - source.get(view) - if isinstance(source, Mapping) - else None - ) - if isinstance(values, Mapping): - for name, count in values.items(): - counts[str(name)] = ( - counts.get(str(name), 0) + int(count) - ) - if counts: - common = sorted( - counts.items(), key=lambda pair: (-pair[1], pair[0]) - )[:3] - parts.append( - "累计拒绝=" - + ",".join( - f"{name}×{count}" for name, count in common - ) - ) - latest = ( - group_reasons.get(view) - if isinstance(group_reasons, Mapping) - else None - ) - if latest and not str(latest).startswith( - "group_initializing:" - ): - parts.append(f"最后状态={latest}") - if parts: - details.append( - f"{VIEW_NAMES_ZH.get(view, view)}=" + "；".join(parts) - ) - reason_text = "、".join(details) or "未形成完整初始化窗口" - return ( - "被测电机已经到达扫描起点，所需Tag也可见，但三维PnP位姿初始化" - f"没有完成（{reason_text}），因此没有生成同步端点帧。", - "不要根据可见性重复粘贴Tag；保留累计拒绝原因并检查PnP候选选择。", - ) - return ( - "被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的" - "同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”，不会触发此错误。", - "只检查标记为✗的实时运动Tag、反光和外部遮挡；不要移动相机或手掌底座。", - ) - if reason == "sweep_timeout": - return ( - "当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。", - "检查电机实际值、Tag连续识别和标定速度，修正后调用resume。", - ) - if reason == "return_baseline_timeout": - return ( - "一个或多个标定电机未在规定时间返回基准命令。", - "检查机械手状态、CAN和机械卡阻，确认安全后调用resume。", - ) - if reason == "validation_move_timeout": - return ( - "随机复测时电机未在规定时间到达目标命令。", - "检查机械手状态和机械卡阻，确认安全后调用resume。", - ) - if reason == "validation_capture_timeout": - return ( - "随机复测位置没有采集到足够的同步有效Tag帧。", - "检查当前机位Tag可见性后调用resume。", - ) - if reason == "palm_orientation_quality_failed": - failures = active.get("failures", []) - detail = ( - str(failures[0].get("reason", "方向观测不足")) - if failures - else "方向观测不足" - ) - if "thumb_cmc_" in detail: - return ( - "拇指CMC yaw无法由顶部Tag 8/9的零位邻近短轨迹稳定确定：" - + detail, - "保持Tag安装不变；确保顶部Tag 8/9在拇指CMC pitch和roll" - "从零位开始的前1/4行程持续可见后重新标定。", - ) - return ( - "掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定：" - + detail, - "保持Tag安装不变；让正面Tag 10–13在对应MCP-pitch起始段" - "至少可见15°行程后重新标定。", - ) - if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}: - metric_names = { - "plane_rms_mm": "平面拟合RMS", - "radial_rms_mm": "圆半径拟合RMS", - "radius_mm": "拟合半径", - "image_radial_rms_px": "二维圆半径拟合RMS", - "image_radial_p95_px": "二维圆半径误差P95", - "image_radius_px": "二维拟合半径", - "arc_deg": "实测圆弧", - "monotonic_correction_deg": "最大单调修正", - "hysteresis_deg": "最大正反程差", - "baseline_hysteresis_deg": "baseline正反程关节角差", - "baseline_directional_gap_deg": "baseline方向分支间隙", - "baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差", - "cycle_travel_range_deg": "三轮行程差", - "rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS", - "axis_plane_rms_mm": "三维圆轴向RMS", - "axis_radial_rms_mm": "三维圆半径RMS", - "axis_pose_line_rms_mm": "姿态轨迹轴线RMS", - "axis_line_cycle_rms_mm": "四轮轴线位置RMS", - "rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角", - "axis_cycle_difference_deg": "各轮转轴方向极差", - "cross_view_roll_curve": "正面/侧面关节角曲线差异RMS", - "third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差", - "third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS", - "third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95", - "zero_cycle_offset_range_deg": "训练轮零位极差", - "zero_confidence_95_half_width_deg": "零位95%置信半宽", - "state_image_sync_p95_ms": "图像与电机状态同步误差P95", - "tag_valid_rate_percent": "所需Tag同时有效率", - } - metric_units = { - "plane_rms_mm": "mm", - "radial_rms_mm": "mm", - "radius_mm": "mm", - "image_radial_rms_px": "px", - "image_radial_p95_px": "px", - "image_radius_px": "px", - "arc_deg": "°", - "monotonic_correction_deg": "°", - "hysteresis_deg": "°", - "baseline_hysteresis_deg": "°", - "baseline_directional_gap_deg": "°", - "baseline_directional_gap_range_deg": "°", - "cycle_travel_range_deg": "°", - "rotation_orthogonal_rms_deg": "°", - "axis_plane_rms_mm": "mm", - "axis_radial_rms_mm": "mm", - "axis_pose_line_rms_mm": "mm", - "axis_line_cycle_rms_mm": "mm", - "rotation_circle_axis_difference_deg": "°", - "axis_cycle_difference_deg": "°", - "third_cycle_axis_holdout_deg": "°", - "third_cycle_axis_line_rms_mm": "mm", - "third_cycle_trajectory_p95_deg": "°", - "zero_cycle_offset_range_deg": "°", - "zero_confidence_95_half_width_deg": "°", - "state_image_sync_p95_ms": "ms", - "tag_valid_rate_percent": "%", - "cross_view_roll_curve": "°", - } - details: list[str] = [] - for failure in active.get("failures", []): - joint = JOINT_NAMES_ZH.get( - str(failure.get("joint")), str(failure.get("joint")) - ) - metric = str(failure.get("metric", "")) - if metric in metric_names: - comparison = str(failure.get("comparison", "")) - requirement = "不超过" if comparison == "maximum" else "至少" - unit = metric_units[metric] - detail = ( - f"{joint}的{metric_names[metric]}为" - f"{float(failure.get('actual', 0.0)):.2f}{unit}，" - f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}" - ) - cycle_travel = failure.get("cycle_travel_deg", []) - if cycle_travel: - detail += "（各轮=" + "/".join( - f"{float(value):.2f}°" for value in cycle_travel - ) + "）" - cycle_values = failure.get("cycle_values_deg", []) - if not cycle_values: - cycle_values = failure.get("cycle_offset_deg", []) - if cycle_values: - detail += "（各轮=" + "/".join( - f"{float(value):.2f}°" for value in cycle_values - ) + "）" - details.append(detail) - else: - cycle = failure.get("cycle") - cycle_text = "" if cycle is None else f"第{cycle}轮" - details.append( - f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败：" - f"{failure.get('reason', '未知原因')}" - ) - detail_text = "；".join(details) or "当前关节的轨迹拟合未通过" - directional_gap_failure = any( - str(failure.get("metric", "")).startswith( - "baseline_directional_gap" - ) - for failure in active.get("failures", []) - ) - if reason == "joint_fit_systematic_failure": - cross_view_systematic = any( - failure.get("classification") - in { - "stable_cross_view_installation_or_model_bias", - "stable_cross_view_direction_conflict", - } - for failure in active.get("failures", []) - ) - suggestion = ( - "四轮都出现稳定的正面/侧面差异，属于Tag安装外参或跨视角模型偏差，" - "继续重扫不会改善；检查Tag刚性安装与跨视角安装变换，不要放宽门限。" - if cross_view_systematic - else "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。" - "请直接复制诊断块给开发者，不要放宽门限。" - ) - else: - source_task_names = set(active.get("source_task_names", [])) - thumb_yaw_source_retry = source_task_names == { - "thumb_cmc_pitch_front", - "thumb_cmc_roll_front", - } - suggestion = ( - "方向分支已由软件保留，不要放宽门限；请检查传动回差或高支架刚度，" - "处理后重新执行一键标定命令，程序会从最近可靠断点继续。" - if directional_gap_failure - else ( - "保持顶部Tag 8/9无遮挡；程序只替换决定yaw零位的" - "CMC pitch/roll顶部轴观测并重扫" - f"{active.get('directions_to_rescan', 16)}个方向，" - "不会无效重扫yaw侧摆。" - if thumb_yaw_source_retry - else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令；" - "程序只清除当前失败关节的数据并重扫" - f"{active.get('directions_to_rescan', 6)}个方向，" - "不需要手工调用ROS服务。" - ) - ) - return ( - detail_text + "。程序已在当前关节结束后立即停止后续步骤。", - suggestion, - ) - if reason == "zero_model_validation_failed": - reason_names = { - "zero_offset_reached_configured_bound": "零位解触及安全边界", - "zero_offset_exceeds_configured_limit": "零位估计超过安全范围", - "zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界", - "zero_offset_cycle_difference_too_large": "三轮零位离散过大", - "zero_offset_not_statistically_significant": "零位偏移未达到统计显著性", - "zero_axis_cone_mismatch_too_large": ( - "父子轴夹角与原始URDF不一致，零位旋转无法解释" - ), - "zero_phase_axis_line_residual_too_large": ( - "整段SE(3)运动无法稳定确定平行轴线相位" - ), - "zero_offset_did_not_improve_with_95pct_confidence": ( - "第三轮留出验证未以95%置信度改善" - ), - } - details: list[str] = [] - for failure in active.get("failures", []): - joint = JOINT_NAMES_ZH.get( - str(failure.get("joint")), str(failure.get("joint")) - ) - if failure.get("metric") == "zero_guard": - reason_text = reason_names.get( - str(failure.get("reason")), str(failure.get("reason")) - ) - if "actual_deg" in failure and "limit_deg" in failure: - reason_text += ( - f"（估计{float(failure['actual_deg']):+.2f}°，" - f"允许±{float(failure['limit_deg']):.2f}°）" - ) - details.append(f"{joint}：{reason_text}") - return ( - "轨迹采集已完成，但零位/URDF几何验证失败" - + ("（" + "；".join(details) + "）" if details else "") - + "。程序没有生成正式JSON或修正URDF。", - "该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；" - "请检查Tag固定、相机外参和原始URDF后重新启动新标定。", - ) - if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}: - return ( - "正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪；此阶段不以Tag可见性阻止基准恢复。", - "保持机械手运动范围无障碍；设备就绪后系统会先安全恢复基准形态，再检查掌部Tag。", - ) - if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}: - return ( - "正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。", - "根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。", - ) - if reason == "waiting_for_baseline_tags_after_recovery": - return ( - "机械手已经稳定恢复到基准形态，正在用新采集的画面确认三台相机各自的固定掌部Tag。", - "若某个掌部Tag持续缺失，只调整遮挡手指或检查Tag固定情况，不要移动相机和手掌底座。", - ) - if reason == "locking_fixed_base_references": - return ( - "基准形态Tag预检已通过，正在把三台相机的固定掌部Tag稳健锁定为本会话参考。", - "无需操作；锁定完成后允许任务姿态遮挡固定掌部Tag。", - ) - if reason == "fixed_base_reference_moved": - return ( - "顶部Tag 8在本会话基准锁定后连续多帧发生角点位移；程序已立即保持机械手当前位置，" - "本会话中已采集数据不再用于发布。", - "Tag 8允许在下一次标定预检前重新摆放，但本次不能继续；固定Tag 8和顶部相机后" - "重新启动新会话。", - ) - if reason == "waiting_for_task_tags_at_sweep_start": - return ( - "电机已到扫描起点，正在等待当前任务的实时运动Tag；显示为“锁”的固定掌部Tag" - "允许被手指遮挡。", - "只检查标记为✗的实时运动Tag；若均为✓或锁，程序会自动开始运动。", - ) - if reason == "call_start_for_baseline_recovery": - return ( - "相机数据和机械手反馈已就绪，等待一键程序触发安全基准恢复。", - "保持机械手运动范围无障碍；程序会自动开始，无需手工调用ROS服务。", - ) - if reason == "call_start": - return ( - "三机位预检已经通过，等待操作员确认开始。", - "清空机械手运动范围后调用/g20_calibration/start。", - ) - if reason == "operator_pause": - return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。" - if reason == "operator_abort": - return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。" - if reason == "collecting_timestamp_synchronised_tag_centres": - return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。" - if reason == "collecting_dedicated_baseline_hold": - return ( - "正在从当前方向到达关节baseline并静止采集Tag与电机反馈；这批数据单独用于回差验收。", - "无需操作，保持相机、标签和底座不动。", - ) - if reason == "steady checkpoint target is missing": - return ( - "首轮稳态检查点已经到达最终端点，但采集状态没有及时切换到端点完成阶段。", - "程序已停止发布并保留已采样数据；这是软件状态切换问题，不需要调整相机、Tag或机械手。", - ) - if reason == "cross_view_roll_front_failure_deferred": - return ( - "正面侧摆回差不合格已保留，诊断模式将继续采集同一手指的侧面数据。", - "无需操作；该诊断会锁定URDF发布。", - ) - if reason == "cross_view_roll_diagnostic_complete": - interpretation = str(active.get("interpretation", "")) - explanations = { - "both_views_confirm_direction_dependent_pose": ( - "正面和侧面都确认了方向相关姿态，优先判断为roll输出机构或共同下游链的真实回差。" - ), - "front_only_difference_check_roll_tag_bracket_or_front_pnp": ( - "只有正面差异超限，优先检查roll Tag高支架刚度和正面PnP。" - ), - "side_only_difference_check_side_tag_chain_or_side_pnp": ( - "只有侧面差异超限，优先检查侧面Tag链和侧面PnP。" - ), - "both_views_within_formal_hysteresis_limit": ( - "两个机位的静止回差均满足正式门限。" - ), - } - return ( - explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。") - + " 本次为诊断会话，不会生成或发布URDF。", - "保存当前状态和raw_samples.jsonl；根据两机位结论处理后重新启动正式标定。", - ) - if reason == "capturing_random_validation_pose": - return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。" - if reason in {"calibration_passed", "calibration_complete"}: - return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。" - if reason == "quality_failed": - return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。" - if reason.startswith("validated_endpoint_zero_state"): - return ( - "轨迹和URDF零位验证已经通过，但发布前检测到端点零位状态缺失或与" - "已验证模型不一致；这是程序内部状态生命周期错误，结果未发布。", - "不要移动相机、Tag或机械手底座；保留当前会话并把原因码交给开发者。", - ) - if reason.startswith("PUB-ARTIFACT-601:"): - return ( - "标定节点已经生成通过质量门限的JSON和候选URDF，但一键程序在正式发布前" - "发现这对产物的坐标、限位、哈希或资源一致性检查失败；原始URDF未被覆盖。", - "不要重新标定相机或调整Tag；保留本会话产物和启动日志供开发者检查发布契约。", - ) - if reason == "combination_pose_prediction_failed": - return ( - "单关节、零位和URDF几何验证已通过，但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。", - "程序会在原姿态重新初始化PnP并自动复测；若最终仍失败，请把raw_samples.jsonl中的" - "combination_validation_failure记录交给开发者，不要重新采集16个单关节任务。", - ) - if reason.startswith("prepare_") or state == "PREPARE_SWEEP": - return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。" - if reason == "holding_same_finger_clearance_before_next_task": - return ( - "同一根手指的上一项已经完成；相邻手指继续保持当前避让姿态，只调整" - "被测关节以衔接下一项。", - "无需操作，不要手动展开正在避让的手指。", - ) - if state == "RETURN_BASELINE": - return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。" - if state == "FITTING": - return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。" - return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。" - - -def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str: - """Render the complete operator status; the JSON topic remains unchanged.""" - state = str(payload.get("state", "")) - active = payload.get("active", {}) - reason_zh, action_zh = three_camera_reason_zh( - state, str(payload.get("reason", "")), active - ) - progress = float(payload.get("progress", 0.0)) - completed = payload.get("completed_sweeps", 0) - total = payload.get("total_sweeps", 0) - scan_progress = float( - payload.get( - "scan_progress", - 0.0 if not total else float(completed) / float(total), - ) - ) - lines = [ - f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）", - f"原因：{reason_zh}", - f"建议：{action_zh}", - f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，" - f"扫描进度{scan_progress:.1%}）", - f"当前任务：{_task_text(active)}", - ] - if state == "RETURN_BASELINE": - baseline_command = payload.get("baseline_command_u8", []) - return_command = payload.get("return_command_u8", baseline_command) - label = "恢复姿态" if return_command != baseline_command else "基准姿态" - lines.append(f"正在确认{label}：{return_command}") - if active and active.get("kind") not in { - "fit_failure", - "zero_model_failure", - "motion_stall", - }: - retry_count = int(active.get("automatic_retry_count", 0)) - if retry_count: - lines.append( - "自动重试：当前方向已自动重扫" - f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，" - f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，" - f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s" - ) - sample = active.get("sample", {}) - motion_progress = active.get("motion_progress") - motion_text = ( - "未知" if motion_progress is None else f"{float(motion_progress):.1%}" - ) - lines.append( - "运动采样：" - f"目标{active.get('target_u8', active.get('command_u8', '?'))}，" - f"实际{_format_u8(active.get('actual_u8'))}，" - f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，" - f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～" - f"{_format_u8(sample.get('maximum_u8'))}" - ) - detection_frames = int(active.get("detection_frames", 0)) - if detection_frames: - lines.append( - "本方向Tag检出：" - f"{float(active.get('detection_rate', 0.0)):.1%}" - f"（{active.get('detection_valid_frames', 0)}/" - f"{detection_frames}帧）" - ) - auxiliary = active.get("auxiliary_motors", []) - if auxiliary: - lines.append( - "避挡姿态：" - + "，".join( - f"电机{item.get('motor_index')}目标" - f"{item.get('command_u8')}、实际" - f"{_format_u8(item.get('actual_u8'))}" - for item in auxiliary - ) - ) - speed = active.get("speed", {}) - if speed: - lines.append( - "阶段速度：五指目标" - f"{speed.get('commanded_finger_speed')}，SDK报告" - f"{speed.get('reported_finger_speed')}" - ) - if active.get("sweep_timeout_seconds") is not None: - lines.append( - "运动保护：扫描超时" - f"{float(active['sweep_timeout_seconds']):.1f}s，" - "连续" - f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s" - "进展不足" - f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}" - "则立即暂停" - ) - lines.append("机位：") - for name, view in payload.get("views", {}).items(): - missing = view.get("missing_tag_ids", []) - missing_text = "无" if not missing else ",".join(map(str, missing)) - lines.append( - f"- {VIEW_NAMES_ZH.get(str(name), str(name))}：" - f"{'就绪' if view.get('ready') else '等待'}，" - f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'}，" - f"{float(view.get('detection_hz', 0.0)):.1f}Hz，" - f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，" - f"当前缺失Tag={missing_text}" - ) - extrinsics_error = payload.get("camera_extrinsics_error") - if extrinsics_error: - lines.append(f"外参文件：{extrinsics_error}") - lines.append(f"JSON结果：{payload.get('result_path') or '尚未生成'}") - lines.append( - f"修正URDF：{payload.get('corrected_urdf_path') or '尚未生成'}" - ) - return "\n".join(lines) +from .models.g20.reporting_zh import * # noqa: F401,F403 +from .models.g20.reporting_zh import _task_text diff --git a/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py b/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py index 6cda1c4..e7ffa2d 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/three_camera_node.py @@ -35,11 +35,11 @@ from .acquisition import ( update_pnp_reset_watchdog, ) from .core import ( - COMMAND_NAMES, DIRECTION_DECREASING, DIRECTION_INCREASING, robust_rotation_summary, ) +from .core.urdf import build_correction_plan from .extrinsics import ( ThreeCameraExtrinsics, camera_info_fingerprint, @@ -74,6 +74,7 @@ from .full_hand import ( 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, @@ -318,13 +319,19 @@ def _palm_axis_observer_schema( 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", [])} - if not profile.palm_axis_observers and capability not in profile.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(profile.capabilities) - {capability} + required_previous = set(current) - {capability} if not required_previous.issubset(previous): raise ValueError("resume checkpoint lacks required capabilities") if capability in previous: @@ -741,7 +748,7 @@ def _requires_pnp_tracker_reset_for_sweep( return False if is_fit_retry: return True - if profile.supports("precheck_sweeps"): + if profile.precheck_sweeps: return bool(item.precheck) return not item.precheck and item.cycle == 0 @@ -2015,7 +2022,7 @@ def _build_sweep_plan( """ plan: list[SweepItem] = [] for spec in profile.sweep_specs: - if profile.supports("precheck_sweeps"): + if profile.precheck_sweeps: plan.extend( SweepItem(spec, -1, direction, precheck=True) for direction in ( @@ -2708,6 +2715,7 @@ class G20ThreeCameraCalibrationNode(Node): ) 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") @@ -5226,7 +5234,19 @@ class G20ThreeCameraCalibrationNode(Node): ( palm_axis_schema_compatible, palm_axis_invalidated_tasks, - ) = _palm_axis_resume_policy(self.profile, start) + ) = _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" @@ -5646,7 +5666,19 @@ class G20ThreeCameraCalibrationNode(Node): self.recalibration_task_keys ), "command_names": list(_command_names(self)), - "capabilities": sorted(self.profile.capabilities), + # 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) @@ -8157,7 +8189,7 @@ class G20ThreeCameraCalibrationNode(Node): } ) continue - if profile.supports("steady_command_checkpoints"): + if profile.steady_command_checkpoints: command_store = getattr( self, "command_records_by_joint", None ) @@ -8438,7 +8470,7 @@ class G20ThreeCameraCalibrationNode(Node): "maximum", ), ) - if not profile.supports("directional_zero"): + if not profile.directional_zero: checks = checks + ( ( "hysteresis_deg", @@ -8789,7 +8821,7 @@ class G20ThreeCameraCalibrationNode(Node): if index != outlier ] failures.append(failure) - if profile.supports("cross_view_roll_curve"): + if profile.cross_view_roll_curve: for primary_name, validation_name in ( profile.axis_validation_sources or {} ).items(): @@ -11565,6 +11597,20 @@ class G20ThreeCameraCalibrationNode(Node): 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, @@ -11572,6 +11618,7 @@ class G20ThreeCameraCalibrationNode(Node): offsets_rad=urdf_offsets, endpoint_anchored_offsets_rad=endpoint_zero_offsets, timestamp=stamp, + correction_plan=correction_plan, ) try: if self.standalone_thumb_calibration: diff --git a/src/linkerhand_calibration/linkerhand_calibration/tools/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/tools/__init__.py new file mode 100644 index 0000000..1782dc0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/tools/__init__.py @@ -0,0 +1 @@ +"""Operator tools for extrinsics, alignment, and runtime bridges.""" diff --git a/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py b/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py index 911e751..e092521 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py +++ b/src/linkerhand_calibration/linkerhand_calibration/urdf_zero.py @@ -18,6 +18,7 @@ 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, @@ -3850,7 +3851,7 @@ def solve_urdf_zero_offsets( if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad: cone_range = float(np.ptp(cone_mismatches)) stable_product_bias = bool( - profile.hand.supports("stable_cross_view_cone_bias") + 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 @@ -4454,6 +4455,7 @@ def write_zero_corrected_urdf( 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() @@ -4475,6 +4477,15 @@ def write_zero_corrected_urdf( } 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() diff --git a/src/linkerhand_calibration/linkerhand_calibration/zero_node.py b/src/linkerhand_calibration/linkerhand_calibration/zero_node.py index ba3885b..07bdfd1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/zero_node.py +++ b/src/linkerhand_calibration/linkerhand_calibration/zero_node.py @@ -21,7 +21,11 @@ from sensor_msgs.msg import Image, JointState from std_msgs.msg import String from std_srvs.srv import Trigger -from .core import BASELINE_COMMAND, build_command, COMMAND_NAMES +from .compat.legacy.thumb_core import ( + BASELINE_COMMAND, + COMMAND_NAMES, + build_command, +) from .storage import atomic_write_json from .zero_calibration import ( build_trajectory_zero_angle_payload, diff --git a/src/linkerhand_calibration/setup.py b/src/linkerhand_calibration/setup.py index 55ffd34..83b6b88 100644 --- a/src/linkerhand_calibration/setup.py +++ b/src/linkerhand_calibration/setup.py @@ -66,11 +66,11 @@ setup( ), ( "calibrate_g20_right = " - "linkerhand_calibration.one_command:main" + "linkerhand_calibration.runtime.runner:main" ), ( "calibrate_hand = " - "linkerhand_calibration.one_command:main" + "linkerhand_calibration.runtime.runner:main" ), ], }, diff --git a/src/linkerhand_calibration/test/test_acquisition.py b/src/linkerhand_calibration/test/test_acquisition.py index c76b8b3..fa340d0 100644 --- a/src/linkerhand_calibration/test/test_acquisition.py +++ b/src/linkerhand_calibration/test/test_acquisition.py @@ -19,7 +19,7 @@ from linkerhand_calibration.acquisition import ( tag_quality_is_valid, update_pnp_reset_watchdog, ) -from linkerhand_calibration.core import PAIR_NAMES +from linkerhand_calibration.acquisition import PAIR_NAMES def test_pnp_watchdog_resets_after_one_continuous_invalid_second() -> None: diff --git a/src/linkerhand_calibration/test/test_architecture.py b/src/linkerhand_calibration/test/test_architecture.py new file mode 100644 index 0000000..5d264b0 --- /dev/null +++ b/src/linkerhand_calibration/test/test_architecture.py @@ -0,0 +1,218 @@ +from pathlib import Path +from types import SimpleNamespace + +from linkerhand_calibration.compat import product_profile_key +from linkerhand_calibration.core import ( + ArtifactPolicy, + CalibrationProfile, + CommandLayout, + MeasurementPolicy, + MeasurementSpec, + MotionPolicy, + ProfileKey, + QualityPolicy, + SampleRecord, + ScopePolicy, + TagSpec, + TaskSpec, + ViewSpec, + VisionRigSpec, + ZeroSolvePolicy, + validate_profile, +) +from linkerhand_calibration.core.solver import ( + SessionSolution, + TaskEvaluation, +) +from linkerhand_calibration.core.urdf import UrdfCorrectionPlan +from linkerhand_calibration.models import ( + EngineBindings, + ProfileRegistry, + RegisteredProfile, + get_default_registry, +) +from linkerhand_calibration.runtime import SessionController, SessionState + + +PACKAGE = Path(__file__).resolve().parents[1] / "linkerhand_calibration" + + +def _source_text(directory: Path) -> str: + return "\n".join( + path.read_text(encoding="utf-8") + for path in sorted(directory.rglob("*.py")) + ) + + +def _small_profile() -> CalibrationProfile: + active = frozenset({"axis_a", "axis_b"}) + return CalibrationProfile( + key=ProfileKey("TEST", "right", "two_view_three_command", 1), + namespace="/calibration_test", + command=CommandLayout( + names=("axis_a", "unused", "axis_b"), + baseline_u8=(255, 0, 127), + command_index_by_joint={"axis_a": 0, "axis_b": 2}, + disabled_indices=frozenset({1}), + ), + vision=VisionRigSpec( + views=( + ViewSpec( + "oblique", + (TagSpec("base_a", 40, fixed_reference=True),), + ), + ViewSpec("wrist", (TagSpec("moving_b", 41),)), + ), + common_frame="fixture", + extrinsic_reference_view="oblique", + ), + motion=MotionPolicy( + tasks=( + TaskSpec("scan_a", "oblique", 0, ("axis_a",)), + TaskSpec("scan_b", "wrist", 2, ("axis_b",)), + ) + ), + measurement=MeasurementPolicy( + measurements={ + "axis_a": MeasurementSpec( + "axis_a", "rotation", "oblique", "base_a", "base_a" + ), + "axis_b": MeasurementSpec( + "axis_b", "circle", "wrist", "moving_b", "moving_b" + ), + } + ), + zero=ZeroSolvePolicy( + active_joints=active, + passive_joints=frozenset(), + direct_zero_joints=("axis_a", "axis_b"), + axis_joints=("axis_a", "axis_b"), + mechanical_endpoint_joints=frozenset({"axis_b"}), + post_solve_endpoint_joints=frozenset(), + mimic_source_by_joint={}, + cad_frozen_joints=frozenset(), + ), + quality=QualityPolicy( + training_cycles=(0, 1), + holdout_cycle=2, + hard_threshold_keys=frozenset({"maximum_error"}), + ), + scope=ScopePolicy( + calibrate_joints={"full": active, "axis_a": frozenset({"axis_a"})}, + frozen_joints={ + "full": frozenset(), + "axis_a": frozenset({"axis_b"}), + }, + ), + artifacts=ArtifactPolicy( + output_schema_version=2, + calibration_filename="calibration.json", + corrected_urdf_filename="corrected.urdf", + protected_input_fields=frozenset({"source_urdf_sha256"}), + ), + ) + + +def _bindings() -> EngineBindings: + no_waypoints = lambda *args, **kwargs: () + return EngineBindings( + hand_profile=SimpleNamespace(), + zero_profile=SimpleNamespace(), + motion_command=lambda *args, **kwargs: [0, 0, 0], + preparation_waypoints=no_waypoints, + return_waypoints=no_waypoints, + cli_main=lambda args=None: None, + ) + + +def test_core_has_no_ros_or_model_dependency() -> None: + text = _source_text(PACKAGE / "core") + for forbidden in ("rclpy", "cv_bridge", " G20", " L6", "..models"): + assert forbidden not in text + + +def test_runtime_has_no_concrete_model_or_view_assumption() -> None: + text = _source_text(PACKAGE / "runtime") + for forbidden in ("G20", "L6", "RIGHT_19", '"front"', '"side"', '"top"'): + assert forbidden not in text + + +def test_every_registered_profile_passes_static_integrity_checks() -> None: + registry = get_default_registry() + assert len(registry) == 3 + for registered in registry: + validate_profile(registered.profile) + assert registered.profile.zero.active_joints + assert registered.profile.motion.tasks + assert registered.profile.vision.tag_ids + + +def test_registry_accepts_different_view_and_command_counts() -> None: + profile = _small_profile() + registry = ProfileRegistry() + registry.register(RegisteredProfile(profile, _bindings())) + + selected = registry.get(profile.key).profile + assert selected.command.command_count == 3 + assert selected.vision.view_names == ("oblique", "wrist") + + +def test_schema_v2_selects_profile_only_by_typed_profile_id() -> None: + key = product_profile_key( + { + "schema_version": 2, + "profile_id": "TEST/right/two_view_three_command/v3", + } + ) + assert key == ProfileKey("TEST", "right", "two_view_three_command", 3) + + +def test_controller_uses_one_evaluator_and_solver_contract() -> None: + profile = _small_profile() + + class Evaluator: + def evaluate_task(self, selected, task, samples): + assert selected is profile + return TaskEvaluation(accepted=True) + + class Solver: + def solve_session(self, selected, samples): + assert selected is profile + assert {row.task_key for row in samples} == {"scan_a", "scan_b"} + return SessionSolution(True, {"ok": True}, {"axis_a": 0.0}) + + controller = SessionController(profile, Evaluator(), Solver()) + controller.start() + controller.finish_preflight(passed=True) + for index, task in enumerate(profile.motion.tasks): + controller.task_pose_ready() + controller.submit_task_samples( + ( + SampleRecord( + task.key, + task.joints[0], + task.view, + 0, + "decreasing", + 255, + index, + {}, + ), + ) + ) + assert controller.state == SessionState.SOLVING + assert controller.solve().passed + controller.finish_release_validation(passed=True) + controller.finish_publication() + assert controller.state == SessionState.COMPLETE + + +def test_urdf_writer_and_validator_can_share_one_edit_plan() -> None: + plan = UrdfCorrectionPlan( + source_sha256="a" * 64, + allowed_active_joints=frozenset({"axis_a", "axis_b"}), + endpoint_limit_joints=frozenset({"axis_b"}), + mimic_source_by_joint={"passive_b": "axis_b"}, + frozen_joints=frozenset({"axis_c"}), + ) + plan.authorize_offsets({"axis_a": 0.0, "axis_b": 0.1}) diff --git a/src/linkerhand_calibration/test/test_core.py b/src/linkerhand_calibration/test/test_core.py index 0eedd42..056a1ef 100644 --- a/src/linkerhand_calibration/test/test_core.py +++ b/src/linkerhand_calibration/test/test_core.py @@ -6,7 +6,7 @@ import numpy as np import pytest from scipy.spatial.transform import Rotation -from linkerhand_calibration.core import ( +from linkerhand_calibration.compat.legacy.thumb_core import ( BASELINE_COMMAND, DIRECTION_DECREASING, DIRECTION_INCREASING, diff --git a/src/linkerhand_calibration/test/test_g20_right_product.py b/src/linkerhand_calibration/test/test_g20_right_product.py index 1c8ec6b..42b2a1b 100644 --- a/src/linkerhand_calibration/test/test_g20_right_product.py +++ b/src/linkerhand_calibration/test/test_g20_right_product.py @@ -28,7 +28,7 @@ from linkerhand_calibration.operator_report import ( classify_error, render_progress_zh, ) -from linkerhand_calibration.one_command import ( +from linkerhand_calibration.models.g20.runner import ( _automatic_resume_candidate, _calibration_node_exited_before_status, _launch_command, diff --git a/src/linkerhand_calibration/test/test_package_rename.py b/src/linkerhand_calibration/test/test_package_rename.py index 8d39fbf..2c9c551 100644 --- a/src/linkerhand_calibration/test/test_package_rename.py +++ b/src/linkerhand_calibration/test/test_package_rename.py @@ -21,6 +21,6 @@ def test_new_and_legacy_executable_names_share_one_implementation() -> None: for executable in ("calibrate_hand", "calibrate_g20_right"): assert re.search( rf'"{executable} = "\s*' - r'"linkerhand_calibration\.one_command:main"', + r'"linkerhand_calibration\.runtime\.runner:main"', setup_text, ) diff --git a/src/linkerhand_calibration/test/test_trajectory.py b/src/linkerhand_calibration/test/test_trajectory.py index 702e54c..ad40ed3 100644 --- a/src/linkerhand_calibration/test/test_trajectory.py +++ b/src/linkerhand_calibration/test/test_trajectory.py @@ -7,11 +7,13 @@ from scipy.spatial.transform import Rotation from linkerhand_calibration.core import ( DIRECTION_DECREASING, DIRECTION_INCREASING, + PHASE_ROOT, + PHASE_TIP, +) +from linkerhand_calibration.compat.legacy.thumb_core import ( PAIR_IP, PAIR_MCP, PAIR_ROOT, - PHASE_ROOT, - PHASE_TIP, create_final_payload, ) from linkerhand_calibration.trajectory import (