refactor: establish reusable calibration architecture
This commit is contained in:
@@ -11,6 +11,29 @@ ros2 run linkerhand_calibration calibrate_hand
|
|||||||
旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名;
|
旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名;
|
||||||
旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。
|
旧 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项任务时,使用:
|
完全独立地只标定大拇指4项任务时,使用:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
|
|||||||
@@ -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"]
|
||||||
|
|||||||
@@ -9,7 +9,7 @@ from typing import Any, Mapping, Sequence
|
|||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
from .core import PAIR_NAMES, robust_rotation_summary
|
from .core import robust_rotation_summary
|
||||||
from .pnp import SquareTagPose
|
from .pnp import SquareTagPose
|
||||||
|
|
||||||
|
|
||||||
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
|
|||||||
"t3_t4": ("t3", "t4"),
|
"t3_t4": ("t3", "t4"),
|
||||||
"t4_t5": ("t4", "t5"),
|
"t4_t5": ("t4", "t5"),
|
||||||
}
|
}
|
||||||
|
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
|
|||||||
@@ -1,5 +1,11 @@
|
|||||||
"""Compatibility adapters for one-release calibration migrations."""
|
"""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
|
from .paths import resolve_renamed_package_path
|
||||||
|
|
||||||
__all__ = ["resolve_renamed_package_path"]
|
__all__ = [
|
||||||
|
"default_product_config_path",
|
||||||
|
"product_profile_key",
|
||||||
|
"resolve_renamed_package_path",
|
||||||
|
]
|
||||||
|
|||||||
@@ -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)
|
||||||
@@ -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"
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Legacy single-camera algorithms retained for one compatibility release."""
|
||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""Artifact schema and release validation contracts."""
|
||||||
|
|
||||||
|
from .release import ReleaseValidation, ReleaseValidator
|
||||||
|
|
||||||
|
__all__ = ["ReleaseValidation", "ReleaseValidator"]
|
||||||
@@ -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: ...
|
||||||
@@ -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
|
||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -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))
|
||||||
@@ -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")
|
||||||
@@ -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
|
||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""Pure curve and axis fitting."""
|
||||||
|
|
||||||
|
from .curve import FitResult, isotonic_nonincreasing
|
||||||
|
|
||||||
|
__all__ = ["FitResult", "isotonic_nonincreasing"]
|
||||||
@@ -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)
|
||||||
@@ -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",
|
||||||
|
]
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -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)
|
||||||
@@ -0,0 +1,15 @@
|
|||||||
|
"""Task acceptance and final-session solver contracts."""
|
||||||
|
|
||||||
|
from .interfaces import (
|
||||||
|
SessionSolution,
|
||||||
|
SessionSolver,
|
||||||
|
TaskEvaluation,
|
||||||
|
TaskEvaluator,
|
||||||
|
)
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
"SessionSolution",
|
||||||
|
"SessionSolver",
|
||||||
|
"TaskEvaluation",
|
||||||
|
"TaskEvaluator",
|
||||||
|
]
|
||||||
@@ -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: ...
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""URDF correction authorization and validation types."""
|
||||||
|
|
||||||
|
from .plan import UrdfCorrectionPlan, build_correction_plan
|
||||||
|
|
||||||
|
__all__ = ["UrdfCorrectionPlan", "build_correction_plan"]
|
||||||
@@ -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,
|
||||||
|
)
|
||||||
@@ -17,7 +17,7 @@ import xml.etree.ElementTree as ET
|
|||||||
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
from .core import COMMAND_NAMES
|
from .models.g20.command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
|
||||||
from .trajectory import (
|
from .trajectory import (
|
||||||
_angle_for_circle,
|
_angle_for_circle,
|
||||||
_fit_circle_with_axis,
|
_fit_circle_with_axis,
|
||||||
@@ -129,6 +129,12 @@ class HandCalibrationProfile:
|
|||||||
command_names: tuple[str, ...] = COMMAND_NAMES
|
command_names: tuple[str, ...] = COMMAND_NAMES
|
||||||
baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND
|
baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND
|
||||||
capabilities: frozenset[str] = frozenset()
|
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
|
@property
|
||||||
def command_count(self) -> int:
|
def command_count(self) -> int:
|
||||||
@@ -703,6 +709,12 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
|||||||
"palm_axis_relative_motion_v3",
|
"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,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -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"]
|
||||||
@@ -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,
|
||||||
|
),
|
||||||
|
)
|
||||||
@@ -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"]
|
||||||
@@ -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"]
|
||||||
@@ -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
|
||||||
|
),
|
||||||
|
)
|
||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -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)
|
||||||
@@ -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,
|
||||||
|
)
|
||||||
@@ -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()
|
||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Future profiles live here; no publishable profile is registered yet."""
|
||||||
@@ -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
|
||||||
@@ -36,7 +36,7 @@ from .acquisition import (
|
|||||||
interpolate_state_u8,
|
interpolate_state_u8,
|
||||||
tag_quality_is_valid,
|
tag_quality_is_valid,
|
||||||
)
|
)
|
||||||
from .core import (
|
from .compat.legacy.thumb_core import (
|
||||||
BASELINE_COMMAND,
|
BASELINE_COMMAND,
|
||||||
COMMAND_NAMES,
|
COMMAND_NAMES,
|
||||||
DIRECTION_DECREASING,
|
DIRECTION_DECREASING,
|
||||||
|
|||||||
@@ -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
|
__all__ = ["main"]
|
||||||
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__":
|
if __name__ == "__main__":
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -13,115 +13,60 @@ import hashlib
|
|||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
import re
|
import re
|
||||||
from typing import Any, Mapping
|
from typing import Any, Mapping
|
||||||
|
import xml.etree.ElementTree as ET
|
||||||
|
|
||||||
import yaml
|
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 .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
|
||||||
from .full_hand import (
|
from .models import RegisteredProfile, get_default_registry
|
||||||
G20_RIGHT_19_LAYOUT,
|
|
||||||
HandCalibrationProfile,
|
|
||||||
get_hand_calibration_profile,
|
|
||||||
)
|
|
||||||
from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile
|
|
||||||
|
|
||||||
|
|
||||||
VIEWS = ("front", "side", "top")
|
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class ProductCalibrationContract:
|
class ProductCalibrationContract:
|
||||||
"""Declarative boundary between one hand model and the shared engine."""
|
"""Compatibility facade over one typed, locally registered profile."""
|
||||||
|
|
||||||
model: str
|
registered: RegisteredProfile
|
||||||
side: str
|
|
||||||
layout_id: str
|
@property
|
||||||
profile: HandCalibrationProfile
|
def model(self) -> str:
|
||||||
zero_profile: ZeroCalibrationProfile
|
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
|
@property
|
||||||
def required_tag_ids(self) -> frozenset[int]:
|
def required_tag_ids(self) -> frozenset[int]:
|
||||||
return frozenset(
|
return self.typed_profile.vision.tag_ids
|
||||||
int(tag_id)
|
|
||||||
for tags in self.profile.view_tags.values()
|
|
||||||
for tag_id in tags.values()
|
|
||||||
)
|
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def views(self) -> tuple[str, ...]:
|
def views(self) -> tuple[str, ...]:
|
||||||
return tuple(self.profile.view_tags)
|
return self.typed_profile.vision.view_names
|
||||||
|
|
||||||
|
|
||||||
_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
|
|
||||||
|
|
||||||
|
|
||||||
def get_product_calibration_contract(
|
def get_product_calibration_contract(
|
||||||
model: str, side: str, layout_id: str
|
model: str, side: str, layout_id: str, revision: int = 1
|
||||||
) -> ProductCalibrationContract:
|
) -> ProductCalibrationContract:
|
||||||
key = (
|
return ProductCalibrationContract(
|
||||||
str(model).strip().upper(),
|
get_default_registry().resolve(model, side, layout_id, revision)
|
||||||
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"
|
|
||||||
),
|
|
||||||
)
|
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -206,10 +151,36 @@ def _custom_pnp_tag_sizes_m_by_id(
|
|||||||
return result
|
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)
|
@dataclass(frozen=True)
|
||||||
class ProductConfig:
|
class ProductConfig:
|
||||||
path: Path
|
path: Path
|
||||||
workspace: Path
|
workspace: Path
|
||||||
|
schema_version: int
|
||||||
|
profile_key: ProfileKey
|
||||||
model: str
|
model: str
|
||||||
side: str
|
side: str
|
||||||
tag_layout: str
|
tag_layout: str
|
||||||
@@ -246,23 +217,17 @@ def load_product_config(
|
|||||||
raise ValueError(f"product config does not exist: {source}")
|
raise ValueError(f"product config does not exist: {source}")
|
||||||
with source.open("r", encoding="utf-8") as stream:
|
with source.open("r", encoding="utf-8") as stream:
|
||||||
raw = _mapping(yaml.safe_load(stream), str(source))
|
raw = _mapping(yaml.safe_load(stream), str(source))
|
||||||
if int(raw.get("schema_version", -1)) != 1:
|
profile_key = product_profile_key(raw)
|
||||||
raise ValueError("product config schema_version must be 1")
|
|
||||||
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
|
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
|
||||||
serial = str(raw.get("serial_number", ""))
|
serial = str(raw.get("serial_number", ""))
|
||||||
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
||||||
raise ValueError("serial_number is invalid")
|
raise ValueError("serial_number is invalid")
|
||||||
model = str(raw.get("model", "")).strip().upper()
|
model = profile_key.model
|
||||||
side = str(raw.get("side", "")).strip().lower()
|
side = profile_key.side
|
||||||
# Keep the deployed G20 schema compatible while making the layout an
|
layout = profile_key.layout
|
||||||
# explicit product choice for all new configurations.
|
contract = get_product_calibration_contract(
|
||||||
layout = str(
|
model, side, layout, profile_key.revision
|
||||||
raw.get(
|
)
|
||||||
"tag_layout",
|
|
||||||
G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "",
|
|
||||||
)
|
|
||||||
).strip().lower()
|
|
||||||
contract = get_product_calibration_contract(model, side, layout)
|
|
||||||
can_interface = str(raw.get("can_interface", "")).strip()
|
can_interface = str(raw.get("can_interface", "")).strip()
|
||||||
if not can_interface:
|
if not can_interface:
|
||||||
raise ValueError("can_interface is required")
|
raise ValueError("can_interface is required")
|
||||||
@@ -315,6 +280,7 @@ def load_product_config(
|
|||||||
actual = sha256_file(candidate)
|
actual = sha256_file(candidate)
|
||||||
if actual != expected:
|
if actual != expected:
|
||||||
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
|
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)
|
expected_tag_ids = set(contract.required_tag_ids)
|
||||||
tag_sizes = _tag_sizes_m_by_id(tag_config)
|
tag_sizes = _tag_sizes_m_by_id(tag_config)
|
||||||
@@ -333,10 +299,6 @@ def load_product_config(
|
|||||||
|
|
||||||
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
||||||
required_views = tuple(contract.views)
|
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):
|
if set(camera_raw) != set(required_views):
|
||||||
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
||||||
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
||||||
@@ -380,6 +342,8 @@ def load_product_config(
|
|||||||
return ProductConfig(
|
return ProductConfig(
|
||||||
path=source,
|
path=source,
|
||||||
workspace=root,
|
workspace=root,
|
||||||
|
schema_version=int(raw["schema_version"]),
|
||||||
|
profile_key=profile_key,
|
||||||
model=model,
|
model=model,
|
||||||
side=side,
|
side=side,
|
||||||
tag_layout=layout,
|
tag_layout=layout,
|
||||||
|
|||||||
@@ -23,6 +23,7 @@ from .full_hand import (
|
|||||||
)
|
)
|
||||||
from .product import ProductConfig, sha256_file
|
from .product import ProductConfig, sha256_file
|
||||||
from .storage import atomic_write_json
|
from .storage import atomic_write_json
|
||||||
|
from .core.urdf import UrdfCorrectionPlan, build_correction_plan
|
||||||
from .urdf_zero import (
|
from .urdf_zero import (
|
||||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||||
get_zero_calibration_profile,
|
get_zero_calibration_profile,
|
||||||
@@ -202,6 +203,7 @@ def verify_corrected_urdf(
|
|||||||
*,
|
*,
|
||||||
expected_offsets_rad: Mapping[str, float] | None = None,
|
expected_offsets_rad: Mapping[str, float] | None = None,
|
||||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||||
|
correction_plan: UrdfCorrectionPlan | None = None,
|
||||||
) -> tuple[str, ...]:
|
) -> tuple[str, ...]:
|
||||||
"""Prove that only active-joint origin.rpy attributes changed.
|
"""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")
|
source_text = Path(source).read_text(encoding="utf-8")
|
||||||
corrected_text = Path(corrected).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)
|
before = _joint_blocks(source_text)
|
||||||
after = _joint_blocks(corrected_text)
|
after = _joint_blocks(corrected_text)
|
||||||
if set(before) != set(after):
|
if set(before) != set(after):
|
||||||
@@ -243,6 +252,12 @@ def verify_corrected_urdf(
|
|||||||
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
||||||
}
|
}
|
||||||
if endpoint_offsets:
|
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)
|
source_joints = _joint_elements(source)
|
||||||
corrected_joints = _joint_elements(corrected)
|
corrected_joints = _joint_elements(corrected)
|
||||||
for name, offset in endpoint_offsets.items():
|
for name, offset in endpoint_offsets.items():
|
||||||
@@ -745,11 +760,20 @@ def finalize_session_artifacts(
|
|||||||
name: offsets[name]
|
name: offsets[name]
|
||||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
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(
|
changed_joints = verify_corrected_urdf(
|
||||||
config.source_urdf,
|
config.source_urdf,
|
||||||
paths["urdf"],
|
paths["urdf"],
|
||||||
expected_offsets_rad=offsets,
|
expected_offsets_rad=offsets,
|
||||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||||
|
correction_plan=correction_plan,
|
||||||
)
|
)
|
||||||
if standalone_thumb:
|
if standalone_thumb:
|
||||||
clipped_runtime_joints = {}
|
clipped_runtime_joints = {}
|
||||||
|
|||||||
@@ -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",
|
||||||
|
]
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Camera, detector, and hand-SDK runtime adapters."""
|
||||||
@@ -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}")
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""ROS message/service shells are migrated here after controller extraction."""
|
||||||
@@ -0,0 +1,3 @@
|
|||||||
|
from .codes import CalibrationErrorCode
|
||||||
|
|
||||||
|
__all__ = ["CalibrationErrorCode"]
|
||||||
@@ -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"
|
||||||
@@ -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)
|
||||||
@@ -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
|
from .core.domain.sample_schema import * # noqa: F401,F403
|
||||||
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
|
|
||||||
|
|||||||
@@ -1,105 +1,3 @@
|
|||||||
"""Crash-safe session storage for hardware calibration."""
|
"""Compatibility import for crash-safe artifact storage."""
|
||||||
|
|
||||||
from __future__ import annotations
|
from .core.artifacts.storage import * # noqa: F401,F403
|
||||||
|
|
||||||
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
|
|
||||||
|
|||||||
@@ -1,893 +1,4 @@
|
|||||||
"""Chinese, operator-facing diagnostics for three-camera calibration."""
|
"""Compatibility import for the installed Chinese model diagnostics."""
|
||||||
|
|
||||||
from __future__ import annotations
|
from .models.g20.reporting_zh import * # noqa: F401,F403
|
||||||
|
from .models.g20.reporting_zh import _task_text
|
||||||
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)
|
|
||||||
|
|||||||
@@ -35,11 +35,11 @@ from .acquisition import (
|
|||||||
update_pnp_reset_watchdog,
|
update_pnp_reset_watchdog,
|
||||||
)
|
)
|
||||||
from .core import (
|
from .core import (
|
||||||
COMMAND_NAMES,
|
|
||||||
DIRECTION_DECREASING,
|
DIRECTION_DECREASING,
|
||||||
DIRECTION_INCREASING,
|
DIRECTION_INCREASING,
|
||||||
robust_rotation_summary,
|
robust_rotation_summary,
|
||||||
)
|
)
|
||||||
|
from .core.urdf import build_correction_plan
|
||||||
from .extrinsics import (
|
from .extrinsics import (
|
||||||
ThreeCameraExtrinsics,
|
ThreeCameraExtrinsics,
|
||||||
camera_info_fingerprint,
|
camera_info_fingerprint,
|
||||||
@@ -74,6 +74,7 @@ from .full_hand import (
|
|||||||
get_hand_calibration_profile,
|
get_hand_calibration_profile,
|
||||||
)
|
)
|
||||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
from .hikrobot_camera import configure_fastdds_large_image_transport
|
||||||
|
from .models.g20.command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
|
||||||
from .pnp import (
|
from .pnp import (
|
||||||
SquareTagGroupPoseTracker,
|
SquareTagGroupPoseTracker,
|
||||||
SquareTagPose,
|
SquareTagPose,
|
||||||
@@ -318,13 +319,19 @@ def _palm_axis_observer_schema(
|
|||||||
def _palm_axis_resume_policy(
|
def _palm_axis_resume_policy(
|
||||||
profile: HandCalibrationProfile,
|
profile: HandCalibrationProfile,
|
||||||
session_start: Mapping[str, Any],
|
session_start: Mapping[str, Any],
|
||||||
|
compatibility_tokens: frozenset[str] | None = None,
|
||||||
) -> tuple[bool, tuple[str, ...]]:
|
) -> tuple[bool, tuple[str, ...]]:
|
||||||
"""Validate optional palm-axis checkpoint data when a model uses it."""
|
"""Validate optional palm-axis checkpoint data when a model uses it."""
|
||||||
capability = "palm_axis_relative_motion_v3"
|
capability = "palm_axis_relative_motion_v3"
|
||||||
previous = {str(value) for value in session_start.get("capabilities", [])}
|
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, ()
|
return True, ()
|
||||||
required_previous = set(profile.capabilities) - {capability}
|
required_previous = set(current) - {capability}
|
||||||
if not required_previous.issubset(previous):
|
if not required_previous.issubset(previous):
|
||||||
raise ValueError("resume checkpoint lacks required capabilities")
|
raise ValueError("resume checkpoint lacks required capabilities")
|
||||||
if capability in previous:
|
if capability in previous:
|
||||||
@@ -741,7 +748,7 @@ def _requires_pnp_tracker_reset_for_sweep(
|
|||||||
return False
|
return False
|
||||||
if is_fit_retry:
|
if is_fit_retry:
|
||||||
return True
|
return True
|
||||||
if profile.supports("precheck_sweeps"):
|
if profile.precheck_sweeps:
|
||||||
return bool(item.precheck)
|
return bool(item.precheck)
|
||||||
return not item.precheck and item.cycle == 0
|
return not item.precheck and item.cycle == 0
|
||||||
|
|
||||||
@@ -2015,7 +2022,7 @@ def _build_sweep_plan(
|
|||||||
"""
|
"""
|
||||||
plan: list[SweepItem] = []
|
plan: list[SweepItem] = []
|
||||||
for spec in profile.sweep_specs:
|
for spec in profile.sweep_specs:
|
||||||
if profile.supports("precheck_sweeps"):
|
if profile.precheck_sweeps:
|
||||||
plan.extend(
|
plan.extend(
|
||||||
SweepItem(spec, -1, direction, precheck=True)
|
SweepItem(spec, -1, direction, precheck=True)
|
||||||
for direction in (
|
for direction in (
|
||||||
@@ -2708,6 +2715,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
)
|
)
|
||||||
self.profile = product_contract.profile
|
self.profile = product_contract.profile
|
||||||
self.zero_profile = product_contract.zero_profile
|
self.zero_profile = product_contract.zero_profile
|
||||||
|
self.calibration_profile = product_contract.typed_profile
|
||||||
self.serial_number = str(value("serial_number"))
|
self.serial_number = str(value("serial_number"))
|
||||||
if self.serial_number == "UNSET":
|
if self.serial_number == "UNSET":
|
||||||
raise ValueError("serial_number is required")
|
raise ValueError("serial_number is required")
|
||||||
@@ -5226,7 +5234,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
(
|
(
|
||||||
palm_axis_schema_compatible,
|
palm_axis_schema_compatible,
|
||||||
palm_axis_invalidated_tasks,
|
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:
|
except ValueError as error:
|
||||||
raise RuntimeError(
|
raise RuntimeError(
|
||||||
"resume checkpoint algorithm capabilities differ"
|
"resume checkpoint algorithm capabilities differ"
|
||||||
@@ -5646,7 +5666,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
self.recalibration_task_keys
|
self.recalibration_task_keys
|
||||||
),
|
),
|
||||||
"command_names": list(_command_names(self)),
|
"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,
|
"reference_finger": self.profile.reference_finger,
|
||||||
"view_tags": {
|
"view_tags": {
|
||||||
view: dict(tags)
|
view: dict(tags)
|
||||||
@@ -8157,7 +8189,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
}
|
}
|
||||||
)
|
)
|
||||||
continue
|
continue
|
||||||
if profile.supports("steady_command_checkpoints"):
|
if profile.steady_command_checkpoints:
|
||||||
command_store = getattr(
|
command_store = getattr(
|
||||||
self, "command_records_by_joint", None
|
self, "command_records_by_joint", None
|
||||||
)
|
)
|
||||||
@@ -8438,7 +8470,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
"maximum",
|
"maximum",
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
if not profile.supports("directional_zero"):
|
if not profile.directional_zero:
|
||||||
checks = checks + (
|
checks = checks + (
|
||||||
(
|
(
|
||||||
"hysteresis_deg",
|
"hysteresis_deg",
|
||||||
@@ -8789,7 +8821,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
if index != outlier
|
if index != outlier
|
||||||
]
|
]
|
||||||
failures.append(failure)
|
failures.append(failure)
|
||||||
if profile.supports("cross_view_roll_curve"):
|
if profile.cross_view_roll_curve:
|
||||||
for primary_name, validation_name in (
|
for primary_name, validation_name in (
|
||||||
profile.axis_validation_sources or {}
|
profile.axis_validation_sources or {}
|
||||||
).items():
|
).items():
|
||||||
@@ -11565,6 +11597,20 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
name: published_zero_offsets[name]
|
name: published_zero_offsets[name]
|
||||||
for name in self.zero_profile.direct_zero_joints
|
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(
|
self.corrected_urdf_path = write_zero_corrected_urdf(
|
||||||
source_urdf=self.source_urdf_path,
|
source_urdf=self.source_urdf_path,
|
||||||
output_directory=self.corrected_urdf_output_dir,
|
output_directory=self.corrected_urdf_output_dir,
|
||||||
@@ -11572,6 +11618,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
|||||||
offsets_rad=urdf_offsets,
|
offsets_rad=urdf_offsets,
|
||||||
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
|
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
|
||||||
timestamp=stamp,
|
timestamp=stamp,
|
||||||
|
correction_plan=correction_plan,
|
||||||
)
|
)
|
||||||
try:
|
try:
|
||||||
if self.standalone_thumb_calibration:
|
if self.standalone_thumb_calibration:
|
||||||
|
|||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Operator tools for extrinsics, alignment, and runtime bridges."""
|
||||||
@@ -18,6 +18,7 @@ from scipy.spatial.transform import Rotation
|
|||||||
from scipy.stats import t as student_t
|
from scipy.stats import t as student_t
|
||||||
|
|
||||||
from .core import fit_rotation_axis, robust_rotation_summary
|
from .core import fit_rotation_axis, robust_rotation_summary
|
||||||
|
from .core.urdf import UrdfCorrectionPlan
|
||||||
from .full_hand import (
|
from .full_hand import (
|
||||||
G20_RIGHT_19_LAYOUT,
|
G20_RIGHT_19_LAYOUT,
|
||||||
IMAGE_TRAJECTORY_JOINTS,
|
IMAGE_TRAJECTORY_JOINTS,
|
||||||
@@ -3850,7 +3851,7 @@ def solve_urdf_zero_offsets(
|
|||||||
if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad:
|
if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad:
|
||||||
cone_range = float(np.ptp(cone_mismatches))
|
cone_range = float(np.ptp(cone_mismatches))
|
||||||
stable_product_bias = bool(
|
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 maximum_systematic_axis_cone_bias_rad is not None
|
||||||
and len(cone_mismatches) >= 4
|
and len(cone_mismatches) >= 4
|
||||||
and maximum_cone_mismatch
|
and maximum_cone_mismatch
|
||||||
@@ -4454,6 +4455,7 @@ def write_zero_corrected_urdf(
|
|||||||
offsets_rad: Mapping[str, float],
|
offsets_rad: Mapping[str, float],
|
||||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||||
timestamp: str | None = None,
|
timestamp: str | None = None,
|
||||||
|
correction_plan: UrdfCorrectionPlan | None = None,
|
||||||
) -> Path:
|
) -> Path:
|
||||||
source = Path(source_urdf).expanduser().resolve()
|
source = Path(source_urdf).expanduser().resolve()
|
||||||
output = Path(output_directory).expanduser().resolve()
|
output = Path(output_directory).expanduser().resolve()
|
||||||
@@ -4475,6 +4477,15 @@ def write_zero_corrected_urdf(
|
|||||||
}
|
}
|
||||||
if not set(endpoint_offsets) <= set(offsets):
|
if not set(endpoint_offsets) <= set(offsets):
|
||||||
raise ValueError("endpoint-anchored offsets must be URDF zero targets")
|
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(
|
if any(
|
||||||
not math.isfinite(value) or abs(value) > math.radians(90.0)
|
not math.isfinite(value) or abs(value) > math.radians(90.0)
|
||||||
for value in offsets.values()
|
for value in offsets.values()
|
||||||
|
|||||||
@@ -21,7 +21,11 @@ from sensor_msgs.msg import Image, JointState
|
|||||||
from std_msgs.msg import String
|
from std_msgs.msg import String
|
||||||
from std_srvs.srv import Trigger
|
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 .storage import atomic_write_json
|
||||||
from .zero_calibration import (
|
from .zero_calibration import (
|
||||||
build_trajectory_zero_angle_payload,
|
build_trajectory_zero_angle_payload,
|
||||||
|
|||||||
@@ -66,11 +66,11 @@ setup(
|
|||||||
),
|
),
|
||||||
(
|
(
|
||||||
"calibrate_g20_right = "
|
"calibrate_g20_right = "
|
||||||
"linkerhand_calibration.one_command:main"
|
"linkerhand_calibration.runtime.runner:main"
|
||||||
),
|
),
|
||||||
(
|
(
|
||||||
"calibrate_hand = "
|
"calibrate_hand = "
|
||||||
"linkerhand_calibration.one_command:main"
|
"linkerhand_calibration.runtime.runner:main"
|
||||||
),
|
),
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
|
|||||||
@@ -19,7 +19,7 @@ from linkerhand_calibration.acquisition import (
|
|||||||
tag_quality_is_valid,
|
tag_quality_is_valid,
|
||||||
update_pnp_reset_watchdog,
|
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:
|
def test_pnp_watchdog_resets_after_one_continuous_invalid_second() -> None:
|
||||||
|
|||||||
@@ -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})
|
||||||
@@ -6,7 +6,7 @@ import numpy as np
|
|||||||
import pytest
|
import pytest
|
||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
from linkerhand_calibration.core import (
|
from linkerhand_calibration.compat.legacy.thumb_core import (
|
||||||
BASELINE_COMMAND,
|
BASELINE_COMMAND,
|
||||||
DIRECTION_DECREASING,
|
DIRECTION_DECREASING,
|
||||||
DIRECTION_INCREASING,
|
DIRECTION_INCREASING,
|
||||||
|
|||||||
@@ -28,7 +28,7 @@ from linkerhand_calibration.operator_report import (
|
|||||||
classify_error,
|
classify_error,
|
||||||
render_progress_zh,
|
render_progress_zh,
|
||||||
)
|
)
|
||||||
from linkerhand_calibration.one_command import (
|
from linkerhand_calibration.models.g20.runner import (
|
||||||
_automatic_resume_candidate,
|
_automatic_resume_candidate,
|
||||||
_calibration_node_exited_before_status,
|
_calibration_node_exited_before_status,
|
||||||
_launch_command,
|
_launch_command,
|
||||||
|
|||||||
@@ -21,6 +21,6 @@ def test_new_and_legacy_executable_names_share_one_implementation() -> None:
|
|||||||
for executable in ("calibrate_hand", "calibrate_g20_right"):
|
for executable in ("calibrate_hand", "calibrate_g20_right"):
|
||||||
assert re.search(
|
assert re.search(
|
||||||
rf'"{executable} = "\s*'
|
rf'"{executable} = "\s*'
|
||||||
r'"linkerhand_calibration\.one_command:main"',
|
r'"linkerhand_calibration\.runtime\.runner:main"',
|
||||||
setup_text,
|
setup_text,
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -7,11 +7,13 @@ from scipy.spatial.transform import Rotation
|
|||||||
from linkerhand_calibration.core import (
|
from linkerhand_calibration.core import (
|
||||||
DIRECTION_DECREASING,
|
DIRECTION_DECREASING,
|
||||||
DIRECTION_INCREASING,
|
DIRECTION_INCREASING,
|
||||||
|
PHASE_ROOT,
|
||||||
|
PHASE_TIP,
|
||||||
|
)
|
||||||
|
from linkerhand_calibration.compat.legacy.thumb_core import (
|
||||||
PAIR_IP,
|
PAIR_IP,
|
||||||
PAIR_MCP,
|
PAIR_MCP,
|
||||||
PAIR_ROOT,
|
PAIR_ROOT,
|
||||||
PHASE_ROOT,
|
|
||||||
PHASE_TIP,
|
|
||||||
create_final_payload,
|
create_final_payload,
|
||||||
)
|
)
|
||||||
from linkerhand_calibration.trajectory import (
|
from linkerhand_calibration.trajectory import (
|
||||||
|
|||||||
Reference in New Issue
Block a user