diff --git a/AGENTS.md b/AGENTS.md index 3b81bad..1744c0a 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -38,7 +38,7 @@ expected next to this one. ## Test ```bash -uv run pytest # 66 tests, no gamepad, arm, browser or daemon needed +uv run pytest # 76 tests, no gamepad, arm, browser or daemon needed uv run ruff check . uv run ruff format --check . ``` @@ -77,6 +77,12 @@ a source goes away**. first time somebody picks up the pad. Errors name the variable. - **`dora_teleop/log.py`** — Timestamped stderr logging. stderr because dora reads a dynamic node's stdout. +- **`dora_teleop/probe.py`** — `dora-teleop-probe`: shows the raw axes, the buttons and the + velocity that would be sent, with no dataflow and no arm. It exists because the indices + in `mapping.py` are SDL's documented layout rather than an observed one, and a pad that + disagrees is otherwise found by watching a real arm move the wrong way. It goes through + the same `Gamepad` and `velocity()` the node runs, so what it prints is what the arm + would get — keep it that way, or it stops being evidence. ## Rules that are not negotiable diff --git a/README.md b/README.md index e05b9bd..45a2d8c 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,46 @@ somebody can read. `home` is deliberately **not** on the pad. It is a large unattended motion and belongs behind a deliberate click, not a thumb resting on a face button. +### Checking your pad + +```bash +uv run dora-teleop-probe +``` + +Prints the raw axes, the buttons it sees, and the velocity this node would send — live, +with no dataflow and no arm. **It cannot move anything.** There is no dora node and no +connection to the robot; it only reads. + +```text + axes (6 reported) + 0 left stick X +0.600 ----------|-----#---- + 1 left stick Y -0.800 --#-------|---------- + 2 left trigger -1.000 #---------|---------- + ... + buttons (6 reported) + 5:yaw right + + velocity this would send to the arm + vx +38.89 mm/s forward / back <-- + vy +23.53 mm/s left / right <-- + vz +20.39 mm/s up / down <-- + wz +30.00 deg/s yaw <-- + + moving: yes +``` + +Run this before trusting the table above. The indices are SDL's documented layout, not +observed ones, and a pad that numbers its axes differently is much better found here than +by watching an arm move the wrong way. Two things to look for: + +- **Every axis is listed, including ones the layout does not use** — an axis marked + `unused` that moves when you push a stick means the layout needs changing. +- **A bar that does not sit centred with the stick released** is a worn pad announcing + itself, and a reason to raise `DEADZONE`. + +It goes through the same `Gamepad` and `velocity()` the node runs, so what it prints is +what the arm would be sent. + ## The lease Two sources, one arm. Whoever holds the lease drives; the other is ignored. @@ -143,7 +183,7 @@ gamepad if you want one — the node starts without and says so. ```bash uv sync --group dev -uv run pytest # 66 tests, no gamepad, arm, browser or daemon needed +uv run pytest # 76 tests, no gamepad, arm, browser or daemon needed uv run ruff check . uv run ruff format --check . ``` diff --git a/dora_teleop/probe.py b/dora_teleop/probe.py new file mode 100644 index 0000000..a99d96f --- /dev/null +++ b/dora_teleop/probe.py @@ -0,0 +1,132 @@ +"""Show what the pad is doing, with no dataflow and no arm. + +`dora-teleop-probe` opens the gamepad and prints, live: the raw axes SDL +reports, the buttons it sees pressed, and the velocity this node would send. + +It exists because the axis and button indices in :mod:`dora_teleop.mapping` are +SDL's documented layout rather than an observed one, and a pad that disagrees is +otherwise discovered by watching a real arm move the wrong way. It goes through +:class:`~dora_teleop.gamepad.Gamepad` and :func:`~dora_teleop.mapping.velocity` +— the same code the node runs — so what it prints is what the arm would get. + +Nothing here can move anything. There is no dora node and no connection to the +arm; this only reads. +""" + +import argparse +import time + +from dora_teleop import mapping +from dora_teleop.config import Config +from dora_teleop.gamepad import Gamepad +from dora_teleop.mapping import LAYOUT + +# What each velocity component is, in the order the arm takes them. +COMPONENTS = ( + ("vx", "mm/s", "forward / back"), + ("vy", "mm/s", "left / right"), + ("vz", "mm/s", "up / down"), + ("wx", "deg/s", "roll"), + ("wy", "deg/s", "pitch"), + ("wz", "deg/s", "yaw"), +) + +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 { + layout.grasp: "grasp", + layout.release: "release", + layout.clear: "clear", + layout.freedrive: "freedrive", + layout.yaw_left: "yaw left", + layout.yaw_right: "yaw right", + layout.estop: "E-STOP", + } + + +def bar(value: float, width: int = 21) -> str: + """Draw a centred bar for a value between -1 and 1. + + Seeing the bar sit off-centre with the stick released is how a worn pad + announces itself, which is most of the point of looking at raw numbers. + """ + middle = width // 2 + position = middle + round(max(-1.0, min(1.0, value)) * middle) + cells = ["-"] * width + cells[middle] = "|" + cells[position] = "#" + return "".join(cells) + + +def render(sample, velocity: list[float], config: Config) -> str: + """Build the whole display for one reading.""" + lines = ["dora-teleop probe — Ctrl-C to stop", ""] + + if not sample.present: + lines += [ + " no gamepad found.", + "", + f" looking for joystick index {config.joystick_index};", + " plug one in and this will pick it up.", + ] + return "\n".join(lines) + + names = axis_names() + lines.append(f" axes ({len(sample.axes)} reported)") + for index, value in enumerate(sample.axes): + name = names.get(index, "unused") + lines.append(f" {index} {name:<15} {value:+.3f} {bar(value)}") + + labels = button_names() + held = [f"{index}:{labels.get(index, 'unused')}" for index, down in enumerate(sample.buttons) if down] + lines += ["", f" buttons ({len(sample.buttons)} reported)", f" {', '.join(held) or 'none pressed'}"] + + lines += ["", " velocity this would send to the arm"] + for (name, units, meaning), value in zip(COMPONENTS, velocity, strict=True): + marker = " <--" if value else "" + lines.append(f" {name} {value:+8.2f} {units:<6} {meaning:<14}{marker}".rstrip()) + + moving = "yes" if mapping.is_moving(velocity) else "no — the arm would be told to hold still" + lines += ["", f" moving: {moving}"] + return "\n".join(lines) + + +def main() -> None: + """Open the pad and print what it is doing until interrupted.""" + parser = argparse.ArgumentParser(description="Show what the gamepad is doing. Moves nothing.") + parser.add_argument("--hz", type=float, default=10.0, help="Redraw rate. Defaults to 10.") + args = parser.parse_args() + + config = Config.from_env() + gamepad = Gamepad(config.joystick_index) + gamepad.start() + + try: + while True: + sample = gamepad.read() + velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6 + print(CLEAR + render(sample, velocity, config), flush=True) + time.sleep(1.0 / args.hz) + except KeyboardInterrupt: + print() + finally: + gamepad.shutdown() + + +if __name__ == "__main__": + main() diff --git a/pyproject.toml b/pyproject.toml index c26a890..ac8c21d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -23,6 +23,7 @@ dev = ["pytest >= 8.1", "ruff >= 0.9"] [project.scripts] dora-teleop = "dora_teleop.main:main" +dora-teleop-probe = "dora_teleop.probe:main" [build-system] requires = ["hatchling"] diff --git a/tests/test_probe.py b/tests/test_probe.py new file mode 100644 index 0000000..279ad01 --- /dev/null +++ b/tests/test_probe.py @@ -0,0 +1,81 @@ +"""The probe: what it draws, without a pad or a terminal.""" + +from dora_teleop import mapping, probe +from dora_teleop.gamepad import Sample +from tests.conftest import stick + + +def test_a_centred_stick_puts_the_marker_in_the_middle(): + drawn = probe.bar(0.0) + + assert drawn[len(drawn) // 2] == "#" + + +def test_the_bar_leans_the_way_the_stick_is_pushed(): + left = probe.bar(-1.0) + right = probe.bar(1.0) + + assert left.index("#") < left.index("|") + assert right.index("#") > right.index("|") + + +def test_a_reading_beyond_full_deflection_stays_on_the_bar(): + # SDL has been known to report slightly outside -1..1 on a worn stick. + assert "#" in probe.bar(-4.0) + assert "#" in probe.bar(4.0) + + +def test_no_gamepad_says_so_and_names_the_index_it_looked_for(config): + config.joystick_index = 2 + + drawn = probe.render(Sample(present=False), [0.0] * 6, config) + + assert "no gamepad found" in drawn + assert "index 2" in drawn + + +def test_every_axis_the_pad_reports_is_shown_even_the_unused_ones(config): + # A pad whose sticks land on indices we do not use is exactly what this is + # for, so nothing may be hidden. + sample = Sample(axes=[0.0] * 8, buttons=[], present=True) + + drawn = probe.render(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]) + + drawn = probe.render(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) + + 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) + + drawn = probe.render(stick(axes=[0.0, -1.0]), velocity, config) + + assert "mm/s" in drawn + assert "deg/s" in drawn + assert f"{config.speed_mm_s:+8.2f}" in drawn + + +def test_a_still_arm_is_reported_as_still(config): + drawn = probe.render(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) + + assert "moving: yes" in drawn