193 lines
8.2 KiB
Python
193 lines
8.2 KiB
Python
"""Map pad deflections and keyboard edges into policy command inputs."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from collections.abc import Iterable, Mapping
|
|
from typing import Any
|
|
|
|
from . import vocabulary as _vocabulary
|
|
from .controls import Command, Controls, KeyAxis
|
|
|
|
__all__ = ["Keyboard", "deflections", "scale", "spans"]
|
|
|
|
|
|
# ── The pad's mapping ───────────────────────────────────────────────────────
|
|
|
|
|
|
def _band(s: float, travel: tuple[float, ...]) -> float:
|
|
"""A signed deflection through one binding's stretch of travel.
|
|
|
|
Zero until `from`, full at `to` and held there beyond it. So `(0.0, 0.5)`
|
|
is full at half travel, and `(0.5, 1.0)` is silent until half travel. With
|
|
a fall, `(0.0, 0.5, 0.5, 0.75)`, it comes back from full at `back` to zero
|
|
at `off` and stays there.
|
|
"""
|
|
lo, hi, *fall = travel
|
|
a = abs(s)
|
|
x = min(max((a - lo) / (hi - lo), 0.0), 1.0)
|
|
if fall:
|
|
back, off = fall
|
|
x *= min(max((off - a) / (off - back), 0.0), 1.0)
|
|
return x if s >= 0.0 else -x
|
|
|
|
|
|
def deflections(
|
|
values: Mapping[str, float], shifted: bool, controls: Controls
|
|
) -> dict[str, float]:
|
|
"""Pad control positions -> each command axis's deflection, -1..1.
|
|
|
|
`values` is keyed by the dictionary's names (`Lx` ... `RT`). A source that
|
|
has a shifted binding drives **only** that binding while shifted; every
|
|
other source carries on as it was. The pad's path only: the keyboard has
|
|
bindings of its own (`Keyboard`).
|
|
"""
|
|
layered = {b.source for parts in controls.bindings.values() for b in parts if b.shifted}
|
|
out = {}
|
|
for axis, parts in controls.bindings.items():
|
|
x = 0.0
|
|
for b in parts:
|
|
live = shifted if b.shifted else not (shifted and b.source in layered)
|
|
if live:
|
|
x += _band(b.sign * values[b.source], b.travel)
|
|
out[axis] = max(-1.0, min(1.0, x))
|
|
return out
|
|
|
|
|
|
def scale(x: float, span: tuple[float, float, float]) -> float:
|
|
"""`full_deflection_is_range_edge`, measured from the axis's rest.
|
|
|
|
The two ends are scaled separately. A range like (-0.5, 1.0) gives forward
|
|
twice what it gives back, which is what the policy trained on; taking the
|
|
larger end for both quietly commands something it never saw.
|
|
"""
|
|
lo, hi, rest = span
|
|
return rest + x * ((hi - rest) if x >= 0.0 else (rest - lo))
|
|
|
|
|
|
def spans(command: Command, cfg: Any) -> dict[str, tuple[float, float, float]]:
|
|
"""`(low, high, rest)` per axis, from a live command config.
|
|
|
|
A range is either a `(low, high)` pair, as the velocity command has, or a
|
|
half-range, as the posture command's angles are. The rest is the config
|
|
field the file names, or zero.
|
|
"""
|
|
out = {}
|
|
for a in command.axes:
|
|
r = getattr(cfg.ranges, a.name)
|
|
lo, hi = (-float(r), float(r)) if isinstance(r, (int, float)) else map(float, r)
|
|
rest = float(getattr(cfg, a.rest)) if a.rest else 0.0
|
|
if not lo <= rest <= hi:
|
|
raise ValueError(
|
|
f"{command.term}.{a.name} rests at {rest:g}, outside its range "
|
|
f"({lo:g}, {hi:g}). A centred stick would command a value the range "
|
|
"clamps away -- name the rest in the controls file (`rest: <field>`)."
|
|
)
|
|
out[a.name] = (lo, hi, rest)
|
|
return out
|
|
|
|
|
|
# ── The keyboard's path ─────────────────────────────────────────────────────
|
|
|
|
|
|
class Keyboard:
|
|
"""The keyboard's half of the operator: which keystrokes are live, since when.
|
|
|
|
Fed a key's two edges by the dictionary's name, and asked what an axis or a
|
|
task control reads. A keystroke held for `full_after_s` is full deflection
|
|
and half that is half; the moment it stops being live it reads nothing.
|
|
Several live on one direction are summed, as two keys on one direction of a
|
|
stick would push it together, and the two directions of an axis cancel.
|
|
"""
|
|
|
|
def __init__(self, controls: Controls):
|
|
self._full = float(controls.full_after_s)
|
|
modifiers = _vocabulary.modifiers()
|
|
#: Keystroke -> (the modifier held with it or `None`, the key or the
|
|
#: modifier it is), every keystroke the file's keyboard block binds.
|
|
self._strokes = {s: _vocabulary.keystroke(s) for s in controls.keystrokes}
|
|
#: Modifier -> its two keys, for the modifiers this block names at all.
|
|
self._modifiers = {
|
|
m: keys for m, keys in modifiers.items()
|
|
if any(m in (mod, key) for mod, key in self._strokes.values())
|
|
}
|
|
#: Key -> the modifiers bound in front of it here. While one of them is
|
|
#: down, the key alone is dead: Shift + J is not J as well.
|
|
self._chorded: dict[str, frozenset[str]] = {}
|
|
for mod, key in self._strokes.values():
|
|
if mod is not None:
|
|
self._chorded[key] = self._chorded.get(key, frozenset()) | {mod}
|
|
#: Every dictionary key this block reads: each keystroke's key, and both
|
|
#: keys of each modifier it names. Any other key is not this file's.
|
|
self.watched = frozenset(
|
|
{key for _mod, key in self._strokes.values() if key not in modifiers}
|
|
| {k for pair in self._modifiers.values() for k in pair}
|
|
)
|
|
#: Keys down, as their edges report them.
|
|
self._down: set[str] = set()
|
|
#: Keys down and pressed since the last `drop`: what drives.
|
|
self._fresh: set[str] = set()
|
|
#: Live keystroke -> when it last became live, on the operator's clock.
|
|
self._live: dict[str, float] = {}
|
|
|
|
def press(self, key: str, now: float) -> set[str] | None:
|
|
"""`key` went down at `now`. The keystrokes that became live, or `None`
|
|
for a key already down -- a repeat, which is the same hold."""
|
|
if key in self._down:
|
|
return None
|
|
self._down.add(key)
|
|
self._fresh.add(key)
|
|
return self._refresh(now)
|
|
|
|
def lift(self, key: str, now: float) -> set[str]:
|
|
"""`key` came up at `now`. The keystrokes that became live: a key whose
|
|
modifier was let go of is itself again."""
|
|
self._down.discard(key)
|
|
self._fresh.discard(key)
|
|
return self._refresh(now)
|
|
|
|
def drop(self) -> None:
|
|
"""Hands off: nothing is live, and a key still down drives nothing --
|
|
as a key or as a modifier -- until it is pressed again."""
|
|
self._fresh.clear()
|
|
self._live.clear()
|
|
|
|
def _refresh(self, now: float) -> set[str]:
|
|
held = {m for m, pair in self._modifiers.items() if self._fresh.intersection(pair)}
|
|
live = set()
|
|
for stroke, (mod, key) in self._strokes.items():
|
|
if mod is not None:
|
|
on = key in self._fresh and mod in held
|
|
elif key in self._modifiers:
|
|
on = key in held
|
|
else:
|
|
on = key in self._fresh and not (self._chorded.get(key, frozenset()) & held)
|
|
if on:
|
|
live.add(stroke)
|
|
became = live - self._live.keys()
|
|
self._live = {s: self._live.get(s, now) for s in live}
|
|
return became
|
|
|
|
@property
|
|
def live(self) -> frozenset[str]:
|
|
return frozenset(self._live)
|
|
|
|
def ramp(self, stroke: str, now: float) -> float:
|
|
"""0..1: how long `stroke` has been live, over `full_after_s`."""
|
|
since = self._live.get(stroke)
|
|
if since is None:
|
|
return 0.0
|
|
return min(1.0, max(0.0, (now - since) / self._full))
|
|
|
|
def deflection(self, axis: KeyAxis, now: float) -> float:
|
|
"""One command axis, -1..1: its `+` keystrokes less its `-` ones."""
|
|
x = sum(self.ramp(s, now) for s in axis.plus) - sum(self.ramp(s, now) for s in axis.minus)
|
|
return max(-1.0, min(1.0, x))
|
|
|
|
def amount(self, strokes: Iterable[str], now: float) -> float:
|
|
"""A task's `amount`: the furthest of its keystrokes' ramps."""
|
|
return max((self.ramp(s, now) for s in strokes), default=0.0)
|
|
|
|
def pressed(self, strokes: Iterable[str]) -> bool:
|
|
"""A task's `press`: any of its keystrokes live, at once and in full."""
|
|
return any(s in self._live for s in strokes)
|