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