Files
chenlin ceca51b0d8
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
Improve generic robot onboarding, walking controls and end-effector interaction
Default humanoid intake to command-following gait and support bounded Cartesian target dragging for arms. Carry model and actuator semantics through calibration, native verification and packaging. Document reusable intake decisions and cover them with regression tests.
2026-11-06 10:54:58 +08:00

204 lines
9.4 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 # Zero holds a position actuator at its calibrated home.
home: float = 0.0
armature: float = 0.0
posture_std: float = 0.35
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=0.0)
_number(f"{self.name}.home", self.home)
_number(f"{self.name}.armature", self.armature, minimum=0.0)
_number(f"{self.name}.posture_std", self.posture_std, minimum=0.0)
if self.posture_std <= 0:
raise ValueError(f"{self.name}.posture_std must be positive")
@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, ...] = ()
foot_sites: 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)
if not isinstance(self.foot_sites, (list, tuple)):
raise ValueError("foot_sites must be a list or tuple of names")
for key in ("foot_geoms", "foot_bodies", "foot_sites", "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")
if self.foot_sites and len(self.foot_sites) != len(self.foot_geoms or self.foot_bodies):
raise ValueError("foot_sites must have one site per named foot support")
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 not any(j.action_scale > 0 for j in self.joints):
raise ValueError("at least one joint must have a positive action_scale")
if self.family == "wheeled_biped":
if len(self.velocity_joints) < 2 or any(
j.kp != 0 or j.kd <= 0 or j.action_scale <= 0 for j in self.velocity_joints
):
raise ValueError("wheeled_biped requires at least two velocity joints with kp=0, kd>0, action_scale>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", "foot_sites", "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