Files
T
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

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)