add the axis mapping

Pure functions, no state and no hardware, because this is the part that has to
be right and the cheapest to check.

Analog and blended: how fast the arm moves depends on how far the stick is
pushed, and several axes move at once. That is the whole reason a pad beats the
web console's four-sector dial pad, which can only ask for one axis at a time at
one fixed speed.

The deadzone rescales what is left of the travel rather than just clipping it,
so the arm starts from zero just outside the band instead of jumping to a
fraction of full speed the instant the stick leaves it. Expo bends the response
because fine positioning happens near centre, where a linear stick gives almost
no resolution.

Up on the stick sends the arm along +X, matching the console's dial, so the two
agree about which way is which.

dora_teleop/mapping.py

@@ -0,0 +1,123 @@
+"""Turning stick deflection into a velocity the arm understands.
+
+Pure functions, no state and no hardware, because this is the part that has to
+be right and the part that is cheapest to check. Everything here is in the
+arm node's units: `[vx, vy, vz, wx, wy, wz]`, mm/s and deg/s, in the robot's
+**base** frame — the same frame `tcp_pose` reports.
+
+The screen convention comes from the old web console, so the two agree: stick
+up is +X, stick right is +Y.
+"""
+
+from dataclasses import dataclass
+
+# The arm takes six numbers; these name them.
+VX, VY, VZ, WX, WY, WZ = range(6)
+
+
+@dataclass(frozen=True)
+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.
+    """
+
+    left_x: int = 0
+    left_y: int = 1
+    left_trigger: int = 2
+    right_x: int = 3
+    right_y: 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
+
+
+LAYOUT = Layout()
+
+
+def deadzone(value: float, width: float) -> float:
+    """Ignore travel near centre, and rescale what is left to the full range.
+
+    Rescaling matters: without it the arm jumps to `width` of full speed the
+    instant the stick leaves the dead band, which feels like a broken control
+    rather than a gentle one.
+    """
+    if width <= 0.0:
+        return value
+    size = abs(value)
+    if size < width:
+        return 0.0
+    scaled = (size - width) / (1.0 - width)
+    return scaled if value > 0 else -scaled
+
+
+def expo(value: float, factor: float) -> float:
+    """Bend the response so small deflections move the arm slowly.
+
+    Fine positioning happens near centre, and a linear stick gives it almost no
+    resolution there. `factor` is how much of the curve is cubic: 0 is linear.
+    """
+    return (1.0 - factor) * value + factor * value**3
+
+
+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.
+    """
+    return max(0.0, min(1.0, (value + 1.0) / 2.0))
+
+
+def axis(values: list[float], index: int) -> float:
+    """Read one axis, treating a pad that does not report it as centred."""
+    return values[index] if index < len(values) else 0.0
+
+
+def pressed(buttons: list[bool], index: int) -> bool:
+    """Read one button, treating a pad that does not report it as unpressed."""
+    return bool(buttons[index]) if index < len(buttons) else False
+
+
+def velocity(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
+    """Map one gamepad reading to a base-frame velocity.
+
+    Translation comes from the left stick and the triggers, rotation from the
+    right stick and the bumpers. Several axes move at once on purpose: a stick
+    is analog and blended, which is exactly what the console's four-sector dial
+    pad could not do.
+    """
+
+    def shaped(value: float) -> float:
+        return expo(deadzone(value, config.deadzone), config.expo)
+
+    out = [0.0] * 6
+    # Up on the stick is negative in SDL, and up should send the arm away from
+    # the operator, along +X.
+    out[VX] = shaped(-axis(axes, layout.left_y)) * config.speed_mm_s
+    out[VY] = shaped(axis(axes, layout.left_x)) * config.speed_mm_s
+
+    lift = trigger(axis(axes, layout.right_trigger)) - trigger(axis(axes, layout.left_trigger))
+    out[VZ] = shaped(lift) * config.speed_mm_s
+
+    out[WX] = shaped(axis(axes, layout.right_x)) * config.speed_deg_s
+    out[WY] = shaped(-axis(axes, layout.right_y)) * config.speed_deg_s
+
+    yaw = pressed(buttons, layout.yaw_right) - pressed(buttons, layout.yaw_left)
+    out[WZ] = yaw * config.speed_deg_s
+    return out
+
+
+def is_moving(velocity_vector: list[float]) -> bool:
+    """Whether this velocity would actually drive the arm."""
+    return any(velocity_vector)

tests/test_mapping.py

@@ -0,0 +1,110 @@
+"""Stick deflection to base-frame velocity: units, signs and the shaping curve."""
+
+import pytest
+
+from dora_teleop import mapping
+from dora_teleop.mapping import VX, VY, VZ, WX, WY, WZ
+from tests.conftest import stick
+
+
+def test_a_stick_at_rest_asks_for_nothing(config):
+    velocity = mapping.velocity(stick().axes, [], config)
+
+    assert velocity == [0.0] * 6
+    assert not mapping.is_moving(velocity)
+
+
+def test_pushing_the_stick_up_moves_the_arm_along_positive_x(config):
+    # Up is negative in SDL, and up should send the arm away from the operator.
+    velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config)
+
+    assert velocity[VX] == pytest.approx(config.speed_mm_s)
+    assert velocity[VY] == 0.0
+
+
+def test_pushing_the_stick_right_moves_the_arm_along_positive_y(config):
+    velocity = mapping.velocity(stick(axes=[1.0, 0.0]).axes, [], config)
+
+    assert velocity[VY] == pytest.approx(config.speed_mm_s)
+
+
+def test_the_right_trigger_lifts_and_the_left_one_lowers(config):
+    up = mapping.velocity(stick(axes=[0, 0, -1.0, 0, 0, 1.0]).axes, [], config)
+    down = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, -1.0]).axes, [], config)
+
+    assert up[VZ] == pytest.approx(config.speed_mm_s)
+    assert down[VZ] == pytest.approx(-config.speed_mm_s)
+
+
+def test_both_triggers_together_cancel(config):
+    velocity = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, 1.0]).axes, [], config)
+
+    assert velocity[VZ] == 0.0
+
+
+def test_an_untouched_trigger_does_not_read_as_half_pressed(config):
+    # SDL reports a trigger from -1 to 1, and one that has never moved sometimes
+    # reads 0 until it does. Treating that as half travel would lift the arm.
+    assert mapping.trigger(-1.0) == 0.0
+    assert mapping.trigger(0.0) == pytest.approx(0.5)
+    assert mapping.trigger(1.0) == 1.0
+
+
+def test_the_right_stick_turns_the_tool_in_degrees_per_second(config):
+    velocity = mapping.velocity(stick(axes=[0, 0, -1, 1.0, -1.0, -1]).axes, [], config)
+
+    assert velocity[WX] == pytest.approx(config.speed_deg_s)
+    assert velocity[WY] == pytest.approx(config.speed_deg_s)
+
+
+def test_the_bumpers_yaw_in_opposite_directions(config):
+    right = mapping.velocity(stick().axes, [False] * 5 + [True], config)
+    left = mapping.velocity(stick().axes, [False] * 4 + [True, False], config)
+
+    assert right[WZ] == pytest.approx(config.speed_deg_s)
+    assert left[WZ] == pytest.approx(-config.speed_deg_s)
+
+
+def test_several_axes_move_at_once(config):
+    # The whole reason a pad beats the console's four-sector dial: the dial can
+    # only ever ask for one axis.
+    velocity = mapping.velocity(stick(axes=[1.0, -1.0, -1, 0, 0, 1.0]).axes, [], config)
+
+    assert velocity[VX] > 0
+    assert velocity[VY] > 0
+    assert velocity[VZ] > 0
+
+
+def test_drift_inside_the_deadzone_is_ignored(config):
+    # A worn stick that does not centre would otherwise drive the arm across the
+    # table with nobody touching it.
+    velocity = mapping.velocity(stick(axes=[config.deadzone / 2, 0.0]).axes, [], config)
+
+    assert velocity == [0.0] * 6
+
+
+def test_motion_starts_from_zero_just_outside_the_deadzone(config):
+    # Without rescaling, the arm would jump to a fraction of full speed the
+    # instant the stick left the dead band.
+    just_outside = mapping.deadzone(config.deadzone + 1e-9, config.deadzone)
+
+    assert just_outside == pytest.approx(0.0, abs=1e-6)
+
+
+def test_the_deadzone_still_reaches_full_speed_at_full_deflection(config):
+    assert mapping.deadzone(1.0, config.deadzone) == pytest.approx(1.0)
+    assert mapping.deadzone(-1.0, config.deadzone) == pytest.approx(-1.0)
+
+
+def test_expo_slows_small_deflections_without_capping_large_ones():
+    assert mapping.expo(0.5, 0.0) == pytest.approx(0.5)
+    assert mapping.expo(0.5, 1.0) == pytest.approx(0.125)
+    assert mapping.expo(1.0, 1.0) == pytest.approx(1.0)
+    assert mapping.expo(-0.5, 1.0) == pytest.approx(-0.125)
+
+
+def test_a_pad_reporting_fewer_axes_is_read_as_centred_not_as_an_error(config):
+    velocity = mapping.velocity([0.0, 0.0], [], config)
+
+    assert velocity[VX] == 0.0
+    assert velocity[WZ] == 0.0