diff --git a/README.md b/README.md index 1d82855..f1b169f 100644 --- a/README.md +++ b/README.md @@ -1,254 +1,46 @@ # dora-teleop -A dora-rs node that hands a robot arm to a human. Pick up a gamepad and move the arm, or -open a browser and move it from there — one of the two at a time, decided here. +dora-rs teleoperation node: normalises two control sources — a gamepad read from +`dora-joy`, and a web console served by this node itself — into one owner's lease, and +emits velocity set-points and robot-agnostic commands. It carries no hardware driver: pad +I/O lives in `dora-joy`, arm I/O in a robot node. -It drives [`dora-ulite6`](https://gitea.liria.cl/liria/dora-ulite6) through that node's -`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.12+, `uv`. -**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), Python 3.11+ and `uv`. +## System -## The gamepad +```mermaid +flowchart LR + pad([gamepad]) --> joy + browser([browser]) -- "ws: held directions, verbs" --> web + web -- "ui_state snapshot" --> browser + joy["dora-joy — sibling repo"] + subgraph teleop["dora-teleop"] + web["web console — built in, HTTP + ws on one port"] + core["command arbitration"] + end + joy -- "joy: axes, buttons, present" --> core + web -- "held directions, ±1" --> core + core -- "twist SI · command verbs (standard)" --> arm + core -- "jog · request (deprecated)" --> arm + arm -. "state · poses · response" .-> core + arm["robot node"] +``` -| 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 base frame — the same -convention the console uses, so the two agree. - -**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. - -### 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 +## Quick start ```bash -source .venv/bin/activate -dora build dataflow-gamepad.yml -dora run dataflow-gamepad.yml -uv run dora-teleop-probe # in another terminal +uv sync +dora run dataflow.yml --uv # teleop + bench stand-in: no pad, no arm ``` -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 --#-------|---------- - 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 <-- - wz +30.00 deg/s yaw <-- - - moving: yes - asking for the arm: yes -``` - -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. - -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 - -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 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. - -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. -- It **gives it back** after `GAMEPAD_IDLE_RELEASE_S` untouched, so a pad left on the - 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, -so another node publishing `request` will preempt an operator mid-motion. - -## The deadman - -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: - -| 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 | - -`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. - -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. - -**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 | -| out | `command` | **Standard.** `{action, value}`, robot-agnostic verbs | -| 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 | - -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. - -## 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 | -| `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, 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. - -## Running it - -| 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 -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 -``` - -`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. - -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. - -**`--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. - -`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. +Open `http://localhost:7000` and drive the robot from the console. The bench answers +services and publishes telemetry so the loop closes. ### Watching the wire -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: +Every dataflow sets `debug: enable_debug_inspection: true`, so dora can inspect the wire +without any node printing. This needs a coordinator, so it works for the arm-free dataflow: ```bash dora up @@ -259,36 +51,64 @@ dora topic hz -d teleop-bench teleop/jog --duration 10 # the re-send caden dora stop --name teleop-bench && dora down ``` -## The console +## Running with hardware -`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. +| 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, no console | a pad and an arm | -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`. +## Gamepad -| 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` | +| Control | Does (base frame) | Index | Units | +|----------------------|----------------------------------------------------------------|--------------|-------| +| Left stick X / Y | Translate in the horizontal plane: `+X` stick up, `+Y` stick right | axes 0, 1 | mm/s | +| Left / right trigger | Translate along Z: right trigger lifts (`+Z`), left lowers | axes 4, 5 | mm/s | +| Right stick X / Y | Rotate: `+roll` stick right, `+pitch` stick up | axes 2, 3 | deg/s | +| LB / RB | Yaw: RB is `+yaw` | buttons 6, 7 | deg/s | +| A | Close the tool | button 0 | — | +| B | Open the tool | button 1 | — | +| X | Release the E-STOP and clear the arm's error | button 3 | — | +| Y | Toggle freedrive | button 4 | — | +| Back | **E-STOP** — latches until X | button 11 | — | -**`Y+` is on the left**, matching xArm Studio and the right-hand rule with `+X` away from -the operator and `+Z` up. Two consoles that disagree about which way is `+Y` is a trap. +The sticks move the arm in its horizontal X/Y plane only; the triggers are the one path +to Z. Up on the stick sends the arm away from the operator, the same `+X` convention the +console draws. -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. +## Inputs and outputs -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**. +| 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 | `request` | *Deprecated.* A service call, `request_id` in metadata | +| out | `ui_state` | The console's telemetry snapshot, as JSON | + +## 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 | +| `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 | ## Development @@ -298,7 +118,3 @@ 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. `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.