Compare commits
17
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
da1b079b7b | ||
|
|
ffbba0f4d1 | ||
|
|
ebd3b3e493 | ||
|
|
1b63ce97c9 | ||
|
|
177511af1f | ||
|
|
d336f8d499 | ||
|
|
885b7a569f | ||
|
|
ac2540629e | ||
|
|
fb14545850 | ||
|
|
e9fa4bf36b | ||
|
|
8a7bfb02e0 | ||
|
|
7250396a1f | ||
|
|
56b86699bd | ||
|
|
9f56b2b334 | ||
|
|
04f0d57068 | ||
|
|
c9cb4b8c9e | ||
|
|
bde8582de3 |
@@ -0,0 +1,70 @@
|
|||||||
|
# A pull request would otherwise run this twice on the same commit: once for the
|
||||||
|
# branch push and once for the PR.
|
||||||
|
|
||||||
|
name: validate
|
||||||
|
|
||||||
|
on:
|
||||||
|
push:
|
||||||
|
branches: [main]
|
||||||
|
pull_request:
|
||||||
|
|
||||||
|
env:
|
||||||
|
DORA_VERSION: v1.0.1
|
||||||
|
DORA_TARGET: x86_64-unknown-linux-gnu
|
||||||
|
|
||||||
|
jobs:
|
||||||
|
validate:
|
||||||
|
runs-on: ubuntu-latest
|
||||||
|
steps:
|
||||||
|
- uses: actions/checkout@v4
|
||||||
|
|
||||||
|
- name: Install the dora CLI
|
||||||
|
run: |
|
||||||
|
curl -fsSL -o dora-cli.tar.gz \
|
||||||
|
"https://github.com/dora-rs/dora/releases/download/${DORA_VERSION}/dora-cli-${DORA_TARGET}.tar.gz"
|
||||||
|
tar xzf dora-cli.tar.gz
|
||||||
|
mkdir -p "$HOME/.local/bin"
|
||||||
|
install -m 755 "dora-cli-${DORA_TARGET}/dora" "$HOME/.local/bin/dora"
|
||||||
|
echo "$HOME/.local/bin" >> "$GITHUB_PATH"
|
||||||
|
rm -rf dora-cli.tar.gz "dora-cli-${DORA_TARGET}"
|
||||||
|
|
||||||
|
- run: dora --version
|
||||||
|
|
||||||
|
# strict_types: true in the dataflow turns a type warning into a non-zero
|
||||||
|
# exit, so a mismatched annotation fails here rather than at runtime.
|
||||||
|
- name: Validate the dataflow
|
||||||
|
run: dora validate dataflow.yml
|
||||||
|
|
||||||
|
# The gamepad dataflow pulls dora-joy by `git:`, but validation resolves
|
||||||
|
# the wiring without fetching it, so this stays offline and still catches
|
||||||
|
# a mistyped port.
|
||||||
|
- name: Validate the gamepad dataflow
|
||||||
|
run: dora validate dataflow-gamepad.yml
|
||||||
|
|
||||||
|
# Same again for the real-arm stack. Its `output_types:` block is what
|
||||||
|
# lets this pass without an arm or a clone of the arm node, so a drift
|
||||||
|
# between it and the tag it pins fails here.
|
||||||
|
- name: Validate the robot dataflow
|
||||||
|
run: dora validate dataflow-robot.yml
|
||||||
|
|
||||||
|
test:
|
||||||
|
runs-on: ubuntu-latest
|
||||||
|
steps:
|
||||||
|
- uses: actions/checkout@v4
|
||||||
|
- uses: astral-sh/setup-uv@v5
|
||||||
|
with:
|
||||||
|
enable-cache: true
|
||||||
|
# --locked fails if uv.lock is out of date with pyproject.toml, so a
|
||||||
|
# dependency change that was never locked cannot reach main.
|
||||||
|
- run: uv sync --locked --group dev
|
||||||
|
- run: uv run ruff check .
|
||||||
|
- run: uv run ruff format --check .
|
||||||
|
- run: uv run pytest -q
|
||||||
|
|
||||||
|
markdownlint:
|
||||||
|
runs-on: ubuntu-latest
|
||||||
|
steps:
|
||||||
|
- uses: actions/checkout@v4
|
||||||
|
- uses: DavidAnson/markdownlint-cli2-action@v16
|
||||||
|
with:
|
||||||
|
globs: "*.md"
|
||||||
@@ -12,3 +12,6 @@ build/
|
|||||||
.ruff_cache/
|
.ruff_cache/
|
||||||
out/
|
out/
|
||||||
.dora/
|
.dora/
|
||||||
|
# Where `dora build` clones a `git:` node source, which
|
||||||
|
# dataflow-robot.yml and dataflow-gamepad.yml both have.
|
||||||
|
git/
|
||||||
|
|||||||
@@ -0,0 +1,228 @@
|
|||||||
|
# AGENTS.md
|
||||||
|
|
||||||
|
This file provides guidance to AI coding assistants when working with code in this repository.
|
||||||
|
|
||||||
|
## What this is
|
||||||
|
|
||||||
|
A dora-rs Python node that gives a human control of a robot arm, from a gamepad or from a
|
||||||
|
web browser, and decides which of the two is driving.
|
||||||
|
|
||||||
|
It is **the piece in the middle**, and it **touches no hardware at all**. The arm is
|
||||||
|
[`dora-ulite6`](https://gitea.liria.cl/liria/dora-ulite6), reached through that node's
|
||||||
|
ordinary `jog` and `request` ports. The gamepad is
|
||||||
|
[`dora-joy`](https://gitea.liria.cl/liria/dora-joy), which publishes raw axes and buttons
|
||||||
|
on `joy` and knows nothing about arms. Do not add either here: a node whose job is policy
|
||||||
|
should not carry a hardware dependency, and a pad nobody else can reach is a pad that gets
|
||||||
|
reimplemented.
|
||||||
|
|
||||||
|
**The web console is the exception, and it is deliberate.** This node serves the page and
|
||||||
|
holds the browsers' sockets itself (`dora_teleop/server.py`), rather than going through
|
||||||
|
`dora-websocket-transceiver` as it did up to v0.2.0. A bridge node for one page meant a
|
||||||
|
second node, a second port and a round trip through the dataflow for every button press,
|
||||||
|
to carry a UI that only this node can interpret anyway. That is not the monolith
|
||||||
|
`dora-ulite6` was rewritten to escape — the monolith was one process owning the robot
|
||||||
|
driver, perception and the UI at once. What is forbidden is the hardware, and that has not
|
||||||
|
moved.
|
||||||
|
|
||||||
|
**What stays here is everything about what a reading *means***: the deadzone, the response
|
||||||
|
curve, the layout (which button is grasp), and the mapping to base-frame mm/s and deg/s.
|
||||||
|
`dora-joy` publishes raw SDL and nothing else, so there is exactly one place shaping each
|
||||||
|
number.
|
||||||
|
|
||||||
|
The web console it serves is ported from `Littlehand/monolitic-littlehand-python`
|
||||||
|
(`littlehand/console/static/`), and **only its Control section**. Perception, calibration,
|
||||||
|
tasks, chat, ROI, settings and health belong to other nodes; porting them here would
|
||||||
|
rebuild the monolith inside a node. That boundary is the one that matters, and the
|
||||||
|
built-in server does not move it: the console can ask for motion and nothing else.
|
||||||
|
|
||||||
|
## Build
|
||||||
|
|
||||||
|
```bash
|
||||||
|
uv sync # runtime deps
|
||||||
|
uv sync --group dev # plus pytest and ruff
|
||||||
|
```
|
||||||
|
|
||||||
|
## Run
|
||||||
|
|
||||||
|
```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:
|
||||||
|
```
|
||||||
|
|
||||||
|
`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`.
|
||||||
|
|
||||||
|
## Test
|
||||||
|
|
||||||
|
```bash
|
||||||
|
uv run pytest # 112 tests, no gamepad, arm, browser or daemon needed
|
||||||
|
uv run ruff check .
|
||||||
|
uv run ruff format --check .
|
||||||
|
```
|
||||||
|
|
||||||
|
## Architecture
|
||||||
|
|
||||||
|
Two things carry the design: **what may touch the hardware**, and **what must happen when
|
||||||
|
a source goes away**.
|
||||||
|
|
||||||
|
- **`dora_teleop/main.py`** — The dora event loop, and the one decision this node exists
|
||||||
|
to make: whether a velocity reaches the arm this tick, and from whom. **It never
|
||||||
|
blocks.** `_drive` and `_wanted_velocity` are the safety-critical pair — every path that
|
||||||
|
is not an owner actively asking for motion returns a stop, so a case nobody thought of
|
||||||
|
fails by halting rather than by carrying on. Keep it that way: the default has to be
|
||||||
|
"still".
|
||||||
|
- **`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.
|
||||||
|
**`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.
|
||||||
|
So silence past `JOY_TTL_S` reads as absent, which is the same answer. This works only
|
||||||
|
because `dora-joy` publishes on every tick whether or not the pad moved; if that ever
|
||||||
|
changes, this layer is gone and nothing here will say so.
|
||||||
|
- **`dora_teleop/message.py`** — The wire shapes, **declared and never inferred**.
|
||||||
|
pyarrow types a struct from its first row, so `{"value": None}` is `null` and
|
||||||
|
`{"value": 1.0}` is `double` — an inferred `command` would change its port's type
|
||||||
|
between one verb and the next.
|
||||||
|
- **`dora_teleop/mapping.py`** — Stick deflection to base-frame velocity, and the layout
|
||||||
|
that says which index is which. Pure functions, no state, no hardware, because this is
|
||||||
|
the part that has to be right and the cheapest to check. Output is always the arm node's
|
||||||
|
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.
|
||||||
|
- **`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
|
||||||
|
`t.owner === "web"` and draws an owner chip. A simpler last-writer-wins scheme would mean
|
||||||
|
rewriting the page to match the node rather than the other way round.
|
||||||
|
- **`dora_teleop/ui.py`** — Translation both ways. The console's telemetry keys are a
|
||||||
|
contract: `app.js` reads `arm_state`, `owner`, `estop`, `vacuum`, `freedrive`,
|
||||||
|
`position`, `joints` by name, so renaming one breaks the page silently. `arm_state()` is
|
||||||
|
where the arm's vocabulary (`connecting|ready|moving|recovering|unavailable`) is
|
||||||
|
reconciled with the console's (`idle|moving|jogging|freedrive|estop|stopped|disconnected`).
|
||||||
|
- **`dora_teleop/config.py`** — Every environment variable, parsed once at startup, so an
|
||||||
|
unusable setting stops the node before it joins the dataflow instead of surfacing the
|
||||||
|
first time somebody picks up the pad. Errors name the variable. It also refuses deadman
|
||||||
|
layers that are out of order, which is otherwise invisible until the day it matters.
|
||||||
|
- **`dora_teleop/log.py`** — Timestamped stderr logging. stderr because dora reads a
|
||||||
|
dynamic node's stdout.
|
||||||
|
- **`dora_teleop/server.py`** — The console's HTTP and WebSocket server, on a thread of
|
||||||
|
its own. Serves `dora_teleop/web/index.html` at `/` and holds the browsers at `/ws`, on
|
||||||
|
one port so the page finds its socket on its own origin. Both queues are bounded: a
|
||||||
|
flood of messages is dropped rather than grown, and a slow browser loses stale snapshots
|
||||||
|
rather than the newest one. **The browser's deadman lives here** — `clients` counts open
|
||||||
|
sockets, and the tick reads it.
|
||||||
|
- **`examples/bench.py`** — A stand-in for the arm, so `dataflow.yml`
|
||||||
|
is self-contained and CI can validate it with no sibling clones. **It is not a
|
||||||
|
simulator** and must not become one: it never integrates a velocity or moves a pose. Its
|
||||||
|
job is to close the loop so this node can be watched, not to stand in for
|
||||||
|
`dora-ulite6` — anything needing the arm to actually respond needs the arm.
|
||||||
|
- **`dora_teleop/probe.py`** — `dora-teleop-probe`: a **dynamic node** that attaches to a
|
||||||
|
running dataflow, reads the same `joy` stream this node reads, and shows the raw axes,
|
||||||
|
the buttons and the velocity that would be sent. It exists because the indices in
|
||||||
|
`mapping.py` are SDL's documented layout rather than an observed one, and a pad that
|
||||||
|
disagrees is otherwise found by watching a real arm move the wrong way. It goes through
|
||||||
|
the same `velocity()` the node runs, on the same readings, so what it prints is what the
|
||||||
|
arm would get — keep it that way, or it stops being evidence. `dataflow-gamepad.yml`
|
||||||
|
declares it as `path: dynamic`. For a pad whose layout is unknown, `dora-joy-probe`
|
||||||
|
needs no dataflow at all.
|
||||||
|
|
||||||
|
## The standard, and why it is one
|
||||||
|
|
||||||
|
This node publishes `twist` (SI) and `command` (robot-agnostic verbs), not one robot's
|
||||||
|
vocabulary. The contract is in the vault at `resources/robotics/teleop-contract.md`, which
|
||||||
|
is where it lives because no source package is shared between node repositories.
|
||||||
|
|
||||||
|
**Normalise first, scale once.** Every control source becomes a `[-1, 1]` six-vector
|
||||||
|
before anything is scaled, and the scaling happens in exactly one function. The deadzone
|
||||||
|
and the response curve are applied only to the gamepad, because they correct a worn
|
||||||
|
physical stick and a browser has none. Break this and the console silently acquires the
|
||||||
|
robot's units, which is what it used to have and the reason it fitted only one arm.
|
||||||
|
|
||||||
|
`jog` and `request` still carry the same intent in `dora-ulite6`'s units and service
|
||||||
|
names. They are deprecated, exist so the two repositories can move independently, and go
|
||||||
|
away once the arm node takes the standard ports. `LEGACY_REQUEST` in `main.py` is the only
|
||||||
|
place that knows what that node calls a verb.
|
||||||
|
|
||||||
|
`joint_jog` is specified in `dora-node.yml` and deliberately **not implemented**. A
|
||||||
|
joint-space robot needs IK and position integration against joint limits, neither of which
|
||||||
|
belongs here, and a port nothing wires reads as supported while having never run.
|
||||||
|
|
||||||
|
## Rules that are not negotiable
|
||||||
|
|
||||||
|
- **Send the change, not the state.** A velocity is a set-point the arm holds. `_drive`
|
||||||
|
sends when it changes and otherwise repeats on `JOG_KEEPALIVE_S`, which exists only to
|
||||||
|
beat the arm's own expiry. Repeating every tick made a real xArm re-plan the same motion
|
||||||
|
twenty times a second, which reads as stepping. Do not restore the every-tick re-send.
|
||||||
|
A change must never be made to wait for the repeat.
|
||||||
|
- **The stop is sent once, not every tick.** A stream of zero velocities keeps
|
||||||
|
`dora-ulite6` believing a jog is live and holds `robot_state` at `moving` forever.
|
||||||
|
- **E-STOP is never gated on the lease, and neither is its release.** From either
|
||||||
|
source, at any time. The latch (`view.estop`) is checked first in
|
||||||
|
`_wanted_velocity`, above the owner — a stop that the next stick movement overrides is
|
||||||
|
not a stop. `_clear` is the only way out and does both halves, this node's latch and the
|
||||||
|
controller's error; the pad's X and the console's RESET both go through it, so a
|
||||||
|
pad-only dataflow is not left with a latch nothing can release.
|
||||||
|
- **The deadman layers must stay in order**: `JOY_TTL_S` (0.2) < `JOG_TTL_S` (0.4) < the
|
||||||
|
arm node's `VELOCITY_COMMAND_TTL_S` (0.5). Each is the backstop for the one inside it,
|
||||||
|
and a backstop that fires first backs up nothing. `Config.from_env` enforces the first
|
||||||
|
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.
|
||||||
|
- **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.
|
||||||
|
- **Declare every published Arrow schema.** See `message.py`.
|
||||||
|
- **Do not add a robot driver or a gamepad driver here.** See above. In particular: no
|
||||||
|
`pygame`, no `evdev`, and no environment variable naming a device. The web server is
|
||||||
|
the one deliberate exception and is scoped to the Control console; anything the console
|
||||||
|
cannot ask the arm to do does not belong on it.
|
||||||
|
- **Nothing on the server thread may block the control loop.** `server.py` meets the loop
|
||||||
|
at two non-blocking points only — the queue the tick drains and the snapshot handed
|
||||||
|
back. A browser that stops reading must never stall the thread that stops the arm.
|
||||||
|
|
||||||
|
## What this node cannot do
|
||||||
|
|
||||||
|
The lease binds only what goes through this node. `dora-ulite6` has no concept of an owner,
|
||||||
|
so another node publishing `request` will preempt an operator mid-motion and nothing here
|
||||||
|
can prevent it. Say so rather than implying otherwise.
|
||||||
|
|
||||||
|
`jog` has no feedback channel at all — a malformed one is logged and dropped by the arm
|
||||||
|
node, with no response and no error output. What this node sends, it learns nothing back
|
||||||
|
about except through `robot_state` and telemetry.
|
||||||
|
|
||||||
|
## Two manifest rules worth knowing
|
||||||
|
|
||||||
|
`dora-node.yml` is the typed contract a consumer wires against, and the only one they get:
|
||||||
|
no source package is shared between node repositories. `dora validate` checks a dataflow
|
||||||
|
against it without running the node, so CI needs neither a gamepad nor a robot.
|
||||||
|
|
||||||
|
`joy`, `jog`, `request`, `response`, `motion_state` and the `ui_` ports are deliberately
|
||||||
|
**untyped**. dora treats a port carrying `request_id` as pattern-correlated and skips type
|
||||||
|
checks on it, several Arrow schemas legitimately flow over each, and dora 1.0 ships no
|
||||||
|
standard joystick type. Annotating them would claim a contract neither side keeps.
|
||||||
|
|
||||||
|
## Key APIs
|
||||||
|
|
||||||
|
- **dora Python API** (`dora`): `Node()`, `node.send_output(id, array, metadata)`, event
|
||||||
|
iteration via `for event in node`.
|
||||||
|
- **The `joy` message** (`dora-joy`): the Arrow struct
|
||||||
|
`{axes: list<float32>, buttons: list<bool>, present: bool}`, published on every tick.
|
||||||
|
Raw SDL — a stick centres near 0, but **a trigger rests at -1.0**. There is no header
|
||||||
|
and no timestamp: freshness is arrival time here.
|
||||||
|
|
||||||
|
## Tools
|
||||||
|
|
||||||
|
- **Issue tracker:** Gitea. Use the Gitea MCP tools (`mcp__gitea__*`) to read and manage
|
||||||
|
issues, pull requests, and other repository operations.
|
||||||
|
|
||||||
|
## Code style
|
||||||
|
|
||||||
|
- Write docstrings in NumPy style (summary line, then `Parameters`, `Returns`, `Raises`
|
||||||
|
sections as needed).
|
||||||
|
- Run `ruff check .` and `ruff format .` before committing. CI enforces both.
|
||||||
|
|
||||||
|
## Rules
|
||||||
|
|
||||||
|
- Do not mention AI tools in commit messages or documentation.
|
||||||
@@ -0,0 +1,433 @@
|
|||||||
|
# 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.
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
## 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.
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
| 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.
|
||||||
|
|
||||||
|
`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.
|
||||||
|
|
||||||
|
**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.
|
||||||
|
|
||||||
|
### Checking your pad
|
||||||
|
|
||||||
|
```bash
|
||||||
|
dora up
|
||||||
|
dora start dataflow-gamepad.yml --uv --name teleop-bench
|
||||||
|
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.
|
||||||
|
|
||||||
|
```text
|
||||||
|
axes (6 reported)
|
||||||
|
0 left stick X +0.600 ----------|-----#----
|
||||||
|
1 left stick Y -0.800 --#-------|----------
|
||||||
|
2 left trigger -1.000 #---------|----------
|
||||||
|
...
|
||||||
|
buttons (6 reported)
|
||||||
|
5:yaw right
|
||||||
|
|
||||||
|
velocity this would send to the arm
|
||||||
|
vx +38.89 mm/s forward / back <--
|
||||||
|
vy +23.53 mm/s left / right <--
|
||||||
|
vz +20.39 mm/s up / down <--
|
||||||
|
wz +30.00 deg/s yaw <--
|
||||||
|
|
||||||
|
moving: yes
|
||||||
|
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:
|
||||||
|
|
||||||
|
- **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.
|
||||||
|
|
||||||
|
## 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.
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
`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 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.
|
||||||
|
|
||||||
|
**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 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:
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
On shutdown the node sends one all-zero velocity before exiting. A motion lives in the
|
||||||
|
controller, not 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.
|
||||||
|
|
||||||
|
## 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 — see below |
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
## 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 |
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
`JOYSTICK_INDEX` moved to `dora-joy` along with the pad.
|
||||||
|
|
||||||
|
## 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://<the machine running teleop>: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.
|
||||||
|
|
||||||
|
```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-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 <name>` 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 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` 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.
|
||||||
|
|
||||||
|
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://<that machine>: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.
|
||||||
|
|
||||||
|
### 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`:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
dora up
|
||||||
|
dora start dataflow-gamepad.yml --uv --name teleop-bench
|
||||||
|
```
|
||||||
|
|
||||||
|
Then, in another terminal:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
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
|
||||||
|
```
|
||||||
|
|
||||||
|
`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.
|
||||||
|
|
||||||
|
**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.
|
||||||
|
|
||||||
|
| 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.
|
||||||
|
|
||||||
|
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 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.
|
||||||
|
|
||||||
|
## Development
|
||||||
|
|
||||||
|
```bash
|
||||||
|
uv sync --group dev
|
||||||
|
uv run pytest # 112 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`.
|
||||||
@@ -0,0 +1,85 @@
|
|||||||
|
# The same bench, with a real gamepad and a probe.
|
||||||
|
#
|
||||||
|
# `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.
|
||||||
|
#
|
||||||
|
# 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.
|
||||||
|
#
|
||||||
|
# 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:
|
||||||
|
#
|
||||||
|
# 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
|
||||||
|
|
||||||
|
strict_types: true
|
||||||
|
|
||||||
|
debug:
|
||||||
|
enable_debug_inspection: true
|
||||||
|
|
||||||
|
nodes:
|
||||||
|
- id: joy
|
||||||
|
git: https://gitea.liria.cl/liria/dora-joy.git
|
||||||
|
tag: v0.1.1
|
||||||
|
build: pip install .
|
||||||
|
path: dora-joy
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/20
|
||||||
|
outputs:
|
||||||
|
- joy
|
||||||
|
|
||||||
|
- id: teleop
|
||||||
|
path: dora-teleop
|
||||||
|
build: pip install .
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/50
|
||||||
|
# queue_size 1: joy ticks at 20 ms and this node at 50, so without it the
|
||||||
|
# loop would work through a backlog of readings that have already been
|
||||||
|
# overtaken. An arm should be driven by the newest one or by none.
|
||||||
|
joy:
|
||||||
|
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
|
||||||
|
outputs:
|
||||||
|
- twist
|
||||||
|
- command
|
||||||
|
- jog
|
||||||
|
- request
|
||||||
|
- ui_state
|
||||||
|
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.
|
||||||
|
WEB_PORT: "0"
|
||||||
|
|
||||||
|
# Declared, not spawned: `dora-teleop-probe` attaches to this id when you run
|
||||||
|
# it, and the dataflow runs perfectly well when nobody does.
|
||||||
|
- id: teleop-probe
|
||||||
|
path: dynamic
|
||||||
|
inputs:
|
||||||
|
joy: joy/joy
|
||||||
|
|
||||||
|
- id: bench
|
||||||
|
path: examples/bench.py
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/200
|
||||||
|
jog: teleop/jog
|
||||||
|
request: teleop/request
|
||||||
|
ui_state: teleop/ui_state
|
||||||
|
outputs:
|
||||||
|
- response
|
||||||
|
- robot_state
|
||||||
|
- tcp_pose
|
||||||
|
- joint_pose
|
||||||
@@ -0,0 +1,111 @@
|
|||||||
|
# Joystick control of a real arm. Three nodes, no bench and no browser.
|
||||||
|
#
|
||||||
|
# This is the one that moves a robot. `dataflow.yml` and `dataflow-gamepad.yml`
|
||||||
|
# drive a stand-in; this one drives `dora-ulite6`, so everything the pad does
|
||||||
|
# here happens in the room. Read `The gamepad` in README.md before picking it up
|
||||||
|
# — Back is the E-STOP.
|
||||||
|
#
|
||||||
|
# Both sibling nodes are pulled by `git:`, so this runs from a fresh clone with
|
||||||
|
# no other checkouts. It needs the network on its first build, and an arm.
|
||||||
|
#
|
||||||
|
# source .venv/bin/activate
|
||||||
|
# dora build dataflow-robot.yml
|
||||||
|
# dora run dataflow-robot.yml
|
||||||
|
#
|
||||||
|
# Not `--uv`: dora 1.0.1 installs a `git:` node into a venv inside its own clone and
|
||||||
|
# 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.
|
||||||
|
#
|
||||||
|
# The web console comes with the node: it serves the page and holds the
|
||||||
|
# browsers' sockets itself, so `http://<this machine>:7000` is a second way to
|
||||||
|
# drive the same arm, with the same lease deciding who gets it. Set WEB_PORT to
|
||||||
|
# 0 below for a pad-only run that opens no port at all.
|
||||||
|
|
||||||
|
strict_types: true
|
||||||
|
|
||||||
|
# Lets `dora topic echo` read what is on the wire — the velocities going to the
|
||||||
|
# arm, above all. Run under `dora start` rather than `dora run` to use it.
|
||||||
|
debug:
|
||||||
|
enable_debug_inspection: true
|
||||||
|
|
||||||
|
nodes:
|
||||||
|
- id: joy
|
||||||
|
git: https://gitea.liria.cl/liria/dora-joy.git
|
||||||
|
tag: v0.1.1
|
||||||
|
build: pip install .
|
||||||
|
path: dora-joy
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/20
|
||||||
|
outputs:
|
||||||
|
- joy
|
||||||
|
|
||||||
|
- id: teleop
|
||||||
|
path: dora-teleop
|
||||||
|
build: pip install .
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/50
|
||||||
|
# queue_size 1: joy ticks at 20 ms and this node at 50, so without it the
|
||||||
|
# loop would work through a backlog of readings that have already been
|
||||||
|
# overtaken. An arm should be driven by the newest one or by none.
|
||||||
|
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:
|
||||||
|
- twist
|
||||||
|
- command
|
||||||
|
- jog
|
||||||
|
- request
|
||||||
|
- ui_state
|
||||||
|
env:
|
||||||
|
SPEED_MM_S: "60"
|
||||||
|
SPEED_DEG_S: "30"
|
||||||
|
# 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"
|
||||||
|
# The console, on this machine. It is bound to every interface because
|
||||||
|
# the operator is rarely sitting at the box wired to the arm.
|
||||||
|
WEB_HOST: "0.0.0.0"
|
||||||
|
WEB_PORT: "7000"
|
||||||
|
|
||||||
|
- 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/100
|
||||||
|
request: teleop/request
|
||||||
|
jog: teleop/jog
|
||||||
|
outputs:
|
||||||
|
- response
|
||||||
|
- 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.192"
|
||||||
|
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"
|
||||||
@@ -0,0 +1,58 @@
|
|||||||
|
# The repo's own example, and what CI validates.
|
||||||
|
#
|
||||||
|
# Self-contained on purpose: teleop plus a bench stand-in for the arm and the
|
||||||
|
# browser, both built from this checkout, so `dora validate` resolves every
|
||||||
|
# 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`.
|
||||||
|
#
|
||||||
|
# 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
|
||||||
|
# consumer actually uses.
|
||||||
|
|
||||||
|
strict_types: true
|
||||||
|
|
||||||
|
# Lets `dora topic echo` and `dora topic hz` read what is on the wire, so the
|
||||||
|
# velocities this node sends can be watched without the bench printing them —
|
||||||
|
# and without a robot. Costs nothing when nobody is looking.
|
||||||
|
debug:
|
||||||
|
enable_debug_inspection: true
|
||||||
|
|
||||||
|
nodes:
|
||||||
|
- id: teleop
|
||||||
|
path: dora-teleop
|
||||||
|
build: pip install .
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/50
|
||||||
|
response: bench/response
|
||||||
|
robot_state: bench/robot_state
|
||||||
|
tcp_pose: bench/tcp_pose
|
||||||
|
joint_pose: bench/joint_pose
|
||||||
|
outputs:
|
||||||
|
- twist
|
||||||
|
- command
|
||||||
|
- jog
|
||||||
|
- request
|
||||||
|
- ui_state
|
||||||
|
env:
|
||||||
|
SPEED_MM_S: "60"
|
||||||
|
SPEED_DEG_S: "30"
|
||||||
|
# The console is served by this node. CI runs this dataflow, so it binds
|
||||||
|
# to localhost and stays off any interface a colleague could reach.
|
||||||
|
WEB_HOST: "127.0.0.1"
|
||||||
|
WEB_PORT: "7000"
|
||||||
|
|
||||||
|
- id: bench
|
||||||
|
path: examples/bench.py
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/200
|
||||||
|
jog: teleop/jog
|
||||||
|
request: teleop/request
|
||||||
|
ui_state: teleop/ui_state
|
||||||
|
outputs:
|
||||||
|
- response
|
||||||
|
- robot_state
|
||||||
|
- tcp_pose
|
||||||
|
- joint_pose
|
||||||
+257
@@ -0,0 +1,257 @@
|
|||||||
|
# Node manifest: the typed contract a consumer wires against.
|
||||||
|
#
|
||||||
|
# No source package is shared between node repositories, so this file and its
|
||||||
|
# type URNs are the only contract a consumer has. `dora validate` checks a
|
||||||
|
# dataflow against it without running the node, so CI needs neither a gamepad
|
||||||
|
# nor a robot.
|
||||||
|
#
|
||||||
|
# `joy`, `jog`, `request`, `response`, `motion_state` and `ui_state` are
|
||||||
|
# deliberately **untyped**. dora treats a port carrying `request_id` as
|
||||||
|
# pattern-correlated and skips type checks on it, several Arrow schemas
|
||||||
|
# legitimately flow over each — one per service, one per console message — and
|
||||||
|
# dora 1.0 ships no standard joystick type. Annotating them would claim a
|
||||||
|
# contract neither side keeps.
|
||||||
|
|
||||||
|
apiVersion: 1
|
||||||
|
name: dora-teleop
|
||||||
|
namespace: liria
|
||||||
|
description: Hands a robot arm to a human, from a gamepad or a browser
|
||||||
|
categories: [robot]
|
||||||
|
keywords: [teleop, gamepad, joystick, jog, arm, manipulator, web-console, operator]
|
||||||
|
|
||||||
|
runtime: python
|
||||||
|
entrypoint: dora-teleop
|
||||||
|
# `dora build --uv` / `dora run --uv` rewrites this to `uv pip install .` and
|
||||||
|
# gives the node its own managed venv under `.dora/python-envs/`, so the
|
||||||
|
# dependencies it sees at runtime are the ones installed here.
|
||||||
|
build: pip install .
|
||||||
|
dora: ">=1.0"
|
||||||
|
|
||||||
|
inputs:
|
||||||
|
tick:
|
||||||
|
description: >-
|
||||||
|
Drives everything time-based: noticing a changed velocity, repeating the
|
||||||
|
held one, expiring a
|
||||||
|
source that has gone quiet, and publishing the console's telemetry. Use 50
|
||||||
|
ms. Slower than about 100 ms and the arm node's own velocity expiry starts
|
||||||
|
firing between ticks, which an operator feels as the arm stuttering.
|
||||||
|
type: std/core/v1/UInt64
|
||||||
|
joy:
|
||||||
|
required: false
|
||||||
|
description: >-
|
||||||
|
One gamepad reading from `dora-joy`, as the Arrow struct
|
||||||
|
`{axes: list<float32>, buttons: list<bool>, present: bool}`. Raw SDL — the
|
||||||
|
deadzone, the response curve and the base-frame mapping all happen here.
|
||||||
|
Wire it with `queue_size: 1`: `dora-joy` ticks faster than this node does,
|
||||||
|
and a backlog of old readings is the last thing an arm should be driven
|
||||||
|
from.
|
||||||
|
|
||||||
|
**`present: false` stops the arm at once**, without waiting out any timer,
|
||||||
|
because an unplugged pad is not a centred pad. So does the stream going
|
||||||
|
quiet for longer than `JOY_TTL_S` — `dora-joy` publishes on every tick
|
||||||
|
whether or not the pad moved, so silence means that node died, and a dead
|
||||||
|
pad node is indistinguishable from an unplugged pad as far as the arm is
|
||||||
|
concerned.
|
||||||
|
|
||||||
|
Leave it unwired for a browser-only console. The node says so and starts.
|
||||||
|
response:
|
||||||
|
required: false
|
||||||
|
description: >-
|
||||||
|
An answer from the arm node, correlated by `request_id` in the metadata. A
|
||||||
|
failure is passed to the console as a toast; a `get_state` answer fills in
|
||||||
|
what telemetry does not carry.
|
||||||
|
motion_state:
|
||||||
|
required: false
|
||||||
|
description: >-
|
||||||
|
How a motion ended. Needed because this node sends motions with
|
||||||
|
`wait: false`, and a non-blocking motion reports its outcome here and
|
||||||
|
nowhere else.
|
||||||
|
robot_state:
|
||||||
|
required: false
|
||||||
|
description: >-
|
||||||
|
The arm's own state, one of `connecting`, `ready`, `moving`, `recovering`
|
||||||
|
or `unavailable`. Translated into the vocabulary the console draws.
|
||||||
|
type: std/core/v1/String
|
||||||
|
tcp_pose:
|
||||||
|
required: false
|
||||||
|
description: >-
|
||||||
|
Tool-tip pose, six float32 as `[x, y, z, roll, pitch, yaw]` in mm and
|
||||||
|
degrees, base frame. Shown in the console's pose readout.
|
||||||
|
type: std/core/v1/Float32
|
||||||
|
joint_pose:
|
||||||
|
required: false
|
||||||
|
description: >-
|
||||||
|
Joint angles, six float32 in degrees. Shown in the console's joint readout.
|
||||||
|
type: std/core/v1/Float32
|
||||||
|
|
||||||
|
outputs:
|
||||||
|
twist:
|
||||||
|
description: >-
|
||||||
|
**The standard motion port.** A velocity, as
|
||||||
|
`{linear: [x, y, z], angular: [x, y, z], frame}` in **SI** — metres per
|
||||||
|
second and radians per second — in the frame named by `frame`, currently
|
||||||
|
always `base`.
|
||||||
|
|
||||||
|
Sent when the velocity **changes**, and otherwise repeated every
|
||||||
|
`JOG_KEEPALIVE_S` so the consumer's own expiry never fires mid-motion — a
|
||||||
|
velocity is a set-point a consumer holds, not a frame in a sequence, and a
|
||||||
|
consumer that re-plans on every message moves in steps if it is fed one
|
||||||
|
every tick. A stop is a change: exactly one all-zero Twist, not repeatedly,
|
||||||
|
because a stream of zeroes keeps a consumer believing a jog is live. There
|
||||||
|
is no answer to a Twist, ever: it expires on the consumer's own TTL.
|
||||||
|
|
||||||
|
Both control sources are normalised to a fraction of full speed before
|
||||||
|
anything is scaled, so a gamepad and the web console asking for the same
|
||||||
|
deflection produce byte-identical Twists.
|
||||||
|
command:
|
||||||
|
description: >-
|
||||||
|
**The standard discrete port.** One thing to do, as `{action, value}`.
|
||||||
|
`action` is robot-agnostic — `gripper_open`, `gripper_close`, `stop`,
|
||||||
|
`home`, `clear`, `freedrive` — so a consumer that swaps a vacuum for a
|
||||||
|
two-finger gripper needs no change here. `value` carries the argument
|
||||||
|
where a verb has one (`freedrive` uses 1.0 and 0.0) and is null otherwise.
|
||||||
|
jog:
|
||||||
|
description: >-
|
||||||
|
**Deprecated; use `twist`.** The same motion in `dora-ulite6`'s units:
|
||||||
|
`{velocity: [vx, vy, vz, wx, wy, wz]}` in mm/s and deg/s, base frame.
|
||||||
|
Published alongside `twist` while that node learns the standard one.
|
||||||
|
|
||||||
|
Wire `twist` **or** `jog`, never both — they carry the same intent, and a
|
||||||
|
consumer reading both would be driven twice.
|
||||||
|
request:
|
||||||
|
description: >-
|
||||||
|
**Deprecated; use `command`.** The same verbs under the service names
|
||||||
|
`dora-ulite6` uses — `stop`, `grasp`, `release`, `clear`, `freedrive`,
|
||||||
|
`home` — with `request_id` in the metadata. `move_to` and `get_state` have
|
||||||
|
no standard equivalent yet and go only here.
|
||||||
|
ui_state:
|
||||||
|
description: >-
|
||||||
|
The console's telemetry snapshot, as a JSON string. Also carries error
|
||||||
|
toasts. Every key in it is read by name in the console's page, so the two
|
||||||
|
change together.
|
||||||
|
|
||||||
|
The browsers get this straight from the node's own server and do not read
|
||||||
|
this port; it is published for anyone else watching — `dora topic echo`,
|
||||||
|
a recorder, a second screen — and costs nothing when nobody is.
|
||||||
|
|
||||||
|
# Reserved, not implemented. A joint-space robot — a lerobot SO-101, say — takes
|
||||||
|
# joint targets rather than a Cartesian velocity, and no amount of scaling turns
|
||||||
|
# one into the other: it needs inverse kinematics, which is another node's job.
|
||||||
|
# When such a consumer exists, this is the port it wires, mirroring ROS's
|
||||||
|
# control_msgs/JointJog beside geometry_msgs/Twist:
|
||||||
|
#
|
||||||
|
# joint_jog:
|
||||||
|
# description: >-
|
||||||
|
# A joint-space velocity, as `{names: [str], velocities: [float]}`, the
|
||||||
|
# joints named rather than positional so a consumer cannot mistake the
|
||||||
|
# order.
|
||||||
|
#
|
||||||
|
# It is written down rather than built because a port nothing wires is worse
|
||||||
|
# than no port: it reads as supported and has never once run.
|
||||||
|
|
||||||
|
env:
|
||||||
|
SPEED_MM_S:
|
||||||
|
type: float
|
||||||
|
default: 60.0
|
||||||
|
description: >-
|
||||||
|
Translation speed at full stick deflection, in mm/s. A stick is analog, so
|
||||||
|
this is a maximum and anything less moves proportionally slower. Defaults
|
||||||
|
to 60.0.
|
||||||
|
SPEED_DEG_S:
|
||||||
|
type: float
|
||||||
|
default: 30.0
|
||||||
|
description: Rotation speed at full deflection, in deg/s. Defaults to 30.0.
|
||||||
|
DEADZONE:
|
||||||
|
type: float
|
||||||
|
default: 0.12
|
||||||
|
description: >-
|
||||||
|
Stick travel near centre to ignore, as a fraction. A worn stick that does
|
||||||
|
not centre would otherwise drive the arm on its own. What is left is
|
||||||
|
rescaled, so motion still starts from zero just outside the band.
|
||||||
|
Defaults to 0.12.
|
||||||
|
EXPO:
|
||||||
|
type: float
|
||||||
|
default: 0.4
|
||||||
|
description: >-
|
||||||
|
How much of the response curve is cubic, from 0 (linear) to 1. Fine
|
||||||
|
positioning happens near centre and a linear stick gives it almost no
|
||||||
|
resolution there. Defaults to 0.4.
|
||||||
|
JOG_TTL_S:
|
||||||
|
type: float
|
||||||
|
default: 0.4
|
||||||
|
description: >-
|
||||||
|
How long the owning source may go quiet before the arm is stopped. Keep it
|
||||||
|
**below** the arm node's VELOCITY_COMMAND_TTL_S (0.5 s by default), which
|
||||||
|
is the controller-side backstop behind this one. Defaults to 0.4.
|
||||||
|
JOG_KEEPALIVE_S:
|
||||||
|
type: float
|
||||||
|
default: 0.2
|
||||||
|
description: >-
|
||||||
|
How often to repeat a velocity that has not changed. Must be **above 0 and
|
||||||
|
below JOG_TTL_S**, which in turn sits below the arm's own
|
||||||
|
VELOCITY_COMMAND_TTL_S — so the repeat outpaces the expiry it exists to
|
||||||
|
beat. Raising it sends fewer messages and asks the controller to re-plan
|
||||||
|
less; too high and the arm stops between repeats. A change is never
|
||||||
|
delayed by it. Defaults to 0.2.
|
||||||
|
JOY_TTL_S:
|
||||||
|
type: float
|
||||||
|
default: 0.2
|
||||||
|
description: >-
|
||||||
|
How long the `joy` stream may go quiet before the pad reads as gone and the
|
||||||
|
arm is stopped. Must be **below** JOG_TTL_S. `dora-joy` publishes on every
|
||||||
|
tick, so this is really "how many missed readings to tolerate" — at a 20 ms
|
||||||
|
joy tick, the default is ten. Defaults to 0.2.
|
||||||
|
GAMEPAD_IDLE_RELEASE_S:
|
||||||
|
type: float
|
||||||
|
default: 2.0
|
||||||
|
description: >-
|
||||||
|
How long a pad may sit untouched before it hands the arm back, so a
|
||||||
|
browser can take over without anyone pressing anything. Defaults to 2.0.
|
||||||
|
STATE_POLL_S:
|
||||||
|
type: float
|
||||||
|
default: 0.5
|
||||||
|
description: >-
|
||||||
|
How often to call `get_state` for what telemetry does not carry: whether
|
||||||
|
the tool is engaged and whether freedrive is on. Defaults to 0.5.
|
||||||
|
WEB_HOST:
|
||||||
|
type: string
|
||||||
|
default: "0.0.0.0"
|
||||||
|
description: >-
|
||||||
|
The address the built-in web console binds to. Defaults to `0.0.0.0`,
|
||||||
|
because the operator is rarely sitting at the machine wired to the arm.
|
||||||
|
Set `127.0.0.1` where they are, or where the network is not yours.
|
||||||
|
WEB_PORT:
|
||||||
|
type: int
|
||||||
|
default: 7000
|
||||||
|
description: >-
|
||||||
|
The port the console is served on — the page and the browsers' WebSocket
|
||||||
|
both, so there is one URL and nothing to keep in step. **Set it to 0 to
|
||||||
|
open no port at all**, which is what a pad-only dataflow wants. Defaults
|
||||||
|
to 7000.
|
||||||
|
|
||||||
|
example: |
|
||||||
|
- 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:
|
||||||
|
- twist
|
||||||
|
- command
|
||||||
|
- jog
|
||||||
|
- request
|
||||||
|
- ui_state
|
||||||
|
env:
|
||||||
|
SPEED_MM_S: "60"
|
||||||
|
SPEED_DEG_S: "30"
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Human control of a robot arm, from a gamepad or a browser."""
|
||||||
@@ -0,0 +1,78 @@
|
|||||||
|
"""Who is allowed to drive the arm.
|
||||||
|
|
||||||
|
Two sources want one arm, and the arm cannot take both. This is the whole of
|
||||||
|
that decision, in one place, so there is exactly one answer to "why did it not
|
||||||
|
move" rather than one per input path.
|
||||||
|
|
||||||
|
The model comes from the monolithic repo's `RobotService` lease, and is kept
|
||||||
|
because the web console is built on it: its controls are gated on owning the
|
||||||
|
arm and it renders an owner chip. A simpler last-writer-wins scheme would mean
|
||||||
|
rewriting the console to match the node rather than the other way round.
|
||||||
|
|
||||||
|
The arm node itself has no idea any of this exists. A lease held here binds only
|
||||||
|
what goes through this node — another node publishing `request` to the arm will
|
||||||
|
preempt an operator mid-motion, and nothing here can stop it.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dora_teleop.log import log
|
||||||
|
|
||||||
|
GAMEPAD = "gamepad"
|
||||||
|
WEB = "web"
|
||||||
|
|
||||||
|
|
||||||
|
class Arbiter:
|
||||||
|
"""The lease on the arm, and the rules for handing it between sources."""
|
||||||
|
|
||||||
|
def __init__(self, idle_release_s: float):
|
||||||
|
self.idle_release_s = idle_release_s
|
||||||
|
self._owner: str | None = None
|
||||||
|
self._last_activity = 0.0
|
||||||
|
|
||||||
|
@property
|
||||||
|
def owner(self) -> str | None:
|
||||||
|
"""Who holds the arm, or None if it is free."""
|
||||||
|
return self._owner
|
||||||
|
|
||||||
|
def take(self, who: str, now: float, preempt: bool = False) -> bool:
|
||||||
|
"""Give ``who`` the arm if it is theirs to take, and say whether it worked.
|
||||||
|
|
||||||
|
A free arm goes to whoever asks. A held one stays held unless the caller
|
||||||
|
asked to preempt, which the console's TAKE CONTROL button offers
|
||||||
|
deliberately: an operator who can see the arm is better placed than this
|
||||||
|
code to decide they should have it.
|
||||||
|
"""
|
||||||
|
if self._owner not in (None, who) and not preempt:
|
||||||
|
return False
|
||||||
|
if self._owner != who:
|
||||||
|
log(f"{who} has the arm" + (f", preempting {self._owner}" if self._owner else ""))
|
||||||
|
self._owner = who
|
||||||
|
self._last_activity = now
|
||||||
|
return True
|
||||||
|
|
||||||
|
def release(self, who: str) -> None:
|
||||||
|
"""Give the arm back, if this source had it."""
|
||||||
|
if self._owner == who:
|
||||||
|
log(f"{who} released the arm")
|
||||||
|
self._owner = None
|
||||||
|
|
||||||
|
def note_activity(self, who: str, now: float) -> None:
|
||||||
|
"""Record that the holder is still there, so the idle timer does not fire."""
|
||||||
|
if self._owner == who:
|
||||||
|
self._last_activity = now
|
||||||
|
|
||||||
|
def expire_idle_gamepad(self, now: float) -> None:
|
||||||
|
"""Hand the arm back after the pad has sat untouched long enough.
|
||||||
|
|
||||||
|
Only the gamepad expires. It takes the arm implicitly, on the first
|
||||||
|
nudge of a stick, so it has to give it back the same way or a pad left
|
||||||
|
on the bench would lock the browser out forever. The web takes the arm
|
||||||
|
by pressing a button and gives it back by pressing another.
|
||||||
|
"""
|
||||||
|
if self._owner != GAMEPAD:
|
||||||
|
return
|
||||||
|
if now - self._last_activity >= self.idle_release_s:
|
||||||
|
self.release(GAMEPAD)
|
||||||
|
|
||||||
|
def may_drive(self, who: str) -> bool:
|
||||||
|
"""Whether this source's jog should reach the arm."""
|
||||||
|
return self._owner == who
|
||||||
@@ -0,0 +1,164 @@
|
|||||||
|
"""Everything the node reads from the environment, parsed once at startup.
|
||||||
|
|
||||||
|
Parsing happens here rather than where each value is used, so an unusable
|
||||||
|
setting stops the node before it joins the dataflow instead of surfacing the
|
||||||
|
first time somebody picks up the gamepad.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import os
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
|
||||||
|
|
||||||
|
def env_float(name: str, default: float) -> float:
|
||||||
|
"""Read a number, naming the variable if it cannot be read as one."""
|
||||||
|
value = os.environ.get(name, "").strip()
|
||||||
|
if not value:
|
||||||
|
return default
|
||||||
|
try:
|
||||||
|
return float(value)
|
||||||
|
except ValueError:
|
||||||
|
raise ValueError(f"{name} must be a number, not {value!r}") from None
|
||||||
|
|
||||||
|
|
||||||
|
def env_int(name: str, default: int) -> int:
|
||||||
|
"""Read a whole number, naming the variable if it cannot be read as one."""
|
||||||
|
value = os.environ.get(name, "").strip()
|
||||||
|
if not value:
|
||||||
|
return default
|
||||||
|
try:
|
||||||
|
return int(value)
|
||||||
|
except ValueError:
|
||||||
|
raise ValueError(f"{name} must be a whole number, not {value!r}") from None
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class ConsoleConfig:
|
||||||
|
"""Where the built-in web console listens.
|
||||||
|
|
||||||
|
The node serves the page and holds the browsers' sockets itself, so there is
|
||||||
|
one address and one port rather than a bridge node and two. `0.0.0.0` is the
|
||||||
|
default because the operator is rarely sitting at the machine driving the
|
||||||
|
arm; bind it to `127.0.0.1` where that is not true.
|
||||||
|
"""
|
||||||
|
|
||||||
|
host: str = "0.0.0.0"
|
||||||
|
port: int = 7000
|
||||||
|
|
||||||
|
@property
|
||||||
|
def enabled(self) -> bool:
|
||||||
|
"""Whether to open the port at all. Port 0 means a pad-only dataflow."""
|
||||||
|
return self.port != 0
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_env(cls) -> "ConsoleConfig":
|
||||||
|
"""Build the console's address from the environment."""
|
||||||
|
port = env_int("WEB_PORT", 7000)
|
||||||
|
if not 0 <= port <= 65535:
|
||||||
|
raise ValueError(f"WEB_PORT must be a port number, not {port}")
|
||||||
|
return cls(host=os.environ.get("WEB_HOST", "").strip() or "0.0.0.0", port=port)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class DeadmanConfig:
|
||||||
|
"""How long a source may go quiet before the arm is told to stop.
|
||||||
|
|
||||||
|
Four layers, outermost last. `joy_ttl_s` is how long a pad reading stays
|
||||||
|
worth acting on; `jog_keepalive_s` is how often an unchanged velocity is
|
||||||
|
repeated; `jog_ttl_s` is how long this node keeps driving without hearing
|
||||||
|
from the source; and behind all of them sits the arm node's own
|
||||||
|
`VELOCITY_COMMAND_TTL_S`, enforced there whether or not this process is
|
||||||
|
alive.
|
||||||
|
|
||||||
|
Keep them in that order — each shorter than the one behind it — or a
|
||||||
|
backstop fires before the thing it is backing up.
|
||||||
|
"""
|
||||||
|
|
||||||
|
jog_ttl_s: float = 0.4
|
||||||
|
# How often to repeat a velocity that has not changed. A velocity is a
|
||||||
|
# set-point, not a sequence: the arm holds the last one until its own expiry
|
||||||
|
# or a new one, so repeating it 20 times a second only makes the controller
|
||||||
|
# re-plan the same motion 20 times, which an operator feels as stepping.
|
||||||
|
# Low enough that the arm's VELOCITY_COMMAND_TTL_S never fires mid-motion,
|
||||||
|
# and no lower. A change — a new direction, or the stop — never waits for it.
|
||||||
|
#
|
||||||
|
# It is checked on the tick, so the period actually achieved is this rounded
|
||||||
|
# *up* to the next tick: 0.2 with a 50 ms tick repeats every 250 ms, because
|
||||||
|
# the tick at 200 ms lands a hair early and does not qualify. Budget against
|
||||||
|
# that number, not this one.
|
||||||
|
jog_keepalive_s: float = 0.2
|
||||||
|
# How long the `joy` stream may go quiet before the pad reads as gone. The
|
||||||
|
# pad lives in another node now, and that node dying looks like silence
|
||||||
|
# rather than like an unplugged pad, so silence has to mean the same thing.
|
||||||
|
# `dora-joy` publishes every tick, so it means nothing else.
|
||||||
|
joy_ttl_s: float = 0.2
|
||||||
|
# How long a gamepad may sit untouched before it gives the arm back, so the
|
||||||
|
# browser can take over without anyone having to press anything.
|
||||||
|
gamepad_idle_release_s: float = 2.0
|
||||||
|
# How often to ask the arm for the things telemetry does not carry: whether
|
||||||
|
# the tool is engaged, whether freedrive is on, whether it is ready.
|
||||||
|
state_poll_s: float = 0.5
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_env(cls) -> "DeadmanConfig":
|
||||||
|
"""Build the timings from the environment."""
|
||||||
|
return cls(
|
||||||
|
jog_ttl_s=env_float("JOG_TTL_S", 0.4),
|
||||||
|
jog_keepalive_s=env_float("JOG_KEEPALIVE_S", 0.2),
|
||||||
|
joy_ttl_s=env_float("JOY_TTL_S", 0.2),
|
||||||
|
gamepad_idle_release_s=env_float("GAMEPAD_IDLE_RELEASE_S", 2.0),
|
||||||
|
state_poll_s=env_float("STATE_POLL_S", 0.5),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class Config:
|
||||||
|
"""The node's settings."""
|
||||||
|
|
||||||
|
# Full-deflection speeds. Both sources are analog — a gamepad stick and the
|
||||||
|
# console's on-screen ones — so these are the maximum and anything less
|
||||||
|
# moves proportionally slower.
|
||||||
|
speed_mm_s: float
|
||||||
|
speed_deg_s: float
|
||||||
|
# Stick travel to ignore. A worn stick that does not centre would otherwise
|
||||||
|
# drive the arm across the table on its own.
|
||||||
|
deadzone: float
|
||||||
|
# How much of the curve is cubic: 0 is linear, 1 is fully cubic. Fine
|
||||||
|
# positioning happens near centre, and expo is what buys resolution there.
|
||||||
|
expo: float
|
||||||
|
deadman: DeadmanConfig = field(default_factory=DeadmanConfig)
|
||||||
|
console: ConsoleConfig = field(default_factory=ConsoleConfig)
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_env(cls) -> "Config":
|
||||||
|
"""Build the configuration from the environment, refusing anything unusable."""
|
||||||
|
deadzone = env_float("DEADZONE", 0.12)
|
||||||
|
if not 0.0 <= deadzone < 1.0:
|
||||||
|
raise ValueError(f"DEADZONE must be at least 0 and below 1, not {deadzone}")
|
||||||
|
expo = env_float("EXPO", 0.4)
|
||||||
|
if not 0.0 <= expo <= 1.0:
|
||||||
|
raise ValueError(f"EXPO must be between 0 and 1, not {expo}")
|
||||||
|
deadman = DeadmanConfig.from_env()
|
||||||
|
# The layers have to fire outermost last, and each one is a backstop for
|
||||||
|
# the one inside it. Behind both of these sits the arm node's own
|
||||||
|
# VELOCITY_COMMAND_TTL_S, which no setting here can see.
|
||||||
|
if deadman.joy_ttl_s >= deadman.jog_ttl_s:
|
||||||
|
raise ValueError(
|
||||||
|
f"JOY_TTL_S must be below JOG_TTL_S ({deadman.jog_ttl_s}), not {deadman.joy_ttl_s}"
|
||||||
|
)
|
||||||
|
# The repeat has to be quicker than the arm's own velocity expiry or the
|
||||||
|
# arm stops between repeats. That expiry cannot be read from here, but
|
||||||
|
# JOG_TTL_S is already required to sit below it, so staying under
|
||||||
|
# JOG_TTL_S keeps the repeat under both.
|
||||||
|
if not 0.0 < deadman.jog_keepalive_s < deadman.jog_ttl_s:
|
||||||
|
raise ValueError(
|
||||||
|
f"JOG_KEEPALIVE_S must be above 0 and below JOG_TTL_S ({deadman.jog_ttl_s}), "
|
||||||
|
f"not {deadman.jog_keepalive_s}"
|
||||||
|
)
|
||||||
|
return cls(
|
||||||
|
speed_mm_s=env_float("SPEED_MM_S", 60.0),
|
||||||
|
speed_deg_s=env_float("SPEED_DEG_S", 30.0),
|
||||||
|
deadzone=deadzone,
|
||||||
|
expo=expo,
|
||||||
|
deadman=deadman,
|
||||||
|
console=ConsoleConfig.from_env(),
|
||||||
|
)
|
||||||
@@ -0,0 +1,83 @@
|
|||||||
|
"""The gamepad, as it arrives from `dora-joy`.
|
||||||
|
|
||||||
|
The pad itself lives in another node now. What reaches this one is a plain
|
||||||
|
reading on the `joy` input, and this module is the seam the loop reads it
|
||||||
|
through: :class:`Sample` is what the rest of the node sees, exactly as it did
|
||||||
|
when the pad was read in-process.
|
||||||
|
|
||||||
|
**The safety-critical part is :meth:`JoyInput.read`.** 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. 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.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import time
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class Sample:
|
||||||
|
"""One reading of the pad.
|
||||||
|
|
||||||
|
``present`` is the one that matters for safety: a pad that has been
|
||||||
|
unplugged reads as absent rather than as centred, so the difference between
|
||||||
|
"the operator let go" and "the operator's pad fell off the desk" is not lost.
|
||||||
|
"""
|
||||||
|
|
||||||
|
axes: list[float] = field(default_factory=list)
|
||||||
|
buttons: list[bool] = field(default_factory=list)
|
||||||
|
present: bool = False
|
||||||
|
|
||||||
|
|
||||||
|
ABSENT = Sample()
|
||||||
|
|
||||||
|
|
||||||
|
def decode(event: dict) -> Sample:
|
||||||
|
"""Read one `joy` message into a reading.
|
||||||
|
|
||||||
|
A message this node cannot make sense of reads as absent rather than as
|
||||||
|
centred, for the same reason an unplugged pad does.
|
||||||
|
"""
|
||||||
|
value = event["value"]
|
||||||
|
if not len(value):
|
||||||
|
return ABSENT
|
||||||
|
row = value[0].as_py()
|
||||||
|
return Sample(
|
||||||
|
axes=list(row.get("axes") or []),
|
||||||
|
buttons=[bool(down) for down in row.get("buttons") or []],
|
||||||
|
present=bool(row.get("present")),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class JoyInput:
|
||||||
|
"""The latest reading, and how long ago it arrived."""
|
||||||
|
|
||||||
|
def __init__(self, ttl_s: float):
|
||||||
|
self.ttl_s = ttl_s
|
||||||
|
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
|
||||||
|
self._seen_at = moment
|
||||||
|
|
||||||
|
def read(self, moment: float) -> Sample:
|
||||||
|
"""Return the current reading, or absence if nobody is still sending one.
|
||||||
|
|
||||||
|
A stale reading is not a reading. `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 is concerned — which is the only safe
|
||||||
|
way to read it.
|
||||||
|
"""
|
||||||
|
if moment - self._seen_at > self.ttl_s:
|
||||||
|
return ABSENT
|
||||||
|
return self._sample
|
||||||
|
|
||||||
|
|
||||||
|
def now() -> float:
|
||||||
|
"""Return the clock the deadman runs on: monotonic, so it cannot step backwards."""
|
||||||
|
return time.monotonic()
|
||||||
@@ -0,0 +1,13 @@
|
|||||||
|
"""Timestamped stderr logging.
|
||||||
|
|
||||||
|
stderr rather than stdout: dora reads a dynamic node's stdout, and a print that
|
||||||
|
lands there is noise in the wrong stream.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
|
||||||
|
def log(message: str) -> None:
|
||||||
|
"""Print a timestamped line for the node."""
|
||||||
|
print(f"[teleop {time.strftime('%H:%M:%S')}] {message}", file=sys.stderr, flush=True)
|
||||||
@@ -0,0 +1,482 @@
|
|||||||
|
"""The dora event loop: routing, the deadman, and nothing that can block.
|
||||||
|
|
||||||
|
The loop owns the one decision this node exists to make — whether a velocity
|
||||||
|
reaches the arm this tick, and from whom. Everything else is delegated:
|
||||||
|
:mod:`dora_teleop.joy` takes the pad off the wire, :mod:`dora_teleop.mapping`
|
||||||
|
turns a reading into a velocity, :mod:`dora_teleop.arbiter` says who is allowed
|
||||||
|
to, :mod:`dora_teleop.ui` speaks the console's language and
|
||||||
|
:mod:`dora_teleop.server` serves it. **No hardware is touched here at all** —
|
||||||
|
the pad is `dora-joy` and the arm is `dora-ulite6`.
|
||||||
|
|
||||||
|
**The safety-critical part is `_drive`.** An arm moving under a velocity command
|
||||||
|
keeps moving until told otherwise, so the question is never "should it start"
|
||||||
|
but "has anything happened that means it must stop". Three things mean that: the
|
||||||
|
source went quiet, the source went away, or this process is going away. All
|
||||||
|
three end in the same place — one all-zero velocity, sent once.
|
||||||
|
|
||||||
|
What goes out is the *change*: a velocity is a set-point the arm holds, not a
|
||||||
|
frame in a sequence, so an unchanged one is repeated only as often as the arm's
|
||||||
|
own expiry demands. A change — a new direction, or the stop — is never made to
|
||||||
|
wait for that timer.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import time
|
||||||
|
|
||||||
|
import pyarrow as pa
|
||||||
|
from dora import Node
|
||||||
|
|
||||||
|
from dora_teleop import joy, mapping, message, server, ui
|
||||||
|
from dora_teleop.arbiter import GAMEPAD, WEB, Arbiter
|
||||||
|
from dora_teleop.config import Config
|
||||||
|
from dora_teleop.joy import JoyInput, now
|
||||||
|
from dora_teleop.log import log
|
||||||
|
from dora_teleop.mapping import LAYOUT, pressed
|
||||||
|
|
||||||
|
REQUEST_ID = "request_id"
|
||||||
|
# Still, in the normalised units both sources speak.
|
||||||
|
STOP_AXES = [0.0] * 6
|
||||||
|
|
||||||
|
# The verbs this node publishes on `command`. Robot-agnostic on purpose: a
|
||||||
|
# consumer that grows a two-finger gripper where the last one had a vacuum
|
||||||
|
# should need no change here, and neither should this node.
|
||||||
|
GRIPPER_CLOSE = "gripper_close"
|
||||||
|
GRIPPER_OPEN = "gripper_open"
|
||||||
|
STOP = "stop"
|
||||||
|
HOME = "home"
|
||||||
|
CLEAR = "clear"
|
||||||
|
FREEDRIVE = "freedrive"
|
||||||
|
|
||||||
|
# The same verbs as `dora-ulite6` names them, for the deprecated `request` port.
|
||||||
|
# This table is the whole of what used to be spread through the loop, and it is
|
||||||
|
# what a robot speaking the standard lets us delete.
|
||||||
|
LEGACY_REQUEST = {
|
||||||
|
GRIPPER_CLOSE: {"service": "grasp"},
|
||||||
|
GRIPPER_OPEN: {"service": "release"},
|
||||||
|
STOP: {"service": "stop"},
|
||||||
|
CLEAR: {"service": "clear"},
|
||||||
|
HOME: {"service": "home", "wait": False},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class Teleop:
|
||||||
|
"""Everything the loop remembers between ticks."""
|
||||||
|
|
||||||
|
def __init__(self, config: Config):
|
||||||
|
self.config = config
|
||||||
|
self.arbiter = Arbiter(config.deadman.gamepad_idle_release_s)
|
||||||
|
self.joy = JoyInput(config.deadman.joy_ttl_s)
|
||||||
|
self.view = ui.ArmView()
|
||||||
|
# What the browser last asked for, and when. A jog is a stream; what
|
||||||
|
# matters is whether the stream is still arriving. Normalised, like the
|
||||||
|
# pad's — the console does not know the robot's units.
|
||||||
|
self.web_axes = STOP_AXES
|
||||||
|
self.web_seen_at = 0.0
|
||||||
|
# How many browsers are on the console right now, read from the server
|
||||||
|
# every tick. The last one leaving has to stop the arm, and a socket
|
||||||
|
# that went away sends nothing to say so.
|
||||||
|
self.clients = 0
|
||||||
|
self.console: server.Console | None = None
|
||||||
|
# Whether the arm is currently being driven by us. Used to send the stop
|
||||||
|
# once rather than every tick: a stream of zeroes would keep the arm
|
||||||
|
# node believing a jog is live and hold `robot_state` at `moving`.
|
||||||
|
self.jogging = False
|
||||||
|
# The last velocity actually put on the wire, and when. `_drive` sends
|
||||||
|
# only when what it wants differs from this, so the arm is told about
|
||||||
|
# changes rather than told the same thing 20 times a second.
|
||||||
|
self.sent_axes = STOP_AXES
|
||||||
|
self.sent_at = 0.0
|
||||||
|
self.buttons: list[bool] = []
|
||||||
|
self.requests = 0
|
||||||
|
self.polled_at = 0.0
|
||||||
|
|
||||||
|
def next_request_id(self, service: str) -> str:
|
||||||
|
"""Mint a correlation id we will recognise coming back."""
|
||||||
|
self.requests += 1
|
||||||
|
return f"teleop-{service}-{self.requests}"
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
"""Connect to the dataflow and run until dora stops us."""
|
||||||
|
args = _parse_args()
|
||||||
|
config = Config.from_env()
|
||||||
|
|
||||||
|
# Passing the name lets the node also run standalone as a dynamic node; when
|
||||||
|
# dora spawns it, the id from the dataflow wins.
|
||||||
|
node = Node(args.name)
|
||||||
|
state = Teleop(config)
|
||||||
|
if config.console.enabled:
|
||||||
|
state.console = server.Console(config.console.host, config.console.port)
|
||||||
|
state.console.start()
|
||||||
|
|
||||||
|
try:
|
||||||
|
for event in node:
|
||||||
|
if event["type"] == "ERROR":
|
||||||
|
raise RuntimeError(event["error"])
|
||||||
|
if event["type"] != "INPUT":
|
||||||
|
continue
|
||||||
|
_handle(node, state, event)
|
||||||
|
finally:
|
||||||
|
# A motion lives in the arm's controller, not in this process. Leaving
|
||||||
|
# without zeroing the velocity leaves it travelling until the
|
||||||
|
# controller's own expiry catches up, which is the one moment nobody is
|
||||||
|
# watching for it.
|
||||||
|
_send_motion(node, state, STOP_AXES)
|
||||||
|
if state.console is not None:
|
||||||
|
state.console.stop()
|
||||||
|
|
||||||
|
|
||||||
|
def _parse_args() -> argparse.Namespace:
|
||||||
|
"""Read the node name, for when dora did not spawn us."""
|
||||||
|
parser = argparse.ArgumentParser(description="Drive a robot arm from a gamepad or a browser.")
|
||||||
|
parser.add_argument(
|
||||||
|
"--name",
|
||||||
|
default="teleop",
|
||||||
|
help="The name of the node in the dataflow, used when running as a dynamic node.",
|
||||||
|
)
|
||||||
|
return parser.parse_args()
|
||||||
|
|
||||||
|
|
||||||
|
def _handle(node: Node, state: Teleop, event: dict) -> None:
|
||||||
|
"""Route one input to whatever deals with it."""
|
||||||
|
event_id = event["id"]
|
||||||
|
if event_id == "tick":
|
||||||
|
_tick(node, state)
|
||||||
|
elif event_id == "joy":
|
||||||
|
state.joy.accept(joy.decode(event), now())
|
||||||
|
elif event_id == "robot_state":
|
||||||
|
state.view.state = event["value"][0].as_py()
|
||||||
|
elif event_id == "tcp_pose":
|
||||||
|
state.view.pose = event["value"].to_pylist()
|
||||||
|
elif event_id == "joint_pose":
|
||||||
|
state.view.joints = event["value"].to_pylist()
|
||||||
|
elif event_id == "response":
|
||||||
|
_accept_response(node, state, event)
|
||||||
|
elif event_id == "motion_state":
|
||||||
|
_accept_motion_state(node, state, event)
|
||||||
|
else:
|
||||||
|
log(f"ignoring unexpected input `{event_id}`")
|
||||||
|
|
||||||
|
|
||||||
|
# --- the tick, where the arm is actually driven ---
|
||||||
|
|
||||||
|
|
||||||
|
def _tick(node: Node, state: Teleop) -> None:
|
||||||
|
"""Decide what the arm should be doing, and say so."""
|
||||||
|
moment = now()
|
||||||
|
# Before anything else: what the browser asked for since the last tick is
|
||||||
|
# part of this tick's picture, and the client count it implies is what the
|
||||||
|
# web deadman reads below.
|
||||||
|
_drain_console(node, state)
|
||||||
|
# Absent covers three things now: no pad plugged in, a pad unplugged
|
||||||
|
# mid-jog, and `dora-joy` no longer sending. The arm cannot tell them apart
|
||||||
|
# and does not need to.
|
||||||
|
sample = state.joy.read(moment)
|
||||||
|
|
||||||
|
# Who owns the arm is settled before anything they pressed is acted on.
|
||||||
|
# The other order drops the first press after the pad has been idle: the
|
||||||
|
# lease is not held yet when the buttons are read, and by the next tick the
|
||||||
|
# button is held rather than newly down, so the edge is gone for good.
|
||||||
|
#
|
||||||
|
# A pad taking the arm the moment a stick moves is what keeps an operator
|
||||||
|
# from having to reach for a browser to use the thing in their hands.
|
||||||
|
active = mapping.is_active(sample)
|
||||||
|
if active and state.arbiter.owner is None:
|
||||||
|
state.arbiter.take(GAMEPAD, moment)
|
||||||
|
if active:
|
||||||
|
state.arbiter.note_activity(GAMEPAD, moment)
|
||||||
|
state.arbiter.expire_idle_gamepad(moment)
|
||||||
|
|
||||||
|
_read_buttons(node, state, sample)
|
||||||
|
|
||||||
|
_drive(node, state, sample, moment)
|
||||||
|
_poll_state(node, state, moment)
|
||||||
|
_publish(node, state)
|
||||||
|
|
||||||
|
|
||||||
|
def _drive(node: Node, state: Teleop, sample, moment: float) -> None:
|
||||||
|
"""Send the velocity the owner is asking for, or the stop that ends it.
|
||||||
|
|
||||||
|
Changes go out at once; an unchanged velocity is repeated only on
|
||||||
|
`jog_keepalive_s`, which exists solely so the arm's own velocity expiry
|
||||||
|
never fires mid-motion. Repeating it every tick instead made the controller
|
||||||
|
re-plan the same motion twenty times a second, which an operator feels as
|
||||||
|
the arm moving in steps rather than travelling.
|
||||||
|
|
||||||
|
The stop is a change like any other, so releasing a button is acted on the
|
||||||
|
tick it happens and never waits for the repeat.
|
||||||
|
"""
|
||||||
|
axes = _wanted_axes(state, sample, moment)
|
||||||
|
|
||||||
|
if axes != state.sent_axes:
|
||||||
|
# Includes the stop: going to all-zero is a change, sent once, and the
|
||||||
|
# `jogging` flag then keeps it from being repeated. A stream of zeroes
|
||||||
|
# would hold the arm node's `robot_state` at `moving` forever.
|
||||||
|
state.jogging = mapping.is_moving(axes)
|
||||||
|
_send_motion(node, state, axes)
|
||||||
|
return
|
||||||
|
|
||||||
|
if state.jogging and moment - state.sent_at >= state.config.deadman.jog_keepalive_s:
|
||||||
|
_send_motion(node, state, axes)
|
||||||
|
|
||||||
|
|
||||||
|
def _wanted_axes(state: Teleop, sample, moment: float) -> list[float]:
|
||||||
|
"""Return what the robot should be doing, normalised, which is usually nothing.
|
||||||
|
|
||||||
|
Every path that is not an owner actively asking for motion returns a stop.
|
||||||
|
That is deliberate: the default has to be "still", so that a case nobody
|
||||||
|
thought of fails by halting rather than by carrying on.
|
||||||
|
"""
|
||||||
|
# First, and above the lease. A stop somebody had to reach for is not undone
|
||||||
|
# by the next thumb movement: it holds until it is deliberately cleared.
|
||||||
|
if state.view.estop:
|
||||||
|
return STOP_AXES
|
||||||
|
|
||||||
|
owner = state.arbiter.owner
|
||||||
|
if owner == GAMEPAD:
|
||||||
|
# An unplugged pad is not a centred pad. Stopping here rather than
|
||||||
|
# waiting out the TTL is the difference between a dropped controller
|
||||||
|
# costing 10 ms of travel and costing 400.
|
||||||
|
if not sample.present:
|
||||||
|
return STOP_AXES
|
||||||
|
return mapping.from_pad(sample.axes, sample.buttons, state.config)
|
||||||
|
|
||||||
|
if owner == WEB:
|
||||||
|
# The console refreshes its jog every 150 ms; the browser going quiet,
|
||||||
|
# or the tab closing, has to stop the arm without anything being sent.
|
||||||
|
if state.clients == 0:
|
||||||
|
return STOP_AXES
|
||||||
|
if moment - state.web_seen_at > state.config.deadman.jog_ttl_s:
|
||||||
|
return STOP_AXES
|
||||||
|
return state.web_axes
|
||||||
|
|
||||||
|
return STOP_AXES
|
||||||
|
|
||||||
|
|
||||||
|
def _read_buttons(node: Node, state: Teleop, sample) -> None:
|
||||||
|
"""Fire the services a gamepad button asks for, on the press only.
|
||||||
|
|
||||||
|
Edge-triggered, because these are one-shot requests rather than a held
|
||||||
|
velocity: holding `grasp` should not send a request every 50 ms.
|
||||||
|
"""
|
||||||
|
previous, state.buttons = state.buttons, list(sample.buttons)
|
||||||
|
if not sample.present:
|
||||||
|
return
|
||||||
|
|
||||||
|
def tapped(index: int) -> bool:
|
||||||
|
return pressed(sample.buttons, index) and not pressed(previous, index)
|
||||||
|
|
||||||
|
# Neither the stop nor its release is gated on the lease. Somebody reaching
|
||||||
|
# for the stop is the one moment when asking who owns the arm is the wrong
|
||||||
|
# question, and a latch that cannot be released without first taking the arm
|
||||||
|
# back is a trap — an idle pad hands the lease away two seconds after the
|
||||||
|
# stop it just made. The console's RESET is ungated for the same reason.
|
||||||
|
if tapped(LAYOUT.estop):
|
||||||
|
_estop(node, state)
|
||||||
|
if tapped(LAYOUT.clear):
|
||||||
|
_clear(node, state)
|
||||||
|
if not state.arbiter.may_drive(GAMEPAD):
|
||||||
|
return
|
||||||
|
if tapped(LAYOUT.grasp):
|
||||||
|
_act(node, state, GRIPPER_CLOSE)
|
||||||
|
if tapped(LAYOUT.release):
|
||||||
|
_act(node, state, GRIPPER_OPEN)
|
||||||
|
if tapped(LAYOUT.freedrive):
|
||||||
|
_freedrive(node, state, not state.view.freedrive_on)
|
||||||
|
|
||||||
|
|
||||||
|
def _poll_state(node: Node, state: Teleop, moment: float) -> None:
|
||||||
|
"""Ask the arm for what telemetry does not carry.
|
||||||
|
|
||||||
|
Whether the tool is engaged and whether freedrive is on only come back from
|
||||||
|
`get_state`, which the arm node answers off its report cache and so can
|
||||||
|
answer during a motion.
|
||||||
|
"""
|
||||||
|
if moment - state.polled_at < state.config.deadman.state_poll_s:
|
||||||
|
return
|
||||||
|
state.polled_at = moment
|
||||||
|
_request(node, state, {"service": "get_state"})
|
||||||
|
|
||||||
|
|
||||||
|
# --- what the console asks for ---
|
||||||
|
|
||||||
|
|
||||||
|
def _drain_console(node: Node, state: Teleop) -> None:
|
||||||
|
"""Act on everything the browsers sent, then note how many are still there."""
|
||||||
|
if state.console is None:
|
||||||
|
return
|
||||||
|
for payload in state.console.take():
|
||||||
|
_accept_ui_command(node, state, payload)
|
||||||
|
_set_clients(state, state.console.clients)
|
||||||
|
|
||||||
|
|
||||||
|
def _accept_ui_command(node: Node, state: Teleop, payload: dict) -> None:
|
||||||
|
"""Take one console message and do what it says."""
|
||||||
|
command = ui.command_of(payload)
|
||||||
|
if command is None:
|
||||||
|
return
|
||||||
|
moment = now()
|
||||||
|
|
||||||
|
if command.type == "estop":
|
||||||
|
_estop(node, state)
|
||||||
|
return
|
||||||
|
if command.type == "control":
|
||||||
|
_control(node, state, command, moment)
|
||||||
|
return
|
||||||
|
if command.type == "reset":
|
||||||
|
_clear(node, state)
|
||||||
|
return
|
||||||
|
|
||||||
|
if not state.arbiter.may_drive(WEB):
|
||||||
|
_post_ui(node, state, ui.error("take control first"))
|
||||||
|
return
|
||||||
|
|
||||||
|
if command.type == "jog":
|
||||||
|
state.web_axes = mapping.from_console(command.axes)
|
||||||
|
state.web_seen_at = moment
|
||||||
|
state.arbiter.note_activity(WEB, moment)
|
||||||
|
elif command.type == "jog_stop":
|
||||||
|
state.web_axes = STOP_AXES
|
||||||
|
state.web_seen_at = moment
|
||||||
|
elif command.type == "home":
|
||||||
|
_act(node, state, HOME)
|
||||||
|
elif command.type == "gripper":
|
||||||
|
_act(node, state, GRIPPER_CLOSE if command.enabled else GRIPPER_OPEN)
|
||||||
|
elif command.type == "freedrive":
|
||||||
|
_freedrive(node, state, command.enabled)
|
||||||
|
elif command.type == "move":
|
||||||
|
_request(node, state, {"service": "move_to", "wait": False, **command.row})
|
||||||
|
|
||||||
|
|
||||||
|
def _control(node: Node, state: Teleop, command, moment: float) -> None:
|
||||||
|
"""Hand the arm to the console, or take it back."""
|
||||||
|
if command.action == "release":
|
||||||
|
state.arbiter.release(WEB)
|
||||||
|
state.web_axes = STOP_AXES
|
||||||
|
return
|
||||||
|
if not state.arbiter.take(WEB, moment, preempt=command.preempt):
|
||||||
|
_post_ui(node, state, ui.error(f"the {state.arbiter.owner} has the arm"))
|
||||||
|
|
||||||
|
|
||||||
|
def _set_clients(state: Teleop, count: int) -> None:
|
||||||
|
"""Track who is watching, because the last one leaving has to stop the arm."""
|
||||||
|
state.clients = count
|
||||||
|
if count == 0:
|
||||||
|
state.arbiter.release(WEB)
|
||||||
|
state.web_axes = STOP_AXES
|
||||||
|
|
||||||
|
|
||||||
|
# --- what the arm says back ---
|
||||||
|
|
||||||
|
|
||||||
|
def _accept_response(node: Node, state: Teleop, event: dict) -> None:
|
||||||
|
"""Absorb one answer, and tell the console about it if it was bad news."""
|
||||||
|
row = event["value"][0].as_py()
|
||||||
|
if row.get("service") == "get_state":
|
||||||
|
state.view.take_get_state(row)
|
||||||
|
return
|
||||||
|
if not row.get("ok", True):
|
||||||
|
message = row.get("message", "")
|
||||||
|
log(f"{row.get('service')} failed: {message}")
|
||||||
|
_post_ui(node, state, ui.error(message))
|
||||||
|
|
||||||
|
|
||||||
|
def _accept_motion_state(node: Node, state: Teleop, event: dict) -> None:
|
||||||
|
"""Report a motion that failed, which a non-blocking request learns no other way."""
|
||||||
|
row = event["value"][0].as_py()
|
||||||
|
if row.get("state") == "failed":
|
||||||
|
_post_ui(node, state, ui.error(row.get("message", "the motion failed")))
|
||||||
|
|
||||||
|
|
||||||
|
# --- talking to the dataflow ---
|
||||||
|
|
||||||
|
|
||||||
|
def _estop(node: Node, state: Teleop) -> None:
|
||||||
|
"""Halt the arm, from any source, held lease or not."""
|
||||||
|
log("e-stop")
|
||||||
|
state.view.estop = True
|
||||||
|
state.jogging = False
|
||||||
|
state.web_axes = STOP_AXES
|
||||||
|
_send_motion(node, state, STOP_AXES)
|
||||||
|
_act(node, state, STOP)
|
||||||
|
|
||||||
|
|
||||||
|
def _clear(node: Node, state: Teleop) -> None:
|
||||||
|
"""Release the latch and clear the controller's error, in that order.
|
||||||
|
|
||||||
|
Both, or one of the two is still saying stop: the latch is this node's
|
||||||
|
business and the error is the arm's. The console's RESET and the pad's
|
||||||
|
`clear` button are the same act and go through here, so a pad-only
|
||||||
|
dataflow is not left with a latch nothing can release.
|
||||||
|
"""
|
||||||
|
log("cleared")
|
||||||
|
state.view.estop = False
|
||||||
|
_act(node, state, CLEAR)
|
||||||
|
|
||||||
|
|
||||||
|
def _send_motion(node: Node, state: Teleop, axes: list[float]) -> None:
|
||||||
|
"""Send one motion command, on the standard port and the deprecated one.
|
||||||
|
|
||||||
|
There is no answer to either, ever — a jog is a stream that expires on the
|
||||||
|
consumer's own TTL, so the sender keeps sending and nobody waits.
|
||||||
|
|
||||||
|
`twist` is the standard: SI, framed, what a robot node should take. `jog`
|
||||||
|
is `dora-ulite6`'s mm/s and deg/s, published alongside until that node
|
||||||
|
learns the standard. Both carry the same intent, so wiring either is safe;
|
||||||
|
wiring both would drive the arm twice.
|
||||||
|
|
||||||
|
This always sends. The decision not to is `_drive`'s, so that the e-stop and
|
||||||
|
the shutdown stop — neither of which goes through it — can never be
|
||||||
|
suppressed by having sent the same thing already.
|
||||||
|
"""
|
||||||
|
config = state.config
|
||||||
|
node.send_output("twist", message.pack_twist(mapping.twist(axes, config)))
|
||||||
|
node.send_output("jog", message.pack_jog(mapping.scale(axes, config)))
|
||||||
|
state.sent_axes = list(axes)
|
||||||
|
state.sent_at = now()
|
||||||
|
|
||||||
|
|
||||||
|
def _act(node: Node, state: Teleop, action: str, value: float | None = None) -> None:
|
||||||
|
"""Ask for one discrete thing, on the standard port and the deprecated one.
|
||||||
|
|
||||||
|
The verb is robot-agnostic; `LEGACY_REQUEST` is the only place that knows
|
||||||
|
what `dora-ulite6` calls it. A robot speaking the standard needs no entry
|
||||||
|
there, which is how that table eventually goes away.
|
||||||
|
"""
|
||||||
|
node.send_output("command", message.pack_command(action, value))
|
||||||
|
row = LEGACY_REQUEST.get(action)
|
||||||
|
if row is not None:
|
||||||
|
_request(node, state, dict(row))
|
||||||
|
|
||||||
|
|
||||||
|
def _freedrive(node: Node, state: Teleop, enabled: bool) -> None:
|
||||||
|
"""Toggle freedrive. Its argument rides as the verb's value."""
|
||||||
|
_act(node, state, FREEDRIVE, value=float(enabled))
|
||||||
|
_request(node, state, {"service": "freedrive", "enabled": enabled})
|
||||||
|
|
||||||
|
|
||||||
|
def _request(node: Node, state: Teleop, row: dict) -> None:
|
||||||
|
"""Call one service on the arm under an id we will recognise coming back."""
|
||||||
|
node.send_output("request", pa.array([row]), metadata={REQUEST_ID: state.next_request_id(row["service"])})
|
||||||
|
|
||||||
|
|
||||||
|
def _publish(node: Node, state: Teleop) -> None:
|
||||||
|
"""Send the console its picture of the arm."""
|
||||||
|
_post_ui(node, state, ui.snapshot(state.view, state.arbiter.owner, state.jogging, time.time()))
|
||||||
|
|
||||||
|
|
||||||
|
def _post_ui(node: Node, state: Teleop, payload: dict) -> None:
|
||||||
|
"""Send one object to every browser, and out on the wire for anyone watching.
|
||||||
|
|
||||||
|
`ui_state` carries no browser any more — the sockets are ours. It stays
|
||||||
|
published because it is the one port that says what an operator is being
|
||||||
|
shown, which is what `dora topic echo` and the bench read it for.
|
||||||
|
"""
|
||||||
|
if state.console is not None:
|
||||||
|
state.console.broadcast(payload)
|
||||||
|
node.send_output("ui_state", pa.array([json.dumps(payload)]), metadata={"encoding": "json"})
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,219 @@
|
|||||||
|
"""Turning a control input into motion, in one place for every source.
|
||||||
|
|
||||||
|
Pure functions, no state and no hardware, because this is the part that has to
|
||||||
|
be right and the part that is cheapest to check.
|
||||||
|
|
||||||
|
Everything passes through a **normalised** six-vector first: `[-1, 1]` per
|
||||||
|
degree of freedom, `[x, y, z, roll, pitch, yaw]`, in the robot's **base**
|
||||||
|
frame. A gamepad reaches it through :func:`from_pad`, which shapes away a worn
|
||||||
|
stick; the browser sends it already clean. Only then is it scaled, and that
|
||||||
|
happens exactly once — which is what lets the same node drive an arm in mm/s
|
||||||
|
and one in m/s without either source knowing the difference.
|
||||||
|
|
||||||
|
Two scalings exist. :func:`twist` is the standard one, SI, and is what a robot
|
||||||
|
should consume. :func:`velocity` is mm/s and deg/s for `dora-ulite6`'s `jog`
|
||||||
|
port, kept while that node learns the standard.
|
||||||
|
|
||||||
|
The screen convention comes from the old web console, so the two agree: stick
|
||||||
|
up is +X, stick right is +Y.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import math
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
# Six degrees of freedom; these name their places in the vector.
|
||||||
|
VX, VY, VZ, WX, WY, WZ = range(6)
|
||||||
|
|
||||||
|
# Operator-facing settings are in mm/s and deg/s because that is what a person
|
||||||
|
# reading a dataflow can judge — 60 mm/s means something, 0.06 m/s less so. The
|
||||||
|
# wire is SI. These convert between the two, and are the only place that does.
|
||||||
|
MM_PER_M = 1000.0
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class Layout:
|
||||||
|
"""Which axis and button index does what, in SDL's numbering.
|
||||||
|
|
||||||
|
The defaults are the layout SDL reports for an Xbox-style pad, which is also
|
||||||
|
what it reports for most PlayStation-style ones. A pad that disagrees is a
|
||||||
|
change here rather than a new environment variable: an index nobody can
|
||||||
|
verify from the outside is worse than a line of code somebody can read.
|
||||||
|
"""
|
||||||
|
|
||||||
|
left_x: int = 0
|
||||||
|
left_y: int = 1
|
||||||
|
left_trigger: int = 2
|
||||||
|
right_x: int = 3
|
||||||
|
right_y: int = 4
|
||||||
|
right_trigger: int = 5
|
||||||
|
|
||||||
|
yaw_left: int = 4 # LB
|
||||||
|
yaw_right: int = 5 # RB
|
||||||
|
grasp: int = 0 # A
|
||||||
|
release: int = 1 # B
|
||||||
|
clear: int = 2 # X
|
||||||
|
freedrive: int = 3 # Y
|
||||||
|
estop: int = 6 # Back
|
||||||
|
|
||||||
|
|
||||||
|
LAYOUT = Layout()
|
||||||
|
|
||||||
|
# The frame a velocity is expressed in. Base is what `tcp_pose` reports and what
|
||||||
|
# the console's arrows mean; tool is reserved for a consumer that wants it.
|
||||||
|
BASE_FRAME = "base"
|
||||||
|
TOOL_FRAME = "tool"
|
||||||
|
|
||||||
|
# How far a control must travel before the pad counts as being used. Raw, with
|
||||||
|
# no deadzone: this decides whether the pad is asking for the arm, and
|
||||||
|
# `velocity` decides how far to move it. A tighter threshold here than there
|
||||||
|
# would mean a stick could claim the arm and then not move it.
|
||||||
|
ACTIVITY = 0.3
|
||||||
|
|
||||||
|
|
||||||
|
def deadzone(value: float, width: float) -> float:
|
||||||
|
"""Ignore travel near centre, and rescale what is left to the full range.
|
||||||
|
|
||||||
|
Rescaling matters: without it the arm jumps to `width` of full speed the
|
||||||
|
instant the stick leaves the dead band, which feels like a broken control
|
||||||
|
rather than a gentle one.
|
||||||
|
"""
|
||||||
|
if width <= 0.0:
|
||||||
|
return value
|
||||||
|
size = abs(value)
|
||||||
|
if size < width:
|
||||||
|
return 0.0
|
||||||
|
scaled = (size - width) / (1.0 - width)
|
||||||
|
return scaled if value > 0 else -scaled
|
||||||
|
|
||||||
|
|
||||||
|
def expo(value: float, factor: float) -> float:
|
||||||
|
"""Bend the response so small deflections move the arm slowly.
|
||||||
|
|
||||||
|
Fine positioning happens near centre, and a linear stick gives it almost no
|
||||||
|
resolution there. `factor` is how much of the curve is cubic: 0 is linear.
|
||||||
|
"""
|
||||||
|
return (1.0 - factor) * value + factor * value**3
|
||||||
|
|
||||||
|
|
||||||
|
def trigger(value: float) -> float:
|
||||||
|
"""Return a trigger's travel as 0 at rest and 1 fully pressed.
|
||||||
|
|
||||||
|
SDL reports a trigger from -1 to 1, unlike a stick, and a trigger that has
|
||||||
|
never been touched sometimes reads 0 instead of -1 until it first moves.
|
||||||
|
Clamping keeps that from reading as half pressed.
|
||||||
|
"""
|
||||||
|
return max(0.0, min(1.0, (value + 1.0) / 2.0))
|
||||||
|
|
||||||
|
|
||||||
|
def axis(values: list[float], index: int) -> float:
|
||||||
|
"""Read one axis, treating a pad that does not report it as centred."""
|
||||||
|
return values[index] if index < len(values) else 0.0
|
||||||
|
|
||||||
|
|
||||||
|
def pressed(buttons: list[bool], index: int) -> bool:
|
||||||
|
"""Read one button, treating a pad that does not report it as unpressed."""
|
||||||
|
return bool(buttons[index]) if index < len(buttons) else False
|
||||||
|
|
||||||
|
|
||||||
|
def from_pad(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
|
||||||
|
"""Map one gamepad reading to the normalised six-vector.
|
||||||
|
|
||||||
|
Translation comes from the left stick and the triggers, rotation from the
|
||||||
|
right stick and the bumpers. Several axes move at once on purpose: a stick
|
||||||
|
is analog and blended, which is exactly what the console's ring sectors cannot do.
|
||||||
|
|
||||||
|
The deadzone and the response curve are applied here and nowhere else,
|
||||||
|
because they exist to correct a physical stick. A browser has no worn
|
||||||
|
centre, so :func:`from_console` does not use them.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def shaped(value: float) -> float:
|
||||||
|
return expo(deadzone(value, config.deadzone), config.expo)
|
||||||
|
|
||||||
|
out = [0.0] * 6
|
||||||
|
# Up on the stick is negative in SDL, and up should send the arm away from
|
||||||
|
# the operator, along +X.
|
||||||
|
out[VX] = shaped(-axis(axes, layout.left_y))
|
||||||
|
out[VY] = shaped(axis(axes, layout.left_x))
|
||||||
|
out[VZ] = shaped(trigger(axis(axes, layout.right_trigger)) - trigger(axis(axes, layout.left_trigger)))
|
||||||
|
out[WX] = shaped(axis(axes, layout.right_x))
|
||||||
|
out[WY] = shaped(-axis(axes, layout.right_y))
|
||||||
|
out[WZ] = float(pressed(buttons, layout.yaw_right) - pressed(buttons, layout.yaw_left))
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def from_console(values) -> list[float]:
|
||||||
|
"""Read the normalised six-vector a browser sent, clamped to what it may ask for.
|
||||||
|
|
||||||
|
The console speaks the same units as a stick — a fraction of full speed per
|
||||||
|
axis — so that how fast anything goes is decided once, here, rather than
|
||||||
|
once per source. A page that has never heard of the robot cannot get its
|
||||||
|
units wrong.
|
||||||
|
"""
|
||||||
|
if not isinstance(values, list) or len(values) != 6:
|
||||||
|
return [0.0] * 6
|
||||||
|
out = []
|
||||||
|
for value in values:
|
||||||
|
if not isinstance(value, (int, float)):
|
||||||
|
return [0.0] * 6
|
||||||
|
out.append(max(-1.0, min(1.0, float(value))))
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def velocity(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
|
||||||
|
"""Map one gamepad reading straight to `dora-ulite6`'s mm/s and deg/s.
|
||||||
|
|
||||||
|
A convenience over `scale(from_pad(...))`, kept because the probe and the
|
||||||
|
tests read better for it.
|
||||||
|
"""
|
||||||
|
return scale(from_pad(axes, buttons, config, layout), config)
|
||||||
|
|
||||||
|
|
||||||
|
def scale(normalised: list[float], config) -> list[float]:
|
||||||
|
"""Scale the normalised six-vector to mm/s and deg/s, base frame.
|
||||||
|
|
||||||
|
`dora-ulite6`'s `jog` units. Deprecated in favour of :func:`twist`, and kept
|
||||||
|
only while that node learns to take the standard one.
|
||||||
|
"""
|
||||||
|
out = [value * config.speed_mm_s for value in normalised[:3]]
|
||||||
|
out += [value * config.speed_deg_s for value in normalised[3:]]
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def twist(normalised: list[float], config, frame: str = BASE_FRAME) -> dict:
|
||||||
|
"""Scale the normalised six-vector to a Twist, in SI.
|
||||||
|
|
||||||
|
Metres per second and radians per second, which is what a robot node should
|
||||||
|
consume: unambiguous, and the same numbers a UR or MoveIt would take. A
|
||||||
|
consumer wanting other units converts them itself, at one known boundary.
|
||||||
|
"""
|
||||||
|
linear = [value * config.speed_mm_s / MM_PER_M for value in normalised[:3]]
|
||||||
|
angular = [math.radians(value * config.speed_deg_s) for value in normalised[3:]]
|
||||||
|
return {"linear": linear, "angular": angular, "frame": frame}
|
||||||
|
|
||||||
|
|
||||||
|
def is_active(sample, layout: Layout = LAYOUT) -> bool:
|
||||||
|
"""Whether anything on the pad is being touched at all.
|
||||||
|
|
||||||
|
A trigger is not a stick. SDL rests a trigger at **-1.0**, so comparing its
|
||||||
|
raw value against a threshold reports every connected pad as being used,
|
||||||
|
for ever — which means the pad claims the arm the moment it is plugged in
|
||||||
|
and `GAMEPAD_IDLE_RELEASE_S` never fires. Triggers go through
|
||||||
|
:func:`trigger` here for the same reason they do in :func:`velocity`.
|
||||||
|
|
||||||
|
Axes the layout does not name are still counted: a pad that reports its
|
||||||
|
d-pad as two extra axes is asking for the arm when somebody presses it.
|
||||||
|
"""
|
||||||
|
if not sample.present:
|
||||||
|
return False
|
||||||
|
if any(sample.buttons):
|
||||||
|
return True
|
||||||
|
triggers = (layout.left_trigger, layout.right_trigger)
|
||||||
|
if any(trigger(axis(sample.axes, index)) > ACTIVITY for index in triggers):
|
||||||
|
return True
|
||||||
|
return any(abs(value) > ACTIVITY for index, value in enumerate(sample.axes) if index not in triggers)
|
||||||
|
|
||||||
|
|
||||||
|
def is_moving(velocity_vector: list[float]) -> bool:
|
||||||
|
"""Whether this velocity would actually drive the arm."""
|
||||||
|
return any(velocity_vector)
|
||||||
@@ -0,0 +1,46 @@
|
|||||||
|
"""The wire shapes this node publishes.
|
||||||
|
|
||||||
|
Declared, never inferred. pyarrow reads `{"value": None}` as `null` and
|
||||||
|
`{"value": 1.0}` as `double`, so an inferred `command` would change the type of
|
||||||
|
its port depending on which verb was pressed — `stop` and `freedrive` would not
|
||||||
|
agree. The same hazard that makes an unplugged gamepad retype a `joy` port.
|
||||||
|
|
||||||
|
dora 1.0 has no standard type for either of these, so these structs and the
|
||||||
|
manifest that describes them are the whole contract a consumer gets.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import pyarrow as pa
|
||||||
|
|
||||||
|
# A velocity, in SI. Metres per second and radians per second, in the frame
|
||||||
|
# named by `frame` — the units a robot node should consume, and the same ones a
|
||||||
|
# UR or MoveIt would take.
|
||||||
|
TWIST = pa.struct(
|
||||||
|
[
|
||||||
|
("linear", pa.list_(pa.float64())),
|
||||||
|
("angular", pa.list_(pa.float64())),
|
||||||
|
("frame", pa.string()),
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
# One discrete thing to do. `action` is a robot-agnostic verb; `value` carries
|
||||||
|
# its argument where it has one and is null otherwise.
|
||||||
|
COMMAND = pa.struct([("action", pa.string()), ("value", pa.float64())])
|
||||||
|
|
||||||
|
# `dora-ulite6`'s velocity, in mm/s and deg/s. Deprecated along with the `jog`
|
||||||
|
# port it goes out on.
|
||||||
|
JOG = pa.struct([("velocity", pa.list_(pa.float64()))])
|
||||||
|
|
||||||
|
|
||||||
|
def pack_twist(twist: dict) -> pa.Array:
|
||||||
|
"""Build one `twist` message."""
|
||||||
|
return pa.array([twist], type=TWIST)
|
||||||
|
|
||||||
|
|
||||||
|
def pack_command(action: str, value: float | None) -> pa.Array:
|
||||||
|
"""Build one `command` message."""
|
||||||
|
return pa.array([{"action": action, "value": value}], type=COMMAND)
|
||||||
|
|
||||||
|
|
||||||
|
def pack_jog(velocity: list[float]) -> pa.Array:
|
||||||
|
"""Build one deprecated `jog` message."""
|
||||||
|
return pa.array([{"velocity": [float(v) for v in velocity]}], type=JOG)
|
||||||
@@ -0,0 +1,151 @@
|
|||||||
|
"""Show what the pad is doing to the arm, without an arm.
|
||||||
|
|
||||||
|
`dora-teleop-probe` attaches to a running dataflow as a dynamic node, reads the
|
||||||
|
same `joy` stream this node reads, and prints: the raw axes, the buttons, and the
|
||||||
|
velocity this node would send. **It cannot move anything.** It publishes no
|
||||||
|
output and never talks to the arm; it only reads.
|
||||||
|
|
||||||
|
It exists because the axis and button indices in :mod:`dora_teleop.mapping` are
|
||||||
|
SDL's documented layout rather than an observed one, and a pad that disagrees is
|
||||||
|
otherwise discovered by watching a real arm move the wrong way. It goes through
|
||||||
|
the same :func:`~dora_teleop.mapping.velocity` the node runs, on the same
|
||||||
|
readings, so what it prints is what the arm would get — keep it that way, or it
|
||||||
|
stops being evidence.
|
||||||
|
|
||||||
|
Because it reads `joy` rather than a pad, the dataflow has to declare it:
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
- id: teleop-probe
|
||||||
|
path: dynamic
|
||||||
|
inputs:
|
||||||
|
joy: joy/joy
|
||||||
|
```
|
||||||
|
|
||||||
|
`dataflow-gamepad.yml` in this repo already does. Start that dataflow under a
|
||||||
|
coordinator (`dora start`, not `dora run`), then run `dora-teleop-probe` in
|
||||||
|
another terminal.
|
||||||
|
|
||||||
|
For a pad whose layout is not known at all — which index is which — use
|
||||||
|
`dora-joy-probe` from the `dora-joy` repo, which needs no dataflow.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
|
||||||
|
from dora import Node
|
||||||
|
|
||||||
|
from dora_teleop import joy, mapping
|
||||||
|
from dora_teleop.config import Config
|
||||||
|
from dora_teleop.mapping import LAYOUT
|
||||||
|
|
||||||
|
# What each velocity component is, in the order the arm takes them.
|
||||||
|
COMPONENTS = (
|
||||||
|
("vx", "mm/s", "forward / back"),
|
||||||
|
("vy", "mm/s", "left / right"),
|
||||||
|
("vz", "mm/s", "up / down"),
|
||||||
|
("wx", "deg/s", "roll"),
|
||||||
|
("wy", "deg/s", "pitch"),
|
||||||
|
("wz", "deg/s", "yaw"),
|
||||||
|
)
|
||||||
|
|
||||||
|
CLEAR = "\033[H\033[J"
|
||||||
|
|
||||||
|
|
||||||
|
def axis_names(layout=LAYOUT) -> dict[int, str]:
|
||||||
|
"""Name each axis index the layout uses."""
|
||||||
|
return {
|
||||||
|
layout.left_x: "left stick X",
|
||||||
|
layout.left_y: "left stick Y",
|
||||||
|
layout.left_trigger: "left trigger",
|
||||||
|
layout.right_x: "right stick X",
|
||||||
|
layout.right_y: "right stick Y",
|
||||||
|
layout.right_trigger: "right trigger",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def button_names(layout=LAYOUT) -> dict[int, str]:
|
||||||
|
"""Name each button index the layout uses."""
|
||||||
|
return {
|
||||||
|
layout.grasp: "grasp",
|
||||||
|
layout.release: "release",
|
||||||
|
layout.clear: "clear",
|
||||||
|
layout.freedrive: "freedrive",
|
||||||
|
layout.yaw_left: "yaw left",
|
||||||
|
layout.yaw_right: "yaw right",
|
||||||
|
layout.estop: "E-STOP",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def bar(value: float, width: int = 21) -> str:
|
||||||
|
"""Draw a centred bar for a value between -1 and 1.
|
||||||
|
|
||||||
|
Seeing the bar sit off-centre with the stick released is how a worn pad
|
||||||
|
announces itself, which is most of the point of looking at raw numbers.
|
||||||
|
"""
|
||||||
|
middle = width // 2
|
||||||
|
position = middle + round(max(-1.0, min(1.0, value)) * middle)
|
||||||
|
cells = ["-"] * width
|
||||||
|
cells[middle] = "|"
|
||||||
|
cells[position] = "#"
|
||||||
|
return "".join(cells)
|
||||||
|
|
||||||
|
|
||||||
|
def render(sample, velocity: list[float], config: Config) -> str:
|
||||||
|
"""Build the whole display for one reading."""
|
||||||
|
lines = ["dora-teleop probe — Ctrl-C to stop", ""]
|
||||||
|
|
||||||
|
if not sample.present:
|
||||||
|
lines += [
|
||||||
|
" no gamepad.",
|
||||||
|
"",
|
||||||
|
" `dora-joy` is reporting no pad, or has stopped reporting at all.",
|
||||||
|
" Either way the arm is being told to hold still.",
|
||||||
|
]
|
||||||
|
return "\n".join(lines)
|
||||||
|
|
||||||
|
names = axis_names()
|
||||||
|
lines.append(f" axes ({len(sample.axes)} reported)")
|
||||||
|
for index, value in enumerate(sample.axes):
|
||||||
|
name = names.get(index, "unused")
|
||||||
|
lines.append(f" {index} {name:<15} {value:+.3f} {bar(value)}")
|
||||||
|
|
||||||
|
labels = button_names()
|
||||||
|
held = [f"{index}:{labels.get(index, 'unused')}" for index, down in enumerate(sample.buttons) if down]
|
||||||
|
lines += ["", f" buttons ({len(sample.buttons)} reported)", f" {', '.join(held) or 'none pressed'}"]
|
||||||
|
|
||||||
|
lines += ["", " velocity this would send to the arm"]
|
||||||
|
for (name, units, meaning), value in zip(COMPONENTS, velocity, strict=True):
|
||||||
|
marker = " <--" if value else ""
|
||||||
|
lines.append(f" {name} {value:+8.2f} {units:<6} {meaning:<14}{marker}".rstrip())
|
||||||
|
|
||||||
|
moving = "yes" if mapping.is_moving(velocity) else "no — the arm would be told to hold still"
|
||||||
|
lines += [
|
||||||
|
"",
|
||||||
|
f" moving: {moving}",
|
||||||
|
f" asking for the arm: {'yes' if mapping.is_active(sample) else 'no'}",
|
||||||
|
]
|
||||||
|
return "\n".join(lines)
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
"""Attach to a running dataflow and print what the pad is doing."""
|
||||||
|
parser = argparse.ArgumentParser(description="Show what the gamepad would do to the arm. Moves nothing.")
|
||||||
|
parser.add_argument(
|
||||||
|
"--name",
|
||||||
|
default="teleop-probe",
|
||||||
|
help="The id this probe is declared under in the dataflow. Defaults to teleop-probe.",
|
||||||
|
)
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
config = Config.from_env()
|
||||||
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,184 @@
|
|||||||
|
"""The console's web server, running inside this node.
|
||||||
|
|
||||||
|
The browser used to reach the node through `dora-websocket-transceiver`, which
|
||||||
|
meant a second node, a second port and a round trip through the dataflow for
|
||||||
|
every button press. This serves the page and holds the sockets directly, so the
|
||||||
|
console is one URL and the operator wires nothing.
|
||||||
|
|
||||||
|
**Nothing here runs on the dora thread.** aiohttp wants an asyncio loop and the
|
||||||
|
node's loop is a blocking `for event in node`, so the server owns a thread of
|
||||||
|
its own and the two sides meet at two places only: :meth:`Console.take`, which
|
||||||
|
the tick drains, and :meth:`Console.broadcast`, which hands a snapshot the other
|
||||||
|
way. Both are non-blocking by construction — a browser that stops reading must
|
||||||
|
never be able to stall the thread that stops the arm.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import asyncio
|
||||||
|
import json
|
||||||
|
import queue
|
||||||
|
import threading
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from aiohttp import WSMsgType, web
|
||||||
|
|
||||||
|
from dora_teleop.log import log
|
||||||
|
|
||||||
|
INDEX = Path(__file__).parent / "web" / "index.html"
|
||||||
|
|
||||||
|
# How many console messages may pile up between two ticks before we start
|
||||||
|
# dropping them. The console sends a jog every 150 ms and the tick runs every
|
||||||
|
# 50, so reaching this at all means something is flooding us; an unbounded
|
||||||
|
# queue would answer that by growing until the node died.
|
||||||
|
INBOX_LIMIT = 256
|
||||||
|
|
||||||
|
# How many snapshots may wait on one socket. Telemetry is a snapshot, not a
|
||||||
|
# log, so a client too slow to keep up wants the newest one and not a backlog
|
||||||
|
# of the arm's recent past.
|
||||||
|
OUTBOX_LIMIT = 8
|
||||||
|
|
||||||
|
|
||||||
|
class _Client:
|
||||||
|
"""One connected browser, and the snapshots waiting to reach it."""
|
||||||
|
|
||||||
|
def __init__(self) -> None:
|
||||||
|
self.outbox: asyncio.Queue[str] = asyncio.Queue(maxsize=OUTBOX_LIMIT)
|
||||||
|
|
||||||
|
def offer(self, text: str) -> None:
|
||||||
|
"""Queue one snapshot, discarding the stalest if this client is behind."""
|
||||||
|
if self.outbox.full():
|
||||||
|
self.outbox.get_nowait()
|
||||||
|
self.outbox.put_nowait(text)
|
||||||
|
|
||||||
|
|
||||||
|
class Console:
|
||||||
|
"""The page, the sockets, and the thread they live on."""
|
||||||
|
|
||||||
|
def __init__(self, host: str, port: int):
|
||||||
|
self.host = host
|
||||||
|
self.port = port
|
||||||
|
# Read every tick by the deadman: the last browser leaving has to stop
|
||||||
|
# the arm, and a closed socket sends nothing to say so.
|
||||||
|
self.clients = 0
|
||||||
|
self._inbox: queue.Queue[dict] = queue.Queue(maxsize=INBOX_LIMIT)
|
||||||
|
self._sockets: set[_Client] = set()
|
||||||
|
self._loop: asyncio.AbstractEventLoop | None = None
|
||||||
|
self._thread: threading.Thread | None = None
|
||||||
|
self._ready = threading.Event()
|
||||||
|
# Whatever stopped the server coming up, kept so `start` can raise it on
|
||||||
|
# the caller's thread instead of leaving a traceback on stderr and a
|
||||||
|
# node that believes it is serving a console.
|
||||||
|
self._failure: BaseException | None = None
|
||||||
|
|
||||||
|
def start(self) -> None:
|
||||||
|
"""Bring the server up, or raise saying why it could not.
|
||||||
|
|
||||||
|
Refusing to start is the point. The port being taken used to leave a
|
||||||
|
traceback on the server thread and a node that carried on announcing a
|
||||||
|
console nobody could open — an operator typing the URL and getting
|
||||||
|
nothing, with the log saying everything was fine.
|
||||||
|
"""
|
||||||
|
self._thread = threading.Thread(target=self._run, name="teleop-console", daemon=True)
|
||||||
|
self._thread.start()
|
||||||
|
if not self._ready.wait(timeout=10.0):
|
||||||
|
raise RuntimeError(f"the console did not come up on {self.host}:{self.port} within 10s")
|
||||||
|
if self._failure is not None:
|
||||||
|
raise self._failure
|
||||||
|
log(f"console on http://{self.host}:{self.port}")
|
||||||
|
|
||||||
|
def take(self) -> list[dict]:
|
||||||
|
"""Return every console message that arrived since the last call."""
|
||||||
|
drained = []
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
drained.append(self._inbox.get_nowait())
|
||||||
|
except queue.Empty:
|
||||||
|
return drained
|
||||||
|
|
||||||
|
def broadcast(self, payload: dict) -> None:
|
||||||
|
"""Hand one object to every connected browser."""
|
||||||
|
if self._loop is None:
|
||||||
|
return
|
||||||
|
text = json.dumps(payload)
|
||||||
|
self._loop.call_soon_threadsafe(self._offer, text)
|
||||||
|
|
||||||
|
def stop(self) -> None:
|
||||||
|
"""Close the sockets and stop the thread."""
|
||||||
|
if self._loop is None:
|
||||||
|
return
|
||||||
|
self._loop.call_soon_threadsafe(self._loop.stop)
|
||||||
|
if self._thread is not None:
|
||||||
|
self._thread.join(timeout=2.0)
|
||||||
|
|
||||||
|
# --- everything below runs on the server thread ---
|
||||||
|
|
||||||
|
def _run(self) -> None:
|
||||||
|
loop = asyncio.new_event_loop()
|
||||||
|
asyncio.set_event_loop(loop)
|
||||||
|
self._loop = loop
|
||||||
|
try:
|
||||||
|
runner = loop.run_until_complete(self._listen())
|
||||||
|
except BaseException as error: # noqa: BLE001 — handed to `start` to raise
|
||||||
|
self._failure = error
|
||||||
|
self._loop = None
|
||||||
|
loop.close()
|
||||||
|
# Releases `start` at once rather than making it wait out its
|
||||||
|
# timeout for a server that is never coming up.
|
||||||
|
self._ready.set()
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
loop.run_forever()
|
||||||
|
finally:
|
||||||
|
loop.run_until_complete(runner.cleanup())
|
||||||
|
loop.close()
|
||||||
|
|
||||||
|
async def _listen(self) -> web.AppRunner:
|
||||||
|
app = web.Application()
|
||||||
|
app.router.add_get("/", self._page)
|
||||||
|
app.router.add_get("/ws", self._socket)
|
||||||
|
runner = web.AppRunner(app)
|
||||||
|
await runner.setup()
|
||||||
|
await web.TCPSite(runner, self.host, self.port).start()
|
||||||
|
self._ready.set()
|
||||||
|
return runner
|
||||||
|
|
||||||
|
async def _page(self, request: web.Request) -> web.FileResponse:
|
||||||
|
"""Serve the console itself."""
|
||||||
|
return web.FileResponse(INDEX)
|
||||||
|
|
||||||
|
async def _socket(self, request: web.Request) -> web.WebSocketResponse:
|
||||||
|
"""Hold one browser's socket for as long as it stays."""
|
||||||
|
socket = web.WebSocketResponse(heartbeat=20)
|
||||||
|
await socket.prepare(request)
|
||||||
|
client = _Client()
|
||||||
|
self._sockets.add(client)
|
||||||
|
self.clients = len(self._sockets)
|
||||||
|
writer = asyncio.create_task(self._write(socket, client))
|
||||||
|
try:
|
||||||
|
async for received in socket:
|
||||||
|
if received.type == WSMsgType.TEXT:
|
||||||
|
self._accept(received.data)
|
||||||
|
finally:
|
||||||
|
writer.cancel()
|
||||||
|
self._sockets.discard(client)
|
||||||
|
self.clients = len(self._sockets)
|
||||||
|
return socket
|
||||||
|
|
||||||
|
def _accept(self, text: str) -> None:
|
||||||
|
"""Park one message from a browser for the next tick to read."""
|
||||||
|
try:
|
||||||
|
payload = json.loads(text)
|
||||||
|
except json.JSONDecodeError:
|
||||||
|
log("ignoring a console message that was not JSON")
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
self._inbox.put_nowait(payload)
|
||||||
|
except queue.Full:
|
||||||
|
log("console inbox full, dropping a message")
|
||||||
|
|
||||||
|
def _offer(self, text: str) -> None:
|
||||||
|
for client in self._sockets:
|
||||||
|
client.offer(text)
|
||||||
|
|
||||||
|
async def _write(self, socket: web.WebSocketResponse, client: _Client) -> None:
|
||||||
|
while True:
|
||||||
|
await socket.send_str(await client.outbox.get())
|
||||||
@@ -0,0 +1,178 @@
|
|||||||
|
"""Translation between the web console and the dataflow.
|
||||||
|
|
||||||
|
The console was written against the monolithic repo's REST API and reads a
|
||||||
|
telemetry snapshot by key. Those keys are the contract here: `app.js` does
|
||||||
|
`t.owner === "web"` and `t.estop` directly, so renaming one breaks the page
|
||||||
|
silently. They are reproduced rather than improved for that reason.
|
||||||
|
|
||||||
|
The two vocabularies do not line up, which is most of what this file is for. The
|
||||||
|
arm node reports `connecting|ready|moving|recovering|unavailable`; the console
|
||||||
|
draws `idle|moving|jogging|freedrive|estop|stopped|disconnected`. Neither is
|
||||||
|
wrong — one describes a driver's lifecycle, the other an operator's screen — and
|
||||||
|
:func:`arm_state` is where the two are reconciled.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
|
||||||
|
# What the arm node publishes on `robot_state`.
|
||||||
|
CONNECTING = "connecting"
|
||||||
|
READY = "ready"
|
||||||
|
MOVING = "moving"
|
||||||
|
RECOVERING = "recovering"
|
||||||
|
UNAVAILABLE = "unavailable"
|
||||||
|
|
||||||
|
# What the console draws.
|
||||||
|
UI_IDLE = "idle"
|
||||||
|
UI_MOVING = "moving"
|
||||||
|
UI_JOGGING = "jogging"
|
||||||
|
UI_FREEDRIVE = "freedrive"
|
||||||
|
UI_ESTOP = "estop"
|
||||||
|
UI_STOPPED = "stopped"
|
||||||
|
UI_DISCONNECTED = "disconnected"
|
||||||
|
|
||||||
|
# An arm the node cannot speak for is `disconnected` whatever the reason, because
|
||||||
|
# that is the only thing an operator can act on. `recovering` is its own case:
|
||||||
|
# the arm is there and answering, it just will not move yet.
|
||||||
|
ARM_STATE = {
|
||||||
|
CONNECTING: UI_DISCONNECTED,
|
||||||
|
UNAVAILABLE: UI_DISCONNECTED,
|
||||||
|
RECOVERING: UI_STOPPED,
|
||||||
|
READY: UI_IDLE,
|
||||||
|
MOVING: UI_MOVING,
|
||||||
|
}
|
||||||
|
|
||||||
|
POSE_KEYS = ("x", "y", "z", "roll", "pitch", "yaw")
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class Command:
|
||||||
|
"""One thing the console asked for."""
|
||||||
|
|
||||||
|
type: str
|
||||||
|
# Normalised, `[-1, 1]` per degree of freedom — a fraction of full speed,
|
||||||
|
# exactly what a stick reports. Not a velocity: the console does not know
|
||||||
|
# the robot's units and must not have to.
|
||||||
|
axes: list[float] = field(default_factory=list)
|
||||||
|
action: str = ""
|
||||||
|
preempt: bool = False
|
||||||
|
enabled: bool = False
|
||||||
|
row: dict = field(default_factory=dict)
|
||||||
|
|
||||||
|
|
||||||
|
def command_of(payload: dict) -> Command | None:
|
||||||
|
"""Read one console message, or None if it is not one we serve.
|
||||||
|
|
||||||
|
A message we do not recognise is dropped rather than refused. The console
|
||||||
|
and this node version independently, and a page from a newer build saying
|
||||||
|
something we have not learned yet must not take the arm down.
|
||||||
|
"""
|
||||||
|
if not isinstance(payload, dict):
|
||||||
|
return None
|
||||||
|
kind = payload.get("type")
|
||||||
|
if not isinstance(kind, str):
|
||||||
|
return None
|
||||||
|
|
||||||
|
if kind == "jog":
|
||||||
|
axes = payload.get("axes") or []
|
||||||
|
if len(axes) != 6 or not all(isinstance(v, (int, float)) for v in axes):
|
||||||
|
return None
|
||||||
|
return Command(type="jog", axes=[float(v) for v in axes])
|
||||||
|
if kind in ("jog_stop", "estop", "reset", "home"):
|
||||||
|
return Command(type=kind)
|
||||||
|
if kind == "control":
|
||||||
|
return Command(
|
||||||
|
type="control",
|
||||||
|
action=str(payload.get("action", "")),
|
||||||
|
preempt=bool(payload.get("preempt", False)),
|
||||||
|
)
|
||||||
|
if kind == "gripper":
|
||||||
|
return Command(type="gripper", enabled=bool(payload.get("closed", False)))
|
||||||
|
if kind == "freedrive":
|
||||||
|
return Command(type="freedrive", enabled=bool(payload.get("enabled", False)))
|
||||||
|
if kind == "move":
|
||||||
|
row = {key: payload[key] for key in POSE_KEYS if isinstance(payload.get(key), (int, float))}
|
||||||
|
if not all(key in row for key in ("x", "y", "z")):
|
||||||
|
return None
|
||||||
|
if isinstance(payload.get("speed"), (int, float)):
|
||||||
|
row["speed"] = float(payload["speed"])
|
||||||
|
return Command(type="move", row=row)
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class ArmView:
|
||||||
|
"""What this node currently believes about the arm.
|
||||||
|
|
||||||
|
Assembled from four places — `robot_state`, `tcp_pose`, `joint_pose` and the
|
||||||
|
answers to `get_state` — because no single one of them carries everything
|
||||||
|
the console draws. Whatever has not arrived yet stays at its default, so a
|
||||||
|
page that loads before the arm is reachable renders as disconnected rather
|
||||||
|
than as a stale pose.
|
||||||
|
"""
|
||||||
|
|
||||||
|
state: str = CONNECTING
|
||||||
|
pose: list[float] = field(default_factory=list)
|
||||||
|
joints: list[float] = field(default_factory=list)
|
||||||
|
connected: bool = False
|
||||||
|
ready: bool = False
|
||||||
|
tool_engaged: bool = False
|
||||||
|
freedrive_on: bool = False
|
||||||
|
estop: bool = False
|
||||||
|
|
||||||
|
def take_get_state(self, row: dict) -> None:
|
||||||
|
"""Absorb a `get_state` answer, which carries what telemetry does not."""
|
||||||
|
self.connected = bool(row.get("connected", False))
|
||||||
|
self.ready = bool(row.get("ready", False))
|
||||||
|
self.tool_engaged = bool(row.get("tool_engaged", False))
|
||||||
|
self.freedrive_on = bool(row.get("freedrive", False))
|
||||||
|
|
||||||
|
|
||||||
|
def arm_state(view: ArmView, jogging: bool) -> str:
|
||||||
|
"""Return the state the console should draw.
|
||||||
|
|
||||||
|
Order matters, and it is the order of what an operator most needs to know.
|
||||||
|
A latched e-stop wins over everything: the arm may well read `ready`, and
|
||||||
|
saying so would invite somebody to push it. Freedrive outranks idle because
|
||||||
|
an arm that can be pushed by hand is not the same as one holding station.
|
||||||
|
"""
|
||||||
|
if view.estop:
|
||||||
|
return UI_ESTOP
|
||||||
|
if view.state in (CONNECTING, UNAVAILABLE):
|
||||||
|
return ARM_STATE[view.state]
|
||||||
|
if view.freedrive_on:
|
||||||
|
return UI_FREEDRIVE
|
||||||
|
if jogging and view.state == MOVING:
|
||||||
|
return UI_JOGGING
|
||||||
|
return ARM_STATE.get(view.state, UI_DISCONNECTED)
|
||||||
|
|
||||||
|
|
||||||
|
def snapshot(view: ArmView, owner: str | None, jogging: bool, timestamp: float) -> dict:
|
||||||
|
"""Build the telemetry object the console reads.
|
||||||
|
|
||||||
|
Every key here is read by name in `app.js`, so this and the page change
|
||||||
|
together. The names are the robot-agnostic ones: `gripper` rather than
|
||||||
|
`vacuum`, because a page that says vacuum only fits a robot with one on it,
|
||||||
|
and `units` so a page can label a pose without guessing.
|
||||||
|
"""
|
||||||
|
return {
|
||||||
|
"type": "telemetry",
|
||||||
|
"ts": timestamp,
|
||||||
|
"connected": view.connected,
|
||||||
|
"ready": view.ready,
|
||||||
|
"arm_state": arm_state(view, jogging),
|
||||||
|
"owner": owner or "none",
|
||||||
|
"estop": view.estop,
|
||||||
|
"gripper": view.tool_engaged,
|
||||||
|
"freedrive": view.freedrive_on,
|
||||||
|
"position": dict(zip(POSE_KEYS, view.pose, strict=False)) if view.pose else None,
|
||||||
|
"joints": list(view.joints),
|
||||||
|
# What `position` is in, so the page labels it rather than assuming. The
|
||||||
|
# telemetry ports carry whatever the robot publishes; only the command
|
||||||
|
# path is normalised.
|
||||||
|
"units": {"position": "mm", "rotation": "deg"},
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def error(message: str) -> dict:
|
||||||
|
"""Build the message the console shows as a toast."""
|
||||||
|
return {"type": "error", "detail": message}
|
||||||
@@ -0,0 +1,449 @@
|
|||||||
|
<title>teleop console</title>
|
||||||
|
<style>
|
||||||
|
:root {
|
||||||
|
color-scheme: dark;
|
||||||
|
--bg: #101016;
|
||||||
|
--card: #1a1a23;
|
||||||
|
--edge: #2b2b38;
|
||||||
|
--ink: #e8e8f0;
|
||||||
|
--dim: #8a8a9e;
|
||||||
|
--live: #7fd1e8;
|
||||||
|
--warn: #e8a37f;
|
||||||
|
--stop: #e2564a;
|
||||||
|
--go: #43c98b;
|
||||||
|
}
|
||||||
|
* { box-sizing: border-box; }
|
||||||
|
body {
|
||||||
|
margin: 0;
|
||||||
|
padding-block: 16px;
|
||||||
|
padding-inline: 16px;
|
||||||
|
background: var(--bg);
|
||||||
|
color: var(--ink);
|
||||||
|
font: 15px/1.5 ui-monospace, SFMono-Regular, Menlo, monospace;
|
||||||
|
-webkit-user-select: none;
|
||||||
|
user-select: none;
|
||||||
|
-webkit-tap-highlight-color: transparent;
|
||||||
|
}
|
||||||
|
.wrap { max-width: 980px; margin: 0 auto; }
|
||||||
|
|
||||||
|
header {
|
||||||
|
display: flex; flex-wrap: wrap; align-items: center; gap: 10px;
|
||||||
|
margin-bottom: 14px;
|
||||||
|
}
|
||||||
|
header h1 { font-size: 15px; margin: 0; margin-right: auto; letter-spacing: .04em; }
|
||||||
|
.chip {
|
||||||
|
padding: 5px 12px; border-radius: 999px; border: 1px solid var(--edge);
|
||||||
|
background: var(--card); font-size: 13px; white-space: nowrap;
|
||||||
|
}
|
||||||
|
.chip b { color: var(--live); font-weight: 600; }
|
||||||
|
.chip.estop { border-color: var(--stop); color: var(--stop); }
|
||||||
|
.chip.mine { border-color: var(--go); }
|
||||||
|
.chip.off { color: var(--dim); }
|
||||||
|
|
||||||
|
.card {
|
||||||
|
background: var(--card); border: 1px solid var(--edge); border-radius: 12px;
|
||||||
|
padding: 14px; margin-bottom: 12px;
|
||||||
|
}
|
||||||
|
.card h2 {
|
||||||
|
font-size: 11px; text-transform: uppercase; letter-spacing: .1em;
|
||||||
|
color: var(--dim); margin: 0 0 12px; font-weight: 600;
|
||||||
|
}
|
||||||
|
|
||||||
|
.bar { display: flex; flex-wrap: wrap; gap: 8px; }
|
||||||
|
button {
|
||||||
|
flex: 1 1 auto; min-height: 48px; min-width: 104px;
|
||||||
|
background: #272733; color: var(--ink); border: 1px solid #383848;
|
||||||
|
border-radius: 9px; padding: 10px 16px; font: inherit; cursor: pointer;
|
||||||
|
touch-action: manipulation;
|
||||||
|
}
|
||||||
|
button:active:not(:disabled) { background: #33334a; }
|
||||||
|
button:disabled { opacity: .3; cursor: not-allowed; }
|
||||||
|
button.stop { background: #55202a; border-color: var(--stop); color: #ffd9d4; }
|
||||||
|
button.on { background: #1f4a5a; border-color: var(--live); }
|
||||||
|
|
||||||
|
/* --- the jog rings --- */
|
||||||
|
|
||||||
|
.decks { display: flex; flex-wrap: wrap; gap: 14px; }
|
||||||
|
.deck {
|
||||||
|
flex: 1 1 340px;
|
||||||
|
background: var(--card); border: 1px solid var(--edge); border-radius: 12px;
|
||||||
|
padding: 14px;
|
||||||
|
}
|
||||||
|
.deck h2 {
|
||||||
|
font-size: 11px; text-transform: uppercase; letter-spacing: .1em;
|
||||||
|
color: var(--dim); margin: 0 0 12px; font-weight: 600;
|
||||||
|
}
|
||||||
|
.pills { display: flex; gap: 8px; margin-bottom: 12px; }
|
||||||
|
|
||||||
|
.ring { display: block; width: 100%; max-width: 330px; margin-inline: auto; touch-action: none; }
|
||||||
|
.ring .sector {
|
||||||
|
fill: #23232f; stroke: var(--edge); stroke-width: 1.5; cursor: pointer;
|
||||||
|
}
|
||||||
|
.ring .sector.holding { fill: var(--live); stroke: #bfe9f5; }
|
||||||
|
.ring .label {
|
||||||
|
fill: #a6a6bd; font-size: 15px; text-anchor: middle; dominant-baseline: middle;
|
||||||
|
pointer-events: none; font-family: inherit;
|
||||||
|
}
|
||||||
|
.ring .sector.holding + .label { fill: #0b1013; font-weight: 700; }
|
||||||
|
.ring .hub {
|
||||||
|
fill: none; stroke: var(--edge); stroke-width: 1.5;
|
||||||
|
}
|
||||||
|
.ring .hubtext {
|
||||||
|
fill: var(--dim); font-size: 13px; text-anchor: middle; dominant-baseline: middle;
|
||||||
|
pointer-events: none; font-family: inherit; letter-spacing: .08em;
|
||||||
|
}
|
||||||
|
.ring[data-off="true"] { opacity: .32; }
|
||||||
|
.ring[data-off="true"] .sector { cursor: not-allowed; }
|
||||||
|
|
||||||
|
label.speed { display: flex; align-items: center; gap: 12px; color: var(--dim); }
|
||||||
|
label.speed input { flex: 1; height: 34px; }
|
||||||
|
label.speed b { color: var(--live); min-width: 4ch; text-align: right; }
|
||||||
|
|
||||||
|
.readout { display: grid; grid-template-columns: repeat(auto-fit, minmax(150px, 1fr)); gap: 2px 18px; }
|
||||||
|
.row { display: flex; justify-content: space-between; gap: 12px; padding: 2px 0; }
|
||||||
|
.row span:last-child { color: var(--live); }
|
||||||
|
|
||||||
|
#toast { color: var(--warn); min-height: 1.5em; padding: 4px 2px; }
|
||||||
|
#link { color: var(--dim); font-size: 12px; }
|
||||||
|
|
||||||
|
@media (max-width: 560px) {
|
||||||
|
body { padding-inline: 10px; }
|
||||||
|
.deck { padding: 10px; }
|
||||||
|
button { min-width: 84px; }
|
||||||
|
}
|
||||||
|
</style>
|
||||||
|
|
||||||
|
<div class="wrap">
|
||||||
|
|
||||||
|
<header>
|
||||||
|
<h1>teleop console</h1>
|
||||||
|
<span class="chip" id="chip-state">—</span>
|
||||||
|
<span class="chip" id="chip-owner">—</span>
|
||||||
|
</header>
|
||||||
|
|
||||||
|
<div class="card">
|
||||||
|
<h2>Control</h2>
|
||||||
|
<div class="bar">
|
||||||
|
<button id="take">Take control</button>
|
||||||
|
<button id="release">Release</button>
|
||||||
|
<button id="reset">Reset</button>
|
||||||
|
<button class="stop" id="estop">E‑STOP</button>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="decks">
|
||||||
|
<div class="deck">
|
||||||
|
<h2>Translate</h2>
|
||||||
|
<div class="pills">
|
||||||
|
<button data-axis="2" data-sign="-1">−Z</button>
|
||||||
|
<button data-axis="2" data-sign="1">+Z</button>
|
||||||
|
</div>
|
||||||
|
<svg class="ring" id="translate" viewBox="0 0 200 200" aria-label="translate">
|
||||||
|
<g data-north="0,1" data-south="0,-1" data-west="1,1" data-east="1,-1"
|
||||||
|
data-labels="+X,−X,Y+,Y−" data-hub="XYZ"></g>
|
||||||
|
</svg>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="deck">
|
||||||
|
<h2>Rotate</h2>
|
||||||
|
<div class="pills">
|
||||||
|
<button data-axis="5" data-sign="-1">−yaw</button>
|
||||||
|
<button data-axis="5" data-sign="1">+yaw</button>
|
||||||
|
</div>
|
||||||
|
<svg class="ring" id="rotate" viewBox="0 0 200 200" aria-label="rotate">
|
||||||
|
<g data-north="4,1" data-south="4,-1" data-west="3,-1" data-east="3,1"
|
||||||
|
data-labels="+pitch,−pitch,−roll,+roll" data-hub="RXYZ"></g>
|
||||||
|
</svg>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="card">
|
||||||
|
<h2>Speed</h2>
|
||||||
|
<label class="speed">
|
||||||
|
<input id="speed" type="range" min="10" max="100" step="5" value="60">
|
||||||
|
<b id="speedout">60%</b>
|
||||||
|
</label>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="card">
|
||||||
|
<h2>Tool</h2>
|
||||||
|
<div class="bar">
|
||||||
|
<button id="close">Close gripper</button>
|
||||||
|
<button id="open">Open gripper</button>
|
||||||
|
<button id="freedrive">Freedrive</button>
|
||||||
|
<button id="home">Home</button>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="card">
|
||||||
|
<h2>State</h2>
|
||||||
|
<div class="readout" id="state"></div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div id="toast"></div>
|
||||||
|
<div id="link"></div>
|
||||||
|
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<script>
|
||||||
|
"use strict";
|
||||||
|
|
||||||
|
// Read by name out of the node's telemetry snapshot. Robot-agnostic: `gripper`,
|
||||||
|
// not `vacuum`, because a page that says vacuum only fits a robot with one.
|
||||||
|
const KEYS = ["arm_state", "owner", "connected", "ready", "estop", "gripper", "freedrive"];
|
||||||
|
|
||||||
|
// The node stops the robot if the jog stream dries up (JOG_TTL_S, 0.4 s).
|
||||||
|
// Resending faster than that is what makes letting go a stop, with nothing
|
||||||
|
// needing to be sent to say so.
|
||||||
|
const REFRESH_MS = 150;
|
||||||
|
|
||||||
|
// The ring, laid out like the arm's own pendant so the two do not disagree:
|
||||||
|
// a quarter each, north at the top, and a hub that is label rather than button.
|
||||||
|
const GAP_DEG = 3;
|
||||||
|
const OUTER = 95;
|
||||||
|
const INNER = 46;
|
||||||
|
const LABEL_R = 71;
|
||||||
|
const COMPASS = [
|
||||||
|
["north", 225, 315],
|
||||||
|
["east", 315, 405],
|
||||||
|
["south", 45, 135],
|
||||||
|
["west", 135, 225],
|
||||||
|
];
|
||||||
|
|
||||||
|
let socket = null;
|
||||||
|
let owner = "none";
|
||||||
|
let current = {};
|
||||||
|
let axes = [0, 0, 0, 0, 0, 0];
|
||||||
|
let refresh = null;
|
||||||
|
|
||||||
|
// Every direction currently held, as element -> {axis, sign}. A map rather than
|
||||||
|
// one value because two fingers may hold two directions, and lifting one must
|
||||||
|
// not cancel the other.
|
||||||
|
const held = new Map();
|
||||||
|
// Which element each finger is on, so a finger that slides off still releases
|
||||||
|
// the direction it started on.
|
||||||
|
const fingers = new Map();
|
||||||
|
|
||||||
|
function send(payload) {
|
||||||
|
if (socket && socket.readyState === WebSocket.OPEN) {
|
||||||
|
socket.send(JSON.stringify(payload));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function scale() {
|
||||||
|
return Number(document.getElementById("speed").value) / 100;
|
||||||
|
}
|
||||||
|
|
||||||
|
function recompute() {
|
||||||
|
axes = [0, 0, 0, 0, 0, 0];
|
||||||
|
const dial = scale();
|
||||||
|
for (const [, { axis, sign }] of held) axes[axis] += sign * dial;
|
||||||
|
axes = axes.map((v) => Math.max(-1, Math.min(1, v)));
|
||||||
|
}
|
||||||
|
|
||||||
|
function moving() {
|
||||||
|
return axes.some((value) => value !== 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
function pump() {
|
||||||
|
if (moving()) send({ type: "jog", axes });
|
||||||
|
}
|
||||||
|
|
||||||
|
// --- drawing the ring ---
|
||||||
|
|
||||||
|
function onCircle(radius, degrees) {
|
||||||
|
const radians = (degrees * Math.PI) / 180;
|
||||||
|
return [100 + radius * Math.cos(radians), 100 + radius * Math.sin(radians)];
|
||||||
|
}
|
||||||
|
|
||||||
|
function sectorPath(from, to) {
|
||||||
|
const [ax, ay] = onCircle(OUTER, from);
|
||||||
|
const [bx, by] = onCircle(OUTER, to);
|
||||||
|
const [cx, cy] = onCircle(INNER, to);
|
||||||
|
const [dx, dy] = onCircle(INNER, from);
|
||||||
|
return `M${ax} ${ay}A${OUTER} ${OUTER} 0 0 1 ${bx} ${by}`
|
||||||
|
+ `L${cx} ${cy}A${INNER} ${INNER} 0 0 0 ${dx} ${dy}Z`;
|
||||||
|
}
|
||||||
|
|
||||||
|
function svg(name, attributes) {
|
||||||
|
const node = document.createElementNS("http://www.w3.org/2000/svg", name);
|
||||||
|
for (const [key, value] of Object.entries(attributes)) node.setAttribute(key, value);
|
||||||
|
return node;
|
||||||
|
}
|
||||||
|
|
||||||
|
function buildRing(ring) {
|
||||||
|
const group = ring.querySelector("g");
|
||||||
|
const labels = group.dataset.labels.split(",");
|
||||||
|
// Labelled in compass order so the markup reads the way the ring looks:
|
||||||
|
// north, south, west, east.
|
||||||
|
const naming = { north: labels[0], south: labels[1], west: labels[2], east: labels[3] };
|
||||||
|
|
||||||
|
for (const [point, from, to] of COMPASS) {
|
||||||
|
const [axis, sign] = group.dataset[point].split(",").map(Number);
|
||||||
|
const sector = svg("path", {
|
||||||
|
d: sectorPath(from + GAP_DEG, to - GAP_DEG),
|
||||||
|
class: "sector",
|
||||||
|
});
|
||||||
|
sector.dataset.axis = axis;
|
||||||
|
sector.dataset.sign = sign;
|
||||||
|
group.appendChild(sector);
|
||||||
|
|
||||||
|
const middle = (from + to) / 2;
|
||||||
|
const [lx, ly] = onCircle(LABEL_R, middle);
|
||||||
|
const text = svg("text", { x: lx, y: ly, class: "label" });
|
||||||
|
text.textContent = naming[point];
|
||||||
|
group.appendChild(text);
|
||||||
|
}
|
||||||
|
|
||||||
|
group.appendChild(svg("circle", { cx: 100, cy: 100, r: INNER - 6, class: "hub" }));
|
||||||
|
const hub = svg("text", { x: 100, y: 100, class: "hubtext" });
|
||||||
|
hub.textContent = group.dataset.hub;
|
||||||
|
group.appendChild(hub);
|
||||||
|
}
|
||||||
|
|
||||||
|
// --- holding a direction ---
|
||||||
|
|
||||||
|
function grab(element) {
|
||||||
|
if (held.has(element) || element.closest("[data-off=\"true\"]")) return;
|
||||||
|
held.set(element, { axis: Number(element.dataset.axis), sign: Number(element.dataset.sign) });
|
||||||
|
element.classList.add("holding");
|
||||||
|
recompute();
|
||||||
|
pump();
|
||||||
|
// Resend while held. The node's deadman expects a stream, not one message.
|
||||||
|
if (refresh === null) refresh = setInterval(pump, REFRESH_MS);
|
||||||
|
}
|
||||||
|
|
||||||
|
function letGo(element) {
|
||||||
|
if (!held.delete(element)) return;
|
||||||
|
element.classList.remove("holding");
|
||||||
|
recompute();
|
||||||
|
if (moving()) return;
|
||||||
|
// Say so rather than going quiet: the deadman would stop it either way, but a
|
||||||
|
// direction released is an intention and 400 ms of travel is not.
|
||||||
|
send({ type: "jog_stop" });
|
||||||
|
clearInterval(refresh);
|
||||||
|
refresh = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
function releaseEverything() {
|
||||||
|
for (const element of [...held.keys()]) letGo(element);
|
||||||
|
fingers.clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
function wireDirection(element) {
|
||||||
|
element.addEventListener("pointerdown", (event) => {
|
||||||
|
// Keeps this finger's events coming even once it slides off, so letting go
|
||||||
|
// always reaches letGo().
|
||||||
|
element.setPointerCapture(event.pointerId);
|
||||||
|
fingers.set(event.pointerId, element);
|
||||||
|
grab(element);
|
||||||
|
});
|
||||||
|
for (const name of ["pointerup", "pointercancel", "lostpointercapture"]) {
|
||||||
|
element.addEventListener(name, (event) => {
|
||||||
|
fingers.delete(event.pointerId);
|
||||||
|
letGo(element);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
element.addEventListener("contextmenu", (event) => event.preventDefault());
|
||||||
|
}
|
||||||
|
|
||||||
|
function wire() {
|
||||||
|
for (const ring of document.querySelectorAll(".ring")) buildRing(ring);
|
||||||
|
for (const element of document.querySelectorAll(".sector, .pills button[data-axis]")) {
|
||||||
|
wireDirection(element);
|
||||||
|
}
|
||||||
|
|
||||||
|
// A tab that goes away mid-jog must not leave the robot travelling. The node
|
||||||
|
// covers this too; doing it here costs one line.
|
||||||
|
window.addEventListener("blur", releaseEverything);
|
||||||
|
document.addEventListener("visibilitychange", () => {
|
||||||
|
if (document.hidden) releaseEverything();
|
||||||
|
});
|
||||||
|
|
||||||
|
const speed = document.getElementById("speed");
|
||||||
|
speed.addEventListener("input", () => {
|
||||||
|
document.getElementById("speedout").textContent = `${speed.value}%`;
|
||||||
|
recompute();
|
||||||
|
});
|
||||||
|
|
||||||
|
const click = {
|
||||||
|
take: { type: "control", action: "take", preempt: true },
|
||||||
|
release: { type: "control", action: "release" },
|
||||||
|
estop: { type: "estop" },
|
||||||
|
reset: { type: "reset" },
|
||||||
|
close: { type: "gripper", closed: true },
|
||||||
|
open: { type: "gripper", closed: false },
|
||||||
|
home: { type: "home" },
|
||||||
|
};
|
||||||
|
for (const [id, payload] of Object.entries(click)) {
|
||||||
|
document.getElementById(id).addEventListener("click", () => send(payload));
|
||||||
|
}
|
||||||
|
document.getElementById("freedrive").addEventListener("click", () => {
|
||||||
|
send({ type: "freedrive", enabled: !current.freedrive });
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
function render(telemetry) {
|
||||||
|
current = telemetry;
|
||||||
|
const rows = KEYS.map((key) =>
|
||||||
|
`<div class="row"><span>${key}</span><span>${telemetry[key]}</span></div>`);
|
||||||
|
const pose = telemetry.position;
|
||||||
|
if (pose) {
|
||||||
|
// The node says what the pose is in rather than the page assuming it.
|
||||||
|
const unit = (telemetry.units && telemetry.units.position) || "";
|
||||||
|
const shown = ["x", "y", "z"].map((axis) => pose[axis].toFixed(1)).join(", ");
|
||||||
|
rows.push(`<div class="row"><span>position</span><span>${shown} ${unit}</span></div>`);
|
||||||
|
}
|
||||||
|
document.getElementById("state").innerHTML = rows.join("");
|
||||||
|
|
||||||
|
const state = document.getElementById("chip-state");
|
||||||
|
state.innerHTML = `arm <b>${telemetry.arm_state}</b>`;
|
||||||
|
state.className = `chip${telemetry.estop ? " estop" : ""}`;
|
||||||
|
|
||||||
|
owner = telemetry.owner;
|
||||||
|
const mine = owner === "web";
|
||||||
|
const chip = document.getElementById("chip-owner");
|
||||||
|
chip.innerHTML = mine ? "<b>you have the arm</b>" : `held by <b>${owner}</b>`;
|
||||||
|
chip.className = `chip ${mine ? "mine" : "off"}`;
|
||||||
|
|
||||||
|
document.getElementById("freedrive").classList.toggle("on", !!telemetry.freedrive);
|
||||||
|
document.getElementById("close").classList.toggle("on", !!telemetry.gripper);
|
||||||
|
|
||||||
|
// Every control is gated on holding the lease, so one that would be refused
|
||||||
|
// looks refused. E-STOP and Reset never are.
|
||||||
|
for (const id of ["close", "open", "freedrive", "home"]) {
|
||||||
|
document.getElementById(id).disabled = !mine;
|
||||||
|
}
|
||||||
|
for (const ring of document.querySelectorAll(".ring")) {
|
||||||
|
ring.dataset.off = String(!mine);
|
||||||
|
}
|
||||||
|
for (const pill of document.querySelectorAll(".pills button[data-axis]")) {
|
||||||
|
pill.disabled = !mine;
|
||||||
|
}
|
||||||
|
if (!mine) releaseEverything();
|
||||||
|
}
|
||||||
|
|
||||||
|
function connect() {
|
||||||
|
// The node serves this page and holds the socket, so the socket is on this
|
||||||
|
// page's own origin — nothing to configure and nothing to keep in step.
|
||||||
|
const scheme = location.protocol === "https:" ? "wss:" : "ws:";
|
||||||
|
const url = `${scheme}//${location.host}/ws`;
|
||||||
|
document.getElementById("link").textContent = `socket: ${url}`;
|
||||||
|
socket = new WebSocket(url);
|
||||||
|
socket.onmessage = (event) => {
|
||||||
|
const message = JSON.parse(event.data);
|
||||||
|
if (message.type === "telemetry") render(message);
|
||||||
|
if (message.type === "error") {
|
||||||
|
document.getElementById("toast").textContent = message.detail;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
socket.onclose = () => {
|
||||||
|
releaseEverything();
|
||||||
|
document.getElementById("link").textContent = `socket: ${url} — closed, retrying`;
|
||||||
|
setTimeout(connect, 2000);
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
wire();
|
||||||
|
connect();
|
||||||
|
</script>
|
||||||
@@ -0,0 +1,107 @@
|
|||||||
|
"""A bench stand-in for the arm and the browser, so teleop can run with neither.
|
||||||
|
|
||||||
|
Run by `dataflow.yml`. It answers every service, publishes plausible telemetry,
|
||||||
|
and prints the velocities it is sent — which is enough to drive the node from a
|
||||||
|
real gamepad and watch what a real arm would have been told to do.
|
||||||
|
|
||||||
|
**It is not a simulator.** It does not integrate the velocity, move a pose, or
|
||||||
|
model the arm in any way; a jog is printed and forgotten. Its job is to close
|
||||||
|
the loop so the node's own behaviour can be watched, not to stand in for
|
||||||
|
`dora-ulite6`. Anything that needs the arm to actually respond needs the arm.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import json
|
||||||
|
|
||||||
|
import pyarrow as pa
|
||||||
|
from dora import Node
|
||||||
|
|
||||||
|
REQUEST_ID = "request_id"
|
||||||
|
|
||||||
|
# Somewhere in front of the robot, tool pointing down. Fixed, because a moving
|
||||||
|
# pose here would look like the arm was responding, which it is not.
|
||||||
|
POSE = [250.0, 0.0, 200.0, 180.0, 0.0, 0.0]
|
||||||
|
JOINTS = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
"""Answer whatever teleop asks and report what it would have moved."""
|
||||||
|
node = Node("bench")
|
||||||
|
announced = False
|
||||||
|
tool_engaged = False
|
||||||
|
freedrive = False
|
||||||
|
jogs = 0
|
||||||
|
shown = None
|
||||||
|
|
||||||
|
for event in node:
|
||||||
|
if event["type"] != "INPUT":
|
||||||
|
continue
|
||||||
|
|
||||||
|
if event["id"] == "tick":
|
||||||
|
if not announced:
|
||||||
|
announced = True
|
||||||
|
node.send_output("robot_state", pa.array(["ready"]))
|
||||||
|
node.send_output("tcp_pose", pa.array(POSE, type=pa.float32()), {"units_position": "mm"})
|
||||||
|
node.send_output("joint_pose", pa.array(JOINTS, type=pa.float32()), {"units": "deg"})
|
||||||
|
|
||||||
|
elif event["id"] == "jog":
|
||||||
|
velocity = event["value"][0].as_py()["velocity"]
|
||||||
|
jogs += 1
|
||||||
|
moving = any(velocity)
|
||||||
|
shown = ", ".join(f"{v:+7.2f}" for v in velocity)
|
||||||
|
print(f"jog {jogs:5d} [{shown}] {'' if moving else '<- stop'}", flush=True)
|
||||||
|
|
||||||
|
elif event["id"] == "request":
|
||||||
|
row = event["value"][0].as_py()
|
||||||
|
service = row.get("service", "")
|
||||||
|
request_id = event["metadata"].get(REQUEST_ID, "")
|
||||||
|
if service == "grasp":
|
||||||
|
tool_engaged = True
|
||||||
|
elif service == "release":
|
||||||
|
tool_engaged = False
|
||||||
|
elif service == "freedrive":
|
||||||
|
freedrive = bool(row.get("enabled", True))
|
||||||
|
if service != "get_state":
|
||||||
|
print(f"service {service} ({request_id})", flush=True)
|
||||||
|
node.send_output(
|
||||||
|
"response",
|
||||||
|
pa.array([_answer(service, tool_engaged, freedrive)]),
|
||||||
|
{REQUEST_ID: request_id},
|
||||||
|
)
|
||||||
|
|
||||||
|
elif event["id"] == "ui_state":
|
||||||
|
# No browser is attached, so this is where the console's picture of
|
||||||
|
# the arm would go. Printing it on change rather than on every tick
|
||||||
|
# makes the things worth watching — the lease moving, the arm
|
||||||
|
# starting and stopping — visible without 20 lines a second.
|
||||||
|
snapshot = json.loads(event["value"][0].as_py())
|
||||||
|
if snapshot.get("type") != "telemetry":
|
||||||
|
continue
|
||||||
|
current = (snapshot["arm_state"], snapshot["owner"], snapshot["estop"])
|
||||||
|
if current != shown:
|
||||||
|
shown = current
|
||||||
|
state, owner, estop = current
|
||||||
|
print(
|
||||||
|
f"console would show: {state}, owner {owner}" + (", E-STOPPED" if estop else ""),
|
||||||
|
flush=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _answer(service: str, tool_engaged: bool, freedrive: bool) -> dict:
|
||||||
|
"""Build the response row the arm node would have sent."""
|
||||||
|
if service == "get_state":
|
||||||
|
return {
|
||||||
|
"ok": True,
|
||||||
|
"service": service,
|
||||||
|
"message": "ready",
|
||||||
|
"connected": True,
|
||||||
|
"ready": True,
|
||||||
|
"tool_engaged": tool_engaged,
|
||||||
|
"freedrive": freedrive,
|
||||||
|
"pose": POSE,
|
||||||
|
"joints": JOINTS,
|
||||||
|
}
|
||||||
|
return {"ok": True, "service": service, "message": f"{service} accepted by the bench"}
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,41 @@
|
|||||||
|
# Manifest for the bench stand-in.
|
||||||
|
#
|
||||||
|
# dora resolves a node's manifest from its `path:`, so without this file the
|
||||||
|
# bench would be validated against the teleop node's own contract and every port
|
||||||
|
# would look wrong.
|
||||||
|
|
||||||
|
apiVersion: 1
|
||||||
|
name: bench
|
||||||
|
namespace: liria
|
||||||
|
description: Bench stand-in for the arm and the browser, so teleop can run with neither
|
||||||
|
|
||||||
|
runtime: python
|
||||||
|
entrypoint: bench.py
|
||||||
|
dora: ">=1.0"
|
||||||
|
|
||||||
|
inputs:
|
||||||
|
tick:
|
||||||
|
description: Drives the telemetry the arm node would publish.
|
||||||
|
type: std/core/v1/UInt64
|
||||||
|
jog:
|
||||||
|
description: A velocity teleop wants the arm to move at. Printed, then forgotten.
|
||||||
|
request:
|
||||||
|
description: A service call. Every one is answered ok.
|
||||||
|
ui_state:
|
||||||
|
description: >-
|
||||||
|
The console snapshot teleop publishes. Printed on change, which is how the
|
||||||
|
lease moving and the arm starting and stopping can be watched without a
|
||||||
|
browser.
|
||||||
|
|
||||||
|
outputs:
|
||||||
|
response:
|
||||||
|
description: One answer per request, under the id it arrived with.
|
||||||
|
robot_state:
|
||||||
|
description: Reports `ready` once, so teleop stops drawing the arm as disconnected.
|
||||||
|
type: std/core/v1/String
|
||||||
|
tcp_pose:
|
||||||
|
description: A fixed tool-tip pose. Fixed on purpose — nothing here integrates a velocity.
|
||||||
|
type: std/core/v1/Float32
|
||||||
|
joint_pose:
|
||||||
|
description: Fixed joint angles, for the same reason.
|
||||||
|
type: std/core/v1/Float32
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
[project]
|
||||||
|
name = "dora-teleop"
|
||||||
|
version = "0.3.0"
|
||||||
|
description = "Dora node giving a human the arm, from a gamepad or a browser"
|
||||||
|
readme = "README.md"
|
||||||
|
license = { text = "MIT" }
|
||||||
|
authors = [{ name = "Liria" }]
|
||||||
|
|
||||||
|
# dora-rs 1.0 requires 3.11; the node uses PEP 604 unions and zip(strict=).
|
||||||
|
requires-python = ">=3.11"
|
||||||
|
|
||||||
|
# No hardware dependency. The pad is `dora-joy` and the arm is `dora-ulite6`;
|
||||||
|
# this node decides, and serves the console that asks it to.
|
||||||
|
dependencies = [
|
||||||
|
"dora-rs >= 1.0.1",
|
||||||
|
"pyarrow >= 17.0",
|
||||||
|
# The web console: the page and the browsers' sockets on one port. Replaces
|
||||||
|
# `dora-websocket-transceiver`, which was a second node for the same job.
|
||||||
|
"aiohttp >= 3.9",
|
||||||
|
]
|
||||||
|
|
||||||
|
[dependency-groups]
|
||||||
|
dev = ["pytest >= 8.1", "ruff >= 0.9"]
|
||||||
|
|
||||||
|
[project.scripts]
|
||||||
|
dora-teleop = "dora_teleop.main:main"
|
||||||
|
dora-teleop-probe = "dora_teleop.probe:main"
|
||||||
|
|
||||||
|
[build-system]
|
||||||
|
requires = ["hatchling"]
|
||||||
|
build-backend = "hatchling.build"
|
||||||
|
|
||||||
|
[tool.hatch.build.targets.wheel]
|
||||||
|
packages = ["dora_teleop"]
|
||||||
|
|
||||||
|
[tool.ruff]
|
||||||
|
line-length = 110
|
||||||
|
|
||||||
|
[tool.ruff.lint]
|
||||||
|
extend-select = ["D", "I", "N", "RET", "UP"]
|
||||||
|
|
||||||
|
[tool.ruff.lint.pydocstyle]
|
||||||
|
convention = "numpy"
|
||||||
|
|
||||||
|
[tool.ruff.lint.per-file-ignores]
|
||||||
|
# Test names are the documentation here; a docstring would only repeat them.
|
||||||
|
"tests/*" = ["D1"]
|
||||||
|
|
||||||
|
[tool.pytest.ini_options]
|
||||||
|
testpaths = ["tests"]
|
||||||
@@ -0,0 +1,119 @@
|
|||||||
|
"""Shared fakes. Nothing here needs a gamepad, an arm, a browser or a daemon."""
|
||||||
|
|
||||||
|
import json
|
||||||
|
|
||||||
|
import pyarrow as pa
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from dora_teleop.config import Config
|
||||||
|
from dora_teleop.joy import Sample
|
||||||
|
|
||||||
|
# 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<null>`,
|
||||||
|
# so a test for an absent pad would otherwise build a message no consumer sees.
|
||||||
|
JOY = pa.struct(
|
||||||
|
[
|
||||||
|
("axes", pa.list_(pa.float32())),
|
||||||
|
("buttons", pa.list_(pa.bool_())),
|
||||||
|
("present", pa.bool_()),
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class RecordingNode:
|
||||||
|
"""A dora node that records outputs *with* their metadata.
|
||||||
|
|
||||||
|
`dora.testing.MockNode` drops the metadata dict on `send_output`, and the
|
||||||
|
metadata is where `request_id` rides. So the tests record it themselves.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self.sent = []
|
||||||
|
|
||||||
|
def send_output(self, output_id, data, metadata=None):
|
||||||
|
self.sent.append((output_id, data, dict(metadata or {})))
|
||||||
|
|
||||||
|
def outputs(self, output_id):
|
||||||
|
"""Return every message sent on ``output_id`` as (value, metadata) pairs."""
|
||||||
|
return [(data[0].as_py(), metadata) for name, data, metadata in self.sent if name == output_id]
|
||||||
|
|
||||||
|
def jogs(self):
|
||||||
|
"""Return every velocity sent on the deprecated `jog` port, in order."""
|
||||||
|
return [row["velocity"] for row, _ in self.outputs("jog")]
|
||||||
|
|
||||||
|
def twists(self):
|
||||||
|
"""Return every Twist sent on the standard port, in order."""
|
||||||
|
return [row for row, _ in self.outputs("twist")]
|
||||||
|
|
||||||
|
def actions(self):
|
||||||
|
"""Return the verb of every standard `command`, in order."""
|
||||||
|
return [row["action"] for row, _ in self.outputs("command")]
|
||||||
|
|
||||||
|
def services(self):
|
||||||
|
"""Return the name of every service called, in order."""
|
||||||
|
return [row["service"] for row, _ in self.outputs("request")]
|
||||||
|
|
||||||
|
def ui(self):
|
||||||
|
"""Return every object handed to the bridge, parsed."""
|
||||||
|
return [json.loads(value) for value, _ in self.outputs("ui_state")]
|
||||||
|
|
||||||
|
def clear(self):
|
||||||
|
self.sent.clear()
|
||||||
|
|
||||||
|
|
||||||
|
def stick(axes=None, buttons=None, present=True):
|
||||||
|
"""Build one gamepad reading, padded to a full six axes."""
|
||||||
|
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):
|
||||||
|
if axes is None or index >= len(axes):
|
||||||
|
values[index] = -1.0
|
||||||
|
return Sample(axes=values, buttons=list(buttons or []), present=present)
|
||||||
|
|
||||||
|
|
||||||
|
def joy_message(sample):
|
||||||
|
"""Build the Arrow row `dora-joy` would publish for one reading."""
|
||||||
|
return pa.array(
|
||||||
|
[
|
||||||
|
{
|
||||||
|
"axes": [float(value) for value in sample.axes],
|
||||||
|
"buttons": [bool(down) for down in sample.buttons],
|
||||||
|
"present": sample.present,
|
||||||
|
}
|
||||||
|
],
|
||||||
|
type=JOY,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def input_event(event_id, value, metadata=None):
|
||||||
|
"""Build the event dict dora hands a node for one input."""
|
||||||
|
return {"type": "INPUT", "id": event_id, "value": value, "metadata": dict(metadata or {})}
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.fixture(autouse=True)
|
||||||
|
def clean_env(monkeypatch):
|
||||||
|
"""Run every test against a bare environment, whatever the shell set."""
|
||||||
|
for name in (
|
||||||
|
"SPEED_MM_S",
|
||||||
|
"SPEED_DEG_S",
|
||||||
|
"DEADZONE",
|
||||||
|
"EXPO",
|
||||||
|
"JOG_TTL_S",
|
||||||
|
"JOG_KEEPALIVE_S",
|
||||||
|
"JOY_TTL_S",
|
||||||
|
"GAMEPAD_IDLE_RELEASE_S",
|
||||||
|
"STATE_POLL_S",
|
||||||
|
"WEB_HOST",
|
||||||
|
"WEB_PORT",
|
||||||
|
):
|
||||||
|
monkeypatch.delenv(name, raising=False)
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.fixture
|
||||||
|
def config():
|
||||||
|
"""Return the default configuration, with expo off so a test can do the arithmetic."""
|
||||||
|
built = Config.from_env()
|
||||||
|
built.expo = 0.0
|
||||||
|
return built
|
||||||
@@ -0,0 +1,106 @@
|
|||||||
|
"""The lease: who may drive the arm, and how it changes hands."""
|
||||||
|
|
||||||
|
from dora_teleop.arbiter import GAMEPAD, WEB, Arbiter
|
||||||
|
|
||||||
|
|
||||||
|
def build(idle_release_s=2.0):
|
||||||
|
return Arbiter(idle_release_s)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_free_arm_goes_to_whoever_asks():
|
||||||
|
arbiter = build()
|
||||||
|
|
||||||
|
assert arbiter.take(WEB, now := 100.0) is True
|
||||||
|
assert arbiter.owner == WEB
|
||||||
|
assert arbiter.may_drive(WEB)
|
||||||
|
assert now == 100.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_held_arm_is_not_handed_over_for_the_asking():
|
||||||
|
arbiter = build()
|
||||||
|
arbiter.take(GAMEPAD, 100.0)
|
||||||
|
|
||||||
|
assert arbiter.take(WEB, 101.0) is False
|
||||||
|
assert arbiter.owner == GAMEPAD
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_non_owner_may_not_drive():
|
||||||
|
arbiter = build()
|
||||||
|
arbiter.take(GAMEPAD, 100.0)
|
||||||
|
|
||||||
|
assert arbiter.may_drive(WEB) is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_preempting_takes_a_held_arm():
|
||||||
|
# The console offers this deliberately: an operator who can see the arm is
|
||||||
|
# better placed than this code to decide they should have it.
|
||||||
|
arbiter = build()
|
||||||
|
arbiter.take(GAMEPAD, 100.0)
|
||||||
|
|
||||||
|
assert arbiter.take(WEB, 101.0, preempt=True) is True
|
||||||
|
assert arbiter.owner == WEB
|
||||||
|
|
||||||
|
|
||||||
|
def test_taking_an_arm_you_already_hold_is_not_a_conflict():
|
||||||
|
arbiter = build()
|
||||||
|
arbiter.take(WEB, 100.0)
|
||||||
|
|
||||||
|
assert arbiter.take(WEB, 101.0) is True
|
||||||
|
|
||||||
|
|
||||||
|
def test_releasing_frees_the_arm():
|
||||||
|
arbiter = build()
|
||||||
|
arbiter.take(WEB, 100.0)
|
||||||
|
|
||||||
|
arbiter.release(WEB)
|
||||||
|
|
||||||
|
assert arbiter.owner is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_releasing_an_arm_somebody_else_holds_does_nothing():
|
||||||
|
arbiter = build()
|
||||||
|
arbiter.take(GAMEPAD, 100.0)
|
||||||
|
|
||||||
|
arbiter.release(WEB)
|
||||||
|
|
||||||
|
assert arbiter.owner == GAMEPAD
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_idle_gamepad_hands_the_arm_back():
|
||||||
|
# A pad left on the bench would otherwise lock the browser out forever.
|
||||||
|
arbiter = build(idle_release_s=2.0)
|
||||||
|
arbiter.take(GAMEPAD, 100.0)
|
||||||
|
|
||||||
|
arbiter.expire_idle_gamepad(102.0)
|
||||||
|
|
||||||
|
assert arbiter.owner is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_gamepad_still_being_used_keeps_the_arm():
|
||||||
|
arbiter = build(idle_release_s=2.0)
|
||||||
|
arbiter.take(GAMEPAD, 100.0)
|
||||||
|
|
||||||
|
arbiter.note_activity(GAMEPAD, 101.5)
|
||||||
|
arbiter.expire_idle_gamepad(102.0)
|
||||||
|
|
||||||
|
assert arbiter.owner == GAMEPAD
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_web_never_expires_on_its_own():
|
||||||
|
# It took the arm by pressing a button and gives it back by pressing another.
|
||||||
|
arbiter = build(idle_release_s=2.0)
|
||||||
|
arbiter.take(WEB, 100.0)
|
||||||
|
|
||||||
|
arbiter.expire_idle_gamepad(999.0)
|
||||||
|
|
||||||
|
assert arbiter.owner == WEB
|
||||||
|
|
||||||
|
|
||||||
|
def test_activity_from_a_non_owner_does_not_extend_anyone_else():
|
||||||
|
arbiter = build(idle_release_s=2.0)
|
||||||
|
arbiter.take(GAMEPAD, 100.0)
|
||||||
|
|
||||||
|
arbiter.note_activity(WEB, 101.9)
|
||||||
|
arbiter.expire_idle_gamepad(102.0)
|
||||||
|
|
||||||
|
assert arbiter.owner is None
|
||||||
@@ -0,0 +1,97 @@
|
|||||||
|
"""Settings: parsed once at startup, and refused there rather than mid-motion."""
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from dora_teleop.config import Config
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_defaults_need_no_environment():
|
||||||
|
config = Config.from_env()
|
||||||
|
|
||||||
|
assert config.speed_mm_s == 60.0
|
||||||
|
assert config.speed_deg_s == 30.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_deadman_is_shorter_than_the_arms_own_expiry():
|
||||||
|
# The arm node's VELOCITY_COMMAND_TTL_S defaults to 0.5 s and is the
|
||||||
|
# backstop. Ours has to fire first, or the backstop backs up nothing.
|
||||||
|
assert Config.from_env().deadman.jog_ttl_s < 0.5
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_layers_fire_innermost_first():
|
||||||
|
# A stale pad reading has to be noticed before the jog deadline it feeds.
|
||||||
|
deadman = Config.from_env().deadman
|
||||||
|
|
||||||
|
assert deadman.joy_ttl_s < deadman.jog_ttl_s
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_blank_variable_falls_back_to_the_default(monkeypatch):
|
||||||
|
monkeypatch.setenv("SPEED_MM_S", " ")
|
||||||
|
|
||||||
|
assert Config.from_env().speed_mm_s == 60.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_speed_that_is_not_a_number_names_itself(monkeypatch):
|
||||||
|
monkeypatch.setenv("SPEED_MM_S", "brisk")
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="SPEED_MM_S"):
|
||||||
|
Config.from_env()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_joy_ttl_at_or_above_the_jog_ttl_is_refused_at_startup(monkeypatch):
|
||||||
|
# Layers out of order means a backstop firing before the thing it backs up,
|
||||||
|
# which is invisible until the day something actually goes wrong.
|
||||||
|
monkeypatch.setenv("JOY_TTL_S", "0.4")
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="JOY_TTL_S"):
|
||||||
|
Config.from_env()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_keepalive_at_or_above_the_jog_ttl_is_refused_at_startup(monkeypatch):
|
||||||
|
# The repeat has to outpace the arm's own velocity expiry. It cannot be read
|
||||||
|
# from here, but JOG_TTL_S already sits below it, so a repeat slower than
|
||||||
|
# JOG_TTL_S is a repeat the arm stops between — an arm that stutters under a
|
||||||
|
# held stick, with nothing in the logs to say why.
|
||||||
|
monkeypatch.setenv("JOG_KEEPALIVE_S", "0.4")
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="JOG_KEEPALIVE_S"):
|
||||||
|
Config.from_env()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_keepalive_of_zero_is_refused_at_startup(monkeypatch):
|
||||||
|
# Zero would mean repeating every tick, which is the behaviour the keepalive
|
||||||
|
# exists to replace.
|
||||||
|
monkeypatch.setenv("JOG_KEEPALIVE_S", "0")
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="JOG_KEEPALIVE_S"):
|
||||||
|
Config.from_env()
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("value", ["1.0", "-0.1", "2"])
|
||||||
|
def test_a_deadzone_outside_the_stick_is_refused_at_startup(monkeypatch, value):
|
||||||
|
# A deadzone of 1 would silently make the pad inert; catching it here beats
|
||||||
|
# finding out when somebody picks it up.
|
||||||
|
monkeypatch.setenv("DEADZONE", value)
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="DEADZONE"):
|
||||||
|
Config.from_env()
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize("value", ["-0.1", "1.1"])
|
||||||
|
def test_an_expo_outside_the_curve_is_refused_at_startup(monkeypatch, value):
|
||||||
|
monkeypatch.setenv("EXPO", value)
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="EXPO"):
|
||||||
|
Config.from_env()
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_timings_can_be_tuned_without_touching_the_speeds(monkeypatch):
|
||||||
|
monkeypatch.setenv("JOG_TTL_S", "0.25")
|
||||||
|
monkeypatch.setenv("JOY_TTL_S", "0.1")
|
||||||
|
monkeypatch.setenv("GAMEPAD_IDLE_RELEASE_S", "5")
|
||||||
|
|
||||||
|
config = Config.from_env()
|
||||||
|
|
||||||
|
assert config.deadman.jog_ttl_s == 0.25
|
||||||
|
assert config.deadman.joy_ttl_s == 0.1
|
||||||
|
assert config.deadman.gamepad_idle_release_s == 5.0
|
||||||
@@ -0,0 +1,89 @@
|
|||||||
|
"""The page and the node are one contract, checked here rather than by eye.
|
||||||
|
|
||||||
|
`ui.py` translates for a console it cannot import, and the page reads telemetry
|
||||||
|
by key. Nothing in either language catches a rename, so a typo would surface as
|
||||||
|
a dead button on a machine that can move — which is the wrong place to find it.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import json
|
||||||
|
import re
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
from dora_teleop import ui
|
||||||
|
|
||||||
|
PAGE = Path(__file__).resolve().parent.parent / "dora_teleop" / "web" / "index.html"
|
||||||
|
|
||||||
|
|
||||||
|
def page():
|
||||||
|
return PAGE.read_text()
|
||||||
|
|
||||||
|
|
||||||
|
def sent_types():
|
||||||
|
"""Return every message type the page can send."""
|
||||||
|
return set(re.findall(r'type:\s*"([a-z_]+)"', page()))
|
||||||
|
|
||||||
|
|
||||||
|
def read_keys():
|
||||||
|
"""Return the telemetry keys the page renders by name."""
|
||||||
|
listed = re.search(r"const KEYS = \[(.*?)\]", page(), re.DOTALL).group(1)
|
||||||
|
return set(re.findall(r'"([a-z_]+)"', listed))
|
||||||
|
|
||||||
|
|
||||||
|
def snapshot_keys():
|
||||||
|
view = ui.ArmView()
|
||||||
|
view.pose = [1.0] * 6
|
||||||
|
return set(ui.snapshot(view, "web", False, 0.0))
|
||||||
|
|
||||||
|
|
||||||
|
def test_every_message_the_page_sends_is_one_the_node_serves():
|
||||||
|
# `data` is the bridge's own envelope, not a console command.
|
||||||
|
for kind in sent_types() - {"data"}:
|
||||||
|
assert ui.command_of({"type": kind}) is not None or kind == "jog", kind
|
||||||
|
|
||||||
|
|
||||||
|
def test_every_telemetry_key_the_page_reads_is_one_the_node_sends():
|
||||||
|
missing = read_keys() - snapshot_keys()
|
||||||
|
|
||||||
|
assert not missing, f"the page reads keys the node does not send: {sorted(missing)}"
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_page_reads_the_pose_units_from_the_node():
|
||||||
|
# Rather than assuming mm, which is only true of one robot.
|
||||||
|
assert "units.position" in page().replace("telemetry.units && telemetry.units.position", "units.position")
|
||||||
|
assert "units" in snapshot_keys()
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_jog_the_page_sends_is_normalised_and_accepted():
|
||||||
|
# Six axes, each a fraction of full deflection. If the page ever sent a
|
||||||
|
# velocity instead, it would have to know the robot's units.
|
||||||
|
command = ui.command_of(json.loads('{"type": "jog", "axes": [0, -0.6, 0, 0, 0, 0]}'))
|
||||||
|
|
||||||
|
assert command is not None
|
||||||
|
assert command.axes == [0.0, -0.6, 0.0, 0.0, 0.0, 0.0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_page_can_ask_for_everything_the_node_serves():
|
||||||
|
# `move` is the one command with no button: it is a pose in the robot's own
|
||||||
|
# units, which is exactly what this page is built not to know.
|
||||||
|
served = {"jog", "jog_stop", "estop", "reset", "home", "control", "gripper", "freedrive"}
|
||||||
|
|
||||||
|
assert served <= sent_types()
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_page_gates_its_jog_pad_on_holding_the_lease():
|
||||||
|
# The node refuses a jog from a non-owner and answers with a toast; a pad
|
||||||
|
# that looks live but is refused teaches an operator to distrust the screen.
|
||||||
|
assert 'owner === "web"' in page()
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_published_schemas_do_not_depend_on_the_value_sent():
|
||||||
|
# pyarrow reads a null `value` as its own type, so an inferred schema would
|
||||||
|
# change the type of the `command` port between one verb and the next.
|
||||||
|
from dora_teleop import message
|
||||||
|
|
||||||
|
assert message.pack_command("stop", None).type == message.COMMAND
|
||||||
|
assert message.pack_command("freedrive", 1.0).type == message.COMMAND
|
||||||
|
assert (
|
||||||
|
message.pack_twist({"linear": [0.0] * 3, "angular": [0.0] * 3, "frame": "base"}).type == message.TWIST
|
||||||
|
)
|
||||||
|
assert message.pack_jog([0.0] * 6).type == message.JOG
|
||||||
@@ -0,0 +1,431 @@
|
|||||||
|
"""The deadman: every way a source can go away, and the stop that follows it."""
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
STOP = [0.0] * 6
|
||||||
|
PUSHED = [0.0, -1.0]
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.fixture
|
||||||
|
def clock(monkeypatch):
|
||||||
|
"""Return a clock the test moves by hand, so no test waits out a real timeout."""
|
||||||
|
|
||||||
|
class Clock:
|
||||||
|
def __init__(self):
|
||||||
|
self.t = 1000.0
|
||||||
|
|
||||||
|
def advance(self, seconds):
|
||||||
|
self.t += seconds
|
||||||
|
|
||||||
|
moved = Clock()
|
||||||
|
monkeypatch.setattr("dora_teleop.main.now", lambda: moved.t)
|
||||||
|
return moved
|
||||||
|
|
||||||
|
|
||||||
|
def build():
|
||||||
|
config = Config.from_env()
|
||||||
|
config.expo = 0.0
|
||||||
|
return RecordingNode(), loop.Teleop(config)
|
||||||
|
|
||||||
|
|
||||||
|
def send_joy(node, state, sample):
|
||||||
|
"""Deliver one reading the way `dora-joy` publishes it, decoding included."""
|
||||||
|
loop._handle(node, state, input_event("joy", joy_message(sample)))
|
||||||
|
|
||||||
|
|
||||||
|
def ticks(node, state, count=1):
|
||||||
|
"""Run `count` ticks with nothing new arriving, and return every velocity sent."""
|
||||||
|
for _ in range(count):
|
||||||
|
loop._tick(node, state)
|
||||||
|
return node.jogs()
|
||||||
|
|
||||||
|
|
||||||
|
def drive(node, state, sample, count=1):
|
||||||
|
"""Hold one reading for `count` ticks, the way a real pad and joy node would."""
|
||||||
|
for _ in range(count):
|
||||||
|
send_joy(node, state, sample)
|
||||||
|
loop._tick(node, state)
|
||||||
|
return node.jogs()
|
||||||
|
|
||||||
|
|
||||||
|
def web_jog(node, state, axes):
|
||||||
|
"""Deliver one console jog message, in the normalised units the page speaks."""
|
||||||
|
loop._accept_ui_command(node, state, {"type": "jog", "axes": axes})
|
||||||
|
|
||||||
|
|
||||||
|
# --- the gamepad ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pushed_stick_takes_the_arm_and_drives_it(clock):
|
||||||
|
# Nobody should have to reach for a browser to use the pad in their hands.
|
||||||
|
node, state = build()
|
||||||
|
|
||||||
|
jogs = drive(node, state, stick(axes=PUSHED))
|
||||||
|
|
||||||
|
assert state.arbiter.owner == GAMEPAD
|
||||||
|
assert jogs[-1][0] > 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_gamepad_that_is_unplugged_mid_jog_stops_the_arm(clock):
|
||||||
|
# An unplugged pad is not a centred pad, and waiting out the TTL here is the
|
||||||
|
# difference between a dropped controller costing 10 ms of travel and 400.
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=PUSHED))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
jogs = drive(node, state, Sample(present=False))
|
||||||
|
|
||||||
|
assert jogs == [STOP]
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_joy_node_dying_stops_the_arm(clock):
|
||||||
|
# The pad is another process now, and that process dying looks like silence
|
||||||
|
# rather than like an unplugged pad. It has to mean the same thing: a
|
||||||
|
# reading nobody is refreshing is not evidence that the stick is still held.
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=PUSHED))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
clock.advance(state.config.deadman.joy_ttl_s + 0.01)
|
||||||
|
jogs = ticks(node, state)
|
||||||
|
|
||||||
|
assert jogs == [STOP]
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_joy_node_that_keeps_publishing_keeps_the_arm_moving(clock):
|
||||||
|
# The mirror of the test above: time passing is not by itself a reason to
|
||||||
|
# stop, or a held stick would stutter.
|
||||||
|
node, state = build()
|
||||||
|
|
||||||
|
sent = []
|
||||||
|
for _ in range(4):
|
||||||
|
clock.advance(0.05)
|
||||||
|
sent = drive(node, state, stick(axes=PUSHED))
|
||||||
|
|
||||||
|
assert sent
|
||||||
|
assert all(jog[0] > 0 for jog in sent)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_stick_returning_to_centre_stops_the_arm(clock):
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=PUSHED))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
jogs = drive(node, state, stick())
|
||||||
|
|
||||||
|
assert jogs == [STOP]
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_stick_at_rest_sends_one_zero_not_a_stream(clock):
|
||||||
|
# A stream of zeroes would keep the arm node believing a jog is live and
|
||||||
|
# hold robot_state at `moving` forever.
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=PUSHED))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
jogs = drive(node, state, stick(), count=5)
|
||||||
|
|
||||||
|
assert jogs == [STOP]
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_held_stick_is_not_resent_every_tick(clock):
|
||||||
|
# A velocity is a set-point the arm holds, not a frame in a sequence.
|
||||||
|
# Repeating it every tick made the controller re-plan the same motion twenty
|
||||||
|
# times a second, which an operator feels as the arm stepping rather than
|
||||||
|
# travelling.
|
||||||
|
node, state = build()
|
||||||
|
|
||||||
|
jogs = drive(node, state, stick(axes=PUSHED), count=4)
|
||||||
|
|
||||||
|
assert len(jogs) == 1
|
||||||
|
assert jogs[0][0] > 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_held_stick_is_repeated_before_the_arm_would_expire_it(clock):
|
||||||
|
# The repeat exists for one reason: the arm's controller drops a velocity on
|
||||||
|
# its own, and the repeat is what keeps that backstop from firing mid-motion.
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=PUSHED))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
clock.advance(state.config.deadman.jog_keepalive_s)
|
||||||
|
jogs = drive(node, state, stick(axes=PUSHED))
|
||||||
|
|
||||||
|
assert len(jogs) == 1
|
||||||
|
assert jogs[0][0] > 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_change_of_direction_never_waits_for_the_repeat(clock):
|
||||||
|
# Sending only on change is worth nothing if the change itself is delayed:
|
||||||
|
# a press or a release has to be acted on the tick it happens, not up to a
|
||||||
|
# keepalive later.
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=PUSHED))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
jogs = drive(node, state, stick(axes=[0.0, 1.0]))
|
||||||
|
|
||||||
|
assert len(jogs) == 1
|
||||||
|
assert jogs[0][0] < 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_that_was_never_wired_up_leaves_the_arm_alone(clock):
|
||||||
|
# `joy` is optional: a browser-only console wires no pad at all, and the
|
||||||
|
# node must not read that as a centred stick asking for the arm.
|
||||||
|
node, state = build()
|
||||||
|
|
||||||
|
jogs = ticks(node, state, count=3)
|
||||||
|
|
||||||
|
assert jogs == []
|
||||||
|
assert state.arbiter.owner is None
|
||||||
|
|
||||||
|
|
||||||
|
# --- the browser ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_last_browser_leaving_stops_the_arm(clock):
|
||||||
|
# Closing the tab has to stop the arm, and nothing is sent when it does.
|
||||||
|
node, state = build()
|
||||||
|
state.clients = 1
|
||||||
|
state.arbiter.take(WEB, clock.t)
|
||||||
|
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||||
|
ticks(node, state)
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
loop._set_clients(state, 0)
|
||||||
|
jogs = ticks(node, state)
|
||||||
|
|
||||||
|
assert jogs == [STOP]
|
||||||
|
assert state.arbiter.owner is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_browser_that_goes_quiet_stops_the_arm(clock):
|
||||||
|
# The console refreshes every 150 ms against a 400 ms deadline.
|
||||||
|
node, state = build()
|
||||||
|
state.clients = 1
|
||||||
|
state.arbiter.take(WEB, clock.t)
|
||||||
|
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||||
|
ticks(node, state)
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
clock.advance(0.5)
|
||||||
|
jogs = ticks(node, state)
|
||||||
|
|
||||||
|
assert jogs == [STOP]
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_browser_that_keeps_refreshing_keeps_the_arm_moving(clock):
|
||||||
|
node, state = build()
|
||||||
|
state.clients = 1
|
||||||
|
state.arbiter.take(WEB, clock.t)
|
||||||
|
|
||||||
|
sent = []
|
||||||
|
for _ in range(4):
|
||||||
|
clock.advance(0.15)
|
||||||
|
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||||
|
loop._tick(node, state)
|
||||||
|
sent = node.jogs()
|
||||||
|
|
||||||
|
assert all(jog[0] == pytest.approx(state.config.speed_mm_s) for jog in sent)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_non_owner_jog_is_ignored(clock):
|
||||||
|
node, state = build()
|
||||||
|
state.clients = 1
|
||||||
|
state.arbiter.take(GAMEPAD, clock.t)
|
||||||
|
|
||||||
|
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||||
|
loop._tick(node, state)
|
||||||
|
|
||||||
|
assert node.jogs() == []
|
||||||
|
assert any(message.get("type") == "error" for message in node.ui())
|
||||||
|
|
||||||
|
|
||||||
|
# --- the stop that is never gated ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_estop_is_never_gated_by_the_lease(clock):
|
||||||
|
# Somebody reaching for the stop is the one moment when asking who owns the
|
||||||
|
# arm is the wrong question.
|
||||||
|
node, state = build()
|
||||||
|
state.clients = 1
|
||||||
|
state.arbiter.take(GAMEPAD, clock.t)
|
||||||
|
|
||||||
|
loop._accept_ui_command(node, state, {"type": "estop"})
|
||||||
|
|
||||||
|
assert node.jogs() == [STOP]
|
||||||
|
assert "stop" in node.services()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_latched_estop_keeps_the_arm_still_under_a_pushed_stick(clock):
|
||||||
|
# A stop somebody had to reach for is not undone by the next thumb
|
||||||
|
# movement. Without this the pad stops the arm for one tick and drives it
|
||||||
|
# again on the next, which is not a stop at all.
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=PUSHED))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
drive(node, state, stick(axes=PUSHED, buttons=[False] * 6 + [True]))
|
||||||
|
node.clear()
|
||||||
|
jogs = drive(node, state, stick(axes=PUSHED), count=4)
|
||||||
|
|
||||||
|
assert jogs == []
|
||||||
|
|
||||||
|
|
||||||
|
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]))
|
||||||
|
assert state.view.estop is True
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
drive(node, state, stick(buttons=[False, False, True]))
|
||||||
|
|
||||||
|
assert state.view.estop is False
|
||||||
|
assert "clear" in node.services()
|
||||||
|
|
||||||
|
|
||||||
|
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]))
|
||||||
|
state.arbiter.release(GAMEPAD)
|
||||||
|
state.arbiter.take(WEB, clock.t)
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
drive(node, state, stick(buttons=[False, False, 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]))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
jogs = drive(node, state, stick(axes=PUSHED))
|
||||||
|
|
||||||
|
assert jogs[-1][0] > 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_latched_estop_stops_the_browser_too(clock):
|
||||||
|
# The latch is the node's, not the pad's. A console jog must not walk
|
||||||
|
# through a stop somebody made on the gamepad.
|
||||||
|
node, state = build()
|
||||||
|
state.clients = 1
|
||||||
|
drive(node, state, stick(buttons=[False] * 6 + [True]))
|
||||||
|
state.arbiter.release(GAMEPAD)
|
||||||
|
state.arbiter.take(WEB, clock.t)
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||||
|
jogs = ticks(node, state)
|
||||||
|
|
||||||
|
assert jogs == []
|
||||||
|
|
||||||
|
|
||||||
|
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]))
|
||||||
|
|
||||||
|
assert "stop" in node.services()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_latched_estop_is_cleared_by_reset(clock):
|
||||||
|
node, state = build()
|
||||||
|
state.view.estop = True
|
||||||
|
|
||||||
|
loop._accept_ui_command(node, state, {"type": "reset"})
|
||||||
|
|
||||||
|
assert state.view.estop is False
|
||||||
|
assert "clear" in node.services()
|
||||||
|
|
||||||
|
|
||||||
|
# --- the standard ports ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_same_deflection_from_either_source_asks_for_the_same_motion(clock):
|
||||||
|
# The whole reason both sources are normalised before they are scaled. If
|
||||||
|
# these ever differ, one path has grown a conversion the other has not —
|
||||||
|
# which is how a console ends up in the robot's units.
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=[0.0, -1.0]))
|
||||||
|
from_pad = node.twists()[-1]
|
||||||
|
|
||||||
|
node, state = build()
|
||||||
|
state.clients = 1
|
||||||
|
state.arbiter.take(WEB, clock.t)
|
||||||
|
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||||
|
ticks(node, state)
|
||||||
|
from_web = node.twists()[-1]
|
||||||
|
|
||||||
|
assert from_pad == from_web
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_twist_is_si_and_the_legacy_jog_is_not(clock):
|
||||||
|
# 60 mm/s is 0.06 m/s. A consumer reading the wrong port at the wrong scale
|
||||||
|
# is a thousandfold error, so this is worth pinning.
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=[0.0, -1.0]))
|
||||||
|
|
||||||
|
twist, jog = node.twists()[-1], node.jogs()[-1]
|
||||||
|
|
||||||
|
assert twist["linear"][0] == pytest.approx(state.config.speed_mm_s / 1000.0)
|
||||||
|
assert jog[0] == pytest.approx(state.config.speed_mm_s)
|
||||||
|
assert twist["frame"] == "base"
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_stop_reaches_both_ports(clock):
|
||||||
|
node, state = build()
|
||||||
|
drive(node, state, stick(axes=PUSHED))
|
||||||
|
node.clear()
|
||||||
|
|
||||||
|
drive(node, state, stick())
|
||||||
|
|
||||||
|
assert node.jogs() == [STOP]
|
||||||
|
assert node.twists() == [{"linear": [0.0] * 3, "angular": [0.0] * 3, "frame": "base"}]
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_button_asks_in_verbs_as_well_as_in_services(clock):
|
||||||
|
# The verb is what a robot speaking the standard reads; the service name is
|
||||||
|
# what dora-ulite6 reads until it does.
|
||||||
|
node, state = build()
|
||||||
|
|
||||||
|
drive(node, state, stick(buttons=[True]))
|
||||||
|
|
||||||
|
assert node.actions() == ["gripper_close"]
|
||||||
|
assert "grasp" in node.services()
|
||||||
|
|
||||||
|
|
||||||
|
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]))
|
||||||
|
|
||||||
|
assert node.actions() == ["gripper_open", "freedrive", "stop", "clear"]
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_first_press_after_the_pad_has_been_idle_is_not_dropped(clock):
|
||||||
|
# The lease has to be settled before the buttons are read. Read them first
|
||||||
|
# and the press arrives while nobody owns the arm, and by the next tick the
|
||||||
|
# button is held rather than newly down — so the edge is gone for good and
|
||||||
|
# the operator presses A and watches nothing happen.
|
||||||
|
node, state = build()
|
||||||
|
assert state.arbiter.owner is None
|
||||||
|
|
||||||
|
drive(node, state, stick(buttons=[True]), count=3)
|
||||||
|
|
||||||
|
assert node.actions() == ["gripper_close"]
|
||||||
@@ -0,0 +1,61 @@
|
|||||||
|
"""The pad as it arrives on the wire, and what silence on that wire means."""
|
||||||
|
|
||||||
|
import pyarrow as pa
|
||||||
|
|
||||||
|
from dora_teleop.joy import ABSENT, JoyInput, Sample, decode
|
||||||
|
from tests.conftest import input_event, joy_message, stick
|
||||||
|
|
||||||
|
|
||||||
|
def joy_event(sample):
|
||||||
|
return input_event("joy", joy_message(sample))
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_reading_survives_the_round_trip():
|
||||||
|
sample = decode(joy_event(stick(axes=[0.5, -0.5], buttons=[False, True])))
|
||||||
|
|
||||||
|
assert sample.axes[:2] == [0.5, -0.5]
|
||||||
|
assert sample.buttons == [False, True]
|
||||||
|
assert sample.present is True
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_absent_pad_decodes_as_absent_not_as_centred():
|
||||||
|
sample = decode(joy_event(ABSENT))
|
||||||
|
|
||||||
|
assert sample.present is False
|
||||||
|
assert sample.axes == []
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_with_fewer_axes_than_the_layout_decodes_without_complaint():
|
||||||
|
# A three-axis flight stick is a strange thing to drive an arm with, but it
|
||||||
|
# must not take the node down; `mapping.axis` treats a missing one as centred.
|
||||||
|
sample = decode(joy_event(Sample(axes=[0.0, 0.0, 0.0], buttons=[False], present=True)))
|
||||||
|
|
||||||
|
assert len(sample.axes) == 3
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_message_with_no_rows_reads_as_absent():
|
||||||
|
empty = input_event("joy", pa.array([], type=joy_message(ABSENT).type))
|
||||||
|
|
||||||
|
assert decode(empty) is 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)
|
||||||
|
|
||||||
|
assert joy.read(100.1) is sample
|
||||||
|
|
||||||
|
|
||||||
|
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(axes=[0.5]), 100.0)
|
||||||
|
|
||||||
|
assert joy.read(100.5) is ABSENT
|
||||||
|
|
||||||
|
|
||||||
|
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
|
||||||
@@ -0,0 +1,142 @@
|
|||||||
|
"""Stick deflection to base-frame velocity: units, signs and the shaping curve."""
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from dora_teleop import mapping
|
||||||
|
from dora_teleop.mapping import VX, VY, VZ, WX, WY, WZ
|
||||||
|
from tests.conftest import stick
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_stick_at_rest_asks_for_nothing(config):
|
||||||
|
velocity = mapping.velocity(stick().axes, [], config)
|
||||||
|
|
||||||
|
assert velocity == [0.0] * 6
|
||||||
|
assert not mapping.is_moving(velocity)
|
||||||
|
|
||||||
|
|
||||||
|
def test_pushing_the_stick_up_moves_the_arm_along_positive_x(config):
|
||||||
|
# Up is negative in SDL, and up should send the arm away from the operator.
|
||||||
|
velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config)
|
||||||
|
|
||||||
|
assert velocity[VX] == pytest.approx(config.speed_mm_s)
|
||||||
|
assert velocity[VY] == 0.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_pushing_the_stick_right_moves_the_arm_along_positive_y(config):
|
||||||
|
velocity = mapping.velocity(stick(axes=[1.0, 0.0]).axes, [], config)
|
||||||
|
|
||||||
|
assert velocity[VY] == pytest.approx(config.speed_mm_s)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_right_trigger_lifts_and_the_left_one_lowers(config):
|
||||||
|
up = mapping.velocity(stick(axes=[0, 0, -1.0, 0, 0, 1.0]).axes, [], config)
|
||||||
|
down = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, -1.0]).axes, [], config)
|
||||||
|
|
||||||
|
assert up[VZ] == pytest.approx(config.speed_mm_s)
|
||||||
|
assert down[VZ] == pytest.approx(-config.speed_mm_s)
|
||||||
|
|
||||||
|
|
||||||
|
def test_both_triggers_together_cancel(config):
|
||||||
|
velocity = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, 1.0]).axes, [], config)
|
||||||
|
|
||||||
|
assert velocity[VZ] == 0.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_untouched_trigger_does_not_read_as_half_pressed(config):
|
||||||
|
# SDL reports a trigger from -1 to 1, and one that has never moved sometimes
|
||||||
|
# reads 0 until it does. Treating that as half travel would lift the arm.
|
||||||
|
assert mapping.trigger(-1.0) == 0.0
|
||||||
|
assert mapping.trigger(0.0) == pytest.approx(0.5)
|
||||||
|
assert mapping.trigger(1.0) == 1.0
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_right_stick_turns_the_tool_in_degrees_per_second(config):
|
||||||
|
velocity = mapping.velocity(stick(axes=[0, 0, -1, 1.0, -1.0, -1]).axes, [], config)
|
||||||
|
|
||||||
|
assert velocity[WX] == pytest.approx(config.speed_deg_s)
|
||||||
|
assert velocity[WY] == pytest.approx(config.speed_deg_s)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_bumpers_yaw_in_opposite_directions(config):
|
||||||
|
right = mapping.velocity(stick().axes, [False] * 5 + [True], config)
|
||||||
|
left = mapping.velocity(stick().axes, [False] * 4 + [True, False], config)
|
||||||
|
|
||||||
|
assert right[WZ] == pytest.approx(config.speed_deg_s)
|
||||||
|
assert left[WZ] == pytest.approx(-config.speed_deg_s)
|
||||||
|
|
||||||
|
|
||||||
|
def test_several_axes_move_at_once(config):
|
||||||
|
# The whole reason a pad beats the console's four-sector dial: the dial can
|
||||||
|
# only ever ask for one axis.
|
||||||
|
velocity = mapping.velocity(stick(axes=[1.0, -1.0, -1, 0, 0, 1.0]).axes, [], config)
|
||||||
|
|
||||||
|
assert velocity[VX] > 0
|
||||||
|
assert velocity[VY] > 0
|
||||||
|
assert velocity[VZ] > 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_drift_inside_the_deadzone_is_ignored(config):
|
||||||
|
# A worn stick that does not centre would otherwise drive the arm across the
|
||||||
|
# table with nobody touching it.
|
||||||
|
velocity = mapping.velocity(stick(axes=[config.deadzone / 2, 0.0]).axes, [], config)
|
||||||
|
|
||||||
|
assert velocity == [0.0] * 6
|
||||||
|
|
||||||
|
|
||||||
|
def test_motion_starts_from_zero_just_outside_the_deadzone(config):
|
||||||
|
# Without rescaling, the arm would jump to a fraction of full speed the
|
||||||
|
# instant the stick left the dead band.
|
||||||
|
just_outside = mapping.deadzone(config.deadzone + 1e-9, config.deadzone)
|
||||||
|
|
||||||
|
assert just_outside == pytest.approx(0.0, abs=1e-6)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_deadzone_still_reaches_full_speed_at_full_deflection(config):
|
||||||
|
assert mapping.deadzone(1.0, config.deadzone) == pytest.approx(1.0)
|
||||||
|
assert mapping.deadzone(-1.0, config.deadzone) == pytest.approx(-1.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_expo_slows_small_deflections_without_capping_large_ones():
|
||||||
|
assert mapping.expo(0.5, 0.0) == pytest.approx(0.5)
|
||||||
|
assert mapping.expo(0.5, 1.0) == pytest.approx(0.125)
|
||||||
|
assert mapping.expo(1.0, 1.0) == pytest.approx(1.0)
|
||||||
|
assert mapping.expo(-0.5, 1.0) == pytest.approx(-0.125)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_reporting_fewer_axes_is_read_as_centred_not_as_an_error(config):
|
||||||
|
velocity = mapping.velocity([0.0, 0.0], [], config)
|
||||||
|
|
||||||
|
assert velocity[VX] == 0.0
|
||||||
|
assert velocity[WZ] == 0.0
|
||||||
|
|
||||||
|
|
||||||
|
# --- is the pad being used at all ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_at_rest_is_not_asking_for_the_arm():
|
||||||
|
# SDL rests a trigger at -1.0, so a raw threshold reports every connected
|
||||||
|
# pad as in use for ever: the pad claims the arm the moment it is plugged
|
||||||
|
# in and GAMEPAD_IDLE_RELEASE_S never fires.
|
||||||
|
assert mapping.is_active(stick()) is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_absent_pad_is_not_asking_for_the_arm():
|
||||||
|
assert mapping.is_active(stick(present=False)) is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_nudged_stick_is_asking_for_the_arm():
|
||||||
|
assert mapping.is_active(stick(axes=[0.5])) is True
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_held_trigger_is_asking_for_the_arm():
|
||||||
|
assert mapping.is_active(stick(axes=[0.0, 0.0, 1.0])) is True
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_held_button_is_asking_for_the_arm():
|
||||||
|
assert mapping.is_active(stick(buttons=[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
|
||||||
@@ -0,0 +1,88 @@
|
|||||||
|
"""The probe: what it draws, without a pad or a terminal."""
|
||||||
|
|
||||||
|
from dora_teleop import mapping, probe
|
||||||
|
from dora_teleop.joy import Sample
|
||||||
|
from tests.conftest import stick
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_centred_stick_puts_the_marker_in_the_middle():
|
||||||
|
drawn = probe.bar(0.0)
|
||||||
|
|
||||||
|
assert drawn[len(drawn) // 2] == "#"
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_bar_leans_the_way_the_stick_is_pushed():
|
||||||
|
left = probe.bar(-1.0)
|
||||||
|
right = probe.bar(1.0)
|
||||||
|
|
||||||
|
assert left.index("#") < left.index("|")
|
||||||
|
assert right.index("#") > right.index("|")
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_reading_beyond_full_deflection_stays_on_the_bar():
|
||||||
|
# SDL has been known to report slightly outside -1..1 on a worn stick.
|
||||||
|
assert "#" in probe.bar(-4.0)
|
||||||
|
assert "#" in probe.bar(4.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_no_gamepad_says_so_and_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)
|
||||||
|
|
||||||
|
assert "no gamepad" in drawn
|
||||||
|
assert "hold still" in drawn
|
||||||
|
|
||||||
|
|
||||||
|
def test_every_axis_the_pad_reports_is_shown_even_the_unused_ones(config):
|
||||||
|
# A pad whose sticks land on indices we do not use is exactly what this is
|
||||||
|
# for, so nothing may be hidden.
|
||||||
|
sample = Sample(axes=[0.0] * 8, buttons=[], present=True)
|
||||||
|
|
||||||
|
drawn = probe.render(sample, [0.0] * 6, config)
|
||||||
|
|
||||||
|
assert "unused" in drawn
|
||||||
|
assert "axes (8 reported)" in drawn
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pressed_button_is_named(config):
|
||||||
|
sample = stick(buttons=[True])
|
||||||
|
|
||||||
|
drawn = probe.render(sample, [0.0] * 6, config)
|
||||||
|
|
||||||
|
assert "grasp" in drawn
|
||||||
|
|
||||||
|
|
||||||
|
def test_nothing_pressed_says_so_rather_than_showing_an_empty_line(config):
|
||||||
|
drawn = probe.render(stick(), [0.0] * 6, config)
|
||||||
|
|
||||||
|
assert "none pressed" in drawn
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_velocity_is_shown_in_the_units_the_arm_takes(config):
|
||||||
|
velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config)
|
||||||
|
|
||||||
|
drawn = probe.render(stick(axes=[0.0, -1.0]), velocity, config)
|
||||||
|
|
||||||
|
assert "mm/s" in drawn
|
||||||
|
assert "deg/s" in drawn
|
||||||
|
assert f"{config.speed_mm_s:+8.2f}" in drawn
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_still_arm_is_reported_as_still(config):
|
||||||
|
drawn = probe.render(stick(), [0.0] * 6, config)
|
||||||
|
|
||||||
|
assert "told to hold still" in drawn
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_moving_arm_is_reported_as_moving(config):
|
||||||
|
drawn = probe.render(stick(axes=[0.0, -1.0]), [10.0, 0, 0, 0, 0, 0], config)
|
||||||
|
|
||||||
|
assert "moving: yes" in drawn
|
||||||
|
|
||||||
|
|
||||||
|
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)
|
||||||
@@ -0,0 +1,223 @@
|
|||||||
|
"""The embedded console server: a real socket, and the deadman it feeds.
|
||||||
|
|
||||||
|
These start the server for real rather than faking it. The thing worth checking
|
||||||
|
is the boundary between the two threads — a browser's message reaching the tick,
|
||||||
|
and a browser going away reaching the deadman — and a fake on either side of
|
||||||
|
that boundary would check nothing.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import asyncio
|
||||||
|
import json
|
||||||
|
import socket
|
||||||
|
import time
|
||||||
|
|
||||||
|
import aiohttp
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from dora_teleop import main as loop
|
||||||
|
from dora_teleop.arbiter import WEB
|
||||||
|
from dora_teleop.config import Config, ConsoleConfig
|
||||||
|
from dora_teleop.server import Console
|
||||||
|
from tests.conftest import RecordingNode
|
||||||
|
|
||||||
|
|
||||||
|
def free_port():
|
||||||
|
"""Ask the OS for a port nobody is using."""
|
||||||
|
with socket.socket() as held:
|
||||||
|
held.bind(("127.0.0.1", 0))
|
||||||
|
return held.getsockname()[1]
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.fixture
|
||||||
|
def console():
|
||||||
|
"""Run a real server on a real port, and stop it however the test ends."""
|
||||||
|
running = Console("127.0.0.1", free_port())
|
||||||
|
running.start()
|
||||||
|
try:
|
||||||
|
yield running
|
||||||
|
finally:
|
||||||
|
running.stop()
|
||||||
|
|
||||||
|
|
||||||
|
def visit(console, exchange):
|
||||||
|
"""Open one browser's socket against the server and run `exchange` on it."""
|
||||||
|
|
||||||
|
async def session():
|
||||||
|
async with aiohttp.ClientSession() as client:
|
||||||
|
url = f"http://{console.host}:{console.port}"
|
||||||
|
async with client.ws_connect(f"{url}/ws") as socket_:
|
||||||
|
return await exchange(client, url, socket_)
|
||||||
|
|
||||||
|
return asyncio.run(session())
|
||||||
|
|
||||||
|
|
||||||
|
def wait_for(predicate):
|
||||||
|
"""Block until the server thread catches up, rather than guessing at a sleep."""
|
||||||
|
for _ in range(200):
|
||||||
|
if predicate():
|
||||||
|
return
|
||||||
|
time.sleep(0.01)
|
||||||
|
raise AssertionError("the server never got there")
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_page_is_served_at_the_root(console):
|
||||||
|
async def exchange(client, url, socket_):
|
||||||
|
async with client.get(url) as answer:
|
||||||
|
return answer.status, await answer.text()
|
||||||
|
|
||||||
|
status, body = visit(console, exchange)
|
||||||
|
|
||||||
|
assert status == 200
|
||||||
|
assert "teleop console" in body
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_message_from_a_browser_reaches_the_tick(console):
|
||||||
|
# The console's half of the boundary: what the page sends has to be waiting
|
||||||
|
# on the queue the next tick drains, in the shape the loop reads.
|
||||||
|
async def exchange(client, url, socket_):
|
||||||
|
await socket_.send_str(json.dumps({"type": "jog", "axes": [1.0, 0, 0, 0, 0, 0]}))
|
||||||
|
await asyncio.sleep(0.05)
|
||||||
|
|
||||||
|
visit(console, exchange)
|
||||||
|
|
||||||
|
assert console.take() == [{"type": "jog", "axes": [1.0, 0, 0, 0, 0, 0]}]
|
||||||
|
assert console.take() == []
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_snapshot_reaches_the_browser(console):
|
||||||
|
# The other half: a telemetry object handed over from the dora thread comes
|
||||||
|
# out of the socket as the page's own JSON, with no envelope around it.
|
||||||
|
async def exchange(client, url, socket_):
|
||||||
|
console.broadcast({"type": "telemetry", "arm_state": "idle"})
|
||||||
|
return json.loads(await socket_.receive_str())
|
||||||
|
|
||||||
|
assert visit(console, exchange) == {"type": "telemetry", "arm_state": "idle"}
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_browser_that_is_not_talking_json_is_ignored(console):
|
||||||
|
# A page from a newer build saying something we cannot parse must not take
|
||||||
|
# the arm down, and must not stop the messages after it being read.
|
||||||
|
async def exchange(client, url, socket_):
|
||||||
|
await socket_.send_str("{not json")
|
||||||
|
await socket_.send_str(json.dumps({"type": "estop"}))
|
||||||
|
await asyncio.sleep(0.05)
|
||||||
|
|
||||||
|
visit(console, exchange)
|
||||||
|
|
||||||
|
assert console.take() == [{"type": "estop"}]
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_server_counts_the_browsers_on_it(console):
|
||||||
|
assert console.clients == 0
|
||||||
|
seen = []
|
||||||
|
|
||||||
|
async def exchange(client, url, socket_):
|
||||||
|
await asyncio.sleep(0.05)
|
||||||
|
seen.append(console.clients)
|
||||||
|
|
||||||
|
visit(console, exchange)
|
||||||
|
|
||||||
|
assert seen == [1]
|
||||||
|
|
||||||
|
# And the socket closing is what the deadman below reads.
|
||||||
|
wait_for(lambda: console.clients == 0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_flood_from_one_browser_cannot_grow_without_bound(console):
|
||||||
|
# An unbounded queue would answer a flood by growing until the node died,
|
||||||
|
# and the node dying is the one thing that must not stop the arm from being
|
||||||
|
# stopped.
|
||||||
|
async def exchange(client, url, socket_):
|
||||||
|
for index in range(600):
|
||||||
|
await socket_.send_str(json.dumps({"type": "jog_stop", "n": index}))
|
||||||
|
await asyncio.sleep(0.2)
|
||||||
|
|
||||||
|
visit(console, exchange)
|
||||||
|
|
||||||
|
assert 0 < len(console.take()) <= 256
|
||||||
|
|
||||||
|
|
||||||
|
# --- where the server meets the deadman ---
|
||||||
|
|
||||||
|
|
||||||
|
def build():
|
||||||
|
config = Config.from_env()
|
||||||
|
config.expo = 0.0
|
||||||
|
return RecordingNode(), loop.Teleop(config)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_last_browser_leaving_takes_the_arm_back(console):
|
||||||
|
# The web deadman used to arrive as a `clients` message from the bridge. It
|
||||||
|
# is now read straight off the server, and it has to mean the same thing.
|
||||||
|
node, state = build()
|
||||||
|
state.console = console
|
||||||
|
state.arbiter.take(WEB, 1000.0)
|
||||||
|
|
||||||
|
loop._drain_console(node, state)
|
||||||
|
|
||||||
|
assert state.clients == 0
|
||||||
|
assert state.arbiter.owner is None
|
||||||
|
assert state.web_axes == [0.0] * 6
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_node_with_no_console_is_left_alone():
|
||||||
|
# A pad-only dataflow opens no port, and the tick must not read a lease
|
||||||
|
# release out of a server that was never started.
|
||||||
|
node, state = build()
|
||||||
|
state.clients = 1
|
||||||
|
state.arbiter.take(WEB, 1000.0)
|
||||||
|
|
||||||
|
loop._drain_console(node, state)
|
||||||
|
|
||||||
|
assert state.arbiter.owner == WEB
|
||||||
|
|
||||||
|
|
||||||
|
# --- the port itself ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_console_is_on_by_default(monkeypatch):
|
||||||
|
monkeypatch.delenv("WEB_PORT", raising=False)
|
||||||
|
settings = ConsoleConfig.from_env()
|
||||||
|
|
||||||
|
assert settings.enabled is True
|
||||||
|
# Pinned because it is the number in the README, the manifest and every
|
||||||
|
# dataflow: a default that drifts from the docs sends an operator to a port
|
||||||
|
# nothing is listening on.
|
||||||
|
assert settings.port == 7000
|
||||||
|
|
||||||
|
|
||||||
|
def test_port_zero_means_no_port_is_opened(monkeypatch):
|
||||||
|
# How `dataflow-gamepad.yml` asks for a pad and nothing else.
|
||||||
|
monkeypatch.setenv("WEB_PORT", "0")
|
||||||
|
assert ConsoleConfig.from_env().enabled is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_port_that_is_not_a_port_stops_the_node(monkeypatch):
|
||||||
|
monkeypatch.setenv("WEB_PORT", "99999")
|
||||||
|
with pytest.raises(ValueError, match="WEB_PORT"):
|
||||||
|
ConsoleConfig.from_env()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_port_that_is_not_a_number_stops_the_node(monkeypatch):
|
||||||
|
monkeypatch.setenv("WEB_PORT", "eight-thousand")
|
||||||
|
with pytest.raises(ValueError, match="WEB_PORT"):
|
||||||
|
ConsoleConfig.from_env()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_port_already_in_use_stops_the_node(console):
|
||||||
|
# Found the hard way: the bind failed on the server thread, `start` waited
|
||||||
|
# out its timeout and then announced a console anyway, and the node ran on
|
||||||
|
# serving nothing. An operator opening the URL got silence while the log
|
||||||
|
# said it was up.
|
||||||
|
second = Console(console.host, console.port)
|
||||||
|
|
||||||
|
with pytest.raises(OSError):
|
||||||
|
second.start()
|
||||||
|
|
||||||
|
second.stop()
|
||||||
|
|
||||||
|
|
||||||
|
def test_stopping_a_console_that_never_started_is_harmless():
|
||||||
|
# `main` stops the console in a `finally`, which runs whether or not the
|
||||||
|
# server ever came up.
|
||||||
|
Console("127.0.0.1", 0).stop()
|
||||||
@@ -0,0 +1,139 @@
|
|||||||
|
"""Translation both ways: what the console asks for, and what it is shown."""
|
||||||
|
|
||||||
|
from dora_teleop import ui
|
||||||
|
from dora_teleop.arbiter import GAMEPAD, WEB
|
||||||
|
|
||||||
|
# --- what the console asks for ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_jog_carries_its_six_numbers():
|
||||||
|
command = ui.command_of({"type": "jog", "axes": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]})
|
||||||
|
|
||||||
|
assert command.type == "jog"
|
||||||
|
assert command.axes == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_jog_of_the_wrong_length_is_not_a_jog():
|
||||||
|
assert ui.command_of({"type": "jog", "axes": [1, 2, 3]}) is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_jog_carrying_something_that_is_not_a_number_is_not_a_jog():
|
||||||
|
assert ui.command_of({"type": "jog", "axes": [1, 2, 3, 4, 5, "fast"]}) is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_message_we_have_never_heard_of_is_dropped_not_refused():
|
||||||
|
# The console and this node version independently; a page from a newer build
|
||||||
|
# must not take the arm down by saying something we have not learned yet.
|
||||||
|
assert ui.command_of({"type": "hologram"}) is None
|
||||||
|
assert ui.command_of({}) is None
|
||||||
|
assert ui.command_of("not even an object") is None
|
||||||
|
|
||||||
|
|
||||||
|
def test_taking_control_carries_whether_it_may_preempt():
|
||||||
|
command = ui.command_of({"type": "control", "action": "take", "preempt": True})
|
||||||
|
|
||||||
|
assert (command.action, command.preempt) == ("take", True)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_gripper_button_says_which_way():
|
||||||
|
assert ui.command_of({"type": "gripper", "closed": True}).enabled is True
|
||||||
|
assert ui.command_of({"type": "gripper", "closed": False}).enabled is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_move_needs_a_position():
|
||||||
|
assert ui.command_of({"type": "move", "x": 1.0, "y": 2.0}) is None
|
||||||
|
|
||||||
|
command = ui.command_of({"type": "move", "x": 250.0, "y": 0.0, "z": 200.0, "speed": 40})
|
||||||
|
|
||||||
|
assert command.row == {"x": 250.0, "y": 0.0, "z": 200.0, "speed": 40.0}
|
||||||
|
|
||||||
|
|
||||||
|
# --- what the console is shown ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_arm_that_cannot_be_reached_reads_as_disconnected():
|
||||||
|
view = ui.ArmView(state=ui.UNAVAILABLE)
|
||||||
|
|
||||||
|
assert ui.arm_state(view, jogging=False) == ui.UI_DISCONNECTED
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_arm_still_starting_up_reads_as_disconnected():
|
||||||
|
view = ui.ArmView(state=ui.CONNECTING)
|
||||||
|
|
||||||
|
assert ui.arm_state(view, jogging=False) == ui.UI_DISCONNECTED
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_latched_estop_outranks_everything_else():
|
||||||
|
# The arm may well read ready, and saying so would invite somebody to push it.
|
||||||
|
view = ui.ArmView(state=ui.READY, estop=True, freedrive_on=True)
|
||||||
|
|
||||||
|
assert ui.arm_state(view, jogging=True) == ui.UI_ESTOP
|
||||||
|
|
||||||
|
|
||||||
|
def test_freedrive_outranks_idle():
|
||||||
|
# An arm that can be pushed by hand is not one holding station.
|
||||||
|
view = ui.ArmView(state=ui.READY, freedrive_on=True)
|
||||||
|
|
||||||
|
assert ui.arm_state(view, jogging=False) == ui.UI_FREEDRIVE
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_moving_arm_we_are_jogging_reads_as_jogging():
|
||||||
|
view = ui.ArmView(state=ui.MOVING)
|
||||||
|
|
||||||
|
assert ui.arm_state(view, jogging=True) == ui.UI_JOGGING
|
||||||
|
assert ui.arm_state(view, jogging=False) == ui.UI_MOVING
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_recovering_arm_is_there_but_will_not_move():
|
||||||
|
view = ui.ArmView(state=ui.RECOVERING)
|
||||||
|
|
||||||
|
assert ui.arm_state(view, jogging=False) == ui.UI_STOPPED
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_state_we_do_not_know_reads_as_disconnected_rather_than_as_ready():
|
||||||
|
view = ui.ArmView(state="dancing")
|
||||||
|
|
||||||
|
assert ui.arm_state(view, jogging=False) == ui.UI_DISCONNECTED
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_snapshot_carries_every_key_the_page_reads_by_name():
|
||||||
|
view = ui.ArmView(state=ui.READY, pose=[1, 2, 3, 4, 5, 6], joints=[0] * 6, connected=True)
|
||||||
|
|
||||||
|
snapshot = ui.snapshot(view, WEB, jogging=False, timestamp=123.0)
|
||||||
|
|
||||||
|
for key in (
|
||||||
|
"type",
|
||||||
|
"ts",
|
||||||
|
"connected",
|
||||||
|
"ready",
|
||||||
|
"arm_state",
|
||||||
|
"owner",
|
||||||
|
"estop",
|
||||||
|
"gripper",
|
||||||
|
"units",
|
||||||
|
"freedrive",
|
||||||
|
"position",
|
||||||
|
"joints",
|
||||||
|
):
|
||||||
|
assert key in snapshot
|
||||||
|
assert snapshot["position"] == {"x": 1, "y": 2, "z": 3, "roll": 4, "pitch": 5, "yaw": 6}
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_arm_with_no_pose_yet_reports_none_rather_than_a_stale_one():
|
||||||
|
snapshot = ui.snapshot(ui.ArmView(), None, jogging=False, timestamp=0.0)
|
||||||
|
|
||||||
|
assert snapshot["position"] is None
|
||||||
|
assert snapshot["owner"] == "none"
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_owner_is_named_so_the_page_can_gate_its_jog_pad():
|
||||||
|
assert ui.snapshot(ui.ArmView(), GAMEPAD, False, 0.0)["owner"] == "gamepad"
|
||||||
|
assert ui.snapshot(ui.ArmView(), WEB, False, 0.0)["owner"] == "web"
|
||||||
|
|
||||||
|
|
||||||
|
def test_get_state_fills_in_what_telemetry_does_not_carry():
|
||||||
|
view = ui.ArmView()
|
||||||
|
|
||||||
|
view.take_get_state({"connected": True, "ready": True, "tool_engaged": True, "freedrive": True})
|
||||||
|
|
||||||
|
assert (view.connected, view.ready, view.tool_engaged, view.freedrive_on) == (True, True, True, True)
|
||||||
Reference in New Issue
Block a user