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

336 lines
16 KiB
Python

"""Deterministic native controller ticks over a normalized MuJoCo model."""
from __future__ import annotations
import json
from collections.abc import Iterable, Mapping
from pathlib import Path
import mujoco
import numpy as np
from controller.controls import Controls, load_controls
from .input import OperatorInput
from .model import JointControl, finite, joint_effort_bounds, joint_values, object_id, vector
from .motion import DampedLeastSquaresIK, Trajectory
from .wheels import WHEEL_AXES, mix_velocity, wheel_mixer
SCHEMA = "mjrl-controller/1"
IK_AXES = ("target_x", "target_y", "target_z")
WHEEL_KINDS = ("differential", "omni")
KINDS = ("direct", "ik", "joint_sliders", "trajectory", "cpg", *WHEEL_KINDS)
def read_config(config: Mapping | Path) -> tuple[dict, Path]:
parent = Path.cwd()
if isinstance(config, (str, Path)):
config = Path(config)
parent = config.resolve().parent
config = json.loads(config.read_text("utf-8"))
if not isinstance(config, Mapping) or config.get("schema", SCHEMA) != SCHEMA:
raise ValueError(f"Expected a {SCHEMA} object")
kind = config.get("kind")
if kind not in KINDS:
raise ValueError(f"Unknown controller kind {kind!r}. Expected one of {KINDS}")
return {**config, "schema": SCHEMA}, parent
class Controller:
"""One controller instance per simulation. Call step before mj_step."""
def __init__(self, model: mujoco.MjModel, config: dict, parent: Path):
self.model = model
self.kind = config["kind"]
timestep = float(model.opt.timestep)
default_hz = 1 / (timestep * max(1, round(0.02 / timestep)))
self.control_hz = finite(config.get("control_hz", default_hz), "control_hz", positive=True)
decimation = 1 / (self.control_hz * timestep)
if decimation < 1 - 1e-8 or not np.isclose(
decimation, round(decimation), rtol=0, atol=1e-8
):
raise ValueError("control_hz must correspond to a whole number of physics steps")
self.command_axes: tuple[str, ...]
self.joints: JointControl | None = None
self._target_rates: np.ndarray | None = None
if "target_rate_limits" in config and self.kind != "joint_sliders":
raise ValueError("target_rate_limits requires joint_sliders")
self.controls: Controls | None = None
self.operator: OperatorInput | None = None
if self.kind == "direct":
self._bind_direct(config)
elif self.kind in WHEEL_KINDS:
self._bind_wheels(config)
else:
actuator = "motor" if self.kind == "cpg" else "position"
self.joints = JointControl.bind(model, config.get("joints"), actuator)
self.ctrl_ids = self.joints.ctrl
self._bind_motion(config, parent)
limits = config.get("command_ranges", {})
if not isinstance(limits, Mapping) or set(limits) - set(self.command_axes):
raise ValueError("command_ranges contains unknown command axes")
self.command_spans = {}
for name, default in zip(self.command_axes, self._spans, strict=True):
lo, hi, rest = default
if name in limits:
low, high = vector(limits[name], 2, f"command_ranges.{name}")
if low < lo or high > hi or not low <= rest <= high:
raise ValueError(f"command_ranges.{name} exceeds limits or excludes the rest")
lo, hi = low, high
self.command_spans[name] = (float(lo), float(hi), float(rest))
self._time: float | None = None
self._motion_time = 0.0
def _bind_direct(self, config: dict) -> None:
model = self.model
names = config.get("actuators")
if names is None:
names = [
mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) for i in range(model.nu)
]
names = [name for name in names if name]
if not isinstance(names, list) or not all(isinstance(n, str) and n for n in names):
raise ValueError("actuators must be a list of actuator names")
if len(set(names)) != len(names):
raise ValueError("actuators must be unique")
self.command_axes = tuple(names)
self.ctrl_ids = np.array(
[object_id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, n) for n in names], dtype=int
)
self._spans = []
for i in self.ctrl_ids:
lo, hi = (
model.actuator_ctrlrange[i] if model.actuator_ctrllimited[i] else (-np.inf, np.inf)
)
self._spans.append((lo, hi, float(np.clip(0, lo, hi))))
def _bind_wheels(self, config: dict) -> None:
names, self._mixer, self._speed = wheel_mixer(config)
self.joints = JointControl.bind(self.model, names, "velocity")
if np.any(np.isfinite(self.joints.lo)):
raise ValueError("Wheels must be continuous joints")
if any(
self.model.jnt_type[self.model.dof_jntid[d]] != mujoco.mjtJoint.mjJNT_HINGE
for d in self.joints.vel
):
raise ValueError("Wheel joints must be hinges")
self.ctrl_ids = self.joints.ctrl
limited = self.model.actuator_ctrllimited[self.ctrl_ids]
lo, hi = self.model.actuator_ctrlrange[self.ctrl_ids].T
if np.any(limited & ((lo > -self._speed) | (hi < self._speed))):
raise ValueError("max_wheel_speed exceeds an actuator's control range")
self.command_axes = WHEEL_AXES
lateral = 0.0 if self.kind == "differential" else np.inf
self._spans = [(-np.inf, np.inf, 0), (-lateral, lateral, 0), (-np.inf, np.inf, 0)]
def _bind_motion(self, config: dict, parent: Path) -> None:
joints = self.joints
assert joints is not None
if self.kind == "ik":
self.ik = DampedLeastSquaresIK(self.model, joints, config)
self._start_at_site = config.get("start_target_at_site", False)
if type(self._start_at_site) is not bool:
raise ValueError("start_target_at_site must be boolean")
self.command_axes = IK_AXES
self._spans = [(-np.inf, np.inf, 0)] * 3
self._origin = self.model.body_pos[
object_id(self.model, mujoco.mjtObj.mjOBJ_BODY, config["mocap_body"])
].copy()
elif self.kind == "joint_sliders":
self.command_axes = joints.names
self._home = np.clip(self.model.qpos0[joints.pos], joints.lo, joints.hi)
if "home" in config:
if not isinstance(config["home"], Mapping):
raise ValueError("home must map every controlled joint by name")
self._home = joint_values(config["home"], joints.names, "home")
if np.any(self._home < joints.lo) or np.any(self._home > joints.hi):
raise ValueError("home lies outside a controlled joint's position limits")
rates = config.get("target_rate_limits", {})
if not isinstance(rates, Mapping) or set(rates) - set(joints.names):
raise ValueError("target_rate_limits must map controlled joint names to rates")
if rates:
self._target_rates = np.array([
finite(rates[name], f"target_rate_limits.{name}", positive=True)
if name in rates else np.inf for name in joints.names
])
self._spans = list(zip(joints.lo, joints.hi, self._home, strict=True))
elif self.kind == "trajectory":
trajectory = config.get("trajectory")
if isinstance(trajectory, str):
path = Path(trajectory)
if path.is_absolute() or ".." in path.parts:
raise ValueError("trajectory must be a path relative to controller.json")
trajectory = parent / path
self.trajectory = Trajectory.from_document(trajectory, joints)
self._loop = config.get("loop", False)
if type(self._loop) is not bool:
raise ValueError("loop must be true or false")
if self._loop and not np.array_equal(self.trajectory.q[0], self.trajectory.q[-1]):
raise ValueError("Looping trajectory must have matching first and last poses")
self.command_axes = ("playback_rate",)
self._spans = [(0, np.inf, 1)]
else:
self._bind_cpg(config, joints)
def _bind_cpg(self, config: dict, joints: JointControl) -> None:
size = len(joints.names)
self._frequency = finite(config.get("frequency_hz"), "frequency_hz", positive=True)
self._amplitude = vector(config.get("amplitude"), size, "amplitude")
if np.any(self._amplitude < 0):
raise ValueError("CPG amplitudes must be nonnegative. Set phase to reverse a joint")
self._phase = vector(config.get("phase", [0] * size), size, "phase")
default = self.model.qpos0[joints.pos].tolist()
self._centre = vector(config.get("centre", default), size, "centre")
if np.any(self._centre - self._amplitude < joints.lo) or np.any(
self._centre + self._amplitude > joints.hi
):
raise ValueError("CPG excursion lies outside a joint's position limits")
self._kp = joint_values(config.get("kp", 40), joints.names, "kp", positive=True)
self._kd = joint_values(config.get("kd", 4), joints.names, "kd", nonnegative=True)
requested = joint_values(config.get("effort_limit", 30), joints.names, "effort_limit", positive=True)
ids = self.model.dof_jntid[joints.vel]
bounds = joint_effort_bounds(self.model, ids, requested)
self._effort = np.minimum(-bounds[:, 0], bounds[:, 1])
self.command_axes = ("amplitude_scale",)
self._spans = [(0, 1, 1)]
def attach_controls(self, controls: Controls) -> None:
stopped = {"trajectory": "playback_rate", "cpg": "amplitude_scale"}.get(self.kind)
self.operator = OperatorInput(
controls, self.command_spans, release={stopped: 0} if stopped else None
)
self.controls = controls
def reset(self, data: mujoco.MjData) -> None:
"""Forget input and timing state after a simulation reset."""
self._time = float(data.time)
self._motion_time = 0.0
if self.kind == "ik":
target = data.mocap_pos[self.ik.mocap]
if self._start_at_site:
mujoco.mj_kinematics(self.model, data)
target[:] = data.site_xpos[self.ik.site]
self._origin = target.copy()
spans = np.array([self.command_spans[axis][:2] for axis in IK_AXES])
self._target_low = self._origin + spans[:, 0]
self._target_high = self._origin + spans[:, 1]
elif self.kind == "joint_sliders":
data.ctrl[self.ctrl_ids] = np.clip(
data.qpos[self.joints.pos], self.joints.lo, self.joints.hi
)
if self._target_rates is not None:
self._applied = data.ctrl[self.ctrl_ids].copy()
self._requested = self._applied.copy()
if self.operator is not None:
self.operator.reset()
def _values(self, command: Mapping[str, float]) -> np.ndarray:
unknown = command.keys() - self.command_spans.keys()
if unknown:
raise ValueError(f"Unknown controller commands {sorted(unknown)}")
return np.array(
[
np.clip(finite(command.get(n, rest), n), lo, hi)
for n, (lo, hi, rest) in self.command_spans.items()
]
)
def step(
self,
data: mujoco.MjData,
command: Mapping[str, float] | None = None,
*,
pad: Mapping[str, float] | None = None,
keys: Iterable[str] | None = None,
) -> np.ndarray:
"""Update controls from explicit commands or raw input, without advancing physics."""
if command is not None and (pad is not None or keys is not None):
raise ValueError("Supply an explicit command or raw operator input")
now = finite(data.time, "simulation time")
if self._time is None or now < self._time:
self.reset(data)
dt = now - self._time
use_operator = pad is not None or keys is not None
if use_operator:
if self.operator is None:
raise ValueError("Load controls.yaml before supplying operator input")
command = self.operator.read(pad or {}, keys or (), now, dt)
if self.kind in {"direct", "joint_sliders"}:
# The passive viewer's Control panel writes data.ctrl. Keep its
# slider values when there is no new keyboard, pad, or API target.
previous_targets = data.ctrl[self.ctrl_ids]
if self._target_rates is not None:
# Viewer edits replace the pending goal only on the axes it changed.
previous_targets = np.where(
previous_targets != self._applied, previous_targets, self._requested
)
previous = dict(zip(self.command_axes, previous_targets, strict=True))
if use_operator and self.operator.releasing:
command = {
name: self.command_spans[name][2] for name in self.operator.controls.bindings
}
elif use_operator and not self.operator.driving:
command = None
command = {**previous, **(command or {})}
values = self._values(command or {})
joints = self.joints
if self.kind in WHEEL_KINDS:
output = mix_velocity(self._mixer, values, self._speed)
elif self.kind == "ik":
# Idle raw input is present on every viewer tick. Preserve a
# target set by the viewer's mocap drag until an input moves it.
if (not use_operator and command is not None) or (
use_operator and (np.any(values) or self.operator.releasing)
):
data.mocap_pos[self.ik.mocap] = self._origin + values
target = data.mocap_pos[self.ik.mocap]
# Native viewer drags write directly to mocap_pos, in world metres.
np.clip(target, self._target_low, self._target_high, out=target)
output = self.ik.solve(data, target)
elif self.kind == "trajectory":
self._motion_time += dt * values[0]
t = self._motion_time
if self._loop:
t %= self.trajectory.time_s[-1]
output = self.trajectory.at(t)
elif self.kind == "cpg":
self._motion_time += dt
omega = 2 * np.pi * self._frequency
phase = omega * self._motion_time + self._phase
amplitude = self._amplitude * values[0]
target = self._centre + amplitude * np.sin(phase)
rate = amplitude * omega * np.cos(phase)
effort = self._kp * (target - data.qpos[joints.pos])
effort += self._kd * (rate - data.qvel[joints.vel])
output = np.clip(effort, -self._effort, self._effort)
elif self._target_rates is not None:
self._requested = values
limited = np.isfinite(self._target_rates)
delta = self._target_rates[limited] * dt
output = values.copy()
output[limited] = np.clip(
values[limited], self._applied[limited] - delta, self._applied[limited] + delta
)
self._applied = output.copy()
else:
output = values
data.ctrl[self.ctrl_ids] = output
self._time = now
return data.ctrl.copy()
def load_controller(
model: mujoco.MjModel,
config: Mapping | Path,
controls: Path | None = None,
) -> Controller:
"""Bind a normalized controller to a compiled model and optional controls file."""
document, parent = read_config(config)
controller = Controller(model, document, parent)
if controls is not None:
controller.attach_controls(load_controls(controls))
return controller