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>

dora_teleop/mapping.py

@@ -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.