fix gamepad layout and refuse a pad whose axes disagree
An Xbox One S pad over Bluetooth numbers its controls differently from the indices this node shipped with: the right stick is on axes 2 and 3, the triggers on 4 and 5, and the buttons carry gaps at 2 and 5. Against the old layout an untouched pad reported a resting trigger as full right-stick deflection, which asked for 30 deg/s of pitch while lying on the desk, held the lease for ever so GAMEPAD_IDLE_RELEASE_S never fired, and moved a real arm into a controller error. X toggled freedrive and LB was the E-STOP, so the resulting latch could not be released from the pad at all. Layout now carries the swept indices. Each pad is also measured the first time it is seen at rest: a stick must rest near centre and a trigger near the bottom of its travel, and a pad that does not fit reads as absent and is named in the log rather than driving an arm from a numbering nobody checked. The measured rest becomes each axis's zero, so a worn stick's tilt is subtracted rather than deadzoned away and a trigger that reads 0 until it first moves stops reading as half pressed. Rest is never re-measured while the pad is in use, because doing that under a held stick would record the deflection as centre and the arm would move when the operator let go. The probe drives from the same calibration, so what it prints is still what the arm would get, and it shows the mismatch. Tests name their controls through axes_for and buttons_for instead of numbering them; the positional lists are why a wrong index passed a green suite. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
Claude Opus 5
+75
-3
@@ -12,11 +12,19 @@ unplugged pad. So silence past `JOY_TTL_S` reads as *absent*, which is the same
|
||||
answer, because a reading nobody is refreshing is not evidence of anything.
|
||||
`dora-joy` publishes on every tick whether or not the pad moved, so silence
|
||||
means one thing only.
|
||||
|
||||
The other thing this seam does is :class:`Calibration`: no two pads rest in
|
||||
quite the same place, and one that rests somewhere the layout does not expect
|
||||
is a pad this node must not drive an arm from. Every reading the loop sees has
|
||||
been through it, so the rest of the node only ever handles a pad that behaves.
|
||||
"""
|
||||
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from dora_teleop import mapping
|
||||
from dora_teleop.log import log
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Sample:
|
||||
@@ -52,17 +60,81 @@ def decode(event: dict) -> Sample:
|
||||
)
|
||||
|
||||
|
||||
class Calibration:
|
||||
"""Where the pad in front of us rests, and whether the layout fits it.
|
||||
|
||||
Captured from the first reading of a pad that is sitting still, and kept
|
||||
until that pad goes away. It is not re-taken while the pad is in use, which
|
||||
would be the one way to get this badly wrong: an operator holding a stick
|
||||
when it happened would have that deflection recorded as centre, and the arm
|
||||
would then move whenever they let go.
|
||||
|
||||
A pad whose resting axes do not fit the layout is reported **absent** rather
|
||||
than corrected. The numbering cannot be guessed from one reading — a trigger
|
||||
resting at -1.0 and a stick held hard over look identical — and guessing
|
||||
wrong drives a real arm. `dora-teleop-probe` shows what the pad reports, and
|
||||
`Layout` in :mod:`dora_teleop.mapping` is where a pad that disagrees is
|
||||
written down.
|
||||
"""
|
||||
|
||||
def __init__(self, layout: mapping.Layout = mapping.LAYOUT):
|
||||
self.layout = layout
|
||||
self.mismatch: str | None = None
|
||||
self._zeros: list[float] | None = None
|
||||
|
||||
@property
|
||||
def known(self) -> bool:
|
||||
"""Whether a pad has been measured at rest and may be driven from."""
|
||||
return self._zeros is not None
|
||||
|
||||
def forget(self) -> None:
|
||||
"""Drop what was learned, so the next pad to appear is measured afresh."""
|
||||
self._zeros = None
|
||||
self.mismatch = None
|
||||
|
||||
def apply(self, sample: Sample) -> Sample:
|
||||
"""Return the reading as the layout's ideal pad would have sent it.
|
||||
|
||||
An absent pad passes straight through and takes the calibration with
|
||||
it. A pad that does not fit the layout comes back absent, which every
|
||||
path in this node already treats as a reason to stop.
|
||||
"""
|
||||
if not sample.present:
|
||||
self.forget()
|
||||
return sample
|
||||
if self._zeros is None or len(self._zeros) != len(sample.axes):
|
||||
try:
|
||||
self._zeros = mapping.rest_zeros(sample.axes, self.layout)
|
||||
except ValueError as wrong:
|
||||
# Logged once per pad: this runs on every reading, and a pad
|
||||
# that does not fit the layout does not fit it fifty times a
|
||||
# second.
|
||||
if self.mismatch is None:
|
||||
log(
|
||||
f"ignoring the gamepad: {wrong}. Run dora-teleop-probe and check Layout in mapping.py"
|
||||
)
|
||||
self.mismatch = str(wrong)
|
||||
return ABSENT
|
||||
self.mismatch = None
|
||||
return Sample(
|
||||
axes=mapping.canonical(sample.axes, self._zeros, self.layout),
|
||||
buttons=sample.buttons,
|
||||
present=True,
|
||||
)
|
||||
|
||||
|
||||
class JoyInput:
|
||||
"""The latest reading, and how long ago it arrived."""
|
||||
|
||||
def __init__(self, ttl_s: float):
|
||||
def __init__(self, ttl_s: float, layout: mapping.Layout = mapping.LAYOUT):
|
||||
self.ttl_s = ttl_s
|
||||
self.calibration = Calibration(layout)
|
||||
self._sample = ABSENT
|
||||
self._seen_at = 0.0
|
||||
|
||||
def accept(self, sample: Sample, moment: float) -> None:
|
||||
"""Take one reading off the wire."""
|
||||
self._sample = sample
|
||||
"""Take one reading off the wire, calibrated to the pad that sent it."""
|
||||
self._sample = self.calibration.apply(sample)
|
||||
self._seen_at = moment
|
||||
|
||||
def read(self, moment: float) -> Sample:
|
||||
|
||||
+117
-16
@@ -34,26 +34,34 @@ MM_PER_M = 1000.0
|
||||
class Layout:
|
||||
"""Which axis and button index does what, in SDL's numbering.
|
||||
|
||||
The defaults are the layout SDL reports for an Xbox-style pad, which is also
|
||||
what it reports for most PlayStation-style ones. A pad that disagrees is a
|
||||
change here rather than a new environment variable: an index nobody can
|
||||
verify from the outside is worse than a line of code somebody can read.
|
||||
These are what an Xbox One S pad reports over Bluetooth, swept one control
|
||||
at a time: sticks on 0-3, both triggers last, and buttons with gaps at 2 and
|
||||
5. A pad that disagrees is a change here rather than a new environment
|
||||
variable: an index nobody can verify from the outside is worse than a line
|
||||
of code somebody can read. `dora-teleop-probe` is how you check.
|
||||
|
||||
**The axes can be checked at runtime and the buttons cannot.**
|
||||
:func:`rest_zeros` refuses a pad whose resting axes say this numbering is
|
||||
wrong, because an axis at rest has a value to compare. An unpressed button
|
||||
reads the same on every pad, so a wrong button index is silent — it was
|
||||
found here by `clear` toggling freedrive and `yaw_left` firing the E-STOP.
|
||||
The probe, and pressing each button once, is the only check there is.
|
||||
"""
|
||||
|
||||
left_x: int = 0
|
||||
left_y: int = 1
|
||||
left_trigger: int = 2
|
||||
right_x: int = 3
|
||||
right_y: int = 4
|
||||
right_x: int = 2
|
||||
right_y: int = 3
|
||||
left_trigger: int = 4
|
||||
right_trigger: int = 5
|
||||
|
||||
yaw_left: int = 4 # LB
|
||||
yaw_right: int = 5 # RB
|
||||
grasp: int = 0 # A
|
||||
release: int = 1 # B
|
||||
clear: int = 2 # X
|
||||
freedrive: int = 3 # Y
|
||||
estop: int = 6 # Back
|
||||
clear: int = 3 # X
|
||||
freedrive: int = 4 # Y
|
||||
yaw_left: int = 6 # LB
|
||||
yaw_right: int = 7 # RB
|
||||
estop: int = 11 # Back
|
||||
|
||||
|
||||
LAYOUT = Layout()
|
||||
@@ -69,6 +77,16 @@ TOOL_FRAME = "tool"
|
||||
# would mean a stick could claim the arm and then not move it.
|
||||
ACTIVITY = 0.3
|
||||
|
||||
# Where an untouched control sits once :func:`canonical` has had it: a stick at
|
||||
# centre, a trigger at the bottom of its travel.
|
||||
STICK_REST = 0.0
|
||||
TRIGGER_REST = -1.0
|
||||
|
||||
# How far an axis may rest from those and still be believed. Wide enough for a
|
||||
# worn stick's tilt and for a pad that reports a trigger it has never felt as 0,
|
||||
# and far short of the full deflection a wrongly numbered axis shows.
|
||||
REST_TOLERANCE = 0.25
|
||||
|
||||
|
||||
def deadzone(value: float, width: float) -> float:
|
||||
"""Ignore travel near centre, and rescale what is left to the full range.
|
||||
@@ -98,11 +116,13 @@ def expo(value: float, factor: float) -> float:
|
||||
def trigger(value: float) -> float:
|
||||
"""Return a trigger's travel as 0 at rest and 1 fully pressed.
|
||||
|
||||
SDL reports a trigger from -1 to 1, unlike a stick, and a trigger that has
|
||||
never been touched sometimes reads 0 instead of -1 until it first moves.
|
||||
Clamping keeps that from reading as half pressed.
|
||||
SDL reports a trigger from -1 to 1, unlike a stick, so `TRIGGER_REST` is
|
||||
where an untouched one sits. A trigger that has never moved sometimes reads
|
||||
0 instead, which this function alone cannot tell from half pressed —
|
||||
:func:`canonical` is what fixes that, by measuring the pad's own rest before
|
||||
anything reaches here.
|
||||
"""
|
||||
return max(0.0, min(1.0, (value + 1.0) / 2.0))
|
||||
return max(0.0, min(1.0, (value - TRIGGER_REST) / (1.0 - TRIGGER_REST)))
|
||||
|
||||
|
||||
def axis(values: list[float], index: int) -> float:
|
||||
@@ -115,6 +135,87 @@ def pressed(buttons: list[bool], index: int) -> bool:
|
||||
return bool(buttons[index]) if index < len(buttons) else False
|
||||
|
||||
|
||||
def axis_names(layout: Layout = LAYOUT) -> dict[int, str]:
|
||||
"""Name each axis index the layout uses."""
|
||||
return {
|
||||
layout.left_x: "left stick X",
|
||||
layout.left_y: "left stick Y",
|
||||
layout.left_trigger: "left trigger",
|
||||
layout.right_x: "right stick X",
|
||||
layout.right_y: "right stick Y",
|
||||
layout.right_trigger: "right trigger",
|
||||
}
|
||||
|
||||
|
||||
def rest_zeros(axes: list[float], layout: Layout = LAYOUT) -> list[float]:
|
||||
"""Return where each axis of an untouched pad sits, to measure deflection from.
|
||||
|
||||
An untouched pad is the one moment this node can check `layout` against the
|
||||
thing in front of it: a stick rests near centre and a trigger near the
|
||||
bottom of its travel, so an axis resting anywhere else means the numbering
|
||||
is wrong, or a control is being held, or the pad is broken. All three look
|
||||
the same from here and all three raise, because all three would otherwise
|
||||
drive the arm from a reading nobody made — a trigger read as a stick asks
|
||||
for full speed while sitting untouched on the desk.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
axes
|
||||
One reading, taken while nobody is touching the pad.
|
||||
layout
|
||||
Which index is which control.
|
||||
|
||||
Returns
|
||||
-------
|
||||
list of float
|
||||
The resting value of each axis, in the order they were reported.
|
||||
|
||||
Raises
|
||||
------
|
||||
ValueError
|
||||
If any axis rests where the layout says it cannot, naming the index,
|
||||
what the layout calls it, and what it read.
|
||||
"""
|
||||
names = axis_names(layout)
|
||||
triggers = (layout.left_trigger, layout.right_trigger)
|
||||
for index, value in enumerate(axes):
|
||||
if index in triggers:
|
||||
# Either end is a resting trigger: -1 is what SDL reports, and 0 is
|
||||
# what some pads report until the trigger first moves.
|
||||
if abs(value - TRIGGER_REST) <= REST_TOLERANCE or abs(value - STICK_REST) <= REST_TOLERANCE:
|
||||
continue
|
||||
elif abs(value - STICK_REST) <= REST_TOLERANCE:
|
||||
continue
|
||||
name = names.get(index, "an axis the layout does not use")
|
||||
raise ValueError(f"axis {index} rests at {value:+.3f}, which is not where {name} sits")
|
||||
return list(axes)
|
||||
|
||||
|
||||
def canonical(axes: list[float], zeros: list[float], layout: Layout = LAYOUT) -> list[float]:
|
||||
"""Return this pad's axes as the layout's ideal pad would have reported them.
|
||||
|
||||
Sticks come back resting at `STICK_REST` and triggers at `TRIGGER_REST`,
|
||||
whatever this particular pad does when untouched, so everything downstream
|
||||
is written once for a pad that behaves. A stick that no longer centres, and
|
||||
a trigger that reads 0 until it first moves, both stop being special cases
|
||||
here rather than in every function that reads one.
|
||||
|
||||
Deflection is measured from `zeros` rather than from the ideal, which is the
|
||||
whole point: the tilt is subtracted, not deadzoned away, so the travel that
|
||||
is left still reaches full speed.
|
||||
"""
|
||||
triggers = (layout.left_trigger, layout.right_trigger)
|
||||
out = []
|
||||
for index, value in enumerate(axes):
|
||||
zero = zeros[index] if index < len(zeros) else STICK_REST
|
||||
if index in triggers:
|
||||
travel = (value - zero) / (1.0 - zero) if zero < 1.0 else 0.0
|
||||
out.append(TRIGGER_REST + (1.0 - TRIGGER_REST) * max(0.0, min(1.0, travel)))
|
||||
else:
|
||||
out.append(max(-1.0, min(1.0, value - zero)))
|
||||
return out
|
||||
|
||||
|
||||
def from_pad(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
|
||||
"""Map one gamepad reading to the normalised six-vector.
|
||||
|
||||
|
||||
+30
-18
@@ -35,7 +35,7 @@ from dora import Node
|
||||
|
||||
from dora_teleop import joy, mapping
|
||||
from dora_teleop.config import Config
|
||||
from dora_teleop.mapping import LAYOUT
|
||||
from dora_teleop.mapping import LAYOUT, axis_names
|
||||
|
||||
# What each velocity component is, in the order the arm takes them.
|
||||
COMPONENTS = (
|
||||
@@ -50,18 +50,6 @@ COMPONENTS = (
|
||||
CLEAR = "\033[H\033[J"
|
||||
|
||||
|
||||
def axis_names(layout=LAYOUT) -> dict[int, str]:
|
||||
"""Name each axis index the layout uses."""
|
||||
return {
|
||||
layout.left_x: "left stick X",
|
||||
layout.left_y: "left stick Y",
|
||||
layout.left_trigger: "left trigger",
|
||||
layout.right_x: "right stick X",
|
||||
layout.right_y: "right stick Y",
|
||||
layout.right_trigger: "right trigger",
|
||||
}
|
||||
|
||||
|
||||
def button_names(layout=LAYOUT) -> dict[int, str]:
|
||||
"""Name each button index the layout uses."""
|
||||
return {
|
||||
@@ -89,8 +77,15 @@ def bar(value: float, width: int = 21) -> str:
|
||||
return "".join(cells)
|
||||
|
||||
|
||||
def render(sample, velocity: list[float], config: Config) -> str:
|
||||
"""Build the whole display for one reading."""
|
||||
def render(sample, driven, velocity: list[float], config: Config, mismatch: str | None = None) -> str:
|
||||
"""Build the whole display for one reading.
|
||||
|
||||
``sample`` is the pad exactly as it was reported, because seeing the raw
|
||||
numbers is the point of this tool. ``driven`` is the same reading after
|
||||
calibration — what the node actually decides from — and ``velocity`` is what
|
||||
it would send. A pad this node refuses reaches here as an absent ``driven``
|
||||
and a ``mismatch`` saying why.
|
||||
"""
|
||||
lines = ["dora-teleop probe — Ctrl-C to stop", ""]
|
||||
|
||||
if not sample.present:
|
||||
@@ -102,6 +97,18 @@ def render(sample, velocity: list[float], config: Config) -> str:
|
||||
]
|
||||
return "\n".join(lines)
|
||||
|
||||
if mismatch:
|
||||
lines += [
|
||||
" this pad does not match the layout, and the arm will not be driven from it.",
|
||||
"",
|
||||
f" {mismatch}",
|
||||
"",
|
||||
" Either something was being held when it was plugged in, or the indices",
|
||||
" below are numbered differently on this pad. The axes are raw, so they",
|
||||
" say which: fix `Layout` in dora_teleop/mapping.py to match them.",
|
||||
"",
|
||||
]
|
||||
|
||||
names = axis_names()
|
||||
lines.append(f" axes ({len(sample.axes)} reported)")
|
||||
for index, value in enumerate(sample.axes):
|
||||
@@ -121,7 +128,7 @@ def render(sample, velocity: list[float], config: Config) -> str:
|
||||
lines += [
|
||||
"",
|
||||
f" moving: {moving}",
|
||||
f" asking for the arm: {'yes' if mapping.is_active(sample) else 'no'}",
|
||||
f" asking for the arm: {'yes' if mapping.is_active(driven) else 'no'}",
|
||||
]
|
||||
return "\n".join(lines)
|
||||
|
||||
@@ -137,14 +144,19 @@ def main() -> None:
|
||||
args = parser.parse_args()
|
||||
|
||||
config = Config.from_env()
|
||||
calibration = joy.Calibration()
|
||||
node = Node(args.name)
|
||||
|
||||
for event in node:
|
||||
if event["type"] != "INPUT" or event["id"] != "joy":
|
||||
continue
|
||||
sample = joy.decode(event)
|
||||
velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6
|
||||
print(CLEAR + render(sample, velocity, config), flush=True)
|
||||
# Through the same calibration the node drives from, so what this prints
|
||||
# stays what the arm would get. What is displayed is still the raw
|
||||
# reading: the indices are what a pad is checked against.
|
||||
driven = calibration.apply(sample)
|
||||
velocity = mapping.velocity(driven.axes, driven.buttons, config) if driven.present else [0.0] * 6
|
||||
print(CLEAR + render(sample, driven, velocity, config, calibration.mismatch), flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
Reference in New Issue
Block a user