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