142 lines
5.5 KiB
Python
142 lines
5.5 KiB
Python
"""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()}
|