"""Shared deterministic command input for MuJoCo controllers and policy replay.""" from __future__ import annotations from collections.abc import Iterable, Mapping import numpy as np from controller import vocabulary from controller.controls import Controls from controller.input import Keyboard, deflections, scale from .model import finite class OperatorInput: """Resolve validated controls against command ranges on the simulator clock.""" def __init__( self, controls: Controls, command_spans: Mapping[str, tuple[float, float, float]], *, release: Mapping[str, float] | None = None, ): axes = {a.name for c in controls.commands for a in c.axes} if not axes <= command_spans.keys(): raise ValueError( f"Controls drive unknown command axes {sorted(axes - command_spans.keys())}" ) for axis in axes: lo, hi, _ = command_spans[axis] if not np.isfinite(lo) or not np.isfinite(hi): raise ValueError(f"Set finite command_ranges for operator axis {axis!r}") if controls.task or any(a.rest for c in controls.commands for a in c.axes): raise ValueError( "Deterministic controllers use their configured axis rests and no task controls" ) self.controls = controls self.command_spans = dict(command_spans) self.release = dict(release or {}) self.reset() def reset(self) -> None: self._keyboard = Keyboard(self.controls) self._held_keys: set[str] = set() self._held_buttons: set[str] = set() self._shift = False self._source = "keys" self._released = False self.releasing = False self.driving = False self._reset_armed = False self._integrated = dict.fromkeys(self.controls.integrating, 0.0) def _read_keys(self, keys: Iterable[str], now: float) -> bool: incoming = set(keys) unknown = incoming - set(vocabulary.keys()) if unknown: raise ValueError(f"Unknown keyboard inputs {sorted(unknown)}") held = incoming & self._keyboard.watched became = set() for key in sorted(self._held_keys - held): became.update(self._keyboard.lift(key, now)) pressed = held - self._held_keys for key in sorted(pressed): became.update(self._keyboard.press(key, now) or ()) self._held_keys = held if set(self.controls.key_release) & self._keyboard.live: return True if pressed and became: self._source, self._released = "keys", False return False def _read_pad(self, pad: Mapping, dt: float) -> tuple[dict[str, float], bool]: ranges = {a["name"]: a["range"] for a in vocabulary.load()["axes"]} buttons = set(vocabulary.buttons()) unknown = pad.keys() - (ranges.keys() | buttons) if unknown: raise ValueError(f"Unknown gamepad inputs {sorted(unknown)}") values = {n: float(np.clip(finite(pad.get(n, 0), n), *r)) for n, r in ranges.items()} held = {n for n in buttons if pad.get(n)} pressed = held - self._held_buttons lifted = self._held_buttons - held self._held_buttons = held controls = self.controls if controls.release_button in held: return values, True if controls.shift_gesture == "hold": self._shift = controls.shift in held elif controls.shift in pressed: self._shift = not self._shift if controls.reset in pressed: self._reset_armed = True if self._reset_armed: relevant = { b.source for axis, bindings in controls.bindings.items() for b in bindings if b.shifted or axis in controls.integrating } if any(values[name] for name in relevant): self._reset_armed = False elif controls.reset in lifted: self._integrated = dict.fromkeys(controls.integrating, 0.0) self._reset_armed = False if any(values.values()) or pressed: if self._source != "pad": self._keyboard.drop() self._source, self._released = "pad", False x = deflections(values, self._shift, controls) if self._source == "pad": for axis, seconds in controls.integrating.items(): self._integrated[axis] = float( np.clip(self._integrated[axis] + x[axis] * dt / seconds, -1, 1) ) x.update(self._integrated) return x, False def read(self, pad: Mapping, keys: Iterable[str], now: float, dt: float) -> dict[str, float]: """Return this tick's command from normalized pad values and physical keys.""" key_release = self._read_keys(keys, now) x, pad_release = self._read_pad(pad, dt) self.releasing = key_release or pad_release if self.releasing: self._keyboard.drop() self._integrated = dict.fromkeys(self.controls.integrating, 0.0) self._released = True if self._released: self.driving = False return dict(self.release) if self._source == "keys": x = { n: self._keyboard.deflection(axis, now) for n, axis in self.controls.key_axes.items() } self.driving = any(x.values()) return {n: scale(v, self.command_spans[n]) for n, v in x.items()}