diff --git a/dora_teleop/joy.py b/dora_teleop/joy.py index ac4e506..62a04e0 100644 --- a/dora_teleop/joy.py +++ b/dora_teleop/joy.py @@ -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: diff --git a/dora_teleop/mapping.py b/dora_teleop/mapping.py index f54d3dc..62c2217 100644 --- a/dora_teleop/mapping.py +++ b/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. diff --git a/dora_teleop/probe.py b/dora_teleop/probe.py index da9a1af..c7029b1 100644 --- a/dora_teleop/probe.py +++ b/dora_teleop/probe.py @@ -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__": diff --git a/tests/conftest.py b/tests/conftest.py index ab074b3..5eab49e 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -7,6 +7,7 @@ import pytest from dora_teleop.config import Config from dora_teleop.joy import Sample +from dora_teleop.mapping import LAYOUT, TRIGGER_REST # The shape `dora-joy` publishes. Declared here rather than inferred for the same # reason it is declared there: pyarrow reads an empty `axes` list as `list`, @@ -66,13 +67,58 @@ def stick(axes=None, buttons=None, present=True): values = list(axes or []) values += [0.0] * (6 - len(values)) # A trigger at rest reads -1, and a test that forgot to say so would look - # like both triggers held down. - for index in (2, 5): + # like both triggers held down. Which indices those are comes from the + # layout, so a test never has to be edited when a pad renumbers them. + for index in (LAYOUT.left_trigger, LAYOUT.right_trigger): if axes is None or index >= len(axes): values[index] = -1.0 return Sample(axes=values, buttons=list(buttons or []), present=present) +def axes_for( + left_x=0.0, + left_y=0.0, + right_x=0.0, + right_y=0.0, + left_trigger=TRIGGER_REST, + right_trigger=TRIGGER_REST, +): + """Lay out a pad's axes by what each control is rather than by its index. + + A test that numbers its axes says nothing about which control it meant, and + goes on passing while meaning something else once a pad renumbers them. + """ + values = [0.0] * 6 + values[LAYOUT.left_x] = left_x + values[LAYOUT.left_y] = left_y + values[LAYOUT.right_x] = right_x + values[LAYOUT.right_y] = right_y + values[LAYOUT.left_trigger] = left_trigger + values[LAYOUT.right_trigger] = right_trigger + return values + + +def buttons_for( + grasp=False, release=False, clear=False, freedrive=False, yaw_left=False, yaw_right=False, estop=False +): + """Lay out a pad's buttons by what each one does rather than by its index. + + The same reason as :func:`axes_for`, and more pressing: an unpressed button + reads identically on every pad, so a test that numbers them is the only + place a wrong index could ever have been caught. + """ + wanted = { + LAYOUT.grasp: grasp, + LAYOUT.release: release, + LAYOUT.clear: clear, + LAYOUT.freedrive: freedrive, + LAYOUT.yaw_left: yaw_left, + LAYOUT.yaw_right: yaw_right, + LAYOUT.estop: estop, + } + return [wanted.get(index, False) for index in range(max(wanted) + 1)] + + def joy_message(sample): """Build the Arrow row `dora-joy` would publish for one reading.""" return pa.array( diff --git a/tests/test_deadman.py b/tests/test_deadman.py index 2a54e04..c01ba99 100644 --- a/tests/test_deadman.py +++ b/tests/test_deadman.py @@ -6,7 +6,7 @@ from dora_teleop import main as loop from dora_teleop.arbiter import GAMEPAD, WEB from dora_teleop.config import Config from dora_teleop.joy import Sample -from tests.conftest import RecordingNode, input_event, joy_message, stick +from tests.conftest import RecordingNode, buttons_for, input_event, joy_message, stick STOP = [0.0] * 6 PUSHED = [0.0, -1.0] @@ -47,7 +47,14 @@ def ticks(node, state, count=1): def drive(node, state, sample, count=1): - """Hold one reading for `count` ticks, the way a real pad and joy node would.""" + """Hold one reading for `count` ticks, the way a real pad and joy node would. + + A pad this node has not yet seen at rest is sent one resting reading first, + because a real one is always at rest before a thumb reaches it — and the + node refuses to drive an arm from a pad it has never seen still. + """ + if not state.joy.calibration.known: + send_joy(node, state, stick()) for _ in range(count): send_joy(node, state, sample) loop._tick(node, state) @@ -271,7 +278,7 @@ def test_a_latched_estop_keeps_the_arm_still_under_a_pushed_stick(clock): drive(node, state, stick(axes=PUSHED)) node.clear() - drive(node, state, stick(axes=PUSHED, buttons=[False] * 6 + [True])) + drive(node, state, stick(axes=PUSHED, buttons=buttons_for(estop=True))) node.clear() jogs = drive(node, state, stick(axes=PUSHED), count=4) @@ -282,11 +289,11 @@ def test_the_pad_can_release_the_latch_it_set(clock): # A pad-only dataflow has no console RESET, so a latch nothing on the pad # can release would strand the arm until the process was restarted. node, state = build() - drive(node, state, stick(buttons=[False] * 6 + [True])) + drive(node, state, stick(buttons=buttons_for(estop=True))) assert state.view.estop is True node.clear() - drive(node, state, stick(buttons=[False, False, True])) + drive(node, state, stick(buttons=buttons_for(clear=True))) assert state.view.estop is False assert "clear" in node.services() @@ -296,20 +303,20 @@ def test_the_latch_is_released_without_holding_the_arm(clock): # An idle pad hands the lease back two seconds after the stop it just made, # so gating the release on the lease would be a trap. node, state = build() - drive(node, state, stick(buttons=[False] * 6 + [True])) + drive(node, state, stick(buttons=buttons_for(estop=True))) state.arbiter.release(GAMEPAD) state.arbiter.take(WEB, clock.t) node.clear() - drive(node, state, stick(buttons=[False, False, True])) + drive(node, state, stick(buttons=buttons_for(clear=True))) assert state.view.estop is False def test_the_arm_drives_again_once_the_latch_is_released(clock): node, state = build() - drive(node, state, stick(buttons=[False] * 6 + [True])) - drive(node, state, stick(buttons=[False, False, True])) + drive(node, state, stick(buttons=buttons_for(estop=True))) + drive(node, state, stick(buttons=buttons_for(clear=True))) node.clear() jogs = drive(node, state, stick(axes=PUSHED)) @@ -322,7 +329,7 @@ def test_a_latched_estop_stops_the_browser_too(clock): # through a stop somebody made on the gamepad. node, state = build() state.clients = 1 - drive(node, state, stick(buttons=[False] * 6 + [True])) + drive(node, state, stick(buttons=buttons_for(estop=True))) state.arbiter.release(GAMEPAD) state.arbiter.take(WEB, clock.t) node.clear() @@ -337,7 +344,7 @@ def test_the_estop_button_works_without_the_pad_holding_the_arm(clock): node, state = build() state.arbiter.take(WEB, clock.t) - drive(node, state, stick(buttons=[False] * 6 + [True])) + drive(node, state, stick(buttons=buttons_for(estop=True))) assert "stop" in node.services() @@ -402,7 +409,7 @@ def test_a_button_asks_in_verbs_as_well_as_in_services(clock): # what dora-ulite6 reads until it does. node, state = build() - drive(node, state, stick(buttons=[True])) + drive(node, state, stick(buttons=buttons_for(grasp=True))) assert node.actions() == ["gripper_close"] assert "grasp" in node.services() @@ -410,10 +417,10 @@ def test_a_button_asks_in_verbs_as_well_as_in_services(clock): def test_every_verb_the_pad_can_ask_for_is_robot_agnostic(clock): node, state = build() - drive(node, state, stick(buttons=[False, True])) - drive(node, state, stick(buttons=[False] * 3 + [True])) - drive(node, state, stick(buttons=[False] * 6 + [True])) - drive(node, state, stick(buttons=[False, False, True])) + drive(node, state, stick(buttons=buttons_for(release=True))) + drive(node, state, stick(buttons=buttons_for(freedrive=True))) + drive(node, state, stick(buttons=buttons_for(estop=True))) + drive(node, state, stick(buttons=buttons_for(clear=True))) assert node.actions() == ["gripper_open", "freedrive", "stop", "clear"] @@ -426,6 +433,6 @@ def test_the_first_press_after_the_pad_has_been_idle_is_not_dropped(clock): node, state = build() assert state.arbiter.owner is None - drive(node, state, stick(buttons=[True]), count=3) + drive(node, state, stick(buttons=buttons_for(grasp=True)), count=3) assert node.actions() == ["gripper_close"] diff --git a/tests/test_joy.py b/tests/test_joy.py index 0cd0571..24a91e7 100644 --- a/tests/test_joy.py +++ b/tests/test_joy.py @@ -1,9 +1,12 @@ """The pad as it arrives on the wire, and what silence on that wire means.""" import pyarrow as pa +import pytest +from dora_teleop import mapping from dora_teleop.joy import ABSENT, JoyInput, Sample, decode -from tests.conftest import input_event, joy_message, stick +from dora_teleop.mapping import LAYOUT +from tests.conftest import axes_for, input_event, joy_message, stick def joy_event(sample): @@ -41,21 +44,107 @@ def test_a_message_with_no_rows_reads_as_absent(): def test_a_fresh_reading_is_handed_back(): joy = JoyInput(ttl_s=0.2) - sample = stick(axes=[0.5]) - joy.accept(sample, 100.0) + joy.accept(stick(), 100.0) + joy.accept(stick(axes=[0.5]), 100.0) - assert joy.read(100.1) is sample + assert joy.read(100.1).axes[0] == 0.5 def test_a_reading_nobody_is_refreshing_reads_as_absent(): # dora-joy publishes every tick, so silence means it died — and a dead pad # node is indistinguishable from an unplugged pad as far as the arm cares. joy = JoyInput(ttl_s=0.2) + joy.accept(stick(), 100.0) joy.accept(stick(axes=[0.5]), 100.0) assert joy.read(100.5) is ABSENT +# --- the pad this node is actually holding --- + + +def test_a_pad_is_measured_where_it_rests_and_the_tilt_is_subtracted(): + # A stick that no longer centres is the ordinary kind of worn pad. Its rest + # becomes zero, so it asks for nothing until it is actually pushed. + joy = JoyInput(ttl_s=0.2) + tilted = axes_for(left_x=0.2, left_y=-0.15) + joy.accept(stick(axes=tilted), 100.0) + + reading = joy.read(100.1) + + assert reading.present is True + assert reading.axes[LAYOUT.left_x] == pytest.approx(0.0) + assert reading.axes[LAYOUT.left_y] == pytest.approx(0.0) + assert mapping.is_active(reading) is False + + +def test_the_tilt_is_subtracted_rather_than_deadzoned_so_full_travel_remains(): + joy = JoyInput(ttl_s=0.2) + joy.accept(stick(axes=axes_for(left_x=0.2)), 100.0) + joy.accept(stick(axes=axes_for(left_x=1.0)), 100.0) + + assert joy.read(100.1).axes[LAYOUT.left_x] == pytest.approx(0.8) + + +def test_a_trigger_that_rests_at_zero_does_not_read_as_half_pressed(): + # Some pads report a trigger as 0 until it first moves. Read as SDL's -1..1 + # that is half travel, which would lift the arm on its own. + joy = JoyInput(ttl_s=0.2) + joy.accept(stick(axes=axes_for(right_trigger=0.0)), 100.0) + + reading = joy.read(100.1) + + assert mapping.trigger(reading.axes[LAYOUT.right_trigger]) == pytest.approx(0.0) + assert mapping.is_active(reading) is False + + +def test_a_pad_that_does_not_fit_the_layout_reads_as_absent(): + # The bug this exists for: a trigger on an index the layout calls a stick + # rests at -1.0, which is full deflection — an untouched pad on the desk + # asking for full speed, and holding the lease so nothing can take it back. + joy = JoyInput(ttl_s=0.2) + wrong = list(axes_for()) + wrong[LAYOUT.right_y] = -1.0 + joy.accept(stick(axes=wrong), 100.0) + + assert joy.read(100.1) is ABSENT + assert "right stick Y" in joy.calibration.mismatch + + +def test_a_pad_held_when_it_appears_is_refused_until_it_is_let_go(): + # A stick held at startup and a wrongly numbered axis look identical in one + # reading, so the safe answer is the same for both: no arm until it rests. + joy = JoyInput(ttl_s=0.2) + joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.0) + + assert joy.read(100.1) is ABSENT + + joy.accept(stick(), 100.2) + + assert joy.read(100.3).present is True + + +def test_rest_is_not_re_measured_while_the_pad_is_in_use(): + # Re-measuring under a held stick would record that deflection as centre, + # and the arm would then move the moment the operator let go. + joy = JoyInput(ttl_s=0.2) + joy.accept(stick(), 100.0) + joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.1) + joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.2) + + assert joy.read(100.3).axes[LAYOUT.left_y] == pytest.approx(-1.0) + + +def test_a_pad_that_goes_away_is_measured_again_when_it_comes_back(): + # The second pad is not the first one, and may not rest where it did. + joy = JoyInput(ttl_s=0.2) + joy.accept(stick(axes=axes_for(left_x=0.2)), 100.0) + joy.accept(Sample(present=False), 100.1) + joy.accept(stick(axes=axes_for(left_x=-0.2)), 100.2) + + assert joy.read(100.3).axes[LAYOUT.left_x] == pytest.approx(0.0) + + def test_nothing_ever_arriving_reads_as_absent(): # A dataflow with no `joy` wired at all, which is a browser-only console. assert JoyInput(ttl_s=0.2).read(100.0) is ABSENT diff --git a/tests/test_mapping.py b/tests/test_mapping.py index 66c5347..dee3ad6 100644 --- a/tests/test_mapping.py +++ b/tests/test_mapping.py @@ -3,8 +3,8 @@ import pytest from dora_teleop import mapping -from dora_teleop.mapping import VX, VY, VZ, WX, WY, WZ -from tests.conftest import stick +from dora_teleop.mapping import LAYOUT, VX, VY, VZ, WX, WY, WZ +from tests.conftest import axes_for, buttons_for, stick def test_a_stick_at_rest_asks_for_nothing(config): @@ -29,15 +29,15 @@ def test_pushing_the_stick_right_moves_the_arm_along_positive_y(config): 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) + up = mapping.velocity(axes_for(right_trigger=1.0), [], config) + down = mapping.velocity(axes_for(left_trigger=1.0), [], 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) + velocity = mapping.velocity(axes_for(left_trigger=1.0, right_trigger=1.0), [], config) assert velocity[VZ] == 0.0 @@ -51,15 +51,15 @@ def test_an_untouched_trigger_does_not_read_as_half_pressed(config): 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) + velocity = mapping.velocity(axes_for(right_x=1.0, right_y=-1.0), [], 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) + right = mapping.velocity(stick().axes, buttons_for(yaw_right=True), config) + left = mapping.velocity(stick().axes, buttons_for(yaw_left=True), config) assert right[WZ] == pytest.approx(config.speed_deg_s) assert left[WZ] == pytest.approx(-config.speed_deg_s) @@ -68,7 +68,7 @@ def test_the_bumpers_yaw_in_opposite_directions(config): 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) + velocity = mapping.velocity(axes_for(left_x=1.0, left_y=-1.0, right_trigger=1.0), [], config) assert velocity[VX] > 0 assert velocity[VY] > 0 @@ -129,14 +129,90 @@ def test_a_nudged_stick_is_asking_for_the_arm(): def test_a_held_trigger_is_asking_for_the_arm(): - assert mapping.is_active(stick(axes=[0.0, 0.0, 1.0])) is True + assert mapping.is_active(stick(axes=axes_for(left_trigger=1.0))) is True def test_a_held_button_is_asking_for_the_arm(): - assert mapping.is_active(stick(buttons=[True])) is True + assert mapping.is_active(stick(buttons=buttons_for(grasp=True))) is True def test_an_axis_the_layout_does_not_use_still_counts(): # A pad reporting its d-pad as two extra axes is asking for the arm when # somebody presses it, whether or not this node maps it to anything. - assert mapping.is_active(stick(axes=[0.0, 0.0, -1.0, 0.0, 0.0, -1.0, 1.0])) is True + assert mapping.is_active(stick(axes=axes_for() + [1.0])) is True + + +# --- reading a pad that is not the layout's ideal one --- + + +def test_a_resting_pad_measures_as_resting(): + zeros = mapping.rest_zeros(axes_for()) + + assert zeros == axes_for() + + +def test_a_small_tilt_is_accepted_as_rest(): + # A worn stick is an ordinary pad, not a broken one. + assert mapping.rest_zeros(axes_for(left_x=0.2, right_y=-0.2)) + + +def test_an_axis_resting_where_the_layout_says_it_cannot_is_refused(): + wrong = axes_for() + wrong[LAYOUT.right_y] = -1.0 + + with pytest.raises(ValueError, match="right stick Y"): + mapping.rest_zeros(wrong) + + +def test_the_refusal_names_the_index_and_what_it_read(): + # It is read off a terminal by somebody deciding what to put in `Layout`. + wrong = axes_for() + wrong[LAYOUT.right_x] = 0.9 + + with pytest.raises(ValueError, match=r"axis 2 rests at \+0.900"): + mapping.rest_zeros(wrong) + + +def test_a_trigger_resting_at_either_end_is_accepted(): + # -1 is what SDL reports; 0 is what some pads report until it first moves. + assert mapping.rest_zeros(axes_for(left_trigger=-1.0, right_trigger=0.0)) + + +def test_an_axis_the_layout_does_not_use_still_has_to_rest(): + # A d-pad reported as two extra axes counts towards `is_active`, so one + # stuck at an extreme would hold the arm as surely as a named one. + with pytest.raises(ValueError, match="axis 6"): + mapping.rest_zeros(axes_for() + [1.0]) + + +def test_canonical_puts_a_tilted_stick_back_at_centre(): + tilted = axes_for(left_x=0.2) + + canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted)) + + assert canonical[LAYOUT.left_x] == pytest.approx(0.0) + + +def test_canonical_puts_a_trigger_back_at_its_own_rest(): + zeroed = axes_for(left_trigger=0.0, right_trigger=0.0) + + canonical = mapping.canonical(zeroed, mapping.rest_zeros(zeroed)) + + assert canonical[LAYOUT.left_trigger] == pytest.approx(mapping.TRIGGER_REST) + assert canonical[LAYOUT.right_trigger] == pytest.approx(mapping.TRIGGER_REST) + + +def test_a_trigger_measured_from_zero_still_reaches_full_travel(): + zeros = mapping.rest_zeros(axes_for(right_trigger=0.0)) + + canonical = mapping.canonical(axes_for(right_trigger=1.0), zeros) + + assert mapping.trigger(canonical[LAYOUT.right_trigger]) == pytest.approx(1.0) + + +def test_a_calibrated_pad_at_rest_asks_for_nothing(config): + tilted = axes_for(left_x=0.2, left_y=-0.15, right_trigger=0.0) + + canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted)) + + assert mapping.velocity(canonical, [], config) == [0.0] * 6 diff --git a/tests/test_probe.py b/tests/test_probe.py index 2dd4e2f..8e1eaab 100644 --- a/tests/test_probe.py +++ b/tests/test_probe.py @@ -2,7 +2,7 @@ from dora_teleop import mapping, probe from dora_teleop.joy import Sample -from tests.conftest import stick +from tests.conftest import buttons_for, stick def test_a_centred_stick_puts_the_marker_in_the_middle(): @@ -28,7 +28,7 @@ def test_a_reading_beyond_full_deflection_stays_on_the_bar(): def test_no_gamepad_says_so_and_says_the_arm_is_being_held_still(config): # Absence covers both "no pad" and "dora-joy stopped sending", and the probe # must not imply the arm is waiting on something it is not. - drawn = probe.render(Sample(present=False), [0.0] * 6, config) + drawn = probe.render(Sample(present=False), Sample(present=False), [0.0] * 6, config) assert "no gamepad" in drawn assert "hold still" in drawn @@ -39,30 +39,31 @@ def test_every_axis_the_pad_reports_is_shown_even_the_unused_ones(config): # for, so nothing may be hidden. sample = Sample(axes=[0.0] * 8, buttons=[], present=True) - drawn = probe.render(sample, [0.0] * 6, config) + drawn = probe.render(sample, sample, [0.0] * 6, config) assert "unused" in drawn assert "axes (8 reported)" in drawn def test_a_pressed_button_is_named(config): - sample = stick(buttons=[True]) + sample = stick(buttons=buttons_for(grasp=True)) - drawn = probe.render(sample, [0.0] * 6, config) + drawn = probe.render(sample, sample, [0.0] * 6, config) assert "grasp" in drawn def test_nothing_pressed_says_so_rather_than_showing_an_empty_line(config): - drawn = probe.render(stick(), [0.0] * 6, config) + drawn = probe.render(stick(), stick(), [0.0] * 6, config) assert "none pressed" in drawn def test_the_velocity_is_shown_in_the_units_the_arm_takes(config): - velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config) + pushed = stick(axes=[0.0, -1.0]) + velocity = mapping.velocity(pushed.axes, [], config) - drawn = probe.render(stick(axes=[0.0, -1.0]), velocity, config) + drawn = probe.render(pushed, pushed, velocity, config) assert "mm/s" in drawn assert "deg/s" in drawn @@ -70,13 +71,14 @@ def test_the_velocity_is_shown_in_the_units_the_arm_takes(config): def test_a_still_arm_is_reported_as_still(config): - drawn = probe.render(stick(), [0.0] * 6, config) + drawn = probe.render(stick(), stick(), [0.0] * 6, config) assert "told to hold still" in drawn def test_a_moving_arm_is_reported_as_moving(config): - drawn = probe.render(stick(axes=[0.0, -1.0]), [10.0, 0, 0, 0, 0, 0], config) + pushed = stick(axes=[0.0, -1.0]) + drawn = probe.render(pushed, pushed, [10.0, 0, 0, 0, 0, 0], config) assert "moving: yes" in drawn @@ -84,5 +86,30 @@ def test_a_moving_arm_is_reported_as_moving(config): def test_whether_the_pad_is_asking_for_the_arm_is_shown(config): # The probe is where a resting trigger claiming the arm would have been # visible, so it says what `is_active` thinks rather than leaving it implied. - assert "asking for the arm: no" in probe.render(stick(), [0.0] * 6, config) - assert "asking for the arm: yes" in probe.render(stick(axes=[0.0, -1.0]), [0.0] * 6, config) + rest, pushed = stick(), stick(axes=[0.0, -1.0]) + + assert "asking for the arm: no" in probe.render(rest, rest, [0.0] * 6, config) + assert "asking for the arm: yes" in probe.render(pushed, pushed, [0.0] * 6, config) + + +def test_a_pad_that_does_not_fit_the_layout_is_called_out(config): + # This is the tool for exactly that pad, so the reason goes on the screen + # rather than only in the node's log. + wrong = stick(axes=[0.0, 0.0, 0.0, -1.0, -1.0, -1.0]) + mismatch = "axis 3 rests at -1.000, which is not where right stick Y sits" + + drawn = probe.render(wrong, Sample(present=False), [0.0] * 6, config, mismatch) + + assert "does not match the layout" in drawn + assert mismatch in drawn + assert "asking for the arm: no" in drawn + + +def test_the_raw_axes_are_still_shown_for_a_pad_that_does_not_fit(config): + # The numbers are the evidence: they are what `Layout` gets corrected from. + wrong = stick(axes=[0.0, 0.0, 0.0, -1.0, -1.0, -1.0]) + + drawn = probe.render(wrong, Sample(present=False), [0.0] * 6, config, "axis 3") + + assert "-1.000" in drawn + assert "axes (6 reported)" in drawn