refactor: establish reusable calibration architecture
This commit is contained in:
@@ -11,6 +11,29 @@ ros2 run linkerhand_calibration calibrate_hand
|
||||
旧 executable `calibrate_g20_right` 在本发行版内保留为同一入口的别名;
|
||||
旧 ROS 包名前缀不再提供。新脚本和部署配置统一使用 `calibrate_hand`。
|
||||
|
||||
构建和正式调用统一为:
|
||||
|
||||
```bash
|
||||
colcon build --packages-select linkerhand_calibration
|
||||
ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml>
|
||||
```
|
||||
|
||||
## 代码边界
|
||||
|
||||
- `core/`:无 ROS、无具体型号,包含领域类型、PnP/旋转数学、拟合接口、统一样本
|
||||
契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。
|
||||
- `runtime/`:通用会话状态机与注册 Profile 分发;ROS 消息和硬件适配只能位于
|
||||
`runtime/nodes`、`runtime/adapters`。
|
||||
- `models/g20/`:G20 right-19、legacy-11、运动、零位、产物和中文诊断策略。
|
||||
后续型号或左右手作为新的独立 Profile 加入 `models/`,不在通用层增加分支。
|
||||
- `compat/`:v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留
|
||||
一版最小转发 shim,不包含算法副本。
|
||||
|
||||
产品配置在启动硬件前通过本地 `ProfileRegistry` 完成命令索引、任务、视角、
|
||||
Tag、零位目标、URDF关节和文件哈希校验。v1 配置原文不改;v2 配置使用
|
||||
`profile_id: MODEL/side/layout/vREVISION`。视角名和数量由 Profile 声明,通用层
|
||||
不要求 `front/side/top`,也不假设固定 20 个命令。
|
||||
|
||||
完全独立地只标定大拇指4项任务时,使用:
|
||||
|
||||
```bash
|
||||
|
||||
@@ -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
|
||||
|
||||
from .core import PAIR_NAMES, robust_rotation_summary
|
||||
from .core import robust_rotation_summary
|
||||
from .pnp import SquareTagPose
|
||||
|
||||
|
||||
@@ -18,6 +18,7 @@ TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
|
||||
"t3_t4": ("t3", "t4"),
|
||||
"t4_t5": ("t4", "t5"),
|
||||
}
|
||||
PAIR_NAMES: tuple[str, ...] = tuple(TAG_PAIR_ROLES)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
|
||||
@@ -1,5 +1,11 @@
|
||||
"""Compatibility adapters for one-release calibration migrations."""
|
||||
|
||||
from .config_v1 import product_profile_key
|
||||
from .defaults import default_product_config_path
|
||||
from .paths import resolve_renamed_package_path
|
||||
|
||||
__all__ = ["resolve_renamed_package_path"]
|
||||
__all__ = [
|
||||
"default_product_config_path",
|
||||
"product_profile_key",
|
||||
"resolve_renamed_package_path",
|
||||
]
|
||||
|
||||
@@ -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
|
||||
|
||||
from .core import COMMAND_NAMES
|
||||
from .models.g20.command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
|
||||
from .trajectory import (
|
||||
_angle_for_circle,
|
||||
_fit_circle_with_axis,
|
||||
@@ -129,6 +129,12 @@ class HandCalibrationProfile:
|
||||
command_names: tuple[str, ...] = COMMAND_NAMES
|
||||
baseline_command: tuple[int, ...] = THREE_CAMERA_BASELINE_COMMAND
|
||||
capabilities: frozenset[str] = frozenset()
|
||||
precheck_sweeps: bool = False
|
||||
steady_command_checkpoints: bool = False
|
||||
directional_zero: bool = False
|
||||
isolated_holdout: bool = False
|
||||
cross_view_roll_curve: bool = False
|
||||
stable_cross_view_cone_bias: bool = False
|
||||
|
||||
@property
|
||||
def command_count(self) -> int:
|
||||
@@ -703,6 +709,12 @@ def _build_right_19_profile() -> HandCalibrationProfile:
|
||||
"palm_axis_relative_motion_v3",
|
||||
}
|
||||
),
|
||||
precheck_sweeps=True,
|
||||
steady_command_checkpoints=True,
|
||||
directional_zero=True,
|
||||
isolated_holdout=True,
|
||||
cross_view_roll_curve=True,
|
||||
stable_cross_view_cone_bias=True,
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -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,
|
||||
tag_quality_is_valid,
|
||||
)
|
||||
from .core import (
|
||||
from .compat.legacy.thumb_core import (
|
||||
BASELINE_COMMAND,
|
||||
COMMAND_NAMES,
|
||||
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
|
||||
from datetime import datetime
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
import traceback
|
||||
from typing import Any, Mapping
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
||||
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
|
||||
from .product import ProductConfig, load_product_config, sha256_file
|
||||
from .publication import atomic_session_pointer, finalize_session_artifacts
|
||||
from .storage import atomic_write_json
|
||||
|
||||
|
||||
EXIT_PASS = 0
|
||||
EXIT_QUALITY = 2
|
||||
EXIT_SAFETY = 3
|
||||
STATUS_TIMEOUT_SECONDS = 90.0
|
||||
FITTING_STATUS_TIMEOUT_SECONDS = 600.0
|
||||
|
||||
|
||||
def _status_timeout_seconds(status: Mapping[str, Any]) -> float:
|
||||
"""Return the watchdog deadline for the node's current phase.
|
||||
|
||||
Motion and acquisition are expected to publish twice a second and retain
|
||||
the strict transport watchdog. The final 3-D fit is intentionally a
|
||||
synchronous, CPU-bound operation, so its executor cannot service the
|
||||
status timer until the fit returns. The node publishes an explicit
|
||||
FITTING status immediately before entering that operation; allow that
|
||||
known phase enough time without weakening motion safety checks.
|
||||
"""
|
||||
if str(status.get("state", "")) == "FITTING":
|
||||
return FITTING_STATUS_TIMEOUT_SECONDS
|
||||
return STATUS_TIMEOUT_SECONDS
|
||||
|
||||
|
||||
def _calibration_node_exited_before_status(log_path: Path) -> bool:
|
||||
"""Detect a launch child crash while the parent launch is still alive."""
|
||||
try:
|
||||
with log_path.open("rb") as stream:
|
||||
stream.seek(0, os.SEEK_END)
|
||||
size = stream.tell()
|
||||
stream.seek(max(0, size - 65536), os.SEEK_SET)
|
||||
tail = stream.read().decode("utf-8", errors="replace")
|
||||
except OSError:
|
||||
return False
|
||||
return (
|
||||
"[three_camera_calibration_node-" in tail
|
||||
and "]: process has died" in tail
|
||||
)
|
||||
|
||||
|
||||
class CalibrationMonitor(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("g20_calibration_product_runner")
|
||||
self.latest_status: dict[str, Any] = {}
|
||||
self.last_status_at = time.monotonic()
|
||||
self.start_requested = False
|
||||
self.start_future: Any = None
|
||||
self.abort_future: Any = None
|
||||
self.create_subscription(String, "/g20_calibration/status", self._status, 10)
|
||||
self.start_client = self.create_client(Trigger, "/g20_calibration/start")
|
||||
self.abort_client = self.create_client(Trigger, "/g20_calibration/abort")
|
||||
|
||||
def _status(self, message: String) -> None:
|
||||
try:
|
||||
payload = json.loads(message.data)
|
||||
except (TypeError, json.JSONDecodeError):
|
||||
return
|
||||
if isinstance(payload, dict):
|
||||
self.latest_status = payload
|
||||
self.last_status_at = time.monotonic()
|
||||
|
||||
def maybe_start(self) -> None:
|
||||
if self.start_requested or self.latest_status.get("state") != "WAIT_START":
|
||||
return
|
||||
if not self.start_client.service_is_ready():
|
||||
self.start_client.wait_for_service(timeout_sec=0.05)
|
||||
return
|
||||
self.start_requested = True
|
||||
self.start_future = self.start_client.call_async(Trigger.Request())
|
||||
|
||||
def abort(self) -> None:
|
||||
if not self.abort_client.service_is_ready():
|
||||
self.abort_client.wait_for_service(timeout_sec=1.0)
|
||||
if self.abort_client.service_is_ready():
|
||||
self.abort_future = self.abort_client.call_async(Trigger.Request())
|
||||
|
||||
|
||||
class ProgressConsole:
|
||||
def __init__(self, serial_number: str) -> None:
|
||||
self.serial_number = serial_number
|
||||
self.estimator = ProgressEstimator.start()
|
||||
self.last_text = ""
|
||||
self.last_issue = ""
|
||||
|
||||
def update(self, status: Mapping[str, Any]) -> None:
|
||||
text = render_progress_zh(self.serial_number, status, self.estimator)
|
||||
if text == self.last_text:
|
||||
return
|
||||
self.last_text = text
|
||||
if sys.stdout.isatty():
|
||||
sys.stdout.write("\x1b[2J\x1b[H" + text + "\n")
|
||||
sys.stdout.flush()
|
||||
else:
|
||||
print(text, flush=True)
|
||||
reason = str(status.get("reason", ""))
|
||||
if reason.startswith("automatic_retry_") and reason != self.last_issue:
|
||||
self.last_issue = reason
|
||||
active = status.get("active", {})
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
f"⚠ 当前任务出现问题:{reason.removeprefix('automatic_retry_')}",
|
||||
f"系统处理:只重扫当前任务(第 {active.get('automatic_retry_count', 1)}/2 次)",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
|
||||
|
||||
def _default_product_config() -> Path:
|
||||
try:
|
||||
installed = Path(
|
||||
get_package_share_directory("linkerhand_calibration")
|
||||
) / "config" / "g20_right_product.yaml"
|
||||
if installed.is_file():
|
||||
return installed
|
||||
except Exception:
|
||||
pass
|
||||
return (
|
||||
Path.cwd()
|
||||
/ "src/linkerhand_calibration/config/g20_right_product.yaml"
|
||||
).resolve()
|
||||
|
||||
|
||||
def _launch_command(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
recalibration_scope: str = "full",
|
||||
) -> list[str]:
|
||||
values = {
|
||||
"model": config.model,
|
||||
"hand_type": config.side,
|
||||
"tag_layout": config.tag_layout,
|
||||
"serial_number": config.serial_number,
|
||||
"can_interface": config.can_interface,
|
||||
"session_dir": str(session),
|
||||
"output_root": str(config.output_root),
|
||||
"camera_extrinsics_file": str(config.camera_extrinsics),
|
||||
"source_urdf_path": str(config.source_urdf),
|
||||
"source_urdf_expected_sha256": config.source_urdf_sha256,
|
||||
"corrected_urdf_output_dir": str(session),
|
||||
"calibration_config": str(config.calibration_config),
|
||||
"tag_config": str(config.tag_config),
|
||||
"commands_enabled": "true",
|
||||
"start_cameras": "true",
|
||||
"start_sdk": "true",
|
||||
"record_bag": "false",
|
||||
"validation_enabled": "false",
|
||||
"recalibration_scope": recalibration_scope,
|
||||
}
|
||||
if resume_from is not None:
|
||||
values["resume_raw_samples_path"] = str(
|
||||
resume_from / "raw_samples.jsonl"
|
||||
)
|
||||
for view, camera in config.cameras.items():
|
||||
values[f"{view}_camera_serial"] = camera["serial_number"]
|
||||
values[f"{view}_camera_name"] = camera["camera_name"]
|
||||
values[f"{view}_camera_info_url"] = camera["camera_info"]
|
||||
return [
|
||||
"ros2",
|
||||
"launch",
|
||||
"linkerhand_calibration",
|
||||
"three_camera_calibration.launch.py",
|
||||
*(f"{name}:={value}" for name, value in values.items()),
|
||||
]
|
||||
|
||||
|
||||
def _stop_stack(process: subprocess.Popen[Any]) -> None:
|
||||
if process.poll() is not None:
|
||||
return
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGINT)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
try:
|
||||
process.wait(timeout=15.0)
|
||||
except subprocess.TimeoutExpired:
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
try:
|
||||
process.wait(timeout=5.0)
|
||||
except subprocess.TimeoutExpired:
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
process.wait(timeout=5.0)
|
||||
|
||||
|
||||
def _write_trace(log_path: Path, error: BaseException) -> None:
|
||||
with log_path.open("a", encoding="utf-8") as stream:
|
||||
stream.write("\n[one-command exception]\n")
|
||||
traceback.print_exception(type(error), error, error.__traceback__, file=stream)
|
||||
|
||||
|
||||
def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None:
|
||||
monitor.abort()
|
||||
deadline = time.monotonic() + float(timeout_seconds)
|
||||
while time.monotonic() < deadline and rclpy.ok():
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
if monitor.latest_status.get("state") == "ABORTED":
|
||||
return
|
||||
|
||||
|
||||
def _run_hardware_session(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
recalibration_scope: str = "full",
|
||||
) -> tuple[dict[str, Any], int]:
|
||||
session.mkdir(parents=True, exist_ok=False)
|
||||
(session / "raw_samples.jsonl").touch()
|
||||
log_path = session / "calibration.log"
|
||||
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
|
||||
atomic_session_pointer(config.session_root, "latest_attempt", session)
|
||||
monitor = CalibrationMonitor()
|
||||
console = ProgressConsole(config.serial_number)
|
||||
process: subprocess.Popen[Any] | None = None
|
||||
latest_status: dict[str, Any] = {
|
||||
"state": "PREFLIGHT",
|
||||
"reason": "starting_ros_stack",
|
||||
"progress": 0.0,
|
||||
"views": {},
|
||||
"feedback_hz": 0.0,
|
||||
}
|
||||
exit_code = EXIT_QUALITY
|
||||
try:
|
||||
process = subprocess.Popen(
|
||||
_launch_command(
|
||||
config,
|
||||
session,
|
||||
resume_from=resume_from,
|
||||
recalibration_scope=recalibration_scope,
|
||||
),
|
||||
cwd=config.workspace,
|
||||
stdout=log_stream,
|
||||
stderr=subprocess.STDOUT,
|
||||
text=True,
|
||||
start_new_session=True,
|
||||
)
|
||||
launched_at = time.monotonic()
|
||||
last_render = 0.0
|
||||
last_startup_log_check = 0.0
|
||||
while True:
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
if monitor.latest_status:
|
||||
latest_status = monitor.latest_status
|
||||
monitor.maybe_start()
|
||||
now = time.monotonic()
|
||||
if now - last_render >= 0.5:
|
||||
console.update(latest_status)
|
||||
last_render = now
|
||||
if monitor.start_future is not None and monitor.start_future.done():
|
||||
response = monitor.start_future.result()
|
||||
if response is None or not response.success:
|
||||
message = "start service failed" if response is None else response.message
|
||||
raise RuntimeError(f"CFG-START-008:{message}")
|
||||
monitor.start_future = None
|
||||
state = str(latest_status.get("state", ""))
|
||||
if state == "COMPLETE":
|
||||
exit_code = EXIT_PASS
|
||||
break
|
||||
if state in {"PAUSED", "ABORTED"}:
|
||||
reason = str(latest_status.get("reason", "calibration_paused"))
|
||||
exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY
|
||||
if state == "PAUSED" and "stall" not in reason:
|
||||
# Ordinary quality failures return to the reviewed baseline
|
||||
# before the process tree is stopped. Mechanical stalls
|
||||
# deliberately skip this path and keep the current pose.
|
||||
failure_status = dict(latest_status)
|
||||
_request_safe_abort(monitor)
|
||||
latest_status = failure_status
|
||||
break
|
||||
if process.poll() is not None:
|
||||
raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}")
|
||||
if (
|
||||
not monitor.latest_status
|
||||
and now - last_startup_log_check >= 0.5
|
||||
):
|
||||
last_startup_log_check = now
|
||||
log_stream.flush()
|
||||
if _calibration_node_exited_before_status(log_path):
|
||||
raise RuntimeError(
|
||||
"CAM-STATUS-202:calibration node exited before status"
|
||||
)
|
||||
if (
|
||||
not monitor.latest_status
|
||||
and now - launched_at > STATUS_TIMEOUT_SECONDS
|
||||
):
|
||||
raise RuntimeError("CAM-STATUS-202:no calibration status received")
|
||||
if (
|
||||
monitor.latest_status
|
||||
and now - monitor.last_status_at
|
||||
> _status_timeout_seconds(monitor.latest_status)
|
||||
):
|
||||
raise RuntimeError("MOTION-COMM-303:calibration status stopped")
|
||||
except KeyboardInterrupt as error:
|
||||
latest_status["state"] = "ABORTED"
|
||||
latest_status["reason"] = "operator_abort"
|
||||
_request_safe_abort(monitor)
|
||||
_write_trace(log_path, error)
|
||||
exit_code = EXIT_SAFETY
|
||||
except BaseException as error:
|
||||
latest_status["state"] = "PAUSED"
|
||||
latest_status["reason"] = str(error)
|
||||
_write_trace(log_path, error)
|
||||
exit_code = EXIT_QUALITY
|
||||
finally:
|
||||
if process is not None:
|
||||
_stop_stack(process)
|
||||
monitor.destroy_node()
|
||||
log_stream.flush()
|
||||
os.fsync(log_stream.fileno())
|
||||
log_stream.close()
|
||||
|
||||
if exit_code != EXIT_PASS:
|
||||
_, block = build_failure_report(
|
||||
config,
|
||||
session,
|
||||
latest_status,
|
||||
reason=str(latest_status.get("reason", "unknown_failure")),
|
||||
)
|
||||
print(block, flush=True)
|
||||
return latest_status, exit_code
|
||||
|
||||
|
||||
def _startup_failure_block(path: Path, error: BaseException) -> str:
|
||||
return "\n".join(
|
||||
[
|
||||
"========== 请复制以下内容给开发者 ==========",
|
||||
"结果:FAIL",
|
||||
"错误代码:CFG-PRODUCT-001",
|
||||
"失败阶段:启动静态预检",
|
||||
f"问题:{error}",
|
||||
f"产品配置:{path}",
|
||||
"自动处理:未启动相机、SDK或机械手运动",
|
||||
"建议:复制本诊断块给开发者,不要手工修改哈希绕过检查。",
|
||||
"========== 复制结束 ==========",
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
||||
"""Return the newest compatible failed attempt, never a passed session.
|
||||
|
||||
Do not trust only ``latest_attempt``. A process interrupted during the
|
||||
device-only startup gate may have already moved that pointer while still
|
||||
containing no ``session_start`` checkpoint. In that case walk backwards
|
||||
to the preceding usable failed session instead of throwing away hours of
|
||||
completed tasks.
|
||||
"""
|
||||
root = config.session_root
|
||||
try:
|
||||
resolved_root = root.resolve(strict=True)
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
candidates: list[Path] = []
|
||||
pointer = config.session_root / "latest_attempt"
|
||||
if pointer.exists():
|
||||
try:
|
||||
candidates.append(pointer.resolve(strict=True))
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
candidates.extend(
|
||||
sorted(
|
||||
(
|
||||
path
|
||||
for path in root.iterdir()
|
||||
if path.is_dir() and not path.name.startswith("latest_")
|
||||
),
|
||||
key=lambda path: path.name,
|
||||
reverse=True,
|
||||
)
|
||||
)
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
passed_pointer = config.session_root / "latest_passed"
|
||||
passed: Path | None = None
|
||||
if passed_pointer.exists():
|
||||
try:
|
||||
passed = passed_pointer.resolve(strict=True)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
seen: set[Path] = set()
|
||||
for unresolved in candidates:
|
||||
try:
|
||||
candidate = unresolved.resolve(strict=True)
|
||||
except OSError:
|
||||
continue
|
||||
if candidate in seen:
|
||||
continue
|
||||
seen.add(candidate)
|
||||
if candidate.parent != resolved_root or not candidate.is_dir():
|
||||
continue
|
||||
# A failed attempt older than the current formal release is stale and
|
||||
# must not seed a new independent calibration.
|
||||
if passed is not None and candidate.name <= passed.name:
|
||||
continue
|
||||
raw_path = candidate / "raw_samples.jsonl"
|
||||
if not raw_path.is_file():
|
||||
continue
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
summary: dict[str, Any] | None = None
|
||||
if summary_path.is_file():
|
||||
try:
|
||||
loaded = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError):
|
||||
continue
|
||||
if not isinstance(loaded, dict) or loaded.get("result") != "FAIL":
|
||||
continue
|
||||
summary = loaded
|
||||
hashes = summary.get("hashes", {})
|
||||
if not isinstance(hashes, Mapping):
|
||||
continue
|
||||
if (
|
||||
str(hashes.get("source_urdf_sha256", ""))
|
||||
!= config.source_urdf_sha256
|
||||
or str(hashes.get("camera_extrinsics_sha256", ""))
|
||||
!= config.camera_extrinsics_sha256
|
||||
):
|
||||
continue
|
||||
start: dict[str, Any] | None = None
|
||||
try:
|
||||
with raw_path.open("r", encoding="utf-8") as stream:
|
||||
for line in stream:
|
||||
if not line.strip():
|
||||
continue
|
||||
value = json.loads(line)
|
||||
if (
|
||||
isinstance(value, dict)
|
||||
and value.get("kind") == "session_start"
|
||||
):
|
||||
start = value
|
||||
break
|
||||
except (OSError, json.JSONDecodeError):
|
||||
continue
|
||||
if (
|
||||
start is None
|
||||
or start.get("hand_type") != config.side
|
||||
or start.get("tag_layout") != config.tag_layout
|
||||
or start.get("source_urdf_sha256")
|
||||
!= config.source_urdf_sha256
|
||||
):
|
||||
continue
|
||||
if summary is None:
|
||||
# Ctrl+C can terminate the ROS launch tree before the wrapper gets
|
||||
# a chance to create calibration_summary_zh.json. The immutable
|
||||
# checkpoint itself is enough to resume only after independently
|
||||
# proving that its external geometry still matches the product.
|
||||
try:
|
||||
checkpoint_extrinsics = Path(
|
||||
str(start["camera_extrinsics_file"])
|
||||
).expanduser().resolve(strict=True)
|
||||
if sha256_file(checkpoint_extrinsics) != (
|
||||
config.camera_extrinsics_sha256
|
||||
):
|
||||
continue
|
||||
except (KeyError, OSError, ValueError):
|
||||
continue
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
def _resolve_partial_base_session(
|
||||
config: ProductConfig, base_session: str | Path | None
|
||||
) -> Path:
|
||||
"""Validate the complete passed session that donates non-target tasks."""
|
||||
if base_session is None or not str(base_session).strip():
|
||||
raise ValueError(
|
||||
"partial scope requires --base-session pointing to a passed "
|
||||
"complete G20 right session"
|
||||
)
|
||||
candidate = Path(base_session).expanduser().resolve(strict=True)
|
||||
root = config.session_root.resolve()
|
||||
if candidate.parent != root or not candidate.is_dir():
|
||||
raise ValueError(
|
||||
"base session must resolve to a direct session directory under "
|
||||
f"{root}"
|
||||
)
|
||||
raw_path = candidate / "raw_samples.jsonl"
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
payload_path = (
|
||||
candidate
|
||||
/ f"g20_right_{config.serial_number}_calibration.json"
|
||||
)
|
||||
for required in (raw_path, summary_path, payload_path):
|
||||
if not required.is_file():
|
||||
raise ValueError(f"base session is missing required artifact: {required}")
|
||||
try:
|
||||
summary = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
except json.JSONDecodeError as error:
|
||||
raise ValueError("base session summary is invalid JSON") from error
|
||||
if (
|
||||
not isinstance(summary, dict)
|
||||
or summary.get("result") != "PASS"
|
||||
or not bool(summary.get("quality", {}).get("passed"))
|
||||
):
|
||||
raise ValueError("base session is not a formally passed session")
|
||||
hashes = summary.get("hashes", {})
|
||||
expected_hashes = {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
}
|
||||
if not isinstance(hashes, Mapping) or any(
|
||||
str(hashes.get(name, "")) != expected
|
||||
for name, expected in expected_hashes.items()
|
||||
):
|
||||
raise ValueError(
|
||||
"base session source URDF, camera extrinsics or calibration "
|
||||
"configuration differs from the current product"
|
||||
)
|
||||
return candidate
|
||||
|
||||
|
||||
def run(
|
||||
config_path: str | Path,
|
||||
*,
|
||||
workspace: str | Path | None = None,
|
||||
preflight_only: bool = False,
|
||||
allow_resume: bool = True,
|
||||
scope: str = "full",
|
||||
base_session: str | Path | None = None,
|
||||
) -> int:
|
||||
path = Path(config_path).expanduser().resolve()
|
||||
try:
|
||||
# Resolve every file and camera identity before allowing a hardware
|
||||
# process to start. A second load enables the real CAN existence gate.
|
||||
config = load_product_config(path, workspace=workspace, check_can=False)
|
||||
load_product_config(path, workspace=workspace, check_can=True)
|
||||
selected_scope = str(scope).strip().lower()
|
||||
if selected_scope not in {"full", "thumb", "fingers"}:
|
||||
raise ValueError("scope must be one of: full, thumb, fingers")
|
||||
if selected_scope == "full" and base_session is not None:
|
||||
raise ValueError(
|
||||
"--base-session is valid only with --scope thumb/fingers"
|
||||
)
|
||||
partial_base = None
|
||||
if selected_scope == "fingers" or base_session is not None:
|
||||
partial_base = _resolve_partial_base_session(config, base_session)
|
||||
except BaseException as error:
|
||||
print(_startup_failure_block(path, error), flush=True)
|
||||
return EXIT_QUALITY
|
||||
if preflight_only:
|
||||
print("PASS:产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。")
|
||||
return EXIT_PASS
|
||||
|
||||
config.session_root.mkdir(parents=True, exist_ok=True)
|
||||
resume_candidate = (
|
||||
partial_base
|
||||
if selected_scope != "full"
|
||||
else (_automatic_resume_candidate(config) if allow_resume else None)
|
||||
)
|
||||
if resume_candidate is not None:
|
||||
if selected_scope == "thumb":
|
||||
print(
|
||||
"拇指专项标定:四指任务继承自已通过会话 "
|
||||
f"{resume_candidate.name};4项拇指任务将全部重新采集,"
|
||||
"四指零位保持不变。",
|
||||
flush=True,
|
||||
)
|
||||
elif selected_scope == "fingers":
|
||||
print(
|
||||
"四指专项标定:拇指任务和4个拇指零位继承自已通过会话 "
|
||||
f"{resume_candidate.name};12项四指任务将全部重新采集。",
|
||||
flush=True,
|
||||
)
|
||||
else:
|
||||
print(
|
||||
"检测到兼容的失败会话,将恢复已完整通过的关节任务:"
|
||||
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
|
||||
flush=True,
|
||||
)
|
||||
elif selected_scope == "thumb":
|
||||
print(
|
||||
"独立拇指标定:不导入四指会话;仅采集4项拇指任务,"
|
||||
"四指URDF零位保持原始CAD值。",
|
||||
flush=True,
|
||||
)
|
||||
maximum_sessions = config.required_independent_passes
|
||||
for pass_index in range(maximum_sessions):
|
||||
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session = config.session_root / stamp
|
||||
while session.exists():
|
||||
time.sleep(1.0)
|
||||
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session = config.session_root / stamp
|
||||
if maximum_sessions > 1:
|
||||
print(f"正式标定复验:第 {pass_index + 1}/{maximum_sessions} 次", flush=True)
|
||||
status, code = _run_hardware_session(
|
||||
config,
|
||||
session,
|
||||
resume_from=(
|
||||
resume_candidate
|
||||
if selected_scope != "full" or pass_index == 0
|
||||
else None
|
||||
),
|
||||
recalibration_scope=selected_scope,
|
||||
)
|
||||
if code != EXIT_PASS:
|
||||
return code
|
||||
# Keep the exact node-side completion contract durable before the
|
||||
# independent publication layer starts. If publication itself fails,
|
||||
# developers can re-run artifact checks without repeating motion or
|
||||
# inventing lost combination-validation metrics.
|
||||
atomic_write_json(session / "node_status.json", status)
|
||||
try:
|
||||
summary, release_ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
)
|
||||
except BaseException as error:
|
||||
_write_trace(session / "calibration.log", error)
|
||||
status = dict(status)
|
||||
status["state"] = "PAUSED"
|
||||
status["reason"] = f"PUB-ARTIFACT-601:{error}"
|
||||
_, block = build_failure_report(config, session, status, reason=status["reason"])
|
||||
print(block, flush=True)
|
||||
return EXIT_QUALITY
|
||||
if release_ready:
|
||||
result_pointer = (
|
||||
config.session_root / "latest_thumb_passed"
|
||||
if selected_scope == "thumb" and partial_base is None
|
||||
else config.session_root / "latest_passed"
|
||||
)
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
f"PASS:{config.model} {config.side} 标定、URDF修正和复验全部通过。",
|
||||
f"正式结果:{result_pointer}",
|
||||
f"JSON:{session / summary['artifacts']['json']}",
|
||||
f"URDF:{summary['artifacts']['urdf']}",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
return EXIT_PASS
|
||||
print("本次会话质量PASS;正在自动执行第二次独立完整复验。", flush=True)
|
||||
return EXIT_QUALITY
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定")
|
||||
parser.add_argument("--config", default=str(_default_product_config()))
|
||||
parser.add_argument("--workspace", default=None)
|
||||
parser.add_argument("--preflight-only", action="store_true")
|
||||
parser.add_argument(
|
||||
"--scope",
|
||||
choices=("full", "thumb", "fingers"),
|
||||
default="full",
|
||||
help=(
|
||||
"full重新标定全手;thumb仅重采4项拇指任务;"
|
||||
"fingers复用已认证拇指并仅重采12项四指任务"
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--base-session",
|
||||
default=None,
|
||||
help=(
|
||||
"可选:thumb模式将结果合并到该完整会话;"
|
||||
"fingers模式必须提供该基础会话"
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-resume",
|
||||
action="store_true",
|
||||
help="忽略失败会话,从第一个关节开始全新采集",
|
||||
)
|
||||
arguments = parser.parse_args(args)
|
||||
configure_fastdds_large_image_transport()
|
||||
ros_log_dir = Path(
|
||||
os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs")
|
||||
)
|
||||
ros_log_dir.mkdir(parents=True, exist_ok=True)
|
||||
rclpy.init()
|
||||
try:
|
||||
code = run(
|
||||
arguments.config,
|
||||
workspace=arguments.workspace,
|
||||
preflight_only=arguments.preflight_only,
|
||||
allow_resume=not arguments.no_resume,
|
||||
scope=arguments.scope,
|
||||
base_session=arguments.base_session,
|
||||
)
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
__all__ = ["main"]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -13,115 +13,60 @@ import hashlib
|
||||
from pathlib import Path
|
||||
import re
|
||||
from typing import Any, Mapping
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import yaml
|
||||
|
||||
from .compat import resolve_renamed_package_path
|
||||
from .compat import product_profile_key, resolve_renamed_package_path
|
||||
from .core import CalibrationProfile, ProfileKey
|
||||
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
|
||||
from .full_hand import (
|
||||
G20_RIGHT_19_LAYOUT,
|
||||
HandCalibrationProfile,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
from .models import RegisteredProfile, get_default_registry
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProductCalibrationContract:
|
||||
"""Declarative boundary between one hand model and the shared engine."""
|
||||
"""Compatibility facade over one typed, locally registered profile."""
|
||||
|
||||
model: str
|
||||
side: str
|
||||
layout_id: str
|
||||
profile: HandCalibrationProfile
|
||||
zero_profile: ZeroCalibrationProfile
|
||||
registered: RegisteredProfile
|
||||
|
||||
@property
|
||||
def model(self) -> str:
|
||||
return self.registered.profile.key.model
|
||||
|
||||
@property
|
||||
def side(self) -> str:
|
||||
return self.registered.profile.key.side
|
||||
|
||||
@property
|
||||
def layout_id(self) -> str:
|
||||
return self.registered.profile.key.layout
|
||||
|
||||
@property
|
||||
def typed_profile(self) -> CalibrationProfile:
|
||||
return self.registered.profile
|
||||
|
||||
@property
|
||||
def profile(self):
|
||||
return self.registered.engine.hand_profile
|
||||
|
||||
@property
|
||||
def zero_profile(self):
|
||||
return self.registered.engine.zero_profile
|
||||
|
||||
@property
|
||||
def required_tag_ids(self) -> frozenset[int]:
|
||||
return frozenset(
|
||||
int(tag_id)
|
||||
for tags in self.profile.view_tags.values()
|
||||
for tag_id in tags.values()
|
||||
)
|
||||
return self.typed_profile.vision.tag_ids
|
||||
|
||||
@property
|
||||
def views(self) -> tuple[str, ...]:
|
||||
return tuple(self.profile.view_tags)
|
||||
|
||||
|
||||
_PRODUCT_CONTRACTS: dict[
|
||||
tuple[str, str, str], ProductCalibrationContract
|
||||
] = {}
|
||||
|
||||
|
||||
def register_product_calibration_contract(
|
||||
contract: ProductCalibrationContract,
|
||||
) -> None:
|
||||
"""Register one model/side/layout without modifying shared workflow code."""
|
||||
model = str(contract.model).strip().upper()
|
||||
side = str(contract.side).strip().lower()
|
||||
layout = str(contract.layout_id).strip().lower()
|
||||
if not model or side not in {"left", "right"} or not layout:
|
||||
raise ValueError("product calibration contract identity is invalid")
|
||||
if contract.profile.side != side:
|
||||
raise ValueError("product contract side differs from hand profile")
|
||||
if contract.profile.layout_id.lower() != layout:
|
||||
raise ValueError("product contract layout differs from hand profile")
|
||||
if contract.zero_profile.hand != contract.profile:
|
||||
raise ValueError("zero-calibration profile differs from hand profile")
|
||||
if len(contract.profile.baseline_command) != contract.profile.command_count:
|
||||
raise ValueError("profile baseline and command names differ in length")
|
||||
if not contract.required_tag_ids:
|
||||
raise ValueError("product contract must declare at least one Tag")
|
||||
key = (model, side, layout)
|
||||
existing = _PRODUCT_CONTRACTS.get(key)
|
||||
if existing is not None and existing != contract:
|
||||
raise ValueError(f"product calibration contract already registered: {key}")
|
||||
_PRODUCT_CONTRACTS[key] = contract
|
||||
return self.typed_profile.vision.view_names
|
||||
|
||||
|
||||
def get_product_calibration_contract(
|
||||
model: str, side: str, layout_id: str
|
||||
model: str, side: str, layout_id: str, revision: int = 1
|
||||
) -> ProductCalibrationContract:
|
||||
key = (
|
||||
str(model).strip().upper(),
|
||||
str(side).strip().lower(),
|
||||
str(layout_id).strip().lower(),
|
||||
)
|
||||
try:
|
||||
return _PRODUCT_CONTRACTS[key]
|
||||
except KeyError as error:
|
||||
supported = ", ".join("/".join(item) for item in sorted(_PRODUCT_CONTRACTS))
|
||||
raise ValueError(
|
||||
f"unsupported calibration product {key}; registered={supported}"
|
||||
) from error
|
||||
|
||||
|
||||
register_product_calibration_contract(
|
||||
ProductCalibrationContract(
|
||||
model="G20",
|
||||
side="right",
|
||||
layout_id=G20_RIGHT_19_LAYOUT,
|
||||
profile=get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||
zero_profile=get_zero_calibration_profile(
|
||||
"right", G20_RIGHT_19_LAYOUT
|
||||
),
|
||||
)
|
||||
)
|
||||
for _legacy_side in ("left", "right"):
|
||||
register_product_calibration_contract(
|
||||
ProductCalibrationContract(
|
||||
model="G20",
|
||||
side=_legacy_side,
|
||||
layout_id="legacy_11",
|
||||
profile=get_hand_calibration_profile(_legacy_side, "legacy_11"),
|
||||
zero_profile=get_zero_calibration_profile(
|
||||
_legacy_side, "legacy_11"
|
||||
),
|
||||
)
|
||||
return ProductCalibrationContract(
|
||||
get_default_registry().resolve(model, side, layout_id, revision)
|
||||
)
|
||||
|
||||
|
||||
@@ -206,10 +151,36 @@ def _custom_pnp_tag_sizes_m_by_id(
|
||||
return result
|
||||
|
||||
|
||||
def _validate_profile_urdf(
|
||||
profile: CalibrationProfile, source_urdf: Path
|
||||
) -> None:
|
||||
"""Verify every zero and mimic target before a hardware command is sent."""
|
||||
root = ET.parse(source_urdf).getroot()
|
||||
urdf_joints = {
|
||||
str(joint.get("name"))
|
||||
for joint in root.findall("joint")
|
||||
if joint.get("name")
|
||||
}
|
||||
required = (
|
||||
set(profile.zero.active_joints)
|
||||
| set(profile.zero.passive_joints)
|
||||
| set(profile.zero.mimic_source_by_joint)
|
||||
| set(profile.zero.mimic_source_by_joint.values())
|
||||
)
|
||||
missing = required - urdf_joints
|
||||
if missing:
|
||||
raise ValueError(
|
||||
"source URDF is missing profile joints: "
|
||||
+ ", ".join(sorted(missing))
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProductConfig:
|
||||
path: Path
|
||||
workspace: Path
|
||||
schema_version: int
|
||||
profile_key: ProfileKey
|
||||
model: str
|
||||
side: str
|
||||
tag_layout: str
|
||||
@@ -246,23 +217,17 @@ def load_product_config(
|
||||
raise ValueError(f"product config does not exist: {source}")
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
raw = _mapping(yaml.safe_load(stream), str(source))
|
||||
if int(raw.get("schema_version", -1)) != 1:
|
||||
raise ValueError("product config schema_version must be 1")
|
||||
profile_key = product_profile_key(raw)
|
||||
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
|
||||
serial = str(raw.get("serial_number", ""))
|
||||
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
||||
raise ValueError("serial_number is invalid")
|
||||
model = str(raw.get("model", "")).strip().upper()
|
||||
side = str(raw.get("side", "")).strip().lower()
|
||||
# Keep the deployed G20 schema compatible while making the layout an
|
||||
# explicit product choice for all new configurations.
|
||||
layout = str(
|
||||
raw.get(
|
||||
"tag_layout",
|
||||
G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "",
|
||||
)
|
||||
).strip().lower()
|
||||
contract = get_product_calibration_contract(model, side, layout)
|
||||
model = profile_key.model
|
||||
side = profile_key.side
|
||||
layout = profile_key.layout
|
||||
contract = get_product_calibration_contract(
|
||||
model, side, layout, profile_key.revision
|
||||
)
|
||||
can_interface = str(raw.get("can_interface", "")).strip()
|
||||
if not can_interface:
|
||||
raise ValueError("can_interface is required")
|
||||
@@ -315,6 +280,7 @@ def load_product_config(
|
||||
actual = sha256_file(candidate)
|
||||
if actual != expected:
|
||||
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
|
||||
_validate_profile_urdf(contract.typed_profile, source_urdf)
|
||||
|
||||
expected_tag_ids = set(contract.required_tag_ids)
|
||||
tag_sizes = _tag_sizes_m_by_id(tag_config)
|
||||
@@ -333,10 +299,6 @@ def load_product_config(
|
||||
|
||||
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
||||
required_views = tuple(contract.views)
|
||||
if set(required_views) != set(VIEWS):
|
||||
raise ValueError(
|
||||
"the current shared engine requires front/side/top views"
|
||||
)
|
||||
if set(camera_raw) != set(required_views):
|
||||
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
||||
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
||||
@@ -380,6 +342,8 @@ def load_product_config(
|
||||
return ProductConfig(
|
||||
path=source,
|
||||
workspace=root,
|
||||
schema_version=int(raw["schema_version"]),
|
||||
profile_key=profile_key,
|
||||
model=model,
|
||||
side=side,
|
||||
tag_layout=layout,
|
||||
|
||||
@@ -23,6 +23,7 @@ from .full_hand import (
|
||||
)
|
||||
from .product import ProductConfig, sha256_file
|
||||
from .storage import atomic_write_json
|
||||
from .core.urdf import UrdfCorrectionPlan, build_correction_plan
|
||||
from .urdf_zero import (
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
get_zero_calibration_profile,
|
||||
@@ -202,6 +203,7 @@ def verify_corrected_urdf(
|
||||
*,
|
||||
expected_offsets_rad: Mapping[str, float] | None = None,
|
||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||
correction_plan: UrdfCorrectionPlan | None = None,
|
||||
) -> tuple[str, ...]:
|
||||
"""Prove that only active-joint origin.rpy attributes changed.
|
||||
|
||||
@@ -210,6 +212,13 @@ def verify_corrected_urdf(
|
||||
"""
|
||||
source_text = Path(source).read_text(encoding="utf-8")
|
||||
corrected_text = Path(corrected).read_text(encoding="utf-8")
|
||||
if correction_plan is not None:
|
||||
correction_plan.verify_source(source)
|
||||
if expected_offsets_rad is None:
|
||||
raise ValueError(
|
||||
"correction-plan validation requires expected offsets"
|
||||
)
|
||||
correction_plan.authorize_offsets(expected_offsets_rad)
|
||||
before = _joint_blocks(source_text)
|
||||
after = _joint_blocks(corrected_text)
|
||||
if set(before) != set(after):
|
||||
@@ -243,6 +252,12 @@ def verify_corrected_urdf(
|
||||
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
||||
}
|
||||
if endpoint_offsets:
|
||||
if correction_plan is not None and not set(endpoint_offsets).issubset(
|
||||
correction_plan.endpoint_limit_joints
|
||||
):
|
||||
raise ValueError(
|
||||
"endpoint offsets are not authorized by the correction plan"
|
||||
)
|
||||
source_joints = _joint_elements(source)
|
||||
corrected_joints = _joint_elements(corrected)
|
||||
for name, offset in endpoint_offsets.items():
|
||||
@@ -745,11 +760,20 @@ def finalize_session_artifacts(
|
||||
name: offsets[name]
|
||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
}
|
||||
typed_profile = config.calibration_contract.typed_profile
|
||||
frozen_names = typed_profile.scope.frozen_joints[calibration_scope]
|
||||
correction_plan = build_correction_plan(
|
||||
typed_profile,
|
||||
source_sha256=config.source_urdf_sha256,
|
||||
scope=calibration_scope,
|
||||
frozen_offsets_rad={name: offsets[name] for name in frozen_names},
|
||||
)
|
||||
changed_joints = verify_corrected_urdf(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||
correction_plan=correction_plan,
|
||||
)
|
||||
if standalone_thumb:
|
||||
clipped_runtime_joints = {}
|
||||
|
||||
@@ -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
|
||||
domains. Durable samples always retain both. Fitting code may still use the
|
||||
historical ``command_u8`` key, but it is created only as an explicit projection
|
||||
of a canonical record at the fitting boundary.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Any, Iterable, Literal, Mapping
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
SAMPLE_KINDS = frozenset(
|
||||
{
|
||||
'sample',
|
||||
'baseline_hold_sample',
|
||||
'steady_command_sample',
|
||||
'palm_axis_sample',
|
||||
}
|
||||
)
|
||||
|
||||
FitDomain = Literal['default', 'requested', 'feedback']
|
||||
|
||||
|
||||
class SampleDataContractError(ValueError):
|
||||
"""A calibration observation mixes or omits command domains."""
|
||||
|
||||
|
||||
def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float:
|
||||
try:
|
||||
number = float(value)
|
||||
except (TypeError, ValueError) as error:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be numeric'
|
||||
) from error
|
||||
if not math.isfinite(number) or not 0.0 <= number <= 255.0:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]'
|
||||
)
|
||||
if integral:
|
||||
rounded = int(round(number))
|
||||
if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9):
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be an integer command'
|
||||
)
|
||||
return rounded
|
||||
return number
|
||||
|
||||
|
||||
def explicit_domain_value(
|
||||
source: Mapping[str, Any], domain: Literal['requested', 'feedback']
|
||||
) -> int | float:
|
||||
"""Read and validate one explicitly named domain from any observation."""
|
||||
field = (
|
||||
'requested_command_u8' if domain == 'requested' else 'feedback_u8'
|
||||
)
|
||||
if field not in source or source[field] is None:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:observation is missing explicit {field}'
|
||||
)
|
||||
return _finite_u8(source[field], field, integral=domain == 'requested')
|
||||
|
||||
|
||||
def canonical_sample_record(
|
||||
source: Mapping[str, Any],
|
||||
*,
|
||||
allow_legacy_command: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Return one durable, unambiguous calibration observation.
|
||||
|
||||
``allow_legacy_command`` is restricted to importing historical sessions
|
||||
and unit fixtures. New online observations must provide both explicit
|
||||
fields and therefore cannot silently reinterpret ``command_u8``.
|
||||
"""
|
||||
record = dict(source)
|
||||
kind = str(record.get('kind', ''))
|
||||
if not kind and allow_legacy_command:
|
||||
# Old in-memory steady-curve fixtures predate durable sample kinds.
|
||||
# This adapter is never enabled by the new online/import contract.
|
||||
kind = 'steady_command_sample'
|
||||
record['kind'] = kind
|
||||
if kind not in SAMPLE_KINDS:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}'
|
||||
)
|
||||
|
||||
requested = record.get('requested_command_u8')
|
||||
feedback = record.get('feedback_u8')
|
||||
legacy = record.get('command_u8')
|
||||
if requested is None or feedback is None:
|
||||
if not allow_legacy_command or legacy is None:
|
||||
missing = [
|
||||
name
|
||||
for name, value in (
|
||||
('requested_command_u8', requested),
|
||||
('feedback_u8', feedback),
|
||||
)
|
||||
if value is None
|
||||
]
|
||||
raise SampleDataContractError(
|
||||
'DATA-CONTRACT-701:'
|
||||
f'{kind} is missing explicit {",".join(missing)}'
|
||||
)
|
||||
# Historical in-memory records used requested commands for settled
|
||||
# checkpoints and feedback bins for dense/baseline/palm observations.
|
||||
if requested is None:
|
||||
requested = legacy
|
||||
if feedback is None:
|
||||
feedback = legacy
|
||||
|
||||
record.pop('command_u8', None)
|
||||
record['requested_command_u8'] = explicit_domain_value(
|
||||
{'requested_command_u8': requested}, 'requested'
|
||||
)
|
||||
record['feedback_u8'] = explicit_domain_value(
|
||||
{'feedback_u8': feedback}, 'feedback'
|
||||
)
|
||||
return record
|
||||
|
||||
|
||||
def fitting_sample_record(
|
||||
source: Mapping[str, Any],
|
||||
*,
|
||||
domain: FitDomain = 'default',
|
||||
allow_legacy_command: bool = False,
|
||||
snap_requested_endpoints: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Project a canonical sample into the legacy curve-fitter interface."""
|
||||
record = canonical_sample_record(
|
||||
source, allow_legacy_command=allow_legacy_command
|
||||
)
|
||||
kind = str(record['kind'])
|
||||
selected = domain
|
||||
if selected == 'default':
|
||||
selected = (
|
||||
'requested' if kind == 'steady_command_sample' else 'feedback'
|
||||
)
|
||||
if selected not in {'requested', 'feedback'}:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}'
|
||||
)
|
||||
requested = int(record['requested_command_u8'])
|
||||
if selected == 'requested' or (
|
||||
snap_requested_endpoints and requested in {0, 255}
|
||||
):
|
||||
index = requested
|
||||
else:
|
||||
index = int(
|
||||
np.clip(np.rint(float(record['feedback_u8'])), 0, 255)
|
||||
)
|
||||
record['command_u8'] = index
|
||||
return record
|
||||
|
||||
|
||||
def fitting_sample_records(
|
||||
records: Iterable[Mapping[str, Any]],
|
||||
*,
|
||||
domain: FitDomain = 'default',
|
||||
allow_legacy_command: bool = False,
|
||||
snap_requested_endpoints: bool = False,
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Project several canonical samples into one explicit fitting domain."""
|
||||
return [
|
||||
fitting_sample_record(
|
||||
record,
|
||||
domain=domain,
|
||||
allow_legacy_command=allow_legacy_command,
|
||||
snap_requested_endpoints=snap_requested_endpoints,
|
||||
)
|
||||
for record in records
|
||||
]
|
||||
|
||||
|
||||
def validate_sample_records(
|
||||
records: Iterable[Mapping[str, Any]],
|
||||
*,
|
||||
allow_legacy_command: bool = False,
|
||||
) -> None:
|
||||
"""Validate a collection without changing its representation."""
|
||||
for index, record in enumerate(records):
|
||||
try:
|
||||
canonical_sample_record(
|
||||
record, allow_legacy_command=allow_legacy_command
|
||||
)
|
||||
except SampleDataContractError as error:
|
||||
raise SampleDataContractError(
|
||||
f'{error};record_index={index}'
|
||||
) from error
|
||||
from .core.domain.sample_schema import * # noqa: F401,F403
|
||||
|
||||
@@ -1,105 +1,3 @@
|
||||
"""Crash-safe session storage for hardware calibration."""
|
||||
"""Compatibility import for crash-safe artifact storage."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
from typing import Any, Iterable, Mapping
|
||||
|
||||
|
||||
def atomic_write_json(path: str | Path, payload: Mapping[str, Any]) -> None:
|
||||
destination = Path(path)
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary = destination.with_suffix(destination.suffix + ".tmp")
|
||||
with temporary.open("w", encoding="utf-8") as stream:
|
||||
json.dump(payload, stream, ensure_ascii=False, indent=2)
|
||||
stream.write("\n")
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
os.replace(temporary, destination)
|
||||
|
||||
|
||||
def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
|
||||
destination = Path(path)
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
line = json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
||||
with destination.open("a", encoding="utf-8") as stream:
|
||||
stream.write(line + "\n")
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
|
||||
|
||||
def append_jsonl_many(
|
||||
path: str | Path, payloads: Iterable[Mapping[str, Any]]
|
||||
) -> None:
|
||||
"""Durably append a batch while paying the fsync cost only once."""
|
||||
destination = Path(path)
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
lines = [
|
||||
json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
||||
for payload in payloads
|
||||
]
|
||||
if not lines:
|
||||
return
|
||||
with destination.open("a", encoding="utf-8") as stream:
|
||||
stream.write("\n".join(lines) + "\n")
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
|
||||
|
||||
def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
|
||||
source = Path(path)
|
||||
if not source.exists():
|
||||
return []
|
||||
records: list[dict[str, Any]] = []
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
lines = stream.readlines()
|
||||
nonempty_lines = [
|
||||
index for index, line in enumerate(lines, 1) if line.strip()
|
||||
]
|
||||
last_nonempty_line = nonempty_lines[-1] if nonempty_lines else 0
|
||||
for line_number, line in enumerate(lines, 1):
|
||||
if not line.strip():
|
||||
continue
|
||||
try:
|
||||
value = json.loads(line)
|
||||
except json.JSONDecodeError as error:
|
||||
if line_number == last_nonempty_line:
|
||||
break
|
||||
raise ValueError(
|
||||
f"{source}:{line_number}: invalid JSONL record"
|
||||
) from error
|
||||
if not isinstance(value, dict):
|
||||
raise ValueError(f"{source}:{line_number}: record must be an object")
|
||||
records.append(value)
|
||||
return records
|
||||
|
||||
|
||||
def load_json(path: str | Path) -> dict[str, Any] | None:
|
||||
source = Path(path)
|
||||
if not source.exists():
|
||||
return None
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
value = json.load(stream)
|
||||
if not isinstance(value, dict):
|
||||
raise ValueError(f"{source} must contain a JSON object")
|
||||
return value
|
||||
|
||||
|
||||
def completed_scan_keys(
|
||||
records: Iterable[Mapping[str, Any]],
|
||||
) -> set[tuple[str, int, str, int]]:
|
||||
keys: set[tuple[str, int, str, int]] = set()
|
||||
for record in records:
|
||||
if record.get("kind", "sample") != "sample":
|
||||
continue
|
||||
keys.add(
|
||||
(
|
||||
str(record["phase"]),
|
||||
int(record["cycle"]),
|
||||
str(record["direction"]),
|
||||
int(record["command_u8"]),
|
||||
)
|
||||
)
|
||||
return keys
|
||||
from .core.artifacts.storage import * # noqa: F401,F403
|
||||
|
||||
@@ -1,893 +1,4 @@
|
||||
"""Chinese, operator-facing diagnostics for three-camera calibration."""
|
||||
"""Compatibility import for the installed Chinese model diagnostics."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
|
||||
STATE_NAMES_ZH = {
|
||||
"PREFLIGHT": "设备和标签预检",
|
||||
"WAIT_START": "等待开始标定",
|
||||
"IMPORTING_BASE": "正在读取基础标定会话",
|
||||
"REVALIDATING_INHERITED": "正在复核继承的四指数据",
|
||||
"RETURN_BASELINE": "正在恢复目标姿态",
|
||||
"PREPARE_SWEEP": "正在到达扫描起点",
|
||||
"SWEEP": "正在采集轨迹",
|
||||
"FITTING": "正在拟合轨迹和零位",
|
||||
"VALIDATION_MOVE": "正在移动到随机复测位置",
|
||||
"VALIDATION_CAPTURE": "正在采集随机复测数据",
|
||||
"PAUSED": "标定已暂停",
|
||||
"ABORTED": "标定已终止",
|
||||
"COMPLETE": "标定已完成",
|
||||
}
|
||||
|
||||
VIEW_NAMES_ZH = {
|
||||
"front": "正面",
|
||||
"side": "侧面",
|
||||
"top": "上面",
|
||||
}
|
||||
|
||||
JOINT_NAMES_ZH = {
|
||||
"thumb_cmc_pitch": "拇指CMC俯仰",
|
||||
"thumb_cmc_roll": "拇指CMC滚转",
|
||||
"thumb_mcp": "拇指MCP",
|
||||
"thumb_ip": "拇指IP(被动)",
|
||||
"index_mcp_roll": "食指MCP侧摆",
|
||||
"index_mcp_pitch": "食指MCP屈伸",
|
||||
"index_pip": "食指PIP",
|
||||
"index_dip": "食指DIP(被动)",
|
||||
"middle_mcp_roll": "中指MCP侧摆",
|
||||
"middle_mcp_pitch": "中指MCP屈伸",
|
||||
"middle_pip": "中指PIP",
|
||||
"middle_dip": "中指DIP(被动)",
|
||||
"ring_mcp_roll": "无名指MCP侧摆",
|
||||
"ring_mcp_pitch": "无名指MCP屈伸",
|
||||
"ring_pip": "无名指PIP",
|
||||
"ring_dip": "无名指DIP(被动)",
|
||||
"pinky_mcp_roll": "小指MCP侧摆",
|
||||
"pinky_mcp_pitch": "小指MCP屈伸",
|
||||
"pinky_pip": "小指PIP",
|
||||
"pinky_dip": "小指DIP(被动)",
|
||||
"thumb_cmc_yaw": "拇指CMC侧摆",
|
||||
"index_mcp_roll_side": "食指MCP侧摆(侧面校验)",
|
||||
"middle_mcp_roll_side": "中指MCP侧摆(侧面校验)",
|
||||
"ring_mcp_roll_side": "无名指MCP侧摆(侧面校验)",
|
||||
"pinky_mcp_roll_side": "小指MCP侧摆(侧面校验)",
|
||||
}
|
||||
|
||||
|
||||
def _format_u8(value: Any) -> str:
|
||||
if value is None:
|
||||
return "尚无反馈"
|
||||
return f"{float(value):.1f}"
|
||||
|
||||
|
||||
def _task_text(active: Mapping[str, Any]) -> str:
|
||||
if not active:
|
||||
return "尚无活动任务"
|
||||
view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
|
||||
if active.get("kind") == "fit_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位,{joint_text}拟合检查失败,"
|
||||
f"电机{active.get('motor_index')},"
|
||||
f"第{active.get('attempt', 1)}次尝试"
|
||||
)
|
||||
if active.get("kind") == "zero_model_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位,{joint_text}零位/URDF验证失败,"
|
||||
f"电机{active.get('motor_index')},不会自动重扫"
|
||||
)
|
||||
if active.get("kind") == "motion_stall":
|
||||
return (
|
||||
f"电机{active.get('motor_index', '?')}运动停滞,目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}"
|
||||
)
|
||||
if active.get("kind") == "cross_view_roll_diagnostic":
|
||||
return (
|
||||
f"{active.get('finger', '?')}侧摆跨机位诊断完成:"
|
||||
f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°,"
|
||||
f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°"
|
||||
)
|
||||
if active.get("kind") == "validation":
|
||||
return (
|
||||
f"{view}机位,随机复测,电机{active.get('motor_index')},"
|
||||
f"目标命令{active.get('command_u8')}"
|
||||
)
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
start = active.get("start_u8")
|
||||
target = active.get("target_u8")
|
||||
cycle = active.get("cycle", "?")
|
||||
repetitions = active.get("repetitions", "?")
|
||||
task = (
|
||||
f"{view}机位,{joint_text},电机{active.get('motor_index')},"
|
||||
f"第{cycle}/{repetitions}轮,{start}→{target}"
|
||||
)
|
||||
fit_attempt = int(active.get("fit_attempt", 1))
|
||||
if fit_attempt > 1:
|
||||
retry_cycles = active.get("fit_retry_cycles", [])
|
||||
if retry_cycles:
|
||||
task += "(补采异常轮" + "/".join(
|
||||
str(cycle) for cycle in retry_cycles
|
||||
) + ")"
|
||||
else:
|
||||
task += f"(拟合补采第{fit_attempt}次)"
|
||||
return task
|
||||
|
||||
|
||||
def three_camera_reason_zh(
|
||||
state: str,
|
||||
reason: str,
|
||||
active: Mapping[str, Any],
|
||||
) -> tuple[str, str]:
|
||||
"""Translate a reason code and provide one concrete operator action."""
|
||||
reason = str(reason)
|
||||
sample = active.get("sample", {}) if active else {}
|
||||
missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
|
||||
sample_range = (
|
||||
f"{_format_u8(sample.get('minimum_u8'))}~"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
tolerance = sample.get("endpoint_tolerance_u8", "?")
|
||||
|
||||
if reason.startswith("motor_state_stalled:"):
|
||||
fields = reason.split(":")
|
||||
context = fields[1] if len(fields) > 1 else "unknown"
|
||||
error_match = re.search(r"error_u8=([0-9.]+)", reason)
|
||||
error = error_match.group(1) if error_match else "未知"
|
||||
timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason)
|
||||
timeout_value = active.get("timeout_seconds")
|
||||
if timeout_value is None and timeout_match is not None:
|
||||
timeout_value = float(timeout_match.group(1))
|
||||
duration = (
|
||||
f"连续{float(timeout_value):g}秒"
|
||||
if timeout_value is not None
|
||||
else "在规定时间内"
|
||||
)
|
||||
motor = active.get("motor_index")
|
||||
if motor is not None:
|
||||
return (
|
||||
f"电机{motor}反馈{duration}没有向目标推进;目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8,"
|
||||
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
|
||||
f"(阶段={context})。程序已保持当前位置。",
|
||||
"若实际反馈是稳定的固件端点,应只配置该电机该端点的专用容差后"
|
||||
"重启;若仍在变化或有摩擦,则先排查机械问题,不要反复resume强推。",
|
||||
)
|
||||
return (
|
||||
f"电机反馈{duration}没有向目标推进;停止位置距目标{error}个u8"
|
||||
f"(阶段={context})。程序已保持当前位置,防止机械碰撞或摩擦加重。",
|
||||
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
|
||||
"正常端点,应配置该电机专用端点容差后重启标定;不要反复调用resume强推。",
|
||||
)
|
||||
|
||||
base_reason, separator, reason_detail = reason.partition(":")
|
||||
if base_reason in {
|
||||
"sweep_missing_endpoint_bin",
|
||||
"sweep_bins_too_few",
|
||||
"sweep_bin_gap_too_large",
|
||||
"task_precheck_missing_command_127",
|
||||
"task_precheck_detection_rate_too_low",
|
||||
"synchronised_tag_state_timeout",
|
||||
}:
|
||||
reason = base_reason
|
||||
detail_label = (
|
||||
JOINT_NAMES_ZH.get(reason_detail, reason_detail)
|
||||
if separator and reason_detail
|
||||
else ""
|
||||
)
|
||||
detail_prefix = f"{detail_label}:" if detail_label else ""
|
||||
|
||||
if "URDF zero offset reached the configured" in reason:
|
||||
bound_match = re.search(
|
||||
r"configured\s+([0-9.]+)\s+degree bound", reason
|
||||
)
|
||||
bound = bound_match.group(1) if bound_match else "配置的"
|
||||
hit_text = ""
|
||||
if "bound:" in reason:
|
||||
hit_text = reason.split("bound:", 1)[1].split(
|
||||
"; all_offsets:", 1
|
||||
)[0]
|
||||
for name, label in JOINT_NAMES_ZH.items():
|
||||
hit_text = hit_text.replace(name, label)
|
||||
hit_suffix = f";触边关节:{hit_text}" if hit_text else ""
|
||||
return (
|
||||
f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
|
||||
"零位结果,而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
|
||||
"不要调用resume,也不要增大零位边界。先确认Tag有效黑框边长、三相机"
|
||||
"内外参和原始CAD URDF;Tag尺寸修正后必须调用start重新采集,旧尺度"
|
||||
"产生的轨迹不能直接生成修正URDF。",
|
||||
)
|
||||
|
||||
if reason == "sweep_missing_endpoint_bin":
|
||||
missing_text = "、".join(str(value) for value in missing) or "0或255"
|
||||
return (
|
||||
f"{detail_prefix}本方向已有{active.get('valid_frames', 0)}帧同步有效数据,但缺少"
|
||||
f"电机端点{missing_text}附近的有效分箱;采样到的实际电机范围为"
|
||||
f"{sample_range},端点容差为±{tolerance}。这通常表示电机虽然运动到"
|
||||
"端点,但该时刻没有同时取得有效Tag图像和电机状态。",
|
||||
"确认当前机位所需Tag在整个行程(尤其缺失端点)均可见,然后调用"
|
||||
"/g20_calibration/resume;程序会重新扫描当前方向,不要调用start。",
|
||||
)
|
||||
if reason == "sweep_bins_too_few":
|
||||
return (
|
||||
f"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个,要求至少"
|
||||
f"{sample.get('minimum_bin_count', '?')}个;当前采样范围{sample_range}。",
|
||||
"检查Tag连续识别和电机状态频率,修正后调用resume重新扫描当前方向。",
|
||||
)
|
||||
if reason == "sweep_bin_gap_too_large":
|
||||
gap_start = sample.get("maximum_bin_gap_start_u8")
|
||||
gap_end = sample.get("maximum_bin_gap_end_u8")
|
||||
gap_range = (
|
||||
""
|
||||
if gap_start is None or gap_end is None
|
||||
else f"({gap_start}→{gap_end})"
|
||||
)
|
||||
return (
|
||||
f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')},"
|
||||
f"{gap_range}允许值不超过"
|
||||
f"{sample.get('allowed_maximum_bin_gap', '?')}。",
|
||||
"检查运动中Tag是否间歇丢失;修正遮挡、反光或对焦后调用resume。",
|
||||
)
|
||||
if reason == "synchronised_tag_state_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
if isinstance(group_reasons, Mapping) and group_reasons:
|
||||
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||
group_rejections = active.get(
|
||||
"group_pnp_rejection_counts", {}
|
||||
)
|
||||
missing_roles = active.get(
|
||||
"group_missing_candidate_roles", {}
|
||||
)
|
||||
candidate_diagnostics = active.get(
|
||||
"pnp_candidate_diagnostics", {}
|
||||
)
|
||||
view_details: list[str] = []
|
||||
for view, value in group_reasons.items():
|
||||
view_name = str(view)
|
||||
parts = [str(value)]
|
||||
missing = (
|
||||
missing_roles.get(view_name, ())
|
||||
if isinstance(missing_roles, Mapping)
|
||||
else ()
|
||||
)
|
||||
if isinstance(missing, Sequence) and not isinstance(
|
||||
missing, (str, bytes)
|
||||
) and missing:
|
||||
parts.append(
|
||||
"缺候选=" + ",".join(str(role) for role in missing)
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for source in (tag_rejections, group_rejections):
|
||||
values = (
|
||||
source.get(view_name)
|
||||
if isinstance(source, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(values, Mapping):
|
||||
for name, count in values.items():
|
||||
counts[str(name)] = counts.get(str(name), 0) + int(
|
||||
count
|
||||
)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:3]
|
||||
parts.append(
|
||||
"累计拒绝="
|
||||
+ ",".join(
|
||||
f"{name}×{count}" for name, count in common
|
||||
)
|
||||
)
|
||||
view_candidates = (
|
||||
candidate_diagnostics.get(view_name, {})
|
||||
if isinstance(candidate_diagnostics, Mapping)
|
||||
else {}
|
||||
)
|
||||
if isinstance(view_candidates, Mapping) and missing:
|
||||
summaries: list[str] = []
|
||||
for role in missing:
|
||||
diagnostic = view_candidates.get(str(role), {})
|
||||
if not isinstance(diagnostic, Mapping):
|
||||
continue
|
||||
summaries.append(
|
||||
f"{role}(solve="
|
||||
f"{int(diagnostic.get('solved_candidate_count', 0))},"
|
||||
"reproj="
|
||||
f"{int(diagnostic.get('reprojection_candidate_count', 0))},"
|
||||
"tilt="
|
||||
f"{int(diagnostic.get('independent_tilt_candidate_count', 0))})"
|
||||
)
|
||||
if summaries:
|
||||
parts.append("候选统计=" + ",".join(summaries))
|
||||
view_details.append(
|
||||
f"{VIEW_NAMES_ZH.get(view_name, view_name)}="
|
||||
+ ";".join(parts)
|
||||
)
|
||||
reason_text = "、".join(view_details)
|
||||
return (
|
||||
f"{detail_prefix}已经取得部分有效轨迹,但Tag仍可见且反馈正常时,"
|
||||
"后续连续图像帧"
|
||||
"被整组PnP几何检查拒绝"
|
||||
f"({reason_text}),因此无法与电机状态形成有效轨迹帧。",
|
||||
"不要调整或反复粘贴Tag;保留当前会话中的"
|
||||
"group_pnp_candidate_event,"
|
||||
"按缺失角色的候选统计检查PnP分支逻辑。",
|
||||
)
|
||||
return (
|
||||
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
"按时间戳配对”的图像帧。",
|
||||
"查看下面活动机位的缺失Tag,确认状态话题仍在更新;修正后调用resume,"
|
||||
"程序会重扫当前方向。",
|
||||
)
|
||||
if reason == "task_precheck_missing_command_127":
|
||||
return (
|
||||
f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。",
|
||||
"检查中位姿态的Tag遮挡和反光;程序只会重扫当前物理任务。",
|
||||
)
|
||||
if reason == "task_precheck_detection_rate_too_low":
|
||||
return (
|
||||
f"{detail_prefix}低速预检的有效Tag识别率低于门限。",
|
||||
"检查该机位当前任务Tag的遮挡、反光和对焦;程序只会重扫当前物理任务。",
|
||||
)
|
||||
if reason == "sweep_start_position_timeout":
|
||||
return (
|
||||
f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
|
||||
f"{active.get('start_u8')},当前实际值{_format_u8(active.get('actual_u8'))}。",
|
||||
"检查CAN、机械手使能和是否存在机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "sweep_start_tag_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
progress_by_view = active.get("pnp_initialization_progress", {})
|
||||
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||
group_rejections = active.get("group_pnp_rejection_counts", {})
|
||||
if any(
|
||||
isinstance(value, Mapping) and bool(value)
|
||||
for value in (
|
||||
group_reasons,
|
||||
progress_by_view,
|
||||
tag_rejections,
|
||||
group_rejections,
|
||||
)
|
||||
):
|
||||
details: list[str] = []
|
||||
views = set()
|
||||
for value in (
|
||||
group_reasons,
|
||||
progress_by_view,
|
||||
tag_rejections,
|
||||
group_rejections,
|
||||
):
|
||||
if isinstance(value, Mapping):
|
||||
views.update(str(view) for view in value)
|
||||
for view in sorted(views):
|
||||
parts: list[str] = []
|
||||
progress = (
|
||||
progress_by_view.get(view)
|
||||
if isinstance(progress_by_view, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(progress, Mapping):
|
||||
parts.append(
|
||||
"初始化"
|
||||
f"{int(progress.get('accepted', 0))}/"
|
||||
f"{int(progress.get('required', 0))}"
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for source in (tag_rejections, group_rejections):
|
||||
values = (
|
||||
source.get(view)
|
||||
if isinstance(source, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(values, Mapping):
|
||||
for name, count in values.items():
|
||||
counts[str(name)] = (
|
||||
counts.get(str(name), 0) + int(count)
|
||||
)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:3]
|
||||
parts.append(
|
||||
"累计拒绝="
|
||||
+ ",".join(
|
||||
f"{name}×{count}" for name, count in common
|
||||
)
|
||||
)
|
||||
latest = (
|
||||
group_reasons.get(view)
|
||||
if isinstance(group_reasons, Mapping)
|
||||
else None
|
||||
)
|
||||
if latest and not str(latest).startswith(
|
||||
"group_initializing:"
|
||||
):
|
||||
parts.append(f"最后状态={latest}")
|
||||
if parts:
|
||||
details.append(
|
||||
f"{VIEW_NAMES_ZH.get(view, view)}=" + ";".join(parts)
|
||||
)
|
||||
reason_text = "、".join(details) or "未形成完整初始化窗口"
|
||||
return (
|
||||
"被测电机已经到达扫描起点,所需Tag也可见,但三维PnP位姿初始化"
|
||||
f"没有完成({reason_text}),因此没有生成同步端点帧。",
|
||||
"不要根据可见性重复粘贴Tag;保留累计拒绝原因并检查PnP候选选择。",
|
||||
)
|
||||
return (
|
||||
"被测电机已经到达扫描起点,但当前任务所需的实时运动Tag没有形成足够的"
|
||||
"同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”,不会触发此错误。",
|
||||
"只检查标记为✗的实时运动Tag、反光和外部遮挡;不要移动相机或手掌底座。",
|
||||
)
|
||||
if reason == "sweep_timeout":
|
||||
return (
|
||||
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
|
||||
"检查电机实际值、Tag连续识别和标定速度,修正后调用resume。",
|
||||
)
|
||||
if reason == "return_baseline_timeout":
|
||||
return (
|
||||
"一个或多个标定电机未在规定时间返回基准命令。",
|
||||
"检查机械手状态、CAN和机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_move_timeout":
|
||||
return (
|
||||
"随机复测时电机未在规定时间到达目标命令。",
|
||||
"检查机械手状态和机械卡阻,确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_capture_timeout":
|
||||
return (
|
||||
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
||||
"检查当前机位Tag可见性后调用resume。",
|
||||
)
|
||||
if reason == "palm_orientation_quality_failed":
|
||||
failures = active.get("failures", [])
|
||||
detail = (
|
||||
str(failures[0].get("reason", "方向观测不足"))
|
||||
if failures
|
||||
else "方向观测不足"
|
||||
)
|
||||
if "thumb_cmc_" in detail:
|
||||
return (
|
||||
"拇指CMC yaw无法由顶部Tag 8/9的零位邻近短轨迹稳定确定:"
|
||||
+ detail,
|
||||
"保持Tag安装不变;确保顶部Tag 8/9在拇指CMC pitch和roll"
|
||||
"从零位开始的前1/4行程持续可见后重新标定。",
|
||||
)
|
||||
return (
|
||||
"掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定:"
|
||||
+ detail,
|
||||
"保持Tag安装不变;让正面Tag 10–13在对应MCP-pitch起始段"
|
||||
"至少可见15°行程后重新标定。",
|
||||
)
|
||||
if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
|
||||
metric_names = {
|
||||
"plane_rms_mm": "平面拟合RMS",
|
||||
"radial_rms_mm": "圆半径拟合RMS",
|
||||
"radius_mm": "拟合半径",
|
||||
"image_radial_rms_px": "二维圆半径拟合RMS",
|
||||
"image_radial_p95_px": "二维圆半径误差P95",
|
||||
"image_radius_px": "二维拟合半径",
|
||||
"arc_deg": "实测圆弧",
|
||||
"monotonic_correction_deg": "最大单调修正",
|
||||
"hysteresis_deg": "最大正反程差",
|
||||
"baseline_hysteresis_deg": "baseline正反程关节角差",
|
||||
"baseline_directional_gap_deg": "baseline方向分支间隙",
|
||||
"baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差",
|
||||
"cycle_travel_range_deg": "三轮行程差",
|
||||
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
|
||||
"axis_plane_rms_mm": "三维圆轴向RMS",
|
||||
"axis_radial_rms_mm": "三维圆半径RMS",
|
||||
"axis_pose_line_rms_mm": "姿态轨迹轴线RMS",
|
||||
"axis_line_cycle_rms_mm": "四轮轴线位置RMS",
|
||||
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
|
||||
"axis_cycle_difference_deg": "各轮转轴方向极差",
|
||||
"cross_view_roll_curve": "正面/侧面关节角曲线差异RMS",
|
||||
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
||||
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
||||
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
||||
"zero_cycle_offset_range_deg": "训练轮零位极差",
|
||||
"zero_confidence_95_half_width_deg": "零位95%置信半宽",
|
||||
"state_image_sync_p95_ms": "图像与电机状态同步误差P95",
|
||||
"tag_valid_rate_percent": "所需Tag同时有效率",
|
||||
}
|
||||
metric_units = {
|
||||
"plane_rms_mm": "mm",
|
||||
"radial_rms_mm": "mm",
|
||||
"radius_mm": "mm",
|
||||
"image_radial_rms_px": "px",
|
||||
"image_radial_p95_px": "px",
|
||||
"image_radius_px": "px",
|
||||
"arc_deg": "°",
|
||||
"monotonic_correction_deg": "°",
|
||||
"hysteresis_deg": "°",
|
||||
"baseline_hysteresis_deg": "°",
|
||||
"baseline_directional_gap_deg": "°",
|
||||
"baseline_directional_gap_range_deg": "°",
|
||||
"cycle_travel_range_deg": "°",
|
||||
"rotation_orthogonal_rms_deg": "°",
|
||||
"axis_plane_rms_mm": "mm",
|
||||
"axis_radial_rms_mm": "mm",
|
||||
"axis_pose_line_rms_mm": "mm",
|
||||
"axis_line_cycle_rms_mm": "mm",
|
||||
"rotation_circle_axis_difference_deg": "°",
|
||||
"axis_cycle_difference_deg": "°",
|
||||
"third_cycle_axis_holdout_deg": "°",
|
||||
"third_cycle_axis_line_rms_mm": "mm",
|
||||
"third_cycle_trajectory_p95_deg": "°",
|
||||
"zero_cycle_offset_range_deg": "°",
|
||||
"zero_confidence_95_half_width_deg": "°",
|
||||
"state_image_sync_p95_ms": "ms",
|
||||
"tag_valid_rate_percent": "%",
|
||||
"cross_view_roll_curve": "°",
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
metric = str(failure.get("metric", ""))
|
||||
if metric in metric_names:
|
||||
comparison = str(failure.get("comparison", ""))
|
||||
requirement = "不超过" if comparison == "maximum" else "至少"
|
||||
unit = metric_units[metric]
|
||||
detail = (
|
||||
f"{joint}的{metric_names[metric]}为"
|
||||
f"{float(failure.get('actual', 0.0)):.2f}{unit},"
|
||||
f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
|
||||
)
|
||||
cycle_travel = failure.get("cycle_travel_deg", [])
|
||||
if cycle_travel:
|
||||
detail += "(各轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_travel
|
||||
) + ")"
|
||||
cycle_values = failure.get("cycle_values_deg", [])
|
||||
if not cycle_values:
|
||||
cycle_values = failure.get("cycle_offset_deg", [])
|
||||
if cycle_values:
|
||||
detail += "(各轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_values
|
||||
) + ")"
|
||||
details.append(detail)
|
||||
else:
|
||||
cycle = failure.get("cycle")
|
||||
cycle_text = "" if cycle is None else f"第{cycle}轮"
|
||||
details.append(
|
||||
f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败:"
|
||||
f"{failure.get('reason', '未知原因')}"
|
||||
)
|
||||
detail_text = ";".join(details) or "当前关节的轨迹拟合未通过"
|
||||
directional_gap_failure = any(
|
||||
str(failure.get("metric", "")).startswith(
|
||||
"baseline_directional_gap"
|
||||
)
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
if reason == "joint_fit_systematic_failure":
|
||||
cross_view_systematic = any(
|
||||
failure.get("classification")
|
||||
in {
|
||||
"stable_cross_view_installation_or_model_bias",
|
||||
"stable_cross_view_direction_conflict",
|
||||
}
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
suggestion = (
|
||||
"四轮都出现稳定的正面/侧面差异,属于Tag安装外参或跨视角模型偏差,"
|
||||
"继续重扫不会改善;检查Tag刚性安装与跨视角安装变换,不要放宽门限。"
|
||||
if cross_view_systematic
|
||||
else "各轮重复出现同一模型冲突,继续运动不会改善;程序已禁止自动重扫。"
|
||||
"请直接复制诊断块给开发者,不要放宽门限。"
|
||||
)
|
||||
else:
|
||||
source_task_names = set(active.get("source_task_names", []))
|
||||
thumb_yaw_source_retry = source_task_names == {
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
}
|
||||
suggestion = (
|
||||
"方向分支已由软件保留,不要放宽门限;请检查传动回差或高支架刚度,"
|
||||
"处理后重新执行一键标定命令,程序会从最近可靠断点继续。"
|
||||
if directional_gap_failure
|
||||
else (
|
||||
"保持顶部Tag 8/9无遮挡;程序只替换决定yaw零位的"
|
||||
"CMC pitch/roll顶部轴观测并重扫"
|
||||
f"{active.get('directions_to_rescan', 16)}个方向,"
|
||||
"不会无效重扫yaw侧摆。"
|
||||
if thumb_yaw_source_retry
|
||||
else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令;"
|
||||
"程序只清除当前失败关节的数据并重扫"
|
||||
f"{active.get('directions_to_rescan', 6)}个方向,"
|
||||
"不需要手工调用ROS服务。"
|
||||
)
|
||||
)
|
||||
return (
|
||||
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
|
||||
suggestion,
|
||||
)
|
||||
if reason == "zero_model_validation_failed":
|
||||
reason_names = {
|
||||
"zero_offset_reached_configured_bound": "零位解触及安全边界",
|
||||
"zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
|
||||
"zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
|
||||
"zero_offset_cycle_difference_too_large": "三轮零位离散过大",
|
||||
"zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
|
||||
"zero_axis_cone_mismatch_too_large": (
|
||||
"父子轴夹角与原始URDF不一致,零位旋转无法解释"
|
||||
),
|
||||
"zero_phase_axis_line_residual_too_large": (
|
||||
"整段SE(3)运动无法稳定确定平行轴线相位"
|
||||
),
|
||||
"zero_offset_did_not_improve_with_95pct_confidence": (
|
||||
"第三轮留出验证未以95%置信度改善"
|
||||
),
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
if failure.get("metric") == "zero_guard":
|
||||
reason_text = reason_names.get(
|
||||
str(failure.get("reason")), str(failure.get("reason"))
|
||||
)
|
||||
if "actual_deg" in failure and "limit_deg" in failure:
|
||||
reason_text += (
|
||||
f"(估计{float(failure['actual_deg']):+.2f}°,"
|
||||
f"允许±{float(failure['limit_deg']):.2f}°)"
|
||||
)
|
||||
details.append(f"{joint}:{reason_text}")
|
||||
return (
|
||||
"轨迹采集已完成,但零位/URDF几何验证失败"
|
||||
+ ("(" + ";".join(details) + ")" if details else "")
|
||||
+ "。程序没有生成正式JSON或修正URDF。",
|
||||
"该类稳定模型失败不能靠重复运动修复,程序不会自动重扫;"
|
||||
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
|
||||
)
|
||||
if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪;此阶段不以Tag可见性阻止基准恢复。",
|
||||
"保持机械手运动范围无障碍;设备就绪后系统会先安全恢复基准形态,再检查掌部Tag。",
|
||||
)
|
||||
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查;全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "waiting_for_baseline_tags_after_recovery":
|
||||
return (
|
||||
"机械手已经稳定恢复到基准形态,正在用新采集的画面确认三台相机各自的固定掌部Tag。",
|
||||
"若某个掌部Tag持续缺失,只调整遮挡手指或检查Tag固定情况,不要移动相机和手掌底座。",
|
||||
)
|
||||
if reason == "locking_fixed_base_references":
|
||||
return (
|
||||
"基准形态Tag预检已通过,正在把三台相机的固定掌部Tag稳健锁定为本会话参考。",
|
||||
"无需操作;锁定完成后允许任务姿态遮挡固定掌部Tag。",
|
||||
)
|
||||
if reason == "fixed_base_reference_moved":
|
||||
return (
|
||||
"顶部Tag 8在本会话基准锁定后连续多帧发生角点位移;程序已立即保持机械手当前位置,"
|
||||
"本会话中已采集数据不再用于发布。",
|
||||
"Tag 8允许在下一次标定预检前重新摆放,但本次不能继续;固定Tag 8和顶部相机后"
|
||||
"重新启动新会话。",
|
||||
)
|
||||
if reason == "waiting_for_task_tags_at_sweep_start":
|
||||
return (
|
||||
"电机已到扫描起点,正在等待当前任务的实时运动Tag;显示为“锁”的固定掌部Tag"
|
||||
"允许被手指遮挡。",
|
||||
"只检查标记为✗的实时运动Tag;若均为✓或锁,程序会自动开始运动。",
|
||||
)
|
||||
if reason == "call_start_for_baseline_recovery":
|
||||
return (
|
||||
"相机数据和机械手反馈已就绪,等待一键程序触发安全基准恢复。",
|
||||
"保持机械手运动范围无障碍;程序会自动开始,无需手工调用ROS服务。",
|
||||
)
|
||||
if reason == "call_start":
|
||||
return (
|
||||
"三机位预检已经通过,等待操作员确认开始。",
|
||||
"清空机械手运动范围后调用/g20_calibration/start。",
|
||||
)
|
||||
if reason == "operator_pause":
|
||||
return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
|
||||
if reason == "operator_abort":
|
||||
return "操作员终止了本次标定,程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
|
||||
if reason == "collecting_timestamp_synchronised_tag_centres":
|
||||
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作,保持相机、标签和底座不动。"
|
||||
if reason == "collecting_dedicated_baseline_hold":
|
||||
return (
|
||||
"正在从当前方向到达关节baseline并静止采集Tag与电机反馈;这批数据单独用于回差验收。",
|
||||
"无需操作,保持相机、标签和底座不动。",
|
||||
)
|
||||
if reason == "steady checkpoint target is missing":
|
||||
return (
|
||||
"首轮稳态检查点已经到达最终端点,但采集状态没有及时切换到端点完成阶段。",
|
||||
"程序已停止发布并保留已采样数据;这是软件状态切换问题,不需要调整相机、Tag或机械手。",
|
||||
)
|
||||
if reason == "cross_view_roll_front_failure_deferred":
|
||||
return (
|
||||
"正面侧摆回差不合格已保留,诊断模式将继续采集同一手指的侧面数据。",
|
||||
"无需操作;该诊断会锁定URDF发布。",
|
||||
)
|
||||
if reason == "cross_view_roll_diagnostic_complete":
|
||||
interpretation = str(active.get("interpretation", ""))
|
||||
explanations = {
|
||||
"both_views_confirm_direction_dependent_pose": (
|
||||
"正面和侧面都确认了方向相关姿态,优先判断为roll输出机构或共同下游链的真实回差。"
|
||||
),
|
||||
"front_only_difference_check_roll_tag_bracket_or_front_pnp": (
|
||||
"只有正面差异超限,优先检查roll Tag高支架刚度和正面PnP。"
|
||||
),
|
||||
"side_only_difference_check_side_tag_chain_or_side_pnp": (
|
||||
"只有侧面差异超限,优先检查侧面Tag链和侧面PnP。"
|
||||
),
|
||||
"both_views_within_formal_hysteresis_limit": (
|
||||
"两个机位的静止回差均满足正式门限。"
|
||||
),
|
||||
}
|
||||
return (
|
||||
explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。")
|
||||
+ " 本次为诊断会话,不会生成或发布URDF。",
|
||||
"保存当前状态和raw_samples.jsonl;根据两机位结论处理后重新启动正式标定。",
|
||||
)
|
||||
if reason == "capturing_random_validation_pose":
|
||||
return "正在当前随机命令位置采集复测数据。", "无需操作,保持设备不动。"
|
||||
if reason in {"calibration_passed", "calibration_complete"}:
|
||||
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
||||
if reason == "quality_failed":
|
||||
return "标定流程完成,但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
||||
if reason.startswith("validated_endpoint_zero_state"):
|
||||
return (
|
||||
"轨迹和URDF零位验证已经通过,但发布前检测到端点零位状态缺失或与"
|
||||
"已验证模型不一致;这是程序内部状态生命周期错误,结果未发布。",
|
||||
"不要移动相机、Tag或机械手底座;保留当前会话并把原因码交给开发者。",
|
||||
)
|
||||
if reason.startswith("PUB-ARTIFACT-601:"):
|
||||
return (
|
||||
"标定节点已经生成通过质量门限的JSON和候选URDF,但一键程序在正式发布前"
|
||||
"发现这对产物的坐标、限位、哈希或资源一致性检查失败;原始URDF未被覆盖。",
|
||||
"不要重新标定相机或调整Tag;保留本会话产物和启动日志供开发者检查发布契约。",
|
||||
)
|
||||
if reason == "combination_pose_prediction_failed":
|
||||
return (
|
||||
"单关节、零位和URDF几何验证已通过,但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
|
||||
"程序会在原姿态重新初始化PnP并自动复测;若最终仍失败,请把raw_samples.jsonl中的"
|
||||
"combination_validation_failure记录交给开发者,不要重新采集16个单关节任务。",
|
||||
)
|
||||
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
|
||||
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
|
||||
if reason == "holding_same_finger_clearance_before_next_task":
|
||||
return (
|
||||
"同一根手指的上一项已经完成;相邻手指继续保持当前避让姿态,只调整"
|
||||
"被测关节以衔接下一项。",
|
||||
"无需操作,不要手动展开正在避让的手指。",
|
||||
)
|
||||
if state == "RETURN_BASELINE":
|
||||
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
return "所有扫描已经完成,正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
|
||||
return f"未分类原因码:{reason}", "保留该原因码和启动终端日志用于进一步定位。"
|
||||
|
||||
|
||||
def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
"""Render the complete operator status; the JSON topic remains unchanged."""
|
||||
state = str(payload.get("state", ""))
|
||||
active = payload.get("active", {})
|
||||
reason_zh, action_zh = three_camera_reason_zh(
|
||||
state, str(payload.get("reason", "")), active
|
||||
)
|
||||
progress = float(payload.get("progress", 0.0))
|
||||
completed = payload.get("completed_sweeps", 0)
|
||||
total = payload.get("total_sweeps", 0)
|
||||
scan_progress = float(
|
||||
payload.get(
|
||||
"scan_progress",
|
||||
0.0 if not total else float(completed) / float(total),
|
||||
)
|
||||
)
|
||||
lines = [
|
||||
f"状态:{STATE_NAMES_ZH.get(state, state)}({state})",
|
||||
f"原因:{reason_zh}",
|
||||
f"建议:{action_zh}",
|
||||
f"总体进度:{progress:.1%}(计划扫描{completed}/{total}个方向,"
|
||||
f"扫描进度{scan_progress:.1%})",
|
||||
f"当前任务:{_task_text(active)}",
|
||||
]
|
||||
if state == "RETURN_BASELINE":
|
||||
baseline_command = payload.get("baseline_command_u8", [])
|
||||
return_command = payload.get("return_command_u8", baseline_command)
|
||||
label = "恢复姿态" if return_command != baseline_command else "基准姿态"
|
||||
lines.append(f"正在确认{label}:{return_command}")
|
||||
if active and active.get("kind") not in {
|
||||
"fit_failure",
|
||||
"zero_model_failure",
|
||||
"motion_stall",
|
||||
}:
|
||||
retry_count = int(active.get("automatic_retry_count", 0))
|
||||
if retry_count:
|
||||
lines.append(
|
||||
"自动重试:当前方向已自动重扫"
|
||||
f"{retry_count}/{active.get('automatic_retry_limit', '?')}次,"
|
||||
f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%},"
|
||||
f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
|
||||
)
|
||||
sample = active.get("sample", {})
|
||||
motion_progress = active.get("motion_progress")
|
||||
motion_text = (
|
||||
"未知" if motion_progress is None else f"{float(motion_progress):.1%}"
|
||||
)
|
||||
lines.append(
|
||||
"运动采样:"
|
||||
f"目标{active.get('target_u8', active.get('command_u8', '?'))},"
|
||||
f"实际{_format_u8(active.get('actual_u8'))},"
|
||||
f"本方向{motion_text},有效帧{active.get('valid_frames', 0)},"
|
||||
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}~"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
detection_frames = int(active.get("detection_frames", 0))
|
||||
if detection_frames:
|
||||
lines.append(
|
||||
"本方向Tag检出:"
|
||||
f"{float(active.get('detection_rate', 0.0)):.1%}"
|
||||
f"({active.get('detection_valid_frames', 0)}/"
|
||||
f"{detection_frames}帧)"
|
||||
)
|
||||
auxiliary = active.get("auxiliary_motors", [])
|
||||
if auxiliary:
|
||||
lines.append(
|
||||
"避挡姿态:"
|
||||
+ ",".join(
|
||||
f"电机{item.get('motor_index')}目标"
|
||||
f"{item.get('command_u8')}、实际"
|
||||
f"{_format_u8(item.get('actual_u8'))}"
|
||||
for item in auxiliary
|
||||
)
|
||||
)
|
||||
speed = active.get("speed", {})
|
||||
if speed:
|
||||
lines.append(
|
||||
"阶段速度:五指目标"
|
||||
f"{speed.get('commanded_finger_speed')},SDK报告"
|
||||
f"{speed.get('reported_finger_speed')}"
|
||||
)
|
||||
if active.get("sweep_timeout_seconds") is not None:
|
||||
lines.append(
|
||||
"运动保护:扫描超时"
|
||||
f"{float(active['sweep_timeout_seconds']):.1f}s,"
|
||||
"连续"
|
||||
f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
|
||||
"进展不足"
|
||||
f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
|
||||
"则立即暂停"
|
||||
)
|
||||
lines.append("机位:")
|
||||
for name, view in payload.get("views", {}).items():
|
||||
missing = view.get("missing_tag_ids", [])
|
||||
missing_text = "无" if not missing else ",".join(map(str, missing))
|
||||
lines.append(
|
||||
f"- {VIEW_NAMES_ZH.get(str(name), str(name))}:"
|
||||
f"{'就绪' if view.get('ready') else '等待'},"
|
||||
f"外参{'匹配' if view.get('camera_extrinsics_valid') else '不匹配'},"
|
||||
f"{float(view.get('detection_hz', 0.0)):.1f}Hz,"
|
||||
f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%},"
|
||||
f"当前缺失Tag={missing_text}"
|
||||
)
|
||||
extrinsics_error = payload.get("camera_extrinsics_error")
|
||||
if extrinsics_error:
|
||||
lines.append(f"外参文件:{extrinsics_error}")
|
||||
lines.append(f"JSON结果:{payload.get('result_path') or '尚未生成'}")
|
||||
lines.append(
|
||||
f"修正URDF:{payload.get('corrected_urdf_path') or '尚未生成'}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
from .models.g20.reporting_zh import * # noqa: F401,F403
|
||||
from .models.g20.reporting_zh import _task_text
|
||||
|
||||
@@ -35,11 +35,11 @@ from .acquisition import (
|
||||
update_pnp_reset_watchdog,
|
||||
)
|
||||
from .core import (
|
||||
COMMAND_NAMES,
|
||||
DIRECTION_DECREASING,
|
||||
DIRECTION_INCREASING,
|
||||
robust_rotation_summary,
|
||||
)
|
||||
from .core.urdf import build_correction_plan
|
||||
from .extrinsics import (
|
||||
ThreeCameraExtrinsics,
|
||||
camera_info_fingerprint,
|
||||
@@ -74,6 +74,7 @@ from .full_hand import (
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
||||
from .models.g20.command_layout import G20_COMMAND_NAMES as COMMAND_NAMES
|
||||
from .pnp import (
|
||||
SquareTagGroupPoseTracker,
|
||||
SquareTagPose,
|
||||
@@ -318,13 +319,19 @@ def _palm_axis_observer_schema(
|
||||
def _palm_axis_resume_policy(
|
||||
profile: HandCalibrationProfile,
|
||||
session_start: Mapping[str, Any],
|
||||
compatibility_tokens: frozenset[str] | None = None,
|
||||
) -> tuple[bool, tuple[str, ...]]:
|
||||
"""Validate optional palm-axis checkpoint data when a model uses it."""
|
||||
capability = "palm_axis_relative_motion_v3"
|
||||
previous = {str(value) for value in session_start.get("capabilities", [])}
|
||||
if not profile.palm_axis_observers and capability not in profile.capabilities:
|
||||
current = (
|
||||
frozenset(profile.capabilities)
|
||||
if compatibility_tokens is None
|
||||
else frozenset(compatibility_tokens)
|
||||
)
|
||||
if not profile.palm_axis_observers and capability not in current:
|
||||
return True, ()
|
||||
required_previous = set(profile.capabilities) - {capability}
|
||||
required_previous = set(current) - {capability}
|
||||
if not required_previous.issubset(previous):
|
||||
raise ValueError("resume checkpoint lacks required capabilities")
|
||||
if capability in previous:
|
||||
@@ -741,7 +748,7 @@ def _requires_pnp_tracker_reset_for_sweep(
|
||||
return False
|
||||
if is_fit_retry:
|
||||
return True
|
||||
if profile.supports("precheck_sweeps"):
|
||||
if profile.precheck_sweeps:
|
||||
return bool(item.precheck)
|
||||
return not item.precheck and item.cycle == 0
|
||||
|
||||
@@ -2015,7 +2022,7 @@ def _build_sweep_plan(
|
||||
"""
|
||||
plan: list[SweepItem] = []
|
||||
for spec in profile.sweep_specs:
|
||||
if profile.supports("precheck_sweeps"):
|
||||
if profile.precheck_sweeps:
|
||||
plan.extend(
|
||||
SweepItem(spec, -1, direction, precheck=True)
|
||||
for direction in (
|
||||
@@ -2708,6 +2715,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
)
|
||||
self.profile = product_contract.profile
|
||||
self.zero_profile = product_contract.zero_profile
|
||||
self.calibration_profile = product_contract.typed_profile
|
||||
self.serial_number = str(value("serial_number"))
|
||||
if self.serial_number == "UNSET":
|
||||
raise ValueError("serial_number is required")
|
||||
@@ -5226,7 +5234,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
(
|
||||
palm_axis_schema_compatible,
|
||||
palm_axis_invalidated_tasks,
|
||||
) = _palm_axis_resume_policy(self.profile, start)
|
||||
) = _palm_axis_resume_policy(
|
||||
self.profile,
|
||||
start,
|
||||
getattr(
|
||||
getattr(
|
||||
getattr(self, "calibration_profile", None),
|
||||
"artifacts",
|
||||
None,
|
||||
),
|
||||
"session_compatibility_tokens",
|
||||
frozenset(self.profile.capabilities),
|
||||
),
|
||||
)
|
||||
except ValueError as error:
|
||||
raise RuntimeError(
|
||||
"resume checkpoint algorithm capabilities differ"
|
||||
@@ -5646,7 +5666,19 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
self.recalibration_task_keys
|
||||
),
|
||||
"command_names": list(_command_names(self)),
|
||||
"capabilities": sorted(self.profile.capabilities),
|
||||
# Retained only as an old-session serialization token. Live
|
||||
# decisions use the typed motion/measurement policies.
|
||||
"capabilities": sorted(
|
||||
getattr(
|
||||
getattr(
|
||||
getattr(self, "calibration_profile", None),
|
||||
"artifacts",
|
||||
None,
|
||||
),
|
||||
"session_compatibility_tokens",
|
||||
frozenset(self.profile.capabilities),
|
||||
)
|
||||
),
|
||||
"reference_finger": self.profile.reference_finger,
|
||||
"view_tags": {
|
||||
view: dict(tags)
|
||||
@@ -8157,7 +8189,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
}
|
||||
)
|
||||
continue
|
||||
if profile.supports("steady_command_checkpoints"):
|
||||
if profile.steady_command_checkpoints:
|
||||
command_store = getattr(
|
||||
self, "command_records_by_joint", None
|
||||
)
|
||||
@@ -8438,7 +8470,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
"maximum",
|
||||
),
|
||||
)
|
||||
if not profile.supports("directional_zero"):
|
||||
if not profile.directional_zero:
|
||||
checks = checks + (
|
||||
(
|
||||
"hysteresis_deg",
|
||||
@@ -8789,7 +8821,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
if index != outlier
|
||||
]
|
||||
failures.append(failure)
|
||||
if profile.supports("cross_view_roll_curve"):
|
||||
if profile.cross_view_roll_curve:
|
||||
for primary_name, validation_name in (
|
||||
profile.axis_validation_sources or {}
|
||||
).items():
|
||||
@@ -11565,6 +11597,20 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
name: published_zero_offsets[name]
|
||||
for name in self.zero_profile.direct_zero_joints
|
||||
}
|
||||
correction_plan = None
|
||||
typed_profile = getattr(self, "calibration_profile", None)
|
||||
if typed_profile is not None:
|
||||
frozen_names = typed_profile.scope.frozen_joints[
|
||||
self.recalibration_scope
|
||||
]
|
||||
correction_plan = build_correction_plan(
|
||||
typed_profile,
|
||||
source_sha256=_file_sha256(self.source_urdf_path),
|
||||
scope=self.recalibration_scope,
|
||||
frozen_offsets_rad={
|
||||
name: urdf_offsets[name] for name in frozen_names
|
||||
},
|
||||
)
|
||||
self.corrected_urdf_path = write_zero_corrected_urdf(
|
||||
source_urdf=self.source_urdf_path,
|
||||
output_directory=self.corrected_urdf_output_dir,
|
||||
@@ -11572,6 +11618,7 @@ class G20ThreeCameraCalibrationNode(Node):
|
||||
offsets_rad=urdf_offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_zero_offsets,
|
||||
timestamp=stamp,
|
||||
correction_plan=correction_plan,
|
||||
)
|
||||
try:
|
||||
if self.standalone_thumb_calibration:
|
||||
|
||||
@@ -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 .core import fit_rotation_axis, robust_rotation_summary
|
||||
from .core.urdf import UrdfCorrectionPlan
|
||||
from .full_hand import (
|
||||
G20_RIGHT_19_LAYOUT,
|
||||
IMAGE_TRAJECTORY_JOINTS,
|
||||
@@ -3850,7 +3851,7 @@ def solve_urdf_zero_offsets(
|
||||
if maximum_cone_mismatch > maximum_axis_cone_mismatch_rad:
|
||||
cone_range = float(np.ptp(cone_mismatches))
|
||||
stable_product_bias = bool(
|
||||
profile.hand.supports("stable_cross_view_cone_bias")
|
||||
profile.hand.stable_cross_view_cone_bias
|
||||
and maximum_systematic_axis_cone_bias_rad is not None
|
||||
and len(cone_mismatches) >= 4
|
||||
and maximum_cone_mismatch
|
||||
@@ -4454,6 +4455,7 @@ def write_zero_corrected_urdf(
|
||||
offsets_rad: Mapping[str, float],
|
||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||
timestamp: str | None = None,
|
||||
correction_plan: UrdfCorrectionPlan | None = None,
|
||||
) -> Path:
|
||||
source = Path(source_urdf).expanduser().resolve()
|
||||
output = Path(output_directory).expanduser().resolve()
|
||||
@@ -4475,6 +4477,15 @@ def write_zero_corrected_urdf(
|
||||
}
|
||||
if not set(endpoint_offsets) <= set(offsets):
|
||||
raise ValueError("endpoint-anchored offsets must be URDF zero targets")
|
||||
if correction_plan is not None:
|
||||
correction_plan.verify_source(source)
|
||||
correction_plan.authorize_offsets(offsets)
|
||||
if not set(endpoint_offsets).issubset(
|
||||
correction_plan.endpoint_limit_joints
|
||||
):
|
||||
raise ValueError(
|
||||
"endpoint offsets are not authorized by the correction plan"
|
||||
)
|
||||
if any(
|
||||
not math.isfinite(value) or abs(value) > math.radians(90.0)
|
||||
for value in offsets.values()
|
||||
|
||||
@@ -21,7 +21,11 @@ from sensor_msgs.msg import Image, JointState
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from .core import BASELINE_COMMAND, build_command, COMMAND_NAMES
|
||||
from .compat.legacy.thumb_core import (
|
||||
BASELINE_COMMAND,
|
||||
COMMAND_NAMES,
|
||||
build_command,
|
||||
)
|
||||
from .storage import atomic_write_json
|
||||
from .zero_calibration import (
|
||||
build_trajectory_zero_angle_payload,
|
||||
|
||||
@@ -66,11 +66,11 @@ setup(
|
||||
),
|
||||
(
|
||||
"calibrate_g20_right = "
|
||||
"linkerhand_calibration.one_command:main"
|
||||
"linkerhand_calibration.runtime.runner:main"
|
||||
),
|
||||
(
|
||||
"calibrate_hand = "
|
||||
"linkerhand_calibration.one_command:main"
|
||||
"linkerhand_calibration.runtime.runner:main"
|
||||
),
|
||||
],
|
||||
},
|
||||
|
||||
@@ -19,7 +19,7 @@ from linkerhand_calibration.acquisition import (
|
||||
tag_quality_is_valid,
|
||||
update_pnp_reset_watchdog,
|
||||
)
|
||||
from linkerhand_calibration.core import PAIR_NAMES
|
||||
from linkerhand_calibration.acquisition import PAIR_NAMES
|
||||
|
||||
|
||||
def test_pnp_watchdog_resets_after_one_continuous_invalid_second() -> None:
|
||||
|
||||
@@ -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
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from linkerhand_calibration.core import (
|
||||
from linkerhand_calibration.compat.legacy.thumb_core import (
|
||||
BASELINE_COMMAND,
|
||||
DIRECTION_DECREASING,
|
||||
DIRECTION_INCREASING,
|
||||
|
||||
@@ -28,7 +28,7 @@ from linkerhand_calibration.operator_report import (
|
||||
classify_error,
|
||||
render_progress_zh,
|
||||
)
|
||||
from linkerhand_calibration.one_command import (
|
||||
from linkerhand_calibration.models.g20.runner import (
|
||||
_automatic_resume_candidate,
|
||||
_calibration_node_exited_before_status,
|
||||
_launch_command,
|
||||
|
||||
@@ -21,6 +21,6 @@ def test_new_and_legacy_executable_names_share_one_implementation() -> None:
|
||||
for executable in ("calibrate_hand", "calibrate_g20_right"):
|
||||
assert re.search(
|
||||
rf'"{executable} = "\s*'
|
||||
r'"linkerhand_calibration\.one_command:main"',
|
||||
r'"linkerhand_calibration\.runtime\.runner:main"',
|
||||
setup_text,
|
||||
)
|
||||
|
||||
@@ -7,11 +7,13 @@ from scipy.spatial.transform import Rotation
|
||||
from linkerhand_calibration.core import (
|
||||
DIRECTION_DECREASING,
|
||||
DIRECTION_INCREASING,
|
||||
PHASE_ROOT,
|
||||
PHASE_TIP,
|
||||
)
|
||||
from linkerhand_calibration.compat.legacy.thumb_core import (
|
||||
PAIR_IP,
|
||||
PAIR_MCP,
|
||||
PAIR_ROOT,
|
||||
PHASE_ROOT,
|
||||
PHASE_TIP,
|
||||
create_final_payload,
|
||||
)
|
||||
from linkerhand_calibration.trajectory import (
|
||||
|
||||
Reference in New Issue
Block a user