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

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,
)