"""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