383 lines
18 KiB
Python
383 lines
18 KiB
Python
"""Strict YAML loader for a model that reuses the unified engine."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import fields
|
|
from pathlib import Path
|
|
import re
|
|
from typing import Any, Mapping
|
|
|
|
import yaml
|
|
from ..core.domain.profile import ReferenceWaypoint, ModelPreparation, ParentReferenceSpec, JointZeroSpec, KnownBaselineGeometry, JointLimitPolicy, RetainedCadScope
|
|
from ..core.domain.motion_path import MotionSegment
|
|
|
|
from ..core import (
|
|
AcquisitionPolicy,
|
|
ArtifactPolicy,
|
|
CalibrationProfile,
|
|
CommandLayout,
|
|
MeasurementPolicy,
|
|
MeasurementSpec,
|
|
MotionPolicy,
|
|
ProfileKey,
|
|
QualityPolicy,
|
|
ScopePolicy,
|
|
TagSpec,
|
|
TaskSpec,
|
|
ViewSpec,
|
|
VisionRigSpec,
|
|
ZeroSolvePolicy,
|
|
validate_profile,
|
|
)
|
|
|
|
|
|
def _map(value: Any, name: str) -> Mapping[str, Any]:
|
|
if not isinstance(value, Mapping):
|
|
raise ValueError(f"{name} must be a mapping")
|
|
return value
|
|
|
|
|
|
def _set(value: Any) -> frozenset[str]:
|
|
return frozenset(str(item) for item in (value or ()))
|
|
|
|
|
|
def _keys(value, allowed, name):
|
|
unknown = set(_map(value, name)) - set(allowed)
|
|
if unknown:
|
|
raise ValueError(f"unknown {name} fields: {sorted(unknown)}")
|
|
|
|
|
|
def _validate_keys(payload):
|
|
# Misspelled safety/geometry fields must not silently select a default.
|
|
_keys(payload, {"schema_version", "profile_id", "namespace", "sdk_adapter", "command",
|
|
"vision", "motion", "measurement", "zero", "quality", "scope", "artifacts",
|
|
"acquisition", "urdf", "joint_coverage"}, "profile")
|
|
for name, contract in (("command", CommandLayout), ("vision", VisionRigSpec),
|
|
("motion", MotionPolicy), ("measurement", MeasurementPolicy), ("zero", ZeroSolvePolicy),
|
|
("quality", QualityPolicy), ("scope", ScopePolicy), ("artifacts", ArtifactPolicy),
|
|
("acquisition", AcquisitionPolicy)):
|
|
_keys(payload.get(name, {}), {f.name for f in fields(contract)}, name)
|
|
if "task_training_cycles" in payload.get("quality", {}):
|
|
raise ValueError("task_training_cycles are journal decisions, not profile configuration")
|
|
_keys(payload.get("urdf", {}), {"authorized_fields", "limit_policies"}, "urdf")
|
|
for policy in payload.get("scope", {}).get("retained_cad_scopes", {}).values():
|
|
_keys(policy, {f.name for f in fields(RetainedCadScope)}, "retained CAD scope")
|
|
for policy in payload.get("urdf", {}).get("limit_policies", {}).values():
|
|
_keys(policy, {f.name for f in fields(JointLimitPolicy)}, "URDF limit policy")
|
|
for view in payload.get("vision", {}).get("views", ()):
|
|
_keys(view, {"name", "tags"}, "view")
|
|
for tag in view.get("tags", ()):
|
|
_keys(tag, {"id", "role", "fixed_reference", "link", "size_m"}, "Tag")
|
|
for task in payload.get("motion", {}).get("tasks", ()):
|
|
_keys(task, {f.name for f in fields(TaskSpec)}, "task")
|
|
if task.get("parent_reference") is not None:
|
|
_keys(task["parent_reference"], {f.name for f in fields(ParentReferenceSpec)}, "parent reference")
|
|
for preparation in task.get("model_preparations", ()):
|
|
_keys(preparation, {f.name for f in fields(ModelPreparation)}, "model preparation")
|
|
for segment in task.get("segments", ()):
|
|
_keys(segment, {f.name for f in fields(MotionSegment)}, "motion segment")
|
|
for waypoint in payload.get("motion", {}).get("resume_verification_waypoints", ()):
|
|
_keys(waypoint, {f.name for f in fields(ReferenceWaypoint)}, "reference waypoint")
|
|
for spec in payload.get("motion", {}).get("joint_zero_references", {}).values():
|
|
_keys(spec, {f.name for f in fields(JointZeroSpec)}, "joint zero reference")
|
|
for spec in payload.get("zero", {}).get("known_baseline_geometry", {}).values():
|
|
_keys(spec, {f.name for f in fields(KnownBaselineGeometry)}, "baseline geometry")
|
|
for name, measurement in payload.get("measurement", {}).get("measurements", {}).items():
|
|
_keys(measurement, {f.name for f in fields(MeasurementSpec)}, "measurement item")
|
|
if measurement.get("joint", name) != name:
|
|
raise ValueError("measurement key and joint field differ")
|
|
|
|
|
|
def load_bundled_hand_profile(layout: str, *, scope: str | None = None) -> CalibrationProfile:
|
|
"""Resolve installed or source-tree YAML without constructing a model.
|
|
|
|
No cache: a product hash check and the subsequently loaded contract must
|
|
see the same on-disk inputs, not a previous in-process Python singleton.
|
|
"""
|
|
if re.fullmatch(r"[a-z0-9_]+", str(layout)) is None:
|
|
raise ValueError("invalid bundled profile layout name")
|
|
source = Path(__file__).resolve().parents[2] / "config" / "profiles" / f"{layout}.yaml"
|
|
if source.is_file():
|
|
return load_hand_profile(source, scope=scope)
|
|
from ament_index_python.packages import get_package_share_directory
|
|
return load_hand_profile(Path(get_package_share_directory("linkerhand_calibration")) / "config" / "profiles" / f"{layout}.yaml", scope=scope)
|
|
|
|
|
|
def load_hand_profile(path: str | Path, *, scope: str | None = None) -> CalibrationProfile:
|
|
"""Load the complete extension contract; no Python model hook is used."""
|
|
source = Path(path).expanduser().resolve()
|
|
payload = _map(yaml.safe_load(source.read_text(encoding="utf-8")), str(source))
|
|
if int(payload.get("schema_version", 0)) != 1:
|
|
raise ValueError("hand profile schema_version must be 1")
|
|
_validate_keys(payload)
|
|
selected_scope = scope
|
|
command = _map(payload.get("command"), "command")
|
|
vision = _map(payload.get("vision"), "vision")
|
|
motion = _map(payload.get("motion"), "motion")
|
|
measurement = _map(payload.get("measurement"), "measurement")
|
|
zero = _map(payload.get("zero"), "zero")
|
|
quality = _map(payload.get("quality"), "quality")
|
|
scope = _map(payload.get("scope"), "scope")
|
|
artifacts = _map(payload.get("artifacts"), "artifacts")
|
|
acquisition = _map(payload.get("acquisition", {}), "acquisition")
|
|
urdf = _map(payload.get("urdf", {}), "urdf")
|
|
|
|
command_layout = CommandLayout(
|
|
names=tuple(str(value) for value in command.get("names", ())),
|
|
baseline_u8=tuple(int(value) for value in command.get("baseline_u8", ())),
|
|
command_index_by_joint={
|
|
str(name): int(index)
|
|
for name, index in _map(
|
|
command.get("command_index_by_joint", {}),
|
|
"command.command_index_by_joint",
|
|
).items()
|
|
},
|
|
disabled_indices=frozenset(
|
|
int(index) for index in command.get("disabled_indices", ())
|
|
),
|
|
urdf_joint_by_joint={
|
|
str(name): str(joint)
|
|
for name, joint in _map(
|
|
command.get("urdf_joint_by_joint", {}),
|
|
"command.urdf_joint_by_joint",
|
|
).items()
|
|
},
|
|
feedback_name_aliases={
|
|
str(name): str(canonical)
|
|
for name, canonical in _map(
|
|
command.get("feedback_name_aliases", {}),
|
|
"command.feedback_name_aliases",
|
|
).items()
|
|
},
|
|
speed_slot_by_command_index={
|
|
int(index): int(slot)
|
|
for index, slot in _map(
|
|
command.get("speed_slot_by_command_index", {}),
|
|
"command.speed_slot_by_command_index",
|
|
).items()
|
|
},
|
|
unit=str(command.get("unit", "u8")),
|
|
baseline=tuple(float(value) for value in command.get("baseline", ())),
|
|
lower_bounds=tuple(float(value) for value in command.get("lower_bounds", ())),
|
|
upper_bounds=tuple(float(value) for value in command.get("upper_bounds", ())),
|
|
feedback_lower_bounds=tuple(
|
|
float(value) for value in command.get("feedback_lower_bounds", ())
|
|
),
|
|
feedback_upper_bounds=tuple(
|
|
float(value) for value in command.get("feedback_upper_bounds", ())
|
|
),
|
|
feedback_by_index=bool(command.get("feedback_by_index", False)),
|
|
sdk_to_joint_direction=tuple(command.get("sdk_to_joint_direction", ())),
|
|
maximum_velocity=tuple(float(value) for value in command.get("maximum_velocity", ())),
|
|
)
|
|
views = tuple(
|
|
ViewSpec(
|
|
str(view["name"]),
|
|
tuple(
|
|
TagSpec(
|
|
str(tag["role"]),
|
|
int(tag["id"]),
|
|
bool(tag.get("fixed_reference", False)),
|
|
None if tag.get("link") is None else str(tag["link"]),
|
|
float(tag.get("size_m", 0.016)),
|
|
)
|
|
for tag in view.get("tags", ())
|
|
),
|
|
)
|
|
for view in vision.get("views", ())
|
|
)
|
|
tasks = tuple(
|
|
TaskSpec(
|
|
key=str(task["key"]),
|
|
view=str(task["view"]),
|
|
command_index=int(task["command_index"]),
|
|
joints=tuple(str(name) for name in task.get("joints", ())),
|
|
auxiliary_commands=tuple(
|
|
(int(pair[0]), float(pair[1]))
|
|
for pair in task.get("auxiliary_commands", ())
|
|
),
|
|
validation_only=bool(task.get("validation_only", False)),
|
|
start_u8=int(task.get("start_u8", 255)),
|
|
end_u8=int(task.get("end_u8", 0)),
|
|
preflight_speed_u8=(
|
|
None if task.get("preflight_speed_u8") is None
|
|
else int(task["preflight_speed_u8"])
|
|
),
|
|
formal_speed_u8=(
|
|
None if task.get("formal_speed_u8") is None
|
|
else int(task["formal_speed_u8"])
|
|
),
|
|
start=None if task.get("start") is None else float(task["start"]),
|
|
end=None if task.get("end") is None else float(task["end"]),
|
|
preflight_speed=(
|
|
None if task.get("preflight_speed") is None
|
|
else float(task["preflight_speed"])
|
|
),
|
|
formal_speed=(
|
|
None if task.get("formal_speed") is None
|
|
else float(task["formal_speed"])
|
|
),
|
|
preparation_groups=tuple(tuple(int(index) for index in group) for group in task.get("preparation_groups", ())),
|
|
entry_waypoints=tuple(tuple((int(index), float(value)) for index, value in waypoint)
|
|
for waypoint in task.get("entry_waypoints", ())),
|
|
exit_waypoints=tuple(tuple((int(index), float(value)) for index, value in waypoint)
|
|
for waypoint in task.get("exit_waypoints", ())),
|
|
model_preparations=tuple(ModelPreparation(str(item["joint"]),
|
|
tuple(map(float, item["command"])),
|
|
tuple(tuple(map(float, pose)) for pose in item["approach_commands"]))
|
|
for item in task.get("model_preparations", ())),
|
|
parent_reference=(ParentReferenceSpec(**task["parent_reference"])
|
|
if task.get("parent_reference") is not None else None),
|
|
segments=tuple(MotionSegment(str(segment["key"]),
|
|
tuple((int(i), float(v)) for i, v in segment["start_commands"]),
|
|
tuple((int(i), float(v)) for i, v in segment["end_commands"]),
|
|
tuple(map(str, segment["joints"]))) for segment in task.get("segments", ())),
|
|
)
|
|
for task in motion.get("tasks", ())
|
|
)
|
|
measurements = {
|
|
str(name): MeasurementSpec(
|
|
joint=str(name),
|
|
kind=str(value["kind"]),
|
|
view=None if value.get("view") is None else str(value["view"]),
|
|
parent_role=(
|
|
None if value.get("parent_role") is None
|
|
else str(value["parent_role"])
|
|
),
|
|
child_role=(
|
|
None if value.get("child_role") is None
|
|
else str(value["child_role"])
|
|
),
|
|
validation_source=(
|
|
None if value.get("validation_source") is None
|
|
else str(value["validation_source"])
|
|
),
|
|
pose_axis_line_required=bool(
|
|
value.get("pose_axis_line_required", True)
|
|
),
|
|
)
|
|
for name, value in _map(
|
|
measurement.get("measurements", {}),
|
|
"measurement.measurements",
|
|
).items()
|
|
}
|
|
profile = CalibrationProfile(
|
|
key=ProfileKey.parse(str(payload["profile_id"])),
|
|
namespace=str(payload["namespace"]),
|
|
command=command_layout,
|
|
vision=VisionRigSpec(
|
|
views,
|
|
str(vision["common_frame"]),
|
|
str(vision["extrinsic_reference_view"]),
|
|
dict(vision.get("extrinsics_quality_limits", {})),
|
|
dict(vision.get("minimum_capture_counts", {})),
|
|
),
|
|
motion=MotionPolicy(
|
|
tasks,
|
|
tuple(tuple(int(item) for item in row) for row in motion.get("preparation_waypoints_u8", ())),
|
|
tuple(tuple(int(item) for item in row) for row in motion.get("safe_return_waypoints_u8", ())),
|
|
dict(motion.get("speed_parameters", {})),
|
|
bool(motion.get("precheck_sweeps", False)),
|
|
bool(motion.get("steady_command_checkpoints", False)),
|
|
tuple(tuple(int(index) for index in group) for group in motion.get("return_groups", ())),
|
|
tuple(ReferenceWaypoint(str(row["key"]), tuple(map(float, row["command"])),
|
|
{str(view): tuple(map(int, ids)) for view, ids in row["tag_ids_by_view"].items()})
|
|
for row in motion.get("resume_verification_waypoints", ())),
|
|
{str(name): JointZeroSpec(str(row["task_key"]), tuple(map(float, row["command"])),
|
|
tuple(tuple(map(float, command)) for command in row.get("approach_commands", ())),
|
|
str(row.get("evidence_scope", "arrival")))
|
|
for name, row in motion.get("joint_zero_references", {}).items()},
|
|
),
|
|
measurement=MeasurementPolicy(
|
|
measurements,
|
|
dict(measurement.get("cross_view_sources", {})),
|
|
_set(measurement.get("image_curve_joints")),
|
|
bool(measurement.get("directional_zero", False)),
|
|
bool(measurement.get("cross_view_roll_curve", False)),
|
|
bool(measurement.get("stable_cross_view_cone_bias", False)),
|
|
_set(measurement.get("candidate_selection_tasks", ())),
|
|
str(measurement.get("input_domain", "")),
|
|
dict(measurement.get("transferred_motion_sources", {})),
|
|
str(measurement.get("release_basis", "feedback_and_command")),
|
|
),
|
|
zero=ZeroSolvePolicy(
|
|
_set(zero.get("active_joints")),
|
|
_set(zero.get("passive_joints")),
|
|
tuple(str(value) for value in zero.get("direct_zero_joints", ())),
|
|
tuple(str(value) for value in zero.get("axis_joints", ())),
|
|
_set(zero.get("mechanical_endpoint_joints")),
|
|
_set(zero.get("post_solve_endpoint_joints")),
|
|
dict(zero.get("mimic_source_by_joint", {})),
|
|
_set(zero.get("cad_frozen_joints")),
|
|
dict(zero.get("endpoint_anchor_by_joint", {})),
|
|
_set(zero.get("fitted_mimic_joints")),
|
|
dict(zero.get("coupling_model_by_joint", {})),
|
|
dict(zero.get("transferred_zero_sources", {})),
|
|
dict(zero.get("transferred_mimic_sources", {})),
|
|
dict(zero.get("spatial", {})),
|
|
{str(name): KnownBaselineGeometry(float(row["cad_angle_rad"]),
|
|
float(row["uncertainty_rad"]), str(row["evidence_source"]))
|
|
for name, row in zero.get("known_baseline_geometry", {}).items()},
|
|
dict(zero.get("cad_zero_assumptions", {})),
|
|
),
|
|
quality=QualityPolicy(
|
|
tuple(int(value) for value in quality.get("training_cycles", ())),
|
|
None if quality.get("holdout_cycle") is None else int(quality["holdout_cycle"]),
|
|
_set(quality.get("hard_threshold_keys")),
|
|
dict(quality.get("retry_metric_scope", {})),
|
|
bool(quality.get("isolated_holdout", False)),
|
|
str(quality.get("training_policy", "fixed")),
|
|
),
|
|
scope=ScopePolicy(
|
|
{
|
|
str(name): _set(values)
|
|
for name, values in _map(scope.get("calibrate_joints", {}), "scope.calibrate_joints").items()
|
|
},
|
|
{
|
|
str(name): _set(values)
|
|
for name, values in _map(scope.get("frozen_joints", {}), "scope.frozen_joints").items()
|
|
},
|
|
str(selected_scope or scope.get("default_scope", "full")),
|
|
{str(name): RetainedCadScope(str(row["reason"]), tuple(row["root_anchor_joints"]),
|
|
str(row["orientation_anchor_joint"]), tuple(row.get("directed_base_axis_joints", ())))
|
|
for name, row in scope.get("retained_cad_scopes", {}).items()},
|
|
),
|
|
artifacts=ArtifactPolicy(
|
|
int(artifacts["output_schema_version"]),
|
|
str(artifacts["calibration_filename"]),
|
|
str(artifacts["corrected_urdf_filename"]),
|
|
_set(artifacts.get("protected_input_fields")),
|
|
str(artifacts.get("publication_pointer", "latest_passed")),
|
|
_set(artifacts.get("session_compatibility_tokens")),
|
|
bool(artifacts.get("publish_corrected_urdf", False)),
|
|
_set(artifacts.get("directional_command_joints")),
|
|
),
|
|
acquisition=AcquisitionPolicy(**dict(acquisition)),
|
|
sdk_adapter=str(payload["sdk_adapter"]),
|
|
urdf_authorized_fields={
|
|
str(name): _set(fields)
|
|
for name, fields in _map(
|
|
urdf.get("authorized_fields", {}), "urdf.authorized_fields"
|
|
).items()
|
|
},
|
|
joint_coverage={
|
|
str(name): str(value)
|
|
for name, value in _map(
|
|
payload.get("joint_coverage", {}), "joint_coverage"
|
|
).items()
|
|
},
|
|
urdf_limit_policies={str(name): JointLimitPolicy(
|
|
source_limits=str(row["source_limits"]), evidence_source=str(row["evidence_source"]),
|
|
independent_safety_limits_cad_rad=(None if row.get("independent_safety_limits_cad_rad") is None
|
|
else tuple(float(v) for v in row["independent_safety_limits_cad_rad"])),
|
|
safety_evidence_source=str(row.get("safety_evidence_source", "")))
|
|
for name, row in _map(urdf.get("limit_policies", {}), "urdf.limit_policies").items()},
|
|
)
|
|
from .scope import compile_capture_scope
|
|
profile = compile_capture_scope(profile)
|
|
validate_profile(profile)
|
|
return profile
|
|
|
|
|
|
__all__ = ["load_hand_profile", "load_bundled_hand_profile"]
|