diff --git a/dora_teleop/mapping.py b/dora_teleop/mapping.py new file mode 100644 index 0000000..93969b9 --- /dev/null +++ b/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) diff --git a/tests/test_mapping.py b/tests/test_mapping.py new file mode 100644 index 0000000..86a7274 --- /dev/null +++ b/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