diff --git a/AGENTS.md b/AGENTS.md index c0ca8d0..31d8340 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -46,16 +46,19 @@ uv sync --group dev # plus pytest and ruff ```bash dora run dataflow.yml --uv # this node plus a bench stand-in, no pad -dora run dataflow-gamepad.yml --uv # the same, with dora-joy pulled in by git: +dora run dataflow-robot.yml # the pad and a real arm, both pulled in by git: +dora run dataflow-gamepad.yml # the same, with the probe declared and no console ``` `dataflow.yml` is self-contained on purpose — it resolves from this checkout alone, which -is what lets CI validate it. Keep it that way; the pad belongs in `dataflow-gamepad.yml`. +is what lets CI validate it. Keep it that way; the pad belongs in the other two. Neither +of those takes `--uv`: dora 1.0.1 installs a `git:` node into a venv inside its own clone +and then looks for its entrypoint in the dataflow's venv. README.md has the detail. ## Test ```bash -uv run pytest # 112 tests, no gamepad, arm, browser or daemon needed +uv run pytest # 149 tests, no gamepad, arm, browser or daemon needed uv run ruff check . uv run ruff format --check . ``` @@ -74,6 +77,14 @@ a source goes away**. - **`dora_teleop/joy.py`** — The pad as it arrives from `dora-joy`. `Sample.present` is the load-bearing field: an unplugged pad reads as *absent*, not as centred, so "the operator let go" and "the pad fell off the desk" stay distinguishable. + **`Calibration` is the other half of that seam.** It measures a pad the first time it is + seen at rest and hands the rest of the node a reading as the layout's ideal pad would + have sent it, so a worn stick's tilt is subtracted rather than deadzoned and a trigger + that rests at 0 stops being a special case. A pad whose resting axes do not fit `Layout` + reads as *absent* and is never corrected: one reading cannot tell a trigger at -1.0 from + a stick held hard over, and guessing wrong drives a real arm. It is not re-measured + while the pad is in use — doing that under a held stick would record the deflection as + centre, and the arm would move when the operator let go. **`JoyInput.read` is the other safety-critical function in this repo.** Moving the pad into another process added a way for it to go away that did not exist before — that process dying — and from here that looks like silence rather than like an unplugged pad. @@ -90,7 +101,12 @@ a source goes away**. units: mm/s and deg/s, base frame. `is_active` lives here rather than on `Sample` because it needs the layout: **a trigger rests at -1.0**, so the pad-is-in-use test has to normalise triggers exactly as `velocity` does, or every connected pad reads as in use - for ever and `GAMEPAD_IDLE_RELEASE_S` never fires. + for ever and `GAMEPAD_IDLE_RELEASE_S` never fires. `rest_zeros` and `canonical` are the + same concern one layer out: they check a resting pad against `Layout` and map it onto + the ideal one. They are the only defence against a wrong index, which is not a + theoretical failure — an Xbox One S pad reporting its triggers on 4 and 5 against a + layout expecting them on 2 and 5 read as a full-speed pitch command from an untouched + pad, held the lease for ever, and moved a real arm into a controller error. - **`dora_teleop/arbiter.py`** — The lease. One place that answers "why did it not move", rather than one answer per input path. Ported from the monolithic repo's `RobotService` lease because the console is already built on it — it gates its controls on @@ -170,6 +186,13 @@ belongs here, and a port nothing wires reads as supported while having never run pair; nothing can enforce the second from here. - **An unplugged pad, a dead `dora-joy` or a departed browser stops the arm immediately**, without waiting out any timer beyond the one that detects it. +- **A pad is driven from only after it has been seen at rest**, and a pad whose rest does + not fit `Layout` is not driven from at all. This covers the axes and **cannot cover the + buttons**: an unpressed button reads the same on every pad, so a wrong button index is + silent until somebody presses it and gets another verb. Sweeping a new pad with the + probe is the only check, and it is worth doing before the arm is powered. The cost is that a pad held when it appears + is ignored until it is let go; the alternative is guessing a numbering from one reading, + against a real arm. - **Both control sources go through `mapping`.** The web path used to bypass it and pass a velocity straight through, which is why the console had to speak mm/s. One conversion point or none. diff --git a/README.md b/README.md index 0f829c5..1d82855 100644 --- a/README.md +++ b/README.md @@ -4,430 +4,301 @@ A dora-rs node that hands a robot arm to a human. Pick up a gamepad and move the open a browser and move it from there — one of the two at a time, decided here. It drives [`dora-ulite6`](https://gitea.liria.cl/liria/dora-ulite6) through that node's -ordinary `jog` and `request` ports, reads the pad from -[`dora-joy`](https://gitea.liria.cl/liria/dora-joy), and **serves the browser console -itself** on one port. It contains no robot driver and no gamepad driver: it is the piece -in the middle that decides what the arm should be doing, and that decision is the only -thing it owns. It touches no hardware at all. +`jog` and `request` ports, reads the pad from +[`dora-joy`](https://gitea.liria.cl/liria/dora-joy), and serves the browser console itself +on one port. It contains no robot driver and no gamepad driver: it is the piece in the +middle that decides what the arm should be doing, and it touches no hardware at all. + +**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), Python 3.11+ and `uv`. ## The gamepad -The pad itself is [`dora-joy`](https://gitea.liria.cl/liria/dora-joy), which publishes raw -axes and buttons and knows nothing about arms. Everything below — the deadzone, the -response curve, which button is which, and what any of it means in mm/s — happens here. +| Control | Does | Units | +|-------------|----------------------------------------------|-------| +| Left stick | Move along X (up/down) and Y (left/right) | mm/s | +| Triggers | Lift on the right, lower on the left | mm/s | +| Right stick | Roll (left/right) and pitch (up/down) | deg/s | +| Bumpers | Yaw, left and right | deg/s | +| A | Close the tool | — | +| B | Open the tool | — | +| X | Release the E-STOP and clear the arm's error | — | +| Y | Toggle freedrive | — | +| Back | **E-STOP** — latches until X | — | -Analog and blended — several axes at once, and how fast depends on how far you push. That -is the whole reason a pad beats the console, whose ring sectors ask for one direction at -the speed the dial is set to. The console can still hold two at once — a sector and a Z -pill — but it cannot ask for a third of full speed on one axis and full on another. +Up on the stick sends the arm away from you, along **+X** in the base frame — the same +convention the console uses, so the two agree. -| Control | Does | Units | -|-------------|-------------------------------------------|-------| -| Left stick | Move along X (up/down) and Y (left/right) | mm/s | -| Triggers | Lift on the right, lower on the left | mm/s | -| Right stick | Roll (left/right) and pitch (up/down) | deg/s | -| Bumpers | Yaw, left and right | deg/s | -| A | Close the tool | — | -| B | Open the tool | — | -| X | Release the E-STOP and clear the arm's error | — | -| Y | Toggle freedrive | — | -| Back | **E-STOP** — latches until X | — | - -Up on the stick sends the arm away from you, along **+X** in the robot's base frame — -the same convention the web console's dial uses, so the two agree. - -Indices are SDL's, which is what most Xbox- and PlayStation-style pads report. A pad that -disagrees is a change to `Layout` in `dora_teleop/mapping.py` rather than a new -environment variable: an index nobody can verify from outside is worse than a line of code -somebody can read. The layout lives here rather than in `dora-joy` for the same reason — -`dora-joy` does not know what a button is for, which is what makes it reusable. +**Back latches.** It stops the arm, tells the controller to stop, and keeps the arm still +no matter what the sticks do. **X is the only way out**, and it does both halves: releases +the latch here and clears the error on the controller. Neither button is gated on the +lease — an idle pad hands the arm back two seconds after the stop it just made, and a +latch you must first take the arm back to release is a trap. `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. +behind a deliberate click. -**Back latches.** It stops the arm, tells the controller to stop, and then keeps the arm -still no matter what the sticks do — a stop somebody had to reach for is not undone by the -next thumb movement. X is the only way back, and it does both halves: releases the latch -here and clears the error on the controller. Neither button is gated on the lease, because -an idle pad hands the arm back two seconds after the stop it just made, and a latch you -must first take the arm back to release is a trap. +### The layout + +These are the indices an Xbox One S pad reports **over Bluetooth**, swept one control at a +time. They are not universal — the same pad on USB numbers its buttons differently — so a +pad that disagrees is an edit to `Layout` in `dora_teleop/mapping.py`, not a new +environment variable. It lives here rather than in `dora-joy` because `dora-joy` does not +know what a button is *for*, which is what makes it reusable. + +| Control | Axis | Control | Button | +|----------------------|------|------------------|--------| +| Left stick X / Y | 0, 1 | A — grasp | 0 | +| Right stick X / Y | 2, 3 | B — release | 1 | +| Left / right trigger | 4, 5 | X — clear | 3 | +| | | Y — freedrive | 4 | +| | | LB / RB — yaw | 6, 7 | +| | | Back — E-STOP | 11 | + +**Sweep a new pad before the arm is powered.** The axes check themselves and the buttons +cannot: + +- **Axes.** Every pad is measured once, on the first reading in which it is sitting still. + A stick must rest near centre and a trigger near the bottom of its travel; an axis + resting anywhere else means the numbering is wrong, something is being held, or the pad + is broken. All three read as **no pad** until it rests, and the log names the axis. A + trigger read as a stick asks for full speed while lying untouched on the desk, so a pad + this node has never seen still never gets the arm. +- **Buttons.** An unpressed button reads the same on every pad, so a wrong button index is + silent until it fires. That is not hypothetical: on this pad, against the indices this + node shipped with, X toggled freedrive and LB was the E-STOP. + +That same measurement makes a worn stick harmless — its resting tilt becomes its zero, so +the tilt is **subtracted** rather than deadzoned away and the travel left still reaches +full speed. A trigger that reads 0 until it first moves, rather than SDL's −1, is handled +the same way instead of reading as half pressed. ### Checking your pad ```bash -dora up -dora start dataflow-gamepad.yml --uv --name teleop-bench +source .venv/bin/activate +dora build dataflow-gamepad.yml +dora run dataflow-gamepad.yml uv run dora-teleop-probe # in another terminal ``` -Prints the raw axes, the buttons it sees, and the velocity this node would send — live, -from the same `joy` stream the node reads. **It cannot move anything.** It attaches as a -dynamic node, publishes nothing and never talks to the arm; it only reads. +The probe prints the raw axes, the buttons it sees and the velocity this node would send, +live, from the same `joy` stream the node reads. **It cannot move anything** — it attaches +as a dynamic node, publishes nothing and never talks to the arm. ```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 + 4 left trigger -1.000 #---------|---------- + buttons (17 reported) + 7: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 asking for the arm: 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: +Two things to look for: an axis marked `unused` that moves when you push a stick, and a +bar that does not sit centred with the stick released. It goes through the same +`velocity()` the node runs, so what it prints is what the arm would be sent. -- **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 `velocity()` the node runs, on the same readings, so what it -prints is what the arm would be sent. - -For a pad whose layout is not known at all — which index is even which — use -`dora-joy-probe` from the `dora-joy` repo, which needs no dataflow and no coordinator. +For a pad whose numbering is not known at all, `dora-joy-probe` in the `dora-joy` repo +needs no dataflow. ## Speaking to any robot -Nothing above is specific to one arm. Both control sources are reduced to a **normalised** -six-vector — `[-1, 1]` per degree of freedom, meaning a fraction of full speed — and only -then scaled, in one function. A gamepad reaches that vector through the deadzone and the -response curve, which exist to correct a worn physical stick; the browser sends it already -clean, because a browser has no worn centre. +Both control sources are reduced to a **normalised** six-vector — `[-1, 1]` per degree of +freedom, a fraction of full speed — and only then scaled, in one function. The gamepad +reaches that vector through the deadzone and the response curve, which correct a worn +physical stick; the browser sends it already clean. -That is what lets the same node and the same console drive a robot in mm/s and one in m/s: -neither source ever knew the difference. It is also why a gamepad and the console asking -for the same deflection produce byte-identical output. +That is what lets one node and one console drive a robot in mm/s and one in m/s. `twist` +is SI, because that is what a UR or MoveIt takes; configuration stays in mm/s and deg/s, +because `60 mm/s` is a number a person reading a dataflow can judge. `command` carries +verbs rather than service names — `gripper_close`, not `grasp` — so swapping a vacuum for +a two-finger gripper needs no change at either end. -`twist` is SI — m/s and rad/s — because that is unambiguous and is what a UR or MoveIt -takes. Configuration stays in mm/s and deg/s, because `60 mm/s` is a number a person -reading a dataflow can judge. - -`command` carries verbs rather than service names: `gripper_close` rather than `grasp`, -so a robot that swaps a vacuum for a two-finger gripper needs no change at either end. - -The full contract, including the reserved `joint_jog` port for joint-space robots, is in -the vault at `resources/robotics/teleop-contract.md`. +The full contract, including the reserved `joint_jog` port, is in the vault at +`resources/robotics/teleop-contract.md`. ## The lease Two sources, one arm. Whoever holds the lease drives; the other is ignored. - The **gamepad takes it implicitly**, the first time a stick moves, if nobody holds it. - Nobody should have to reach for a browser to use the thing in their hands. - It **gives it back** after `GAMEPAD_IDLE_RELEASE_S` untouched, so a pad left on the - bench does not lock the browser out forever. -- The **browser takes it explicitly**, with the console's TAKE CONTROL button, and gives - it back with RELEASE. It can preempt a held arm, because an operator who can see the arm - is better placed than this code to decide they should have it. -- **E-STOP is never gated.** From either source, held lease or not. Somebody reaching for - the stop is the one moment when asking who owns the arm is the wrong question. + bench does not lock the browser out. +- The **browser takes it explicitly** with TAKE CONTROL and returns it with RELEASE. It + can preempt: an operator who can see the arm is better placed than this code to decide. +- **E-STOP is never gated**, from either source, held lease or not. -**The lease binds only what goes through this node.** `dora-ulite6` has no idea it exists. -Another node publishing `request` will preempt an operator mid-motion and nothing here can -stop it — if that matters for your dataflow, do not wire one. +**The lease binds only what goes through this node.** `dora-ulite6` has no idea it exists, +so another node publishing `request` will preempt an operator mid-motion. ## The deadman -An arm under a velocity command keeps moving until told otherwise, so the question is -never whether to start but whether anything has happened that means it must stop. Four -layers, outermost last: +An arm under a velocity command keeps moving until told otherwise. Four layers, each +shorter than the one behind it — a backstop that fires first backs up nothing: -1. **The repeat** (`JOG_KEEPALIVE_S`, 0.2 s) while a source is driving. -2. **The pad's freshness** (`JOY_TTL_S`, 0.2 s). The pad is another process now, and that - process dying looks like silence rather than like an unplugged pad — so silence means - the same thing. `dora-joy` publishes on every tick whether or not the pad moved, which - is what makes silence unambiguous. -3. **This node's watchdog** (`JOG_TTL_S`, 0.4 s) stops the arm when the owner goes quiet — - and stops it *immediately*, without waiting out the timer, when the gamepad reports - itself gone or the last browser disconnects. An unplugged pad is not a centred pad. -4. **The arm's own expiry** (`VELOCITY_COMMAND_TTL_S` in `dora-ulite6`, 0.5 s), enforced by - the controller whether or not this process is alive. +| Layer | Setting | Default | +|--------------------------|-------------------------------------|---------| +| Held velocity repeated | `JOG_KEEPALIVE_S` (teleop) | 0.2 s | +| Pad reading goes stale | `JOY_TTL_S` (teleop) | 0.2 s | +| Owner goes quiet | `JOG_TTL_S` (teleop) | 0.4 s | +| Jog sender goes quiet | `VELOCITY_COMMAND_TTL_S` (ulite6) | 0.5 s | -Keep them in that order, each shorter than the one behind it. A backstop that fires before -the thing it is backing up backs up nothing. `JOY_TTL_S >= JOG_TTL_S` stops the node at -startup rather than letting that happen quietly. +`JOY_TTL_S >= JOG_TTL_S` stops the node at startup rather than failing quietly later. The +last layer is enforced by `dora-ulite6` whether or not this process is alive. -On shutdown the node sends one all-zero velocity before exiting. A motion lives in the -controller, not in this process. +A pad reporting itself gone, or the last browser disconnecting, stops the arm +**immediately** without waiting out any timer: an unplugged pad is not a centred pad. On +shutdown the node sends one all-zero velocity, because a motion lives in the controller +rather than in this process. -The stop is sent **once**, not every tick. A stream of zeroes would keep `dora-ulite6` -believing a jog is live and hold `robot_state` at `moving` forever. - -### What goes on the wire is the change - -A velocity is a set-point the arm holds, not a frame in a sequence. This node sends one -when it **changes** — a new direction, or the stop — and otherwise repeats the current one -only every `JOG_KEEPALIVE_S` (0.2 s), which exists solely so the arm's own expiry never -fires mid-motion. - -It used to repeat every tick. On a real xArm that means the controller is handed the same -velocity twenty times a second and re-plans the same motion each time, which an operator -feels as the arm moving in steps rather than travelling. Keep the repeat well under the -arm's `VELOCITY_COMMAND_TTL_S` and no faster than it needs to be. - -**A change never waits for the repeat.** Releasing a button is acted on the tick it -happens, so sending less does not cost anything at the operator's end. +**What goes on the wire is the change.** A velocity is a set-point the arm holds, so one +is sent when it *changes* and otherwise repeated only every `JOG_KEEPALIVE_S`. Repeating +every tick made a real xArm re-plan the same motion twenty times a second, which reads as +stepping. The stop is sent **once** — a stream of zeroes keeps `dora-ulite6` believing a +jog is live. A change never waits for the repeat. ## Ports -| Direction | Name | Description | -|-----------|----------------|------------------------------------------------------| -| in | `tick` | 50 ms — change detection, deadman, telemetry | -| in | `joy` | One pad reading from `dora-joy` | -| in | `response` | An answer from the arm | -| in | `motion_state` | How a motion ended | -| in | `robot_state` | The arm's own state | -| in | `tcp_pose` | Tool-tip pose, six float32, mm and degrees | -| in | `joint_pose` | Joint angles, six float32, degrees | -| out | `twist` | **Standard.** `{linear, angular, frame}`, SI | +| Direction | Name | Description | +|-----------|----------------|-------------------------------------------------------| +| in | `tick` | 50 ms — change detection, deadman, telemetry | +| in | `joy` | One pad reading from `dora-joy` | +| in | `response` | An answer from the arm | +| in | `motion_state` | How a motion ended | +| in | `robot_state` | The arm's own state | +| in | `tcp_pose` | Tool-tip pose, six float32, mm and degrees | +| in | `joint_pose` | Joint angles, six float32, degrees | +| out | `twist` | **Standard.** `{linear, angular, frame}`, SI | | out | `command` | **Standard.** `{action, value}`, robot-agnostic verbs | -| out | `jog` | *Deprecated.* mm/s + deg/s, for `dora-ulite6` | +| out | `jog` | *Deprecated.* mm/s + deg/s, for `dora-ulite6` | | out | `request` | *Deprecated.* A service call, `request_id` in metadata | -| out | `ui_state` | The console's telemetry snapshot, as JSON — see below | +| out | `ui_state` | The console's telemetry snapshot, as JSON | -Wire `twist` **or** `jog`, never both: they carry the same intent, and a robot reading +Wire `twist` **or** `jog`, never both — they carry the same intent, and a robot reading both is driven twice. Same for `command` and `request`. -The browser is not on this table. It reaches the node over the node's own HTTP and -WebSocket port rather than through the dataflow, so there is nothing to wire for it — -`ui_state` is published for anyone else watching and has no consumer in the stacks below. +The browser is not on this table: it reaches the node over the node's own HTTP and +WebSocket port rather than through the dataflow, so there is nothing to wire for it. -## Environment Variables +## Environment variables -| Variable | Default | Description | -|--------------------------|---------|--------------------------------------------------------| -| `SPEED_MM_S` | `60.0` | Translation speed at full deflection | -| `SPEED_DEG_S` | `30.0` | Rotation speed at full deflection | -| `DEADZONE` | `0.12` | Stick travel near centre to ignore | -| `EXPO` | `0.4` | How much of the curve is cubic; 0 is linear | -| `JOG_TTL_S` | `0.4` | Owner silence before the arm is stopped | -| `JOY_TTL_S` | `0.2` | `joy` silence before the pad reads as gone | -| `GAMEPAD_IDLE_RELEASE_S` | `2.0` | Untouched pad before it hands the arm back | -| `STATE_POLL_S` | `0.5` | How often to ask the arm what telemetry does not carry | +| Variable | Default | Description | +|--------------------------|----------|--------------------------------------------------------| +| `SPEED_MM_S` | `60.0` | Translation speed at full deflection | +| `SPEED_DEG_S` | `30.0` | Rotation speed at full deflection | +| `DEADZONE` | `0.12` | Stick travel near centre to ignore | +| `EXPO` | `0.4` | How much of the curve is cubic; 0 is linear | +| `JOG_TTL_S` | `0.4` | Owner silence before the arm is stopped | +| `JOG_KEEPALIVE_S` | `0.2` | How often an unchanged velocity is repeated | +| `JOY_TTL_S` | `0.2` | `joy` silence before the pad reads as gone | +| `GAMEPAD_IDLE_RELEASE_S` | `2.0` | Untouched pad before it hands the arm back | +| `STATE_POLL_S` | `0.5` | How often to ask the arm what telemetry does not carry | +| `WEB_HOST` | `0.0.0.0`| Address the console binds to | +| `WEB_PORT` | `7000` | Console port; **`0` opens no port at all** | -A value that cannot be read, a `DEADZONE` outside the stick, an `EXPO` off the curve or a -`JOY_TTL_S` at or above `JOG_TTL_S` stops the node at startup rather than the first time -somebody picks up the pad. +A value that cannot be read, or one that breaks the deadman order, stops the node at +startup rather than the first time somebody picks up the pad. `JOYSTICK_INDEX` moved to +`dora-joy` along with the pad. -`JOYSTICK_INDEX` moved to `dora-joy` along with the pad. +## Running it -## Usage - -The real stack is three nodes: this one, the pad and the arm. The console comes with -this node. - -```yaml - - id: teleop - git: ssh://git@gitea.liria.cl:5001/liria/dora-teleop.git - tag: v0.3.0 - build: pip install . - path: dora-teleop - inputs: - tick: dora/timer/millis/50 - joy: - source: joy/joy - queue_size: 1 - queue_policy: drop_oldest - response: ulite6/response - motion_state: ulite6/motion_state - robot_state: ulite6/robot_state - tcp_pose: ulite6/tcp_pose - joint_pose: ulite6/joint_pose - outputs: [jog, request, ui_state] - env: - WEB_PORT: "7000" -``` - -Wire `ulite6`'s `request` and `jog` from this node, and add `dora-joy` on a 20 ms tick for -the pad — `queue_size: 1` matters, because it ticks faster than this node does and an arm -should be driven by the newest reading or by none. - -Then open `http://:7000` for the console. Nothing else needs -wiring: the node serves the page and holds the browsers' sockets, and the page finds the -socket on its own origin. - -Leave `joy` unwired for a browser-only console; the node starts without it. Set -`WEB_PORT: "0"` for a pad-only one, and no port is opened. - -**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), Python 3.11+ and `uv`. No -hardware: the pad and the arm are other nodes, and the browser is any browser. - -### Driving a real arm - -`dataflow-robot.yml` is the one that moves a robot: the pad, this node and -[`dora-ulite6`](https://gitea.liria.cl/liria/dora-ulite6), with no bench and no browser. -Both sibling nodes come in by `git:`, so it runs from a fresh clone with nothing else -checked out. +| Dataflow | Nodes | Needs | +|-------------------------|------------------------------|-----------------| +| `dataflow.yml` | teleop + bench stand-in | nothing | +| `dataflow-robot.yml` | joy + teleop + ulite6 | a pad and an arm | +| `dataflow-gamepad.yml` | the same, plus the probe, console shut | a pad and an arm | ```bash -source .venv/bin/activate # see the note below before reaching for --uv -dora build dataflow-robot.yml # clones and installs both, needs the network +dora run dataflow.yml --uv # no pad, no arm, browser path only + +source .venv/bin/activate # a real arm: see the --uv note below +dora build dataflow-robot.yml # clones and installs both siblings dora run dataflow-robot.yml ``` -**`--uv` does not work for a dataflow with `git:` nodes** (dora 1.0.1). `dora build --uv` -installs each `git:` node into a venv inside its own clone directory, while `dora run ---uv` resolves the entrypoint by running `uv run which ` from the *dataflow's* -directory — which only ever sees the dataflow's own venv. The node is installed where the -spawner does not look, and the run fails with `Could not find source path dora-joy within -uv`. Building into one activated environment, as above, puts every node's entrypoint -somewhere the spawner finds it. The venv needs `pip` for that (`uv pip install pip`), -since `uv` does not create one. +`dataflow.yml` is self-contained on purpose — it resolves from this checkout alone, which +is what lets CI validate it. The bench answers services and publishes telemetry but is +**not a simulator**: it never integrates a velocity or moves a pose. -`dataflow.yml` is unaffected and `--uv` is fine there: its only Python node is this repo, -which is already in this repo's venv. +Set `ROBOT_IP` and `TOOL_TYPE` to match the arm in front of you. `INIT_MODE` is `none`, so +nothing moves until somebody pushes a stick. The console comes with the node — open +`http://:7000` for the same arm the pad drives, under the same lease. -Set `ROBOT_IP` and `TOOL_TYPE` in that file to match the arm in front of you; the defaults -are `dora-ulite6`'s own, `192.168.1.192` and a vacuum tool. `INIT_MODE` is `none`, so -nothing moves until somebody pushes a stick. +**`--uv` does not work for a dataflow with `git:` nodes** (dora 1.0.1): each is installed +into a venv inside its own clone, while the spawner resolves entrypoints from the +dataflow's venv, and the run fails with `Could not find source path dora-joy within uv`. +Build into one activated environment instead, as above; it needs `pip` +(`uv pip install pip`). **`dora start` refuses them outright** — "node joy has git source, +but no `dora build` was run yet", straight after a build that succeeded — so use +`dora run`, which also means `dora topic` is unavailable for those two dataflows. -The deadman timings have to stay ordered, each below the one behind it, and the file pins -all of them so the chain is visible in one place: - -| Layer | Setting | Default | -|---|---|---| -| Pad reading goes stale | `JOY_TTL_S` (teleop) | 0.2 s | -| Held velocity repeated | `JOG_KEEPALIVE_S` (teleop) | 0.2 s | -| Owner goes quiet | `JOG_TTL_S` (teleop) | 0.4 s | -| Jog sender goes quiet | `VELOCITY_COMMAND_TTL_S` (ulite6) | 0.5 s | - -The last one is enforced by `dora-ulite6` itself from v0.3.1 on, not by the controller: -the velocity is set once and held, so that node stops the arm rather than letting a -controller-side timer do it. - -`dora` type-checks a dataflow before it fetches a `git:` source, so it cannot read the -arm's manifest from here. The `output_types:` block on the `ulite6` node repeats what -`dora-ulite6` v0.3.1 annotates, which is also what pinning that tag asserts — re-check it -when moving to a newer one. - -The console is already there — `dataflow-robot.yml` sets `WEB_PORT` to 7000, so opening -`http://:7000` gets you the same arm the pad drives, under the same lease. - -### Running it with no arm and no browser - -`dataflow.yml` in this repo is **not** that stack. It is this node plus a bench stand-in, -with **no pad**: pulling `dora-joy` in by `git:` would cost that file the one property it -has, which is that it resolves and runs from this checkout alone, with no network and no -sibling clones. - -```bash -dora run dataflow.yml --uv # bench only, browser path -dora run dataflow-gamepad.yml --uv # the same bench with a real pad -``` - -`dataflow-gamepad.yml` adds `dora-joy` by `git:` and declares the probe. It needs the -network on its first build. - -The bench answers every service, publishes plausible telemetry, and prints each velocity -it is sent: - -```text -jog 1 [ +38.89, +23.53, +0.00, +0.00, +0.00, +0.00] -jog 2 [ +38.89, +23.53, +0.00, +0.00, +0.00, +0.00] -jog 3 [ +0.00, +0.00, +0.00, +0.00, +0.00, +0.00] <- stop -``` - -It is **not a simulator**: it never integrates the velocity or moves the pose, and a jog -is printed and forgotten. It closes the loop so this node's behaviour can be watched — -the lease changing hands, the deadman firing when you let go — not so the arm can be -pretended at. +`dora` type-checks a dataflow before fetching a `git:` source, so it cannot read the arm's +manifest. The `output_types:` block on the `ulite6` node repeats what `dora-ulite6` v0.3.1 +annotates — re-check it when moving to a newer tag. ### Watching the wire -`dataflow.yml` sets `debug: enable_debug_inspection: true`, so dora will let you read what -is actually on the wire without the bench printing anything. Run it under a coordinator -rather than `dora run`: +Every dataflow sets `debug: enable_debug_inspection: true`, so dora will let you read the +wire without any node printing it. This needs a coordinator, so it works for the +arm-free dataflow: ```bash dora up -dora start dataflow-gamepad.yml --uv --name teleop-bench -``` - -Then, in another terminal: - -```bash +dora start dataflow.yml --uv --name teleop-bench dora topic list -d teleop-bench # every edge in the graph -dora topic echo -d teleop-bench joy/joy # what the pad is reporting dora topic echo -d teleop-bench teleop/jog # every velocity, live -dora topic echo -d teleop-bench teleop/ui_state # what the console would draw dora topic hz -d teleop-bench teleop/jog --duration 10 # the re-send cadence +dora stop --name teleop-bench && dora down ``` -`hz` is the one that checks the timings this node promises. The `get_state` poll measures -at 1.88 Hz against a configured `STATE_POLL_S` of 0.5, and with a pad held over the -deadzone `teleop/jog` should sit near the 50 ms tick. - -Tear it down with `dora stop --name teleop-bench && dora down`. - ## The console -`dora_teleop/web/index.html` is the browser half: two analog sticks, a lift lane and a -yaw lane, plus gripper, freedrive, home, the lease buttons and E-STOP over a telemetry -readout. Plain HTML with no build step — the node serves the file as it stands, and a -console you cannot fix without a toolchain is the wrong thing to have between a person and -a robot. +`dora_teleop/web/index.html` is the browser half: two analog rings, a lift lane and a yaw +lane, plus gripper, freedrive, home, the lease buttons and E-STOP over a telemetry +readout. Plain HTML with no build step — a console you cannot fix without a toolchain is +the wrong thing to have between a person and a robot. -**Laid out like the arm's own pendant.** Each ring is quartered into arc sectors you hold, -with the out-of-plane axis as a pair of pills above it — the same shape as xArm Studio's -jog control on the controller at `:18333`, and deliberately the same directions, because -two consoles driving one arm that disagree about which way is `+Y` is a trap rather than a -preference. +It is laid out like the arm's own pendant: each ring is quartered into sectors you hold, +with the out-of-plane axis as a pair of pills above it — the same shape and the same +directions as xArm Studio's jog control at `:18333`. -| Ring | Sectors | Pills above | -|-----------|------------------------------------------------|----------------| -| translate | `+X` north, `−X` south, `Y+` **west**, `Y−` east | `−Z` `+Z` | -| rotate | `+pitch` north, `−pitch` south, `−roll` west, `+roll` east | `−yaw` `+yaw` | +| Ring | Sectors | Pills above | +|-----------|------------------------------------------------------------|---------------| +| translate | `+X` north, `−X` south, `Y+` **west**, `Y−` east | `−Z` `+Z` | +| rotate | `+pitch` north, `−pitch` south, `−roll` west, `+roll` east | `−yaw` `+yaw` | **`Y+` is on the left**, matching xArm Studio and the right-hand rule with `+X` away from -the operator and `+Z` up. It is the one direction that does not read as "screen right is -right", and it is that way on purpose. +the operator and `+Z` up. Two consoles that disagree about which way is `+Y` is a trap. -Directions are held, not analog: speed comes from the dial, and every held direction is a -separate pointer, so two fingers work at once — driving `+X` while lifting on `+Z` is one -gesture. +The node serves the page and holds the WebSocket on `/ws` of the same port, so there is +one URL and nothing to keep in step. That runs on its own thread and meets the control +loop at two non-blocking points only, because a browser that stops reading must never +stall the thread that stops the arm. **The browser's deadman is the socket itself** — the +last one closing releases the lease and stops the arm. -The node serves it on `WEB_PORT` (7000) and holds the WebSocket on `/ws` of that same -port, so the console is one URL. That work happens on a thread of its own and meets the -control loop at two points only — the messages the next tick drains, and the snapshot -handed back — because a browser that stops reading must never be able to stall the thread -that stops the arm. - -**The browser's deadman is the socket itself.** The last one closing releases the lease -and stops the arm, which is why the tab closing mid-jog does not leave it travelling. - -Hold a direction to move, release to stop. The page re-sends its jog every 150 ms against -the node's 400 ms deadline, so letting go stops the robot even if the release is lost. It -sends a **fraction of full deflection**, never a speed, so it never learns the robot's -units; the speed dial sets that fraction. Every control is dimmed and inert unless the -console holds the lease, so one that would be refused looks refused. - -If `WEB_PORT` is already taken the node **refuses to start** and says so, rather than -carrying on with a console nobody can open. - -`tests/test_console.py` checks the page against `ui.py` — every message type it sends is -one the node serves, and every telemetry key it reads is one the node sends. Neither -language catches a rename, and the wrong place to discover one is on a machine that moves. +Directions are held, not analog: speed comes from the dial, and each held direction is a +separate pointer, so two fingers work at once. The page re-sends its jog every 150 ms +against the node's 400 ms deadline, and sends a **fraction of full deflection**, never a +speed, so it never learns the robot's units. Controls are dimmed and inert unless the +console holds the lease. If `WEB_PORT` is taken the node **refuses to start**. ## Development ```bash uv sync --group dev -uv run pytest # 112 tests, no gamepad, arm, browser or daemon needed +uv run pytest # 149 tests, no gamepad, arm, browser or daemon needed uv run ruff check . uv run ruff format --check . ``` -Every file here is testable without hardware, because there is no hardware. The pad -arrives on `joy` as a plain `Sample`. +Every file here is testable without hardware, because there is no hardware. `tests/` names +its controls (`axes_for`, `buttons_for`) rather than numbering them — a test that numbers +its axes goes on passing while meaning something else once a pad renumbers them. diff --git a/dataflow-gamepad.yml b/dataflow-gamepad.yml index 23057f8..f06f204 100644 --- a/dataflow-gamepad.yml +++ b/dataflow-gamepad.yml @@ -1,23 +1,29 @@ -# The same bench, with a real gamepad and a probe. +# The pad, a real arm and the probe. No bench and no browser. # # `dataflow.yml` is what CI validates, and it deliberately has no pad in it: the # pad is `dora-joy`, another repo, and pulling it in by `git:` would cost that # file its one property — that it resolves and runs from this checkout alone. -# This file is the other half. It needs the network on first build, and a pad. +# This file is the other half. It needs the network on first build, a pad, and +# an arm. # -# The bench answers services and publishes telemetry but never moves anything. -# What this is for is watching the node decide: the lease changing hands, the -# deadman firing when you let go, and the arm being stopped when the pad — or -# the node reading it — goes away. +# It is `dataflow-robot.yml` with the probe attached and the console shut, for +# the one job the probe exists for: checking that the pad in your hands has the +# layout `mapping.py` assumes, against the arm that will actually move. Use +# `dataflow-robot.yml` for ordinary driving. # -# Run it under a coordinator rather than `dora run`, so `dora topic` can read the -# wire and so the probe has something to attach to: +# source .venv/bin/activate # not --uv; README.md says why +# dora build dataflow-gamepad.yml +# dora run dataflow-gamepad.yml # -# dora up -# dora start dataflow-gamepad.yml --uv --name teleop-bench -# dora-teleop-probe # in another terminal -# dora topic echo -d teleop-bench teleop/jog -# dora stop --name teleop-bench && dora down +# `dora run`, not `dora up` + `dora start`: dora 1.0.1 refuses to start a +# dataflow holding `git:` nodes from a coordinator — "node joy has git source, +# but no `dora build` was run yet", straight after a build that succeeded. It is +# the same shape of gap as the `--uv` one above, and it costs this file +# `dora topic`, which needs a coordinator to read the wire. +# +# ROBOT_IP and TOOL_TYPE below name the arm in front of you: the bench arm at +# 192.168.1.162, with a vacuum tool. Read +# `The gamepad` in README.md before picking it up — Back is the E-STOP. strict_types: true @@ -47,10 +53,11 @@ nodes: source: joy/joy queue_size: 1 queue_policy: drop_oldest - response: bench/response - robot_state: bench/robot_state - tcp_pose: bench/tcp_pose - joint_pose: bench/joint_pose + response: ulite6/response + motion_state: ulite6/motion_state + robot_state: ulite6/robot_state + tcp_pose: ulite6/tcp_pose + joint_pose: ulite6/joint_pose outputs: - twist - command @@ -60,8 +67,13 @@ nodes: env: SPEED_MM_S: "60" SPEED_DEG_S: "30" - # This one is about the pad, so the console stays shut. Set a port to - # bring it up alongside and watch the lease move between the two. + # Keep the deadman layers ordered: JOY_TTL_S < JOG_TTL_S < the arm's own + # VELOCITY_COMMAND_TTL_S, which is 0.5 s below. Each backs up the one + # inside it, and a backstop that fires first backs up nothing. + JOY_TTL_S: "0.2" + JOG_TTL_S: "0.4" + # This one is about the pad, so the console stays shut and no port is + # opened. `dataflow-robot.yml` is the one that serves it. WEB_PORT: "0" # Declared, not spawned: `dora-teleop-probe` attaches to this id when you run @@ -71,15 +83,34 @@ nodes: inputs: joy: joy/joy - - id: bench - path: examples/bench.py + - id: ulite6 + git: https://gitea.liria.cl/liria/dora-ulite6.git + tag: v0.3.1 + build: pip install . + path: dora-ulite6 inputs: - tick: dora/timer/millis/200 - jog: teleop/jog + tick: dora/timer/millis/100 request: teleop/request - ui_state: teleop/ui_state + jog: teleop/jog outputs: - response - - robot_state - - tcp_pose + - motion_state - joint_pose + - tcp_pose + - robot_state + # dora type-checks before it fetches a `git:` source, so it cannot read the + # arm's own manifest from here. These three repeat what `dora-ulite6` + # v0.3.1 annotates; `response` and `motion_state` stay untyped there and so + # stay untyped here. Re-check them when moving to a new tag. + output_types: + joint_pose: std/core/v1/Float32 + tcp_pose: std/core/v1/Float32 + robot_state: std/core/v1/String + env: + ROBOT_IP: "192.168.1.162" + TOOL_TYPE: "vacuum" + # The outermost deadman, enforced by the controller whether or not any of + # this is alive. Every timer above must stay below it. + VELOCITY_COMMAND_TTL_S: "0.5" + # The pad does not home. Nothing moves until somebody pushes a stick. + INIT_MODE: "none" diff --git a/dataflow-robot.yml b/dataflow-robot.yml index d113d5a..91b16db 100644 --- a/dataflow-robot.yml +++ b/dataflow-robot.yml @@ -16,8 +16,8 @@ # then looks for its entrypoint in this dataflow's venv instead. README.md has the # detail. `dataflow.yml` has no `git:` nodes and is fine with --uv. # -# Set ROBOT_IP and TOOL_TYPE below to match the arm in front of you. The -# defaults are `dora-ulite6`'s own: 192.168.1.192 and a vacuum tool. +# ROBOT_IP and TOOL_TYPE below name the arm in front of you: the bench arm at +# 192.168.1.162, with a vacuum tool. # # The web console comes with the node: it serves the page and holds the # browsers' sockets itself, so `http://:7000` is a second way to @@ -102,7 +102,7 @@ nodes: tcp_pose: std/core/v1/Float32 robot_state: std/core/v1/String env: - ROBOT_IP: "192.168.1.192" + ROBOT_IP: "192.168.1.162" TOOL_TYPE: "vacuum" # The outermost deadman, enforced by the controller whether or not any of # this is alive. Every timer above must stay below it. diff --git a/dataflow.yml b/dataflow.yml index 33e3cbb..5a58791 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -5,8 +5,9 @@ # manifest from `path:` and CI needs no gamepad, no robot and no sibling clones. # # There is **no gamepad here**, because the pad is another node and wiring it in -# by `git:` would cost this file the one property it has. For a real pad on a -# real bench, use `dataflow-gamepad.yml`. +# by `git:` would cost this file the one property it has. For a real pad, use +# `dataflow-robot.yml`, or `dataflow-gamepad.yml` to check the pad with the +# probe — both of which drive a real arm. # # This is **not** the real stack. The bench answers services and publishes # telemetry but never moves anything. README.md carries the four-node wiring a diff --git a/dora-node.yml b/dora-node.yml index 55c6ac9..9977de6 100644 --- a/dora-node.yml +++ b/dora-node.yml @@ -53,6 +53,12 @@ inputs: pad node is indistinguishable from an unplugged pad as far as the arm is concerned. + Each pad is measured the first time it is seen sitting still, so a stick + that no longer centres has its tilt subtracted rather than deadzoned away. + A pad whose resting axes do not fit the layout this node compiles in — a + trigger where a stick should be, say — reads the same as no pad at all + and is named in the log, rather than being guessed at against a real arm. + Leave it unwired for a browser-only console. The node says so and starts. response: required: false 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