refactor: establish reusable calibration architecture

This commit is contained in:
lxp
2026-08-31 18:33:31 +08:00
parent 0d606c2ba2
commit ba9f1b25e8
66 changed files with 5771 additions and 3792 deletions
+23
View File
@@ -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 1013在对应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 1013在对应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,
+2 -2
View File
@@ -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})
+1 -1
View File
@@ -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 (