Files
chenlin c32df70326
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
release: v0.1 generic robot training and application platform
2026-11-05 11:05:01 +08:00

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()}