ceca51b0d8
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.
231 lines
9.0 KiB
Python
231 lines
9.0 KiB
Python
"""Normalize actuator semantics and generate validated operator bindings."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import json
|
|
import os
|
|
from collections.abc import Mapping
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
from tempfile import TemporaryDirectory
|
|
|
|
import mujoco
|
|
import numpy as np
|
|
import yaml
|
|
|
|
from controller.controls import load_controls
|
|
from mjrl.onboarding._xml import checked_mjcf
|
|
from mjrl.onboarding.build import write_mjcf
|
|
|
|
from .bindings import AxisBinding, controls_document, velocity_controls
|
|
from .model import finite, joint_values, normalize_actuators, object_id, vector
|
|
from .runtime import WHEEL_KINDS, Controller, read_config
|
|
from .wheels import wheel_mixer
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class ControllerSetup:
|
|
model: Path
|
|
config: Path
|
|
controls: Path | None
|
|
|
|
|
|
def _gains(config: Mapping) -> tuple:
|
|
names = config.get("joints")
|
|
kp, kd, effort = config.get("kp", 40), config.get("kd", 4), config.get("effort_limit", 30)
|
|
joint_values(kp, names, "kp", positive=True)
|
|
joint_values(kd, names, "kd", nonnegative=True)
|
|
joint_values(effort, names, "effort_limit", positive=True)
|
|
return kp, kd, effort
|
|
|
|
|
|
def _compile_with_named_links(spec: mujoco.MjSpec) -> mujoco.MjModel:
|
|
authored = spec.copy()
|
|
authored.compiler.fusestatic = False
|
|
return authored.compile()
|
|
|
|
|
|
def _prepare_ik(spec: mujoco.MjSpec, model: mujoco.MjModel, config: dict) -> None:
|
|
site = config.setdefault("site", "mjrl_ee")
|
|
if spec.site(site) is None:
|
|
body_name = config.get("end_effector_body")
|
|
if body_name is None:
|
|
joint = object_id(model, mujoco.mjtObj.mjOBJ_JOINT, config["joints"][-1])
|
|
bid = int(model.jnt_bodyid[joint])
|
|
while len(children := np.flatnonzero(model.body_parentid[1:] == bid)) == 1:
|
|
bid = int(children[0]) + 1
|
|
if len(children):
|
|
raise ValueError("Specify end_effector_body for a branching chain")
|
|
body_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, bid)
|
|
body = spec.body(body_name) if body_name else None
|
|
if body is None:
|
|
raise ValueError(f"Missing end-effector body {body_name!r}")
|
|
offset = vector(config.get("end_effector_offset", [0, 0, 0]), 3, "end_effector_offset")
|
|
body.add_site(name=site, pos=offset, size=[0.012, 0, 0], rgba=[0.1, 0.8, 1, 1])
|
|
mocap = config.setdefault("mocap_body", "mjrl_target")
|
|
if spec.body(mocap) is None:
|
|
config.setdefault("start_target_at_site", True)
|
|
model = _compile_with_named_links(spec)
|
|
data = mujoco.MjData(model)
|
|
mujoco.mj_kinematics(model, data)
|
|
index = object_id(model, mujoco.mjtObj.mjOBJ_SITE, site)
|
|
target = spec.worldbody.add_body(name=mocap, mocap=1, pos=data.site_xpos[index])
|
|
size = finite(config.get("target_radius", 0.025), "target_radius", positive=True)
|
|
target.add_geom(
|
|
name=f"{mocap}_sphere",
|
|
type=mujoco.mjtGeom.mjGEOM_SPHERE,
|
|
size=[size, 0, 0],
|
|
rgba=[0.9, 0.3, 0.1, 0.7],
|
|
contype=0,
|
|
conaffinity=0,
|
|
)
|
|
|
|
|
|
def _prepare_trajectory(config: dict, parent: Path, stage: Path) -> None:
|
|
raw = config.get("trajectory")
|
|
if isinstance(raw, str):
|
|
raw = json.loads((parent / raw).read_text("utf-8"))
|
|
if not isinstance(raw, Mapping):
|
|
raise ValueError("trajectory must be a keyframe object or JSON path")
|
|
name = "motion.trajectory.json"
|
|
(stage / name).write_text(json.dumps(raw, indent=2, allow_nan=False) + "\n", "utf-8")
|
|
config["trajectory"] = name
|
|
|
|
|
|
def _range_defaults(controller: Controller, config: dict) -> None:
|
|
spans = config["command_ranges"] = dict(config.get("command_ranges", {}))
|
|
wheel_defaults = {
|
|
"lin_vel_x": [-0.5, 0.5],
|
|
"lin_vel_y": [-0.5, 0.5],
|
|
"ang_vel_z": [-1, 1],
|
|
}
|
|
for axis, (lo, hi, rest) in controller.command_spans.items():
|
|
if axis in spans:
|
|
continue
|
|
if controller.kind in WHEEL_KINDS:
|
|
low, high = wheel_defaults[axis]
|
|
elif controller.kind == "ik":
|
|
low, high = -0.2, 0.2
|
|
elif controller.kind == "trajectory":
|
|
low, high = 0, 2
|
|
else:
|
|
low, high = rest - 1, rest + 1
|
|
spans[axis] = [
|
|
float(max(lo, low)) if np.isinf(lo) else lo,
|
|
float(min(hi, high)) if np.isinf(hi) else hi,
|
|
]
|
|
|
|
|
|
def _controls_document(controller: Controller, config: dict) -> dict | None:
|
|
# Many-DOF hands expose every joint through direct sliders. A pad can only
|
|
# steer a few axes at once, so keep a named subset in the operator file.
|
|
available = controller.command_axes
|
|
if controller.kind == "differential":
|
|
available = ("lin_vel_x", "ang_vel_z")
|
|
axes = config.get("operator_axes", list(available[:4]))
|
|
if not isinstance(axes, list) or len(axes) > 4 or len(set(axes)) != len(axes):
|
|
raise ValueError("operator_axes must list at most four distinct axes")
|
|
if set(axes) - set(available):
|
|
raise ValueError("operator_axes contains unknown controller commands")
|
|
if not axes:
|
|
return None
|
|
if controller.kind in WHEEL_KINDS:
|
|
return velocity_controls(axes, differential=controller.kind == "differential")
|
|
generic = (
|
|
("Ly", -1, "key_w", "key_s"),
|
|
("Lx", -1, "key_a", "key_d"),
|
|
("Rx", -1, "key_j", "key_l"),
|
|
("Ry", -1, "key_i", "key_k"),
|
|
)
|
|
bindings = {}
|
|
for i, axis in enumerate(axes):
|
|
unit = "m" if controller.kind == "ik" else "1"
|
|
if controller.kind == "joint_sliders":
|
|
joint = object_id(controller.model, mujoco.mjtObj.mjOBJ_JOINT, axis)
|
|
unit = "rad" if controller.model.jnt_type[joint] == mujoco.mjtJoint.mjJNT_HINGE else "m"
|
|
bindings[axis] = AxisBinding(*generic[i], unit, "increase target")
|
|
return controls_document(
|
|
bindings,
|
|
term="target",
|
|
feeds=controller.kind,
|
|
frame="world" if controller.kind == "ik" else "joint",
|
|
)
|
|
|
|
|
|
def setup_controller(model_xml: Path, config: Mapping | Path, out: Path) -> ControllerSetup:
|
|
"""Write normalized robot.xml, controller.json and its operator bindings.
|
|
|
|
The XML continues to reference existing mesh and texture files. A bundle
|
|
packages those dependencies when it copies the normalized XML.
|
|
"""
|
|
model_xml = Path(model_xml).resolve(strict=True)
|
|
out = Path(out).absolute()
|
|
config, parent = read_config(config)
|
|
kind = config["kind"]
|
|
checked_mjcf(model_xml)
|
|
spec = mujoco.MjSpec.from_file(str(model_xml))
|
|
# Compiling a spec with fusestatic enabled can mutate its authored
|
|
# geometry, which is still needed when writing the normalized XML.
|
|
model = _compile_with_named_links(spec)
|
|
|
|
if kind in WHEEL_KINDS:
|
|
names, _, speed = wheel_mixer(config)
|
|
gain = finite(config.get("kv", 5), "kv", positive=True)
|
|
effort = finite(config.get("effort_limit", 30), "effort_limit", positive=True)
|
|
normalize_actuators(
|
|
spec, model, names, "velocity", kp=0, kd=gain, effort=effort, speed=speed
|
|
)
|
|
elif kind != "direct":
|
|
kp, kd, effort = _gains(config)
|
|
normalize_actuators(
|
|
spec,
|
|
model,
|
|
config.get("joints"),
|
|
"motor" if kind == "cpg" else "position",
|
|
kp=kp,
|
|
kd=kd,
|
|
effort=effort,
|
|
speed=0,
|
|
)
|
|
if kind == "ik":
|
|
_prepare_ik(spec, model, config)
|
|
|
|
out.parent.mkdir(parents=True, exist_ok=True)
|
|
with TemporaryDirectory(prefix=".controller-", dir=out.parent) as tmp:
|
|
stage = Path(tmp)
|
|
if kind == "trajectory":
|
|
_prepare_trajectory(config, parent, stage)
|
|
model = write_mjcf(spec, model_xml, stage / "robot.xml")
|
|
controller = Controller(model, config, stage)
|
|
config["control_hz"] = controller.control_hz
|
|
_range_defaults(controller, config)
|
|
controller = Controller(model, config, stage)
|
|
controls = _controls_document(controller, config)
|
|
if controls is not None:
|
|
path = stage / "controls.yaml"
|
|
path.write_text(yaml.safe_dump(controls, sort_keys=False), "utf-8")
|
|
controller.attach_controls(load_controls(path))
|
|
(stage / "controller.json").write_text(
|
|
json.dumps(config, indent=2, allow_nan=False) + "\n", "utf-8"
|
|
)
|
|
files = list(stage.iterdir())
|
|
conflicts = [entry.name for entry in files if (out / entry.name).exists()]
|
|
if conflicts:
|
|
raise FileExistsError(f"Controller setup would overwrite {sorted(conflicts)} in {out}")
|
|
out.mkdir(exist_ok=True)
|
|
published = []
|
|
try:
|
|
for entry in files:
|
|
target = out / entry.name
|
|
os.link(entry, target)
|
|
published.append(target)
|
|
except OSError:
|
|
for target in published:
|
|
target.unlink()
|
|
raise
|
|
return ControllerSetup(
|
|
out / "robot.xml",
|
|
out / "controller.json",
|
|
out / "controls.yaml" if controls is not None else None,
|
|
)
|