193 lines
8.6 KiB
Python
193 lines
8.6 KiB
Python
"""Literal robot calibration data and the onboarding writer."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import ast
|
|
import keyword
|
|
import math
|
|
import os
|
|
import re
|
|
from dataclasses import asdict, dataclass
|
|
from pathlib import Path
|
|
from pprint import pformat
|
|
from tempfile import NamedTemporaryFile
|
|
from typing import Any
|
|
|
|
FAMILIES = ("biped", "quadruped", "wheeled_biped", "multipod")
|
|
_RESERVED_IDS = {"registry", "paths", "robot_spec", "templates"}
|
|
MODEL_SUFFIX = ".xml"
|
|
|
|
|
|
def _number(name: str, value: float, *, minimum: float = -math.inf) -> None:
|
|
if not isinstance(value, (int, float)) or isinstance(value, bool) or not (
|
|
minimum <= value < math.inf and value > -math.inf
|
|
):
|
|
raise ValueError(f"{name} must be finite and at least {minimum}; got {value!r}")
|
|
|
|
|
|
def _names(name: str, values: tuple[str, ...]) -> None:
|
|
if any(not isinstance(value, str) or not value.strip() for value in values):
|
|
raise ValueError(f"{name} must contain nonempty names")
|
|
if any("/" in value for value in values):
|
|
raise ValueError(f"{name} contains '/'; use unqualified model names for training")
|
|
if len(set(values)) != len(values):
|
|
raise ValueError(f"{name} contains duplicate names")
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class JointSpec:
|
|
"""One actuated joint and its calibrated control parameters."""
|
|
|
|
name: str
|
|
kp: float
|
|
kd: float
|
|
effort_limit: float
|
|
action_scale: float
|
|
home: float = 0.0
|
|
armature: float = 0.0
|
|
|
|
def __post_init__(self) -> None:
|
|
_names("joint", (self.name,))
|
|
_number(f"{self.name}.kp", self.kp, minimum=0.0)
|
|
_number(f"{self.name}.kd", self.kd, minimum=0.0)
|
|
_number(f"{self.name}.effort_limit", self.effort_limit, minimum=1e-12)
|
|
_number(f"{self.name}.action_scale", self.action_scale, minimum=1e-12)
|
|
_number(f"{self.name}.home", self.home)
|
|
_number(f"{self.name}.armature", self.armature, minimum=0.0)
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class RobotSpec:
|
|
"""Physical calibration and file paths accepted by a locked template."""
|
|
|
|
robot_id: str
|
|
family: str
|
|
model_path: Path
|
|
base_body: str
|
|
base_height: float
|
|
foot_geoms: tuple[str, ...] = ()
|
|
position_joints: tuple[JointSpec, ...] = ()
|
|
velocity_joints: tuple[JointSpec, ...] = ()
|
|
controls_path: Path | None = None
|
|
foot_bodies: tuple[str, ...] = ()
|
|
penalized_contact_geoms: tuple[str, ...] = ()
|
|
termination_contact_geoms: tuple[str, ...] = ()
|
|
|
|
def __post_init__(self) -> None:
|
|
if (
|
|
not isinstance(self.robot_id, str)
|
|
or re.fullmatch(r"[a-z][a-z0-9_]*", self.robot_id) is None
|
|
or keyword.iskeyword(self.robot_id)
|
|
or self.robot_id in _RESERVED_IDS
|
|
):
|
|
raise ValueError(f"invalid robot_id {self.robot_id!r}; use lower_snake_case")
|
|
if self.family not in FAMILIES:
|
|
raise ValueError(f"family must be one of {FAMILIES}; got {self.family!r}")
|
|
if not isinstance(self.model_path, Path) or self.model_path.suffix != MODEL_SUFFIX:
|
|
raise ValueError("model_path must identify a robot MJCF with the lowercase .xml suffix")
|
|
if self.controls_path is not None and not isinstance(self.controls_path, Path):
|
|
raise ValueError("controls_path must be a Path or None")
|
|
_names("base_body", (self.base_body,))
|
|
_number("base_height", self.base_height, minimum=1e-12)
|
|
for key in ("foot_geoms", "foot_bodies", "penalized_contact_geoms", "termination_contact_geoms"):
|
|
_names(key, getattr(self, key))
|
|
if bool(self.foot_geoms) == bool(self.foot_bodies):
|
|
raise ValueError("declare either foot_geoms or foot_bodies")
|
|
min_contacts = {"biped": 2, "quadruped": 4, "wheeled_biped": 2, "multipod": 5}
|
|
if len(self.foot_geoms or self.foot_bodies) < min_contacts[self.family]:
|
|
raise ValueError(f"{self.family} requires at least {min_contacts[self.family]} supports")
|
|
forbidden_supports = set(self.penalized_contact_geoms) | set(self.termination_contact_geoms)
|
|
if set(self.foot_geoms) & forbidden_supports:
|
|
raise ValueError("support geoms cannot also be penalized or termination contacts")
|
|
_names("position_joints and velocity_joints", tuple(j.name for j in self.joints))
|
|
if not self.position_joints or any(j.kp <= 0 for j in self.position_joints):
|
|
raise ValueError("position_joints must contain joints with kp > 0")
|
|
if self.family == "wheeled_biped":
|
|
if len(self.velocity_joints) < 2 or any(
|
|
j.kp != 0 or j.kd <= 0 for j in self.velocity_joints
|
|
):
|
|
raise ValueError("wheeled_biped requires at least two velocity joints with kp=0, kd>0")
|
|
elif self.velocity_joints:
|
|
raise ValueError("velocity_joints are supported by wheeled_biped only")
|
|
|
|
@property
|
|
def joints(self) -> tuple[JointSpec, ...]:
|
|
return self.position_joints + self.velocity_joints
|
|
|
|
@classmethod
|
|
def from_dict(cls, config: dict[str, Any], *, base_dir: Path) -> RobotSpec:
|
|
"""Resolve relative paths beside the source config and reject unknown fields."""
|
|
data = dict(config)
|
|
for key in ("model_path", "controls_path"):
|
|
if key in data and data[key] is not None:
|
|
data[key] = (base_dir / Path(data[key])).resolve()
|
|
for key in ("foot_geoms", "foot_bodies", "penalized_contact_geoms", "termination_contact_geoms"):
|
|
if key in data:
|
|
if not isinstance(data[key], (tuple, list)):
|
|
raise ValueError(f"{key} must be a list or tuple of names")
|
|
data[key] = tuple(data[key])
|
|
for key in ("position_joints", "velocity_joints"):
|
|
if key in data:
|
|
data[key] = tuple(JointSpec(**joint) for joint in data[key])
|
|
return cls(**data)
|
|
|
|
|
|
def read_robot_spec(path: Path) -> RobotSpec:
|
|
"""Read the literal without importing code or evaluating expressions."""
|
|
try:
|
|
statements = ast.parse(path.read_text(encoding="utf-8"), filename=str(path)).body
|
|
if len(statements) != 1:
|
|
raise ValueError("expected a single literal ROBOT_SPEC assignment")
|
|
statement = statements[0]
|
|
if (
|
|
not isinstance(statement, ast.Assign)
|
|
or len(statement.targets) != 1
|
|
or not isinstance(statement.targets[0], ast.Name)
|
|
or statement.targets[0].id != "ROBOT_SPEC"
|
|
or not isinstance(statement.value, ast.Dict)
|
|
):
|
|
raise ValueError("expected a single literal ROBOT_SPEC dictionary")
|
|
for node in ast.walk(statement.value):
|
|
if isinstance(node, ast.Dict):
|
|
keys = [ast.literal_eval(key) for key in node.keys]
|
|
if any(not isinstance(key, str) for key in keys) or len(keys) != len(set(keys)):
|
|
raise ValueError("ROBOT_SPEC dictionaries require unique string keys")
|
|
spec = RobotSpec.from_dict(ast.literal_eval(statement.value), base_dir=path.parent)
|
|
if spec.robot_id != path.parent.name:
|
|
raise ValueError(f"robot_id {spec.robot_id!r} must match its directory name")
|
|
return spec
|
|
except (SyntaxError, TypeError, ValueError) as exc:
|
|
raise ValueError(f"{path}: {exc}") from exc
|
|
|
|
|
|
def scaffold_robot_spec(spec: RobotSpec, *, tasks_root: Path | None = None) -> Path:
|
|
"""Write one robot's literal data file without replacing an existing spec."""
|
|
root = tasks_root if tasks_root is not None else Path(__file__).resolve().parent
|
|
directory = root / spec.robot_id
|
|
path = directory / "robot_spec.py"
|
|
data = asdict(spec)
|
|
for key in ("model_path", "controls_path"):
|
|
source = data[key]
|
|
if source is not None:
|
|
source = source.resolve()
|
|
if not source.is_file():
|
|
raise FileNotFoundError(f"{key}: {source} does not exist")
|
|
try:
|
|
data[key] = os.path.relpath(source, directory.resolve())
|
|
except ValueError: # Different drives on Windows.
|
|
data[key] = str(source)
|
|
directory.mkdir(parents=True, exist_ok=True)
|
|
source = "# Generated by scripts/onboard.py. This file contains calibration data.\n\n"
|
|
source += f"ROBOT_SPEC = {pformat(data, sort_dicts=False, width=100)}\n"
|
|
# A concurrent registration must never read a half-written spec. A hard link
|
|
# publishes the complete file and refuses to replace an existing destination.
|
|
with NamedTemporaryFile(mode="w", encoding="utf-8", dir=directory, delete=False) as temp:
|
|
temp_path = Path(temp.name)
|
|
try:
|
|
temp.write(source)
|
|
temp.close()
|
|
os.link(temp_path, path)
|
|
finally:
|
|
temp_path.unlink()
|
|
return path
|