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.
336 lines
16 KiB
Python
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
|