Compare commits
3
Commits
e3fc538e5d
...
5ce83ae945
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
5ce83ae945 | ||
|
|
f7257ab903 | ||
|
|
d0f884c7e2 |
@@ -46,16 +46,19 @@ uv sync --group dev # plus pytest and ruff
|
|||||||
|
|
||||||
```bash
|
```bash
|
||||||
dora run dataflow.yml --uv # this node plus a bench stand-in, no pad
|
dora run dataflow.yml --uv # this node plus a bench stand-in, no pad
|
||||||
dora run dataflow-gamepad.yml --uv # the same, with dora-joy pulled in by git:
|
dora run dataflow-robot.yml # the pad and a real arm, both pulled in by git:
|
||||||
|
dora run dataflow-gamepad.yml # the same, with the probe declared and no console
|
||||||
```
|
```
|
||||||
|
|
||||||
`dataflow.yml` is self-contained on purpose — it resolves from this checkout alone, which
|
`dataflow.yml` is self-contained on purpose — it resolves from this checkout alone, which
|
||||||
is what lets CI validate it. Keep it that way; the pad belongs in `dataflow-gamepad.yml`.
|
is what lets CI validate it. Keep it that way; the pad belongs in the other two. Neither
|
||||||
|
of those takes `--uv`: dora 1.0.1 installs a `git:` node into a venv inside its own clone
|
||||||
|
and then looks for its entrypoint in the dataflow's venv. README.md has the detail.
|
||||||
|
|
||||||
## Test
|
## Test
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
uv run pytest # 112 tests, no gamepad, arm, browser or daemon needed
|
uv run pytest # 149 tests, no gamepad, arm, browser or daemon needed
|
||||||
uv run ruff check .
|
uv run ruff check .
|
||||||
uv run ruff format --check .
|
uv run ruff format --check .
|
||||||
```
|
```
|
||||||
@@ -74,6 +77,14 @@ a source goes away**.
|
|||||||
- **`dora_teleop/joy.py`** — The pad as it arrives from `dora-joy`. `Sample.present` is
|
- **`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
|
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.
|
operator let go" and "the pad fell off the desk" stay distinguishable.
|
||||||
|
**`Calibration` is the other half of that seam.** It measures a pad the first time it is
|
||||||
|
seen at rest and hands the rest of the node a reading as the layout's ideal pad would
|
||||||
|
have sent it, so a worn stick's tilt is subtracted rather than deadzoned and a trigger
|
||||||
|
that rests at 0 stops being a special case. A pad whose resting axes do not fit `Layout`
|
||||||
|
reads as *absent* and is never corrected: one reading cannot tell a trigger at -1.0 from
|
||||||
|
a stick held hard over, and guessing wrong drives a real arm. It is not re-measured
|
||||||
|
while the pad is in use — doing that under a held stick would record the deflection as
|
||||||
|
centre, and the arm would move when the operator let go.
|
||||||
**`JoyInput.read` is the other safety-critical function in this repo.** Moving the pad
|
**`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
|
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.
|
process dying — and from here that looks like silence rather than like an unplugged pad.
|
||||||
@@ -90,7 +101,12 @@ a source goes away**.
|
|||||||
units: mm/s and deg/s, base frame. `is_active` lives here rather than on `Sample`
|
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
|
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
|
to normalise triggers exactly as `velocity` does, or every connected pad reads as in use
|
||||||
for ever and `GAMEPAD_IDLE_RELEASE_S` never fires.
|
for ever and `GAMEPAD_IDLE_RELEASE_S` never fires. `rest_zeros` and `canonical` are the
|
||||||
|
same concern one layer out: they check a resting pad against `Layout` and map it onto
|
||||||
|
the ideal one. They are the only defence against a wrong index, which is not a
|
||||||
|
theoretical failure — an Xbox One S pad reporting its triggers on 4 and 5 against a
|
||||||
|
layout expecting them on 2 and 5 read as a full-speed pitch command from an untouched
|
||||||
|
pad, held the lease for ever, and moved a real arm into a controller error.
|
||||||
- **`dora_teleop/arbiter.py`** — The lease. One place that answers "why did it not move",
|
- **`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`
|
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
|
lease because the console is already built on it — it gates its controls on
|
||||||
@@ -170,6 +186,13 @@ belongs here, and a port nothing wires reads as supported while having never run
|
|||||||
pair; nothing can enforce the second from here.
|
pair; nothing can enforce the second from here.
|
||||||
- **An unplugged pad, a dead `dora-joy` or a departed browser stops the arm immediately**,
|
- **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.
|
without waiting out any timer beyond the one that detects it.
|
||||||
|
- **A pad is driven from only after it has been seen at rest**, and a pad whose rest does
|
||||||
|
not fit `Layout` is not driven from at all. This covers the axes and **cannot cover the
|
||||||
|
buttons**: an unpressed button reads the same on every pad, so a wrong button index is
|
||||||
|
silent until somebody presses it and gets another verb. Sweeping a new pad with the
|
||||||
|
probe is the only check, and it is worth doing before the arm is powered. The cost is that a pad held when it appears
|
||||||
|
is ignored until it is let go; the alternative is guessing a numbering from one reading,
|
||||||
|
against a real arm.
|
||||||
- **Both control sources go through `mapping`.** The web path used to bypass it and pass
|
- **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
|
a velocity straight through, which is why the console had to speak mm/s. One conversion
|
||||||
point or none.
|
point or none.
|
||||||
|
|||||||
@@ -4,25 +4,17 @@ A dora-rs node that hands a robot arm to a human. Pick up a gamepad and move the
|
|||||||
open a browser and move it from there — one of the two at a time, decided here.
|
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
|
It drives [`dora-ulite6`](https://gitea.liria.cl/liria/dora-ulite6) through that node's
|
||||||
ordinary `jog` and `request` ports, reads the pad from
|
`jog` and `request` ports, reads the pad from
|
||||||
[`dora-joy`](https://gitea.liria.cl/liria/dora-joy), and **serves the browser console
|
[`dora-joy`](https://gitea.liria.cl/liria/dora-joy), and serves the browser console itself
|
||||||
itself** on one port. It contains no robot driver and no gamepad driver: it is the piece
|
on one port. It contains no robot driver and no gamepad driver: it is the piece in the
|
||||||
in the middle that decides what the arm should be doing, and that decision is the only
|
middle that decides what the arm should be doing, and it touches no hardware at all.
|
||||||
thing it owns. It touches no hardware at all.
|
|
||||||
|
**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), Python 3.11+ and `uv`.
|
||||||
|
|
||||||
## The gamepad
|
## The 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 |
|
| Control | Does | Units |
|
||||||
|-------------|-------------------------------------------|-------|
|
|-------------|----------------------------------------------|-------|
|
||||||
| Left stick | Move along X (up/down) and Y (left/right) | mm/s |
|
| Left stick | Move along X (up/down) and Y (left/right) | mm/s |
|
||||||
| Triggers | Lift on the right, lower on the left | mm/s |
|
| Triggers | Lift on the right, lower on the left | mm/s |
|
||||||
| Right stick | Roll (left/right) and pitch (up/down) | deg/s |
|
| Right stick | Roll (left/right) and pitch (up/down) | deg/s |
|
||||||
@@ -33,157 +25,150 @@ pill — but it cannot ask for a third of full speed on one axis and full on ano
|
|||||||
| Y | Toggle freedrive | — |
|
| Y | Toggle freedrive | — |
|
||||||
| Back | **E-STOP** — latches until X | — |
|
| Back | **E-STOP** — latches until X | — |
|
||||||
|
|
||||||
Up on the stick sends the arm away from you, along **+X** in the robot's base frame —
|
Up on the stick sends the arm away from you, along **+X** in the base frame — the same
|
||||||
the same convention the web console's dial uses, so the two agree.
|
convention the console uses, so the two agree.
|
||||||
|
|
||||||
Indices are SDL's, which is what most Xbox- and PlayStation-style pads report. A pad that
|
**Back latches.** It stops the arm, tells the controller to stop, and keeps the arm still
|
||||||
disagrees is a change to `Layout` in `dora_teleop/mapping.py` rather than a new
|
no matter what the sticks do. **X is the only way out**, and it does both halves: releases
|
||||||
environment variable: an index nobody can verify from outside is worse than a line of code
|
the latch here and clears the error on the controller. Neither button is gated on the
|
||||||
somebody can read. The layout lives here rather than in `dora-joy` for the same reason —
|
lease — an idle pad hands the arm back two seconds after the stop it just made, and a
|
||||||
`dora-joy` does not know what a button is for, which is what makes it reusable.
|
latch you must first take the arm back to release is a trap.
|
||||||
|
|
||||||
`home` is deliberately **not** on the pad. It is a large unattended motion and belongs
|
`home` is deliberately **not** on the pad. It is a large unattended motion and belongs
|
||||||
behind a deliberate click, not a thumb resting on a face button.
|
behind a deliberate click.
|
||||||
|
|
||||||
**Back latches.** It stops the arm, tells the controller to stop, and then keeps the arm
|
### The layout
|
||||||
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
|
These are the indices an Xbox One S pad reports **over Bluetooth**, swept one control at a
|
||||||
here and clears the error on the controller. Neither button is gated on the lease, because
|
time. They are not universal — the same pad on USB numbers its buttons differently — so a
|
||||||
an idle pad hands the arm back two seconds after the stop it just made, and a latch you
|
pad that disagrees is an edit to `Layout` in `dora_teleop/mapping.py`, not a new
|
||||||
must first take the arm back to release is a trap.
|
environment variable. It lives here rather than in `dora-joy` because `dora-joy` does not
|
||||||
|
know what a button is *for*, which is what makes it reusable.
|
||||||
|
|
||||||
|
| Control | Axis | Control | Button |
|
||||||
|
|----------------------|------|------------------|--------|
|
||||||
|
| Left stick X / Y | 0, 1 | A — grasp | 0 |
|
||||||
|
| Right stick X / Y | 2, 3 | B — release | 1 |
|
||||||
|
| Left / right trigger | 4, 5 | X — clear | 3 |
|
||||||
|
| | | Y — freedrive | 4 |
|
||||||
|
| | | LB / RB — yaw | 6, 7 |
|
||||||
|
| | | Back — E-STOP | 11 |
|
||||||
|
|
||||||
|
**Sweep a new pad before the arm is powered.** The axes check themselves and the buttons
|
||||||
|
cannot:
|
||||||
|
|
||||||
|
- **Axes.** Every pad is measured once, on the first reading in which it is sitting still.
|
||||||
|
A stick must rest near centre and a trigger near the bottom of its travel; an axis
|
||||||
|
resting anywhere else means the numbering is wrong, something is being held, or the pad
|
||||||
|
is broken. All three read as **no pad** until it rests, and the log names the axis. A
|
||||||
|
trigger read as a stick asks for full speed while lying untouched on the desk, so a pad
|
||||||
|
this node has never seen still never gets the arm.
|
||||||
|
- **Buttons.** An unpressed button reads the same on every pad, so a wrong button index is
|
||||||
|
silent until it fires. That is not hypothetical: on this pad, against the indices this
|
||||||
|
node shipped with, X toggled freedrive and LB was the E-STOP.
|
||||||
|
|
||||||
|
That same measurement makes a worn stick harmless — its resting tilt becomes its zero, so
|
||||||
|
the tilt is **subtracted** rather than deadzoned away and the travel left still reaches
|
||||||
|
full speed. A trigger that reads 0 until it first moves, rather than SDL's −1, is handled
|
||||||
|
the same way instead of reading as half pressed.
|
||||||
|
|
||||||
### Checking your pad
|
### Checking your pad
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
dora up
|
source .venv/bin/activate
|
||||||
dora start dataflow-gamepad.yml --uv --name teleop-bench
|
dora build dataflow-gamepad.yml
|
||||||
|
dora run dataflow-gamepad.yml
|
||||||
uv run dora-teleop-probe # in another terminal
|
uv run dora-teleop-probe # in another terminal
|
||||||
```
|
```
|
||||||
|
|
||||||
Prints the raw axes, the buttons it sees, and the velocity this node would send — live,
|
The probe prints the raw axes, the buttons it sees and the velocity this node would send,
|
||||||
from the same `joy` stream the node reads. **It cannot move anything.** It attaches as a
|
live, from the same `joy` stream the node reads. **It cannot move anything** — it attaches
|
||||||
dynamic node, publishes nothing and never talks to the arm; it only reads.
|
as a dynamic node, publishes nothing and never talks to the arm.
|
||||||
|
|
||||||
```text
|
```text
|
||||||
axes (6 reported)
|
axes (6 reported)
|
||||||
0 left stick X +0.600 ----------|-----#----
|
0 left stick X +0.600 ----------|-----#----
|
||||||
1 left stick Y -0.800 --#-------|----------
|
1 left stick Y -0.800 --#-------|----------
|
||||||
2 left trigger -1.000 #---------|----------
|
4 left trigger -1.000 #---------|----------
|
||||||
...
|
buttons (17 reported)
|
||||||
buttons (6 reported)
|
7:yaw right
|
||||||
5:yaw right
|
|
||||||
|
|
||||||
velocity this would send to the arm
|
velocity this would send to the arm
|
||||||
vx +38.89 mm/s forward / back <--
|
vx +38.89 mm/s forward / back <--
|
||||||
vy +23.53 mm/s left / right <--
|
vy +23.53 mm/s left / right <--
|
||||||
vz +20.39 mm/s up / down <--
|
|
||||||
wz +30.00 deg/s yaw <--
|
wz +30.00 deg/s yaw <--
|
||||||
|
|
||||||
moving: yes
|
moving: yes
|
||||||
asking for the arm: yes
|
asking for the arm: yes
|
||||||
```
|
```
|
||||||
|
|
||||||
Run this before trusting the table above. The indices are SDL's documented layout, not
|
Two things to look for: an axis marked `unused` that moves when you push a stick, and a
|
||||||
observed ones, and a pad that numbers its axes differently is much better found here than
|
bar that does not sit centred with the stick released. It goes through the same
|
||||||
by watching an arm move the wrong way. Two things to look for:
|
`velocity()` the node runs, so what it prints is what the arm would be sent.
|
||||||
|
|
||||||
- **Every axis is listed, including ones the layout does not use** — an axis marked
|
For a pad whose numbering is not known at all, `dora-joy-probe` in the `dora-joy` repo
|
||||||
`unused` that moves when you push a stick means the layout needs changing.
|
needs no dataflow.
|
||||||
- **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
|
## Speaking to any robot
|
||||||
|
|
||||||
Nothing above is specific to one arm. Both control sources are reduced to a **normalised**
|
Both control sources are reduced to a **normalised** six-vector — `[-1, 1]` per degree of
|
||||||
six-vector — `[-1, 1]` per degree of freedom, meaning a fraction of full speed — and only
|
freedom, a fraction of full speed — and only then scaled, in one function. The gamepad
|
||||||
then scaled, in one function. A gamepad reaches that vector through the deadzone and the
|
reaches that vector through the deadzone and the response curve, which correct a worn
|
||||||
response curve, which exist to correct a worn physical stick; the browser sends it already
|
physical stick; the browser sends it already clean.
|
||||||
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:
|
That is what lets one node and one console drive a robot in mm/s and one in m/s. `twist`
|
||||||
neither source ever knew the difference. It is also why a gamepad and the console asking
|
is SI, because that is what a UR or MoveIt takes; configuration stays in mm/s and deg/s,
|
||||||
for the same deflection produce byte-identical output.
|
because `60 mm/s` is a number a person reading a dataflow can judge. `command` carries
|
||||||
|
verbs rather than service names — `gripper_close`, not `grasp` — so swapping a vacuum for
|
||||||
|
a two-finger gripper needs no change at either end.
|
||||||
|
|
||||||
`twist` is SI — m/s and rad/s — because that is unambiguous and is what a UR or MoveIt
|
The full contract, including the reserved `joint_jog` port, is in the vault at
|
||||||
takes. Configuration stays in mm/s and deg/s, because `60 mm/s` is a number a person
|
`resources/robotics/teleop-contract.md`.
|
||||||
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
|
## The lease
|
||||||
|
|
||||||
Two sources, one arm. Whoever holds the lease drives; the other is ignored.
|
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.
|
- 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
|
- It **gives it back** after `GAMEPAD_IDLE_RELEASE_S` untouched, so a pad left on the
|
||||||
bench does not lock the browser out forever.
|
bench does not lock the browser out.
|
||||||
- The **browser takes it explicitly**, with the console's TAKE CONTROL button, and gives
|
- The **browser takes it explicitly** with TAKE CONTROL and returns it with RELEASE. It
|
||||||
it back with RELEASE. It can preempt a held arm, because an operator who can see the arm
|
can preempt: an operator who can see the arm is better placed than this code to decide.
|
||||||
is better placed than this code to decide they should have it.
|
- **E-STOP is never gated**, from either source, held lease or not.
|
||||||
- **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.
|
**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
|
so another node publishing `request` will preempt an operator mid-motion.
|
||||||
stop it — if that matters for your dataflow, do not wire one.
|
|
||||||
|
|
||||||
## The deadman
|
## The deadman
|
||||||
|
|
||||||
An arm under a velocity command keeps moving until told otherwise, so the question is
|
An arm under a velocity command keeps moving until told otherwise. Four layers, each
|
||||||
never whether to start but whether anything has happened that means it must stop. Four
|
shorter than the one behind it — a backstop that fires first backs up nothing:
|
||||||
layers, outermost last:
|
|
||||||
|
|
||||||
1. **The repeat** (`JOG_KEEPALIVE_S`, 0.2 s) while a source is driving.
|
| Layer | Setting | Default |
|
||||||
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
|
| Held velocity repeated | `JOG_KEEPALIVE_S` (teleop) | 0.2 s |
|
||||||
the same thing. `dora-joy` publishes on every tick whether or not the pad moved, which
|
| Pad reading goes stale | `JOY_TTL_S` (teleop) | 0.2 s |
|
||||||
is what makes silence unambiguous.
|
| Owner goes quiet | `JOG_TTL_S` (teleop) | 0.4 s |
|
||||||
3. **This node's watchdog** (`JOG_TTL_S`, 0.4 s) stops the arm when the owner goes quiet —
|
| Jog sender goes quiet | `VELOCITY_COMMAND_TTL_S` (ulite6) | 0.5 s |
|
||||||
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
|
`JOY_TTL_S >= JOG_TTL_S` stops the node at startup rather than failing quietly later. The
|
||||||
the thing it is backing up backs up nothing. `JOY_TTL_S >= JOG_TTL_S` stops the node at
|
last layer is enforced by `dora-ulite6` whether or not this process is alive.
|
||||||
startup rather than letting that happen quietly.
|
|
||||||
|
|
||||||
On shutdown the node sends one all-zero velocity before exiting. A motion lives in the
|
A pad reporting itself gone, or the last browser disconnecting, stops the arm
|
||||||
controller, not in this process.
|
**immediately** without waiting out any timer: an unplugged pad is not a centred pad. On
|
||||||
|
shutdown the node sends one all-zero velocity, because a motion lives in the controller
|
||||||
|
rather than in this process.
|
||||||
|
|
||||||
The stop is sent **once**, not every tick. A stream of zeroes would keep `dora-ulite6`
|
**What goes on the wire is the change.** A velocity is a set-point the arm holds, so one
|
||||||
believing a jog is live and hold `robot_state` at `moving` forever.
|
is sent when it *changes* and otherwise repeated only every `JOG_KEEPALIVE_S`. Repeating
|
||||||
|
every tick made a real xArm re-plan the same motion twenty times a second, which reads as
|
||||||
### What goes on the wire is the change
|
stepping. The stop is sent **once** — a stream of zeroes keeps `dora-ulite6` believing a
|
||||||
|
jog is live. A change never waits for the repeat.
|
||||||
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
|
## Ports
|
||||||
|
|
||||||
| Direction | Name | Description |
|
| Direction | Name | Description |
|
||||||
|-----------|----------------|------------------------------------------------------|
|
|-----------|----------------|-------------------------------------------------------|
|
||||||
| in | `tick` | 50 ms — change detection, deadman, telemetry |
|
| in | `tick` | 50 ms — change detection, deadman, telemetry |
|
||||||
| in | `joy` | One pad reading from `dora-joy` |
|
| in | `joy` | One pad reading from `dora-joy` |
|
||||||
| in | `response` | An answer from the arm |
|
| in | `response` | An answer from the arm |
|
||||||
@@ -195,239 +180,125 @@ happens, so sending less does not cost anything at the operator's end.
|
|||||||
| out | `command` | **Standard.** `{action, value}`, robot-agnostic verbs |
|
| out | `command` | **Standard.** `{action, value}`, robot-agnostic verbs |
|
||||||
| out | `jog` | *Deprecated.* mm/s + deg/s, for `dora-ulite6` |
|
| out | `jog` | *Deprecated.* mm/s + deg/s, for `dora-ulite6` |
|
||||||
| out | `request` | *Deprecated.* A service call, `request_id` in metadata |
|
| out | `request` | *Deprecated.* A service call, `request_id` in metadata |
|
||||||
| out | `ui_state` | The console's telemetry snapshot, as JSON — see below |
|
| out | `ui_state` | The console's telemetry snapshot, as JSON |
|
||||||
|
|
||||||
Wire `twist` **or** `jog`, never both: they carry the same intent, and a robot reading
|
Wire `twist` **or** `jog`, never both — they carry the same intent, and a robot reading
|
||||||
both is driven twice. Same for `command` and `request`.
|
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
|
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 —
|
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
|
## Environment variables
|
||||||
|
|
||||||
| Variable | Default | Description |
|
| Variable | Default | Description |
|
||||||
|--------------------------|---------|--------------------------------------------------------|
|
|--------------------------|----------|--------------------------------------------------------|
|
||||||
| `SPEED_MM_S` | `60.0` | Translation speed at full deflection |
|
| `SPEED_MM_S` | `60.0` | Translation speed at full deflection |
|
||||||
| `SPEED_DEG_S` | `30.0` | Rotation speed at full deflection |
|
| `SPEED_DEG_S` | `30.0` | Rotation speed at full deflection |
|
||||||
| `DEADZONE` | `0.12` | Stick travel near centre to ignore |
|
| `DEADZONE` | `0.12` | Stick travel near centre to ignore |
|
||||||
| `EXPO` | `0.4` | How much of the curve is cubic; 0 is linear |
|
| `EXPO` | `0.4` | How much of the curve is cubic; 0 is linear |
|
||||||
| `JOG_TTL_S` | `0.4` | Owner silence before the arm is stopped |
|
| `JOG_TTL_S` | `0.4` | Owner silence before the arm is stopped |
|
||||||
|
| `JOG_KEEPALIVE_S` | `0.2` | How often an unchanged velocity is repeated |
|
||||||
| `JOY_TTL_S` | `0.2` | `joy` silence before the pad reads as gone |
|
| `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 |
|
| `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 |
|
| `STATE_POLL_S` | `0.5` | How often to ask the arm what telemetry does not carry |
|
||||||
|
| `WEB_HOST` | `0.0.0.0`| Address the console binds to |
|
||||||
|
| `WEB_PORT` | `7000` | Console port; **`0` opens no port at all** |
|
||||||
|
|
||||||
A value that cannot be read, a `DEADZONE` outside the stick, an `EXPO` off the curve or a
|
A value that cannot be read, or one that breaks the deadman order, stops the node at
|
||||||
`JOY_TTL_S` at or above `JOG_TTL_S` stops the node at startup rather than the first time
|
startup rather than the first time somebody picks up the pad. `JOYSTICK_INDEX` moved to
|
||||||
somebody picks up the pad.
|
`dora-joy` along with the pad.
|
||||||
|
|
||||||
`JOYSTICK_INDEX` moved to `dora-joy` along with the pad.
|
## Running it
|
||||||
|
|
||||||
## Usage
|
| Dataflow | Nodes | Needs |
|
||||||
|
|-------------------------|------------------------------|-----------------|
|
||||||
The real stack is three nodes: this one, the pad and the arm. The console comes with
|
| `dataflow.yml` | teleop + bench stand-in | nothing |
|
||||||
this node.
|
| `dataflow-robot.yml` | joy + teleop + ulite6 | a pad and an arm |
|
||||||
|
| `dataflow-gamepad.yml` | the same, plus the probe, console shut | a pad and an arm |
|
||||||
```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
|
```bash
|
||||||
source .venv/bin/activate # see the note below before reaching for --uv
|
dora run dataflow.yml --uv # no pad, no arm, browser path only
|
||||||
dora build dataflow-robot.yml # clones and installs both, needs the network
|
|
||||||
|
source .venv/bin/activate # a real arm: see the --uv note below
|
||||||
|
dora build dataflow-robot.yml # clones and installs both siblings
|
||||||
dora run dataflow-robot.yml
|
dora run dataflow-robot.yml
|
||||||
```
|
```
|
||||||
|
|
||||||
**`--uv` does not work for a dataflow with `git:` nodes** (dora 1.0.1). `dora build --uv`
|
`dataflow.yml` is self-contained on purpose — it resolves from this checkout alone, which
|
||||||
installs each `git:` node into a venv inside its own clone directory, while `dora run
|
is what lets CI validate it. The bench answers services and publishes telemetry but is
|
||||||
--uv` resolves the entrypoint by running `uv run which <name>` from the *dataflow's*
|
**not a simulator**: it never integrates a velocity or moves a pose.
|
||||||
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,
|
Set `ROBOT_IP` and `TOOL_TYPE` to match the arm in front of you. `INIT_MODE` is `none`, so
|
||||||
which is already in this repo's venv.
|
nothing moves until somebody pushes a stick. The console comes with the node — open
|
||||||
|
`http://<that machine>:7000` for the same arm the pad drives, under the same lease.
|
||||||
|
|
||||||
Set `ROBOT_IP` and `TOOL_TYPE` in that file to match the arm in front of you; the defaults
|
**`--uv` does not work for a dataflow with `git:` nodes** (dora 1.0.1): each is installed
|
||||||
are `dora-ulite6`'s own, `192.168.1.192` and a vacuum tool. `INIT_MODE` is `none`, so
|
into a venv inside its own clone, while the spawner resolves entrypoints from the
|
||||||
nothing moves until somebody pushes a stick.
|
dataflow's venv, and the run fails with `Could not find source path dora-joy within uv`.
|
||||||
|
Build into one activated environment instead, as above; it needs `pip`
|
||||||
|
(`uv pip install pip`). **`dora start` refuses them outright** — "node joy has git source,
|
||||||
|
but no `dora build` was run yet", straight after a build that succeeded — so use
|
||||||
|
`dora run`, which also means `dora topic` is unavailable for those two dataflows.
|
||||||
|
|
||||||
The deadman timings have to stay ordered, each below the one behind it, and the file pins
|
`dora` type-checks a dataflow before fetching a `git:` source, so it cannot read the arm's
|
||||||
all of them so the chain is visible in one place:
|
manifest. The `output_types:` block on the `ulite6` node repeats what `dora-ulite6` v0.3.1
|
||||||
|
annotates — re-check it when moving to a newer tag.
|
||||||
| 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
|
### Watching the wire
|
||||||
|
|
||||||
`dataflow.yml` sets `debug: enable_debug_inspection: true`, so dora will let you read what
|
Every dataflow sets `debug: enable_debug_inspection: true`, so dora will let you read the
|
||||||
is actually on the wire without the bench printing anything. Run it under a coordinator
|
wire without any node printing it. This needs a coordinator, so it works for the
|
||||||
rather than `dora run`:
|
arm-free dataflow:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
dora up
|
dora up
|
||||||
dora start dataflow-gamepad.yml --uv --name teleop-bench
|
dora start dataflow.yml --uv --name teleop-bench
|
||||||
```
|
|
||||||
|
|
||||||
Then, in another terminal:
|
|
||||||
|
|
||||||
```bash
|
|
||||||
dora topic list -d teleop-bench # every edge in the graph
|
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/jog # every velocity, live
|
||||||
dora topic echo -d teleop-bench teleop/ui_state # what the console would draw
|
|
||||||
dora topic hz -d teleop-bench teleop/jog --duration 10 # the re-send cadence
|
dora topic hz -d teleop-bench teleop/jog --duration 10 # the re-send cadence
|
||||||
|
dora stop --name teleop-bench && dora down
|
||||||
```
|
```
|
||||||
|
|
||||||
`hz` is the one that checks the timings this node promises. The `get_state` poll measures
|
|
||||||
at 1.88 Hz against a configured `STATE_POLL_S` of 0.5, and with a pad held over the
|
|
||||||
deadzone `teleop/jog` should sit near the 50 ms tick.
|
|
||||||
|
|
||||||
Tear it down with `dora stop --name teleop-bench && dora down`.
|
|
||||||
|
|
||||||
## The console
|
## The console
|
||||||
|
|
||||||
`dora_teleop/web/index.html` is the browser half: two analog sticks, a lift lane and a
|
`dora_teleop/web/index.html` is the browser half: two analog rings, a lift lane and a yaw
|
||||||
yaw lane, plus gripper, freedrive, home, the lease buttons and E-STOP over a telemetry
|
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
|
readout. Plain HTML with no build step — a console you cannot fix without a toolchain is
|
||||||
console you cannot fix without a toolchain is the wrong thing to have between a person and
|
the wrong thing to have between a person and a robot.
|
||||||
a robot.
|
|
||||||
|
|
||||||
**Laid out like the arm's own pendant.** Each ring is quartered into arc sectors you hold,
|
It is laid out like the arm's own pendant: each ring is quartered into sectors you hold,
|
||||||
with the out-of-plane axis as a pair of pills above it — the same shape as xArm Studio's
|
with the out-of-plane axis as a pair of pills above it — the same shape and the same
|
||||||
jog control on the controller at `:18333`, and deliberately the same directions, because
|
directions as xArm Studio's jog control at `:18333`.
|
||||||
two consoles driving one arm that disagree about which way is `+Y` is a trap rather than a
|
|
||||||
preference.
|
|
||||||
|
|
||||||
| Ring | Sectors | Pills above |
|
| Ring | Sectors | Pills above |
|
||||||
|-----------|------------------------------------------------|----------------|
|
|-----------|------------------------------------------------------------|---------------|
|
||||||
| translate | `+X` north, `−X` south, `Y+` **west**, `Y−` east | `−Z` `+Z` |
|
| translate | `+X` north, `−X` south, `Y+` **west**, `Y−` east | `−Z` `+Z` |
|
||||||
| rotate | `+pitch` north, `−pitch` south, `−roll` west, `+roll` east | `−yaw` `+yaw` |
|
| 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
|
**`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
|
the operator and `+Z` up. Two consoles that disagree about which way is `+Y` is a trap.
|
||||||
right", and it is that way on purpose.
|
|
||||||
|
|
||||||
Directions are held, not analog: speed comes from the dial, and every held direction is a
|
The node serves the page and holds the WebSocket on `/ws` of the same port, so there is
|
||||||
separate pointer, so two fingers work at once — driving `+X` while lifting on `+Z` is one
|
one URL and nothing to keep in step. That runs on its own thread and meets the control
|
||||||
gesture.
|
loop at two non-blocking points only, because a browser that stops reading must never
|
||||||
|
stall the thread that stops the arm. **The browser's deadman is the socket itself** — the
|
||||||
|
last one closing releases the lease and stops the arm.
|
||||||
|
|
||||||
The node serves it on `WEB_PORT` (7000) and holds the WebSocket on `/ws` of that same
|
Directions are held, not analog: speed comes from the dial, and each held direction is a
|
||||||
port, so the console is one URL. That work happens on a thread of its own and meets the
|
separate pointer, so two fingers work at once. The page re-sends its jog every 150 ms
|
||||||
control loop at two points only — the messages the next tick drains, and the snapshot
|
against the node's 400 ms deadline, and sends a **fraction of full deflection**, never a
|
||||||
handed back — because a browser that stops reading must never be able to stall the thread
|
speed, so it never learns the robot's units. Controls are dimmed and inert unless the
|
||||||
that stops the arm.
|
console holds the lease. If `WEB_PORT` is taken the node **refuses to start**.
|
||||||
|
|
||||||
**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
|
## Development
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
uv sync --group dev
|
uv sync --group dev
|
||||||
uv run pytest # 112 tests, no gamepad, arm, browser or daemon needed
|
uv run pytest # 149 tests, no gamepad, arm, browser or daemon needed
|
||||||
uv run ruff check .
|
uv run ruff check .
|
||||||
uv run ruff format --check .
|
uv run ruff format --check .
|
||||||
```
|
```
|
||||||
|
|
||||||
Every file here is testable without hardware, because there is no hardware. The pad
|
Every file here is testable without hardware, because there is no hardware. `tests/` names
|
||||||
arrives on `joy` as a plain `Sample`.
|
its controls (`axes_for`, `buttons_for`) rather than numbering them — a test that numbers
|
||||||
|
its axes goes on passing while meaning something else once a pad renumbers them.
|
||||||
|
|||||||
+57
-26
@@ -1,23 +1,29 @@
|
|||||||
# The same bench, with a real gamepad and a probe.
|
# The pad, a real arm and the probe. No bench and no browser.
|
||||||
#
|
#
|
||||||
# `dataflow.yml` is what CI validates, and it deliberately has no pad in it: the
|
# `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
|
# 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.
|
# file its one property — that it resolves and runs from this checkout alone.
|
||||||
# This file is the other half. It needs the network on first build, and a pad.
|
# This file is the other half. It needs the network on first build, a pad, and
|
||||||
|
# an arm.
|
||||||
#
|
#
|
||||||
# The bench answers services and publishes telemetry but never moves anything.
|
# It is `dataflow-robot.yml` with the probe attached and the console shut, for
|
||||||
# What this is for is watching the node decide: the lease changing hands, the
|
# the one job the probe exists for: checking that the pad in your hands has the
|
||||||
# deadman firing when you let go, and the arm being stopped when the pad — or
|
# layout `mapping.py` assumes, against the arm that will actually move. Use
|
||||||
# the node reading it — goes away.
|
# `dataflow-robot.yml` for ordinary driving.
|
||||||
#
|
#
|
||||||
# Run it under a coordinator rather than `dora run`, so `dora topic` can read the
|
# source .venv/bin/activate # not --uv; README.md says why
|
||||||
# wire and so the probe has something to attach to:
|
# dora build dataflow-gamepad.yml
|
||||||
|
# dora run dataflow-gamepad.yml
|
||||||
#
|
#
|
||||||
# dora up
|
# `dora run`, not `dora up` + `dora start`: dora 1.0.1 refuses to start a
|
||||||
# dora start dataflow-gamepad.yml --uv --name teleop-bench
|
# dataflow holding `git:` nodes from a coordinator — "node joy has git source,
|
||||||
# dora-teleop-probe # in another terminal
|
# but no `dora build` was run yet", straight after a build that succeeded. It is
|
||||||
# dora topic echo -d teleop-bench teleop/jog
|
# the same shape of gap as the `--uv` one above, and it costs this file
|
||||||
# dora stop --name teleop-bench && dora down
|
# `dora topic`, which needs a coordinator to read the wire.
|
||||||
|
#
|
||||||
|
# ROBOT_IP and TOOL_TYPE below name the arm in front of you: the bench arm at
|
||||||
|
# 192.168.1.162, with a vacuum tool. Read
|
||||||
|
# `The gamepad` in README.md before picking it up — Back is the E-STOP.
|
||||||
|
|
||||||
strict_types: true
|
strict_types: true
|
||||||
|
|
||||||
@@ -47,10 +53,11 @@ nodes:
|
|||||||
source: joy/joy
|
source: joy/joy
|
||||||
queue_size: 1
|
queue_size: 1
|
||||||
queue_policy: drop_oldest
|
queue_policy: drop_oldest
|
||||||
response: bench/response
|
response: ulite6/response
|
||||||
robot_state: bench/robot_state
|
motion_state: ulite6/motion_state
|
||||||
tcp_pose: bench/tcp_pose
|
robot_state: ulite6/robot_state
|
||||||
joint_pose: bench/joint_pose
|
tcp_pose: ulite6/tcp_pose
|
||||||
|
joint_pose: ulite6/joint_pose
|
||||||
outputs:
|
outputs:
|
||||||
- twist
|
- twist
|
||||||
- command
|
- command
|
||||||
@@ -60,8 +67,13 @@ nodes:
|
|||||||
env:
|
env:
|
||||||
SPEED_MM_S: "60"
|
SPEED_MM_S: "60"
|
||||||
SPEED_DEG_S: "30"
|
SPEED_DEG_S: "30"
|
||||||
# This one is about the pad, so the console stays shut. Set a port to
|
# Keep the deadman layers ordered: JOY_TTL_S < JOG_TTL_S < the arm's own
|
||||||
# bring it up alongside and watch the lease move between the two.
|
# VELOCITY_COMMAND_TTL_S, which is 0.5 s below. Each backs up the one
|
||||||
|
# inside it, and a backstop that fires first backs up nothing.
|
||||||
|
JOY_TTL_S: "0.2"
|
||||||
|
JOG_TTL_S: "0.4"
|
||||||
|
# This one is about the pad, so the console stays shut and no port is
|
||||||
|
# opened. `dataflow-robot.yml` is the one that serves it.
|
||||||
WEB_PORT: "0"
|
WEB_PORT: "0"
|
||||||
|
|
||||||
# Declared, not spawned: `dora-teleop-probe` attaches to this id when you run
|
# Declared, not spawned: `dora-teleop-probe` attaches to this id when you run
|
||||||
@@ -71,15 +83,34 @@ nodes:
|
|||||||
inputs:
|
inputs:
|
||||||
joy: joy/joy
|
joy: joy/joy
|
||||||
|
|
||||||
- id: bench
|
- id: ulite6
|
||||||
path: examples/bench.py
|
git: https://gitea.liria.cl/liria/dora-ulite6.git
|
||||||
|
tag: v0.3.1
|
||||||
|
build: pip install .
|
||||||
|
path: dora-ulite6
|
||||||
inputs:
|
inputs:
|
||||||
tick: dora/timer/millis/200
|
tick: dora/timer/millis/100
|
||||||
jog: teleop/jog
|
|
||||||
request: teleop/request
|
request: teleop/request
|
||||||
ui_state: teleop/ui_state
|
jog: teleop/jog
|
||||||
outputs:
|
outputs:
|
||||||
- response
|
- response
|
||||||
- robot_state
|
- motion_state
|
||||||
- tcp_pose
|
|
||||||
- joint_pose
|
- joint_pose
|
||||||
|
- tcp_pose
|
||||||
|
- robot_state
|
||||||
|
# dora type-checks before it fetches a `git:` source, so it cannot read the
|
||||||
|
# arm's own manifest from here. These three repeat what `dora-ulite6`
|
||||||
|
# v0.3.1 annotates; `response` and `motion_state` stay untyped there and so
|
||||||
|
# stay untyped here. Re-check them when moving to a new tag.
|
||||||
|
output_types:
|
||||||
|
joint_pose: std/core/v1/Float32
|
||||||
|
tcp_pose: std/core/v1/Float32
|
||||||
|
robot_state: std/core/v1/String
|
||||||
|
env:
|
||||||
|
ROBOT_IP: "192.168.1.162"
|
||||||
|
TOOL_TYPE: "vacuum"
|
||||||
|
# The outermost deadman, enforced by the controller whether or not any of
|
||||||
|
# this is alive. Every timer above must stay below it.
|
||||||
|
VELOCITY_COMMAND_TTL_S: "0.5"
|
||||||
|
# The pad does not home. Nothing moves until somebody pushes a stick.
|
||||||
|
INIT_MODE: "none"
|
||||||
|
|||||||
+3
-3
@@ -16,8 +16,8 @@
|
|||||||
# then looks for its entrypoint in this dataflow's venv instead. README.md has the
|
# 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.
|
# 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
|
# ROBOT_IP and TOOL_TYPE below name the arm in front of you: the bench arm at
|
||||||
# defaults are `dora-ulite6`'s own: 192.168.1.192 and a vacuum tool.
|
# 192.168.1.162, with a vacuum tool.
|
||||||
#
|
#
|
||||||
# The web console comes with the node: it serves the page and holds the
|
# 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
|
# browsers' sockets itself, so `http://<this machine>:7000` is a second way to
|
||||||
@@ -102,7 +102,7 @@ nodes:
|
|||||||
tcp_pose: std/core/v1/Float32
|
tcp_pose: std/core/v1/Float32
|
||||||
robot_state: std/core/v1/String
|
robot_state: std/core/v1/String
|
||||||
env:
|
env:
|
||||||
ROBOT_IP: "192.168.1.192"
|
ROBOT_IP: "192.168.1.162"
|
||||||
TOOL_TYPE: "vacuum"
|
TOOL_TYPE: "vacuum"
|
||||||
# The outermost deadman, enforced by the controller whether or not any of
|
# The outermost deadman, enforced by the controller whether or not any of
|
||||||
# this is alive. Every timer above must stay below it.
|
# this is alive. Every timer above must stay below it.
|
||||||
|
|||||||
+3
-2
@@ -5,8 +5,9 @@
|
|||||||
# manifest from `path:` and CI needs no gamepad, no robot and no sibling clones.
|
# 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
|
# 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
|
# by `git:` would cost this file the one property it has. For a real pad, use
|
||||||
# real bench, use `dataflow-gamepad.yml`.
|
# `dataflow-robot.yml`, or `dataflow-gamepad.yml` to check the pad with the
|
||||||
|
# probe — both of which drive a real arm.
|
||||||
#
|
#
|
||||||
# This is **not** the real stack. The bench answers services and publishes
|
# 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
|
# telemetry but never moves anything. README.md carries the four-node wiring a
|
||||||
|
|||||||
@@ -53,6 +53,12 @@ inputs:
|
|||||||
pad node is indistinguishable from an unplugged pad as far as the arm is
|
pad node is indistinguishable from an unplugged pad as far as the arm is
|
||||||
concerned.
|
concerned.
|
||||||
|
|
||||||
|
Each pad is measured the first time it is seen sitting still, so a stick
|
||||||
|
that no longer centres has its tilt subtracted rather than deadzoned away.
|
||||||
|
A pad whose resting axes do not fit the layout this node compiles in — a
|
||||||
|
trigger where a stick should be, say — reads the same as no pad at all
|
||||||
|
and is named in the log, rather than being guessed at against a real arm.
|
||||||
|
|
||||||
Leave it unwired for a browser-only console. The node says so and starts.
|
Leave it unwired for a browser-only console. The node says so and starts.
|
||||||
response:
|
response:
|
||||||
required: false
|
required: false
|
||||||
|
|||||||
+75
-3
@@ -12,11 +12,19 @@ unplugged pad. So silence past `JOY_TTL_S` reads as *absent*, which is the same
|
|||||||
answer, because a reading nobody is refreshing is not evidence of anything.
|
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
|
`dora-joy` publishes on every tick whether or not the pad moved, so silence
|
||||||
means one thing only.
|
means one thing only.
|
||||||
|
|
||||||
|
The other thing this seam does is :class:`Calibration`: no two pads rest in
|
||||||
|
quite the same place, and one that rests somewhere the layout does not expect
|
||||||
|
is a pad this node must not drive an arm from. Every reading the loop sees has
|
||||||
|
been through it, so the rest of the node only ever handles a pad that behaves.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import time
|
import time
|
||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
|
|
||||||
|
from dora_teleop import mapping
|
||||||
|
from dora_teleop.log import log
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class Sample:
|
class Sample:
|
||||||
@@ -52,17 +60,81 @@ def decode(event: dict) -> Sample:
|
|||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class Calibration:
|
||||||
|
"""Where the pad in front of us rests, and whether the layout fits it.
|
||||||
|
|
||||||
|
Captured from the first reading of a pad that is sitting still, and kept
|
||||||
|
until that pad goes away. It is not re-taken while the pad is in use, which
|
||||||
|
would be the one way to get this badly wrong: an operator holding a stick
|
||||||
|
when it happened would have that deflection recorded as centre, and the arm
|
||||||
|
would then move whenever they let go.
|
||||||
|
|
||||||
|
A pad whose resting axes do not fit the layout is reported **absent** rather
|
||||||
|
than corrected. The numbering cannot be guessed from one reading — a trigger
|
||||||
|
resting at -1.0 and a stick held hard over look identical — and guessing
|
||||||
|
wrong drives a real arm. `dora-teleop-probe` shows what the pad reports, and
|
||||||
|
`Layout` in :mod:`dora_teleop.mapping` is where a pad that disagrees is
|
||||||
|
written down.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, layout: mapping.Layout = mapping.LAYOUT):
|
||||||
|
self.layout = layout
|
||||||
|
self.mismatch: str | None = None
|
||||||
|
self._zeros: list[float] | None = None
|
||||||
|
|
||||||
|
@property
|
||||||
|
def known(self) -> bool:
|
||||||
|
"""Whether a pad has been measured at rest and may be driven from."""
|
||||||
|
return self._zeros is not None
|
||||||
|
|
||||||
|
def forget(self) -> None:
|
||||||
|
"""Drop what was learned, so the next pad to appear is measured afresh."""
|
||||||
|
self._zeros = None
|
||||||
|
self.mismatch = None
|
||||||
|
|
||||||
|
def apply(self, sample: Sample) -> Sample:
|
||||||
|
"""Return the reading as the layout's ideal pad would have sent it.
|
||||||
|
|
||||||
|
An absent pad passes straight through and takes the calibration with
|
||||||
|
it. A pad that does not fit the layout comes back absent, which every
|
||||||
|
path in this node already treats as a reason to stop.
|
||||||
|
"""
|
||||||
|
if not sample.present:
|
||||||
|
self.forget()
|
||||||
|
return sample
|
||||||
|
if self._zeros is None or len(self._zeros) != len(sample.axes):
|
||||||
|
try:
|
||||||
|
self._zeros = mapping.rest_zeros(sample.axes, self.layout)
|
||||||
|
except ValueError as wrong:
|
||||||
|
# Logged once per pad: this runs on every reading, and a pad
|
||||||
|
# that does not fit the layout does not fit it fifty times a
|
||||||
|
# second.
|
||||||
|
if self.mismatch is None:
|
||||||
|
log(
|
||||||
|
f"ignoring the gamepad: {wrong}. Run dora-teleop-probe and check Layout in mapping.py"
|
||||||
|
)
|
||||||
|
self.mismatch = str(wrong)
|
||||||
|
return ABSENT
|
||||||
|
self.mismatch = None
|
||||||
|
return Sample(
|
||||||
|
axes=mapping.canonical(sample.axes, self._zeros, self.layout),
|
||||||
|
buttons=sample.buttons,
|
||||||
|
present=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
class JoyInput:
|
class JoyInput:
|
||||||
"""The latest reading, and how long ago it arrived."""
|
"""The latest reading, and how long ago it arrived."""
|
||||||
|
|
||||||
def __init__(self, ttl_s: float):
|
def __init__(self, ttl_s: float, layout: mapping.Layout = mapping.LAYOUT):
|
||||||
self.ttl_s = ttl_s
|
self.ttl_s = ttl_s
|
||||||
|
self.calibration = Calibration(layout)
|
||||||
self._sample = ABSENT
|
self._sample = ABSENT
|
||||||
self._seen_at = 0.0
|
self._seen_at = 0.0
|
||||||
|
|
||||||
def accept(self, sample: Sample, moment: float) -> None:
|
def accept(self, sample: Sample, moment: float) -> None:
|
||||||
"""Take one reading off the wire."""
|
"""Take one reading off the wire, calibrated to the pad that sent it."""
|
||||||
self._sample = sample
|
self._sample = self.calibration.apply(sample)
|
||||||
self._seen_at = moment
|
self._seen_at = moment
|
||||||
|
|
||||||
def read(self, moment: float) -> Sample:
|
def read(self, moment: float) -> Sample:
|
||||||
|
|||||||
+117
-16
@@ -34,26 +34,34 @@ MM_PER_M = 1000.0
|
|||||||
class Layout:
|
class Layout:
|
||||||
"""Which axis and button index does what, in SDL's numbering.
|
"""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
|
These are what an Xbox One S pad reports over Bluetooth, swept one control
|
||||||
what it reports for most PlayStation-style ones. A pad that disagrees is a
|
at a time: sticks on 0-3, both triggers last, and buttons with gaps at 2 and
|
||||||
change here rather than a new environment variable: an index nobody can
|
5. A pad that disagrees is a change here rather than a new environment
|
||||||
verify from the outside is worse than a line of code somebody can read.
|
variable: an index nobody can verify from the outside is worse than a line
|
||||||
|
of code somebody can read. `dora-teleop-probe` is how you check.
|
||||||
|
|
||||||
|
**The axes can be checked at runtime and the buttons cannot.**
|
||||||
|
:func:`rest_zeros` refuses a pad whose resting axes say this numbering is
|
||||||
|
wrong, because an axis at rest has a value to compare. An unpressed button
|
||||||
|
reads the same on every pad, so a wrong button index is silent — it was
|
||||||
|
found here by `clear` toggling freedrive and `yaw_left` firing the E-STOP.
|
||||||
|
The probe, and pressing each button once, is the only check there is.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
left_x: int = 0
|
left_x: int = 0
|
||||||
left_y: int = 1
|
left_y: int = 1
|
||||||
left_trigger: int = 2
|
right_x: int = 2
|
||||||
right_x: int = 3
|
right_y: int = 3
|
||||||
right_y: int = 4
|
left_trigger: int = 4
|
||||||
right_trigger: int = 5
|
right_trigger: int = 5
|
||||||
|
|
||||||
yaw_left: int = 4 # LB
|
|
||||||
yaw_right: int = 5 # RB
|
|
||||||
grasp: int = 0 # A
|
grasp: int = 0 # A
|
||||||
release: int = 1 # B
|
release: int = 1 # B
|
||||||
clear: int = 2 # X
|
clear: int = 3 # X
|
||||||
freedrive: int = 3 # Y
|
freedrive: int = 4 # Y
|
||||||
estop: int = 6 # Back
|
yaw_left: int = 6 # LB
|
||||||
|
yaw_right: int = 7 # RB
|
||||||
|
estop: int = 11 # Back
|
||||||
|
|
||||||
|
|
||||||
LAYOUT = Layout()
|
LAYOUT = Layout()
|
||||||
@@ -69,6 +77,16 @@ TOOL_FRAME = "tool"
|
|||||||
# would mean a stick could claim the arm and then not move it.
|
# would mean a stick could claim the arm and then not move it.
|
||||||
ACTIVITY = 0.3
|
ACTIVITY = 0.3
|
||||||
|
|
||||||
|
# Where an untouched control sits once :func:`canonical` has had it: a stick at
|
||||||
|
# centre, a trigger at the bottom of its travel.
|
||||||
|
STICK_REST = 0.0
|
||||||
|
TRIGGER_REST = -1.0
|
||||||
|
|
||||||
|
# How far an axis may rest from those and still be believed. Wide enough for a
|
||||||
|
# worn stick's tilt and for a pad that reports a trigger it has never felt as 0,
|
||||||
|
# and far short of the full deflection a wrongly numbered axis shows.
|
||||||
|
REST_TOLERANCE = 0.25
|
||||||
|
|
||||||
|
|
||||||
def deadzone(value: float, width: float) -> float:
|
def deadzone(value: float, width: float) -> float:
|
||||||
"""Ignore travel near centre, and rescale what is left to the full range.
|
"""Ignore travel near centre, and rescale what is left to the full range.
|
||||||
@@ -98,11 +116,13 @@ def expo(value: float, factor: float) -> float:
|
|||||||
def trigger(value: float) -> float:
|
def trigger(value: float) -> float:
|
||||||
"""Return a trigger's travel as 0 at rest and 1 fully pressed.
|
"""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
|
SDL reports a trigger from -1 to 1, unlike a stick, so `TRIGGER_REST` is
|
||||||
never been touched sometimes reads 0 instead of -1 until it first moves.
|
where an untouched one sits. A trigger that has never moved sometimes reads
|
||||||
Clamping keeps that from reading as half pressed.
|
0 instead, which this function alone cannot tell from half pressed —
|
||||||
|
:func:`canonical` is what fixes that, by measuring the pad's own rest before
|
||||||
|
anything reaches here.
|
||||||
"""
|
"""
|
||||||
return max(0.0, min(1.0, (value + 1.0) / 2.0))
|
return max(0.0, min(1.0, (value - TRIGGER_REST) / (1.0 - TRIGGER_REST)))
|
||||||
|
|
||||||
|
|
||||||
def axis(values: list[float], index: int) -> float:
|
def axis(values: list[float], index: int) -> float:
|
||||||
@@ -115,6 +135,87 @@ def pressed(buttons: list[bool], index: int) -> bool:
|
|||||||
return bool(buttons[index]) if index < len(buttons) else False
|
return bool(buttons[index]) if index < len(buttons) else False
|
||||||
|
|
||||||
|
|
||||||
|
def axis_names(layout: Layout = LAYOUT) -> dict[int, str]:
|
||||||
|
"""Name each axis index the layout uses."""
|
||||||
|
return {
|
||||||
|
layout.left_x: "left stick X",
|
||||||
|
layout.left_y: "left stick Y",
|
||||||
|
layout.left_trigger: "left trigger",
|
||||||
|
layout.right_x: "right stick X",
|
||||||
|
layout.right_y: "right stick Y",
|
||||||
|
layout.right_trigger: "right trigger",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def rest_zeros(axes: list[float], layout: Layout = LAYOUT) -> list[float]:
|
||||||
|
"""Return where each axis of an untouched pad sits, to measure deflection from.
|
||||||
|
|
||||||
|
An untouched pad is the one moment this node can check `layout` against the
|
||||||
|
thing in front of it: a stick rests near centre and a trigger near the
|
||||||
|
bottom of its travel, so an axis resting anywhere else means the numbering
|
||||||
|
is wrong, or a control is being held, or the pad is broken. All three look
|
||||||
|
the same from here and all three raise, because all three would otherwise
|
||||||
|
drive the arm from a reading nobody made — a trigger read as a stick asks
|
||||||
|
for full speed while sitting untouched on the desk.
|
||||||
|
|
||||||
|
Parameters
|
||||||
|
----------
|
||||||
|
axes
|
||||||
|
One reading, taken while nobody is touching the pad.
|
||||||
|
layout
|
||||||
|
Which index is which control.
|
||||||
|
|
||||||
|
Returns
|
||||||
|
-------
|
||||||
|
list of float
|
||||||
|
The resting value of each axis, in the order they were reported.
|
||||||
|
|
||||||
|
Raises
|
||||||
|
------
|
||||||
|
ValueError
|
||||||
|
If any axis rests where the layout says it cannot, naming the index,
|
||||||
|
what the layout calls it, and what it read.
|
||||||
|
"""
|
||||||
|
names = axis_names(layout)
|
||||||
|
triggers = (layout.left_trigger, layout.right_trigger)
|
||||||
|
for index, value in enumerate(axes):
|
||||||
|
if index in triggers:
|
||||||
|
# Either end is a resting trigger: -1 is what SDL reports, and 0 is
|
||||||
|
# what some pads report until the trigger first moves.
|
||||||
|
if abs(value - TRIGGER_REST) <= REST_TOLERANCE or abs(value - STICK_REST) <= REST_TOLERANCE:
|
||||||
|
continue
|
||||||
|
elif abs(value - STICK_REST) <= REST_TOLERANCE:
|
||||||
|
continue
|
||||||
|
name = names.get(index, "an axis the layout does not use")
|
||||||
|
raise ValueError(f"axis {index} rests at {value:+.3f}, which is not where {name} sits")
|
||||||
|
return list(axes)
|
||||||
|
|
||||||
|
|
||||||
|
def canonical(axes: list[float], zeros: list[float], layout: Layout = LAYOUT) -> list[float]:
|
||||||
|
"""Return this pad's axes as the layout's ideal pad would have reported them.
|
||||||
|
|
||||||
|
Sticks come back resting at `STICK_REST` and triggers at `TRIGGER_REST`,
|
||||||
|
whatever this particular pad does when untouched, so everything downstream
|
||||||
|
is written once for a pad that behaves. A stick that no longer centres, and
|
||||||
|
a trigger that reads 0 until it first moves, both stop being special cases
|
||||||
|
here rather than in every function that reads one.
|
||||||
|
|
||||||
|
Deflection is measured from `zeros` rather than from the ideal, which is the
|
||||||
|
whole point: the tilt is subtracted, not deadzoned away, so the travel that
|
||||||
|
is left still reaches full speed.
|
||||||
|
"""
|
||||||
|
triggers = (layout.left_trigger, layout.right_trigger)
|
||||||
|
out = []
|
||||||
|
for index, value in enumerate(axes):
|
||||||
|
zero = zeros[index] if index < len(zeros) else STICK_REST
|
||||||
|
if index in triggers:
|
||||||
|
travel = (value - zero) / (1.0 - zero) if zero < 1.0 else 0.0
|
||||||
|
out.append(TRIGGER_REST + (1.0 - TRIGGER_REST) * max(0.0, min(1.0, travel)))
|
||||||
|
else:
|
||||||
|
out.append(max(-1.0, min(1.0, value - zero)))
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
def from_pad(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
|
def from_pad(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
|
||||||
"""Map one gamepad reading to the normalised six-vector.
|
"""Map one gamepad reading to the normalised six-vector.
|
||||||
|
|
||||||
|
|||||||
+30
-18
@@ -35,7 +35,7 @@ from dora import Node
|
|||||||
|
|
||||||
from dora_teleop import joy, mapping
|
from dora_teleop import joy, mapping
|
||||||
from dora_teleop.config import Config
|
from dora_teleop.config import Config
|
||||||
from dora_teleop.mapping import LAYOUT
|
from dora_teleop.mapping import LAYOUT, axis_names
|
||||||
|
|
||||||
# What each velocity component is, in the order the arm takes them.
|
# What each velocity component is, in the order the arm takes them.
|
||||||
COMPONENTS = (
|
COMPONENTS = (
|
||||||
@@ -50,18 +50,6 @@ COMPONENTS = (
|
|||||||
CLEAR = "\033[H\033[J"
|
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]:
|
def button_names(layout=LAYOUT) -> dict[int, str]:
|
||||||
"""Name each button index the layout uses."""
|
"""Name each button index the layout uses."""
|
||||||
return {
|
return {
|
||||||
@@ -89,8 +77,15 @@ def bar(value: float, width: int = 21) -> str:
|
|||||||
return "".join(cells)
|
return "".join(cells)
|
||||||
|
|
||||||
|
|
||||||
def render(sample, velocity: list[float], config: Config) -> str:
|
def render(sample, driven, velocity: list[float], config: Config, mismatch: str | None = None) -> str:
|
||||||
"""Build the whole display for one reading."""
|
"""Build the whole display for one reading.
|
||||||
|
|
||||||
|
``sample`` is the pad exactly as it was reported, because seeing the raw
|
||||||
|
numbers is the point of this tool. ``driven`` is the same reading after
|
||||||
|
calibration — what the node actually decides from — and ``velocity`` is what
|
||||||
|
it would send. A pad this node refuses reaches here as an absent ``driven``
|
||||||
|
and a ``mismatch`` saying why.
|
||||||
|
"""
|
||||||
lines = ["dora-teleop probe — Ctrl-C to stop", ""]
|
lines = ["dora-teleop probe — Ctrl-C to stop", ""]
|
||||||
|
|
||||||
if not sample.present:
|
if not sample.present:
|
||||||
@@ -102,6 +97,18 @@ def render(sample, velocity: list[float], config: Config) -> str:
|
|||||||
]
|
]
|
||||||
return "\n".join(lines)
|
return "\n".join(lines)
|
||||||
|
|
||||||
|
if mismatch:
|
||||||
|
lines += [
|
||||||
|
" this pad does not match the layout, and the arm will not be driven from it.",
|
||||||
|
"",
|
||||||
|
f" {mismatch}",
|
||||||
|
"",
|
||||||
|
" Either something was being held when it was plugged in, or the indices",
|
||||||
|
" below are numbered differently on this pad. The axes are raw, so they",
|
||||||
|
" say which: fix `Layout` in dora_teleop/mapping.py to match them.",
|
||||||
|
"",
|
||||||
|
]
|
||||||
|
|
||||||
names = axis_names()
|
names = axis_names()
|
||||||
lines.append(f" axes ({len(sample.axes)} reported)")
|
lines.append(f" axes ({len(sample.axes)} reported)")
|
||||||
for index, value in enumerate(sample.axes):
|
for index, value in enumerate(sample.axes):
|
||||||
@@ -121,7 +128,7 @@ def render(sample, velocity: list[float], config: Config) -> str:
|
|||||||
lines += [
|
lines += [
|
||||||
"",
|
"",
|
||||||
f" moving: {moving}",
|
f" moving: {moving}",
|
||||||
f" asking for the arm: {'yes' if mapping.is_active(sample) else 'no'}",
|
f" asking for the arm: {'yes' if mapping.is_active(driven) else 'no'}",
|
||||||
]
|
]
|
||||||
return "\n".join(lines)
|
return "\n".join(lines)
|
||||||
|
|
||||||
@@ -137,14 +144,19 @@ def main() -> None:
|
|||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
|
|
||||||
config = Config.from_env()
|
config = Config.from_env()
|
||||||
|
calibration = joy.Calibration()
|
||||||
node = Node(args.name)
|
node = Node(args.name)
|
||||||
|
|
||||||
for event in node:
|
for event in node:
|
||||||
if event["type"] != "INPUT" or event["id"] != "joy":
|
if event["type"] != "INPUT" or event["id"] != "joy":
|
||||||
continue
|
continue
|
||||||
sample = joy.decode(event)
|
sample = joy.decode(event)
|
||||||
velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6
|
# Through the same calibration the node drives from, so what this prints
|
||||||
print(CLEAR + render(sample, velocity, config), flush=True)
|
# stays what the arm would get. What is displayed is still the raw
|
||||||
|
# reading: the indices are what a pad is checked against.
|
||||||
|
driven = calibration.apply(sample)
|
||||||
|
velocity = mapping.velocity(driven.axes, driven.buttons, config) if driven.present else [0.0] * 6
|
||||||
|
print(CLEAR + render(sample, driven, velocity, config, calibration.mismatch), flush=True)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
|||||||
+48
-2
@@ -7,6 +7,7 @@ import pytest
|
|||||||
|
|
||||||
from dora_teleop.config import Config
|
from dora_teleop.config import Config
|
||||||
from dora_teleop.joy import Sample
|
from dora_teleop.joy import Sample
|
||||||
|
from dora_teleop.mapping import LAYOUT, TRIGGER_REST
|
||||||
|
|
||||||
# The shape `dora-joy` publishes. Declared here rather than inferred for the same
|
# 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>`,
|
# reason it is declared there: pyarrow reads an empty `axes` list as `list<null>`,
|
||||||
@@ -66,13 +67,58 @@ def stick(axes=None, buttons=None, present=True):
|
|||||||
values = list(axes or [])
|
values = list(axes or [])
|
||||||
values += [0.0] * (6 - len(values))
|
values += [0.0] * (6 - len(values))
|
||||||
# A trigger at rest reads -1, and a test that forgot to say so would look
|
# A trigger at rest reads -1, and a test that forgot to say so would look
|
||||||
# like both triggers held down.
|
# like both triggers held down. Which indices those are comes from the
|
||||||
for index in (2, 5):
|
# layout, so a test never has to be edited when a pad renumbers them.
|
||||||
|
for index in (LAYOUT.left_trigger, LAYOUT.right_trigger):
|
||||||
if axes is None or index >= len(axes):
|
if axes is None or index >= len(axes):
|
||||||
values[index] = -1.0
|
values[index] = -1.0
|
||||||
return Sample(axes=values, buttons=list(buttons or []), present=present)
|
return Sample(axes=values, buttons=list(buttons or []), present=present)
|
||||||
|
|
||||||
|
|
||||||
|
def axes_for(
|
||||||
|
left_x=0.0,
|
||||||
|
left_y=0.0,
|
||||||
|
right_x=0.0,
|
||||||
|
right_y=0.0,
|
||||||
|
left_trigger=TRIGGER_REST,
|
||||||
|
right_trigger=TRIGGER_REST,
|
||||||
|
):
|
||||||
|
"""Lay out a pad's axes by what each control is rather than by its index.
|
||||||
|
|
||||||
|
A test that numbers its axes says nothing about which control it meant, and
|
||||||
|
goes on passing while meaning something else once a pad renumbers them.
|
||||||
|
"""
|
||||||
|
values = [0.0] * 6
|
||||||
|
values[LAYOUT.left_x] = left_x
|
||||||
|
values[LAYOUT.left_y] = left_y
|
||||||
|
values[LAYOUT.right_x] = right_x
|
||||||
|
values[LAYOUT.right_y] = right_y
|
||||||
|
values[LAYOUT.left_trigger] = left_trigger
|
||||||
|
values[LAYOUT.right_trigger] = right_trigger
|
||||||
|
return values
|
||||||
|
|
||||||
|
|
||||||
|
def buttons_for(
|
||||||
|
grasp=False, release=False, clear=False, freedrive=False, yaw_left=False, yaw_right=False, estop=False
|
||||||
|
):
|
||||||
|
"""Lay out a pad's buttons by what each one does rather than by its index.
|
||||||
|
|
||||||
|
The same reason as :func:`axes_for`, and more pressing: an unpressed button
|
||||||
|
reads identically on every pad, so a test that numbers them is the only
|
||||||
|
place a wrong index could ever have been caught.
|
||||||
|
"""
|
||||||
|
wanted = {
|
||||||
|
LAYOUT.grasp: grasp,
|
||||||
|
LAYOUT.release: release,
|
||||||
|
LAYOUT.clear: clear,
|
||||||
|
LAYOUT.freedrive: freedrive,
|
||||||
|
LAYOUT.yaw_left: yaw_left,
|
||||||
|
LAYOUT.yaw_right: yaw_right,
|
||||||
|
LAYOUT.estop: estop,
|
||||||
|
}
|
||||||
|
return [wanted.get(index, False) for index in range(max(wanted) + 1)]
|
||||||
|
|
||||||
|
|
||||||
def joy_message(sample):
|
def joy_message(sample):
|
||||||
"""Build the Arrow row `dora-joy` would publish for one reading."""
|
"""Build the Arrow row `dora-joy` would publish for one reading."""
|
||||||
return pa.array(
|
return pa.array(
|
||||||
|
|||||||
+24
-17
@@ -6,7 +6,7 @@ from dora_teleop import main as loop
|
|||||||
from dora_teleop.arbiter import GAMEPAD, WEB
|
from dora_teleop.arbiter import GAMEPAD, WEB
|
||||||
from dora_teleop.config import Config
|
from dora_teleop.config import Config
|
||||||
from dora_teleop.joy import Sample
|
from dora_teleop.joy import Sample
|
||||||
from tests.conftest import RecordingNode, input_event, joy_message, stick
|
from tests.conftest import RecordingNode, buttons_for, input_event, joy_message, stick
|
||||||
|
|
||||||
STOP = [0.0] * 6
|
STOP = [0.0] * 6
|
||||||
PUSHED = [0.0, -1.0]
|
PUSHED = [0.0, -1.0]
|
||||||
@@ -47,7 +47,14 @@ def ticks(node, state, count=1):
|
|||||||
|
|
||||||
|
|
||||||
def drive(node, state, sample, count=1):
|
def drive(node, state, sample, count=1):
|
||||||
"""Hold one reading for `count` ticks, the way a real pad and joy node would."""
|
"""Hold one reading for `count` ticks, the way a real pad and joy node would.
|
||||||
|
|
||||||
|
A pad this node has not yet seen at rest is sent one resting reading first,
|
||||||
|
because a real one is always at rest before a thumb reaches it — and the
|
||||||
|
node refuses to drive an arm from a pad it has never seen still.
|
||||||
|
"""
|
||||||
|
if not state.joy.calibration.known:
|
||||||
|
send_joy(node, state, stick())
|
||||||
for _ in range(count):
|
for _ in range(count):
|
||||||
send_joy(node, state, sample)
|
send_joy(node, state, sample)
|
||||||
loop._tick(node, state)
|
loop._tick(node, state)
|
||||||
@@ -271,7 +278,7 @@ def test_a_latched_estop_keeps_the_arm_still_under_a_pushed_stick(clock):
|
|||||||
drive(node, state, stick(axes=PUSHED))
|
drive(node, state, stick(axes=PUSHED))
|
||||||
node.clear()
|
node.clear()
|
||||||
|
|
||||||
drive(node, state, stick(axes=PUSHED, buttons=[False] * 6 + [True]))
|
drive(node, state, stick(axes=PUSHED, buttons=buttons_for(estop=True)))
|
||||||
node.clear()
|
node.clear()
|
||||||
jogs = drive(node, state, stick(axes=PUSHED), count=4)
|
jogs = drive(node, state, stick(axes=PUSHED), count=4)
|
||||||
|
|
||||||
@@ -282,11 +289,11 @@ def test_the_pad_can_release_the_latch_it_set(clock):
|
|||||||
# A pad-only dataflow has no console RESET, so a latch nothing on the pad
|
# 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.
|
# can release would strand the arm until the process was restarted.
|
||||||
node, state = build()
|
node, state = build()
|
||||||
drive(node, state, stick(buttons=[False] * 6 + [True]))
|
drive(node, state, stick(buttons=buttons_for(estop=True)))
|
||||||
assert state.view.estop is True
|
assert state.view.estop is True
|
||||||
node.clear()
|
node.clear()
|
||||||
|
|
||||||
drive(node, state, stick(buttons=[False, False, True]))
|
drive(node, state, stick(buttons=buttons_for(clear=True)))
|
||||||
|
|
||||||
assert state.view.estop is False
|
assert state.view.estop is False
|
||||||
assert "clear" in node.services()
|
assert "clear" in node.services()
|
||||||
@@ -296,20 +303,20 @@ def test_the_latch_is_released_without_holding_the_arm(clock):
|
|||||||
# An idle pad hands the lease back two seconds after the stop it just made,
|
# 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.
|
# so gating the release on the lease would be a trap.
|
||||||
node, state = build()
|
node, state = build()
|
||||||
drive(node, state, stick(buttons=[False] * 6 + [True]))
|
drive(node, state, stick(buttons=buttons_for(estop=True)))
|
||||||
state.arbiter.release(GAMEPAD)
|
state.arbiter.release(GAMEPAD)
|
||||||
state.arbiter.take(WEB, clock.t)
|
state.arbiter.take(WEB, clock.t)
|
||||||
node.clear()
|
node.clear()
|
||||||
|
|
||||||
drive(node, state, stick(buttons=[False, False, True]))
|
drive(node, state, stick(buttons=buttons_for(clear=True)))
|
||||||
|
|
||||||
assert state.view.estop is False
|
assert state.view.estop is False
|
||||||
|
|
||||||
|
|
||||||
def test_the_arm_drives_again_once_the_latch_is_released(clock):
|
def test_the_arm_drives_again_once_the_latch_is_released(clock):
|
||||||
node, state = build()
|
node, state = build()
|
||||||
drive(node, state, stick(buttons=[False] * 6 + [True]))
|
drive(node, state, stick(buttons=buttons_for(estop=True)))
|
||||||
drive(node, state, stick(buttons=[False, False, True]))
|
drive(node, state, stick(buttons=buttons_for(clear=True)))
|
||||||
node.clear()
|
node.clear()
|
||||||
|
|
||||||
jogs = drive(node, state, stick(axes=PUSHED))
|
jogs = drive(node, state, stick(axes=PUSHED))
|
||||||
@@ -322,7 +329,7 @@ def test_a_latched_estop_stops_the_browser_too(clock):
|
|||||||
# through a stop somebody made on the gamepad.
|
# through a stop somebody made on the gamepad.
|
||||||
node, state = build()
|
node, state = build()
|
||||||
state.clients = 1
|
state.clients = 1
|
||||||
drive(node, state, stick(buttons=[False] * 6 + [True]))
|
drive(node, state, stick(buttons=buttons_for(estop=True)))
|
||||||
state.arbiter.release(GAMEPAD)
|
state.arbiter.release(GAMEPAD)
|
||||||
state.arbiter.take(WEB, clock.t)
|
state.arbiter.take(WEB, clock.t)
|
||||||
node.clear()
|
node.clear()
|
||||||
@@ -337,7 +344,7 @@ def test_the_estop_button_works_without_the_pad_holding_the_arm(clock):
|
|||||||
node, state = build()
|
node, state = build()
|
||||||
state.arbiter.take(WEB, clock.t)
|
state.arbiter.take(WEB, clock.t)
|
||||||
|
|
||||||
drive(node, state, stick(buttons=[False] * 6 + [True]))
|
drive(node, state, stick(buttons=buttons_for(estop=True)))
|
||||||
|
|
||||||
assert "stop" in node.services()
|
assert "stop" in node.services()
|
||||||
|
|
||||||
@@ -402,7 +409,7 @@ def test_a_button_asks_in_verbs_as_well_as_in_services(clock):
|
|||||||
# what dora-ulite6 reads until it does.
|
# what dora-ulite6 reads until it does.
|
||||||
node, state = build()
|
node, state = build()
|
||||||
|
|
||||||
drive(node, state, stick(buttons=[True]))
|
drive(node, state, stick(buttons=buttons_for(grasp=True)))
|
||||||
|
|
||||||
assert node.actions() == ["gripper_close"]
|
assert node.actions() == ["gripper_close"]
|
||||||
assert "grasp" in node.services()
|
assert "grasp" in node.services()
|
||||||
@@ -410,10 +417,10 @@ def test_a_button_asks_in_verbs_as_well_as_in_services(clock):
|
|||||||
|
|
||||||
def test_every_verb_the_pad_can_ask_for_is_robot_agnostic(clock):
|
def test_every_verb_the_pad_can_ask_for_is_robot_agnostic(clock):
|
||||||
node, state = build()
|
node, state = build()
|
||||||
drive(node, state, stick(buttons=[False, True]))
|
drive(node, state, stick(buttons=buttons_for(release=True)))
|
||||||
drive(node, state, stick(buttons=[False] * 3 + [True]))
|
drive(node, state, stick(buttons=buttons_for(freedrive=True)))
|
||||||
drive(node, state, stick(buttons=[False] * 6 + [True]))
|
drive(node, state, stick(buttons=buttons_for(estop=True)))
|
||||||
drive(node, state, stick(buttons=[False, False, True]))
|
drive(node, state, stick(buttons=buttons_for(clear=True)))
|
||||||
|
|
||||||
assert node.actions() == ["gripper_open", "freedrive", "stop", "clear"]
|
assert node.actions() == ["gripper_open", "freedrive", "stop", "clear"]
|
||||||
|
|
||||||
@@ -426,6 +433,6 @@ def test_the_first_press_after_the_pad_has_been_idle_is_not_dropped(clock):
|
|||||||
node, state = build()
|
node, state = build()
|
||||||
assert state.arbiter.owner is None
|
assert state.arbiter.owner is None
|
||||||
|
|
||||||
drive(node, state, stick(buttons=[True]), count=3)
|
drive(node, state, stick(buttons=buttons_for(grasp=True)), count=3)
|
||||||
|
|
||||||
assert node.actions() == ["gripper_close"]
|
assert node.actions() == ["gripper_close"]
|
||||||
|
|||||||
+93
-4
@@ -1,9 +1,12 @@
|
|||||||
"""The pad as it arrives on the wire, and what silence on that wire means."""
|
"""The pad as it arrives on the wire, and what silence on that wire means."""
|
||||||
|
|
||||||
import pyarrow as pa
|
import pyarrow as pa
|
||||||
|
import pytest
|
||||||
|
|
||||||
|
from dora_teleop import mapping
|
||||||
from dora_teleop.joy import ABSENT, JoyInput, Sample, decode
|
from dora_teleop.joy import ABSENT, JoyInput, Sample, decode
|
||||||
from tests.conftest import input_event, joy_message, stick
|
from dora_teleop.mapping import LAYOUT
|
||||||
|
from tests.conftest import axes_for, input_event, joy_message, stick
|
||||||
|
|
||||||
|
|
||||||
def joy_event(sample):
|
def joy_event(sample):
|
||||||
@@ -41,21 +44,107 @@ def test_a_message_with_no_rows_reads_as_absent():
|
|||||||
|
|
||||||
def test_a_fresh_reading_is_handed_back():
|
def test_a_fresh_reading_is_handed_back():
|
||||||
joy = JoyInput(ttl_s=0.2)
|
joy = JoyInput(ttl_s=0.2)
|
||||||
sample = stick(axes=[0.5])
|
joy.accept(stick(), 100.0)
|
||||||
joy.accept(sample, 100.0)
|
joy.accept(stick(axes=[0.5]), 100.0)
|
||||||
|
|
||||||
assert joy.read(100.1) is sample
|
assert joy.read(100.1).axes[0] == 0.5
|
||||||
|
|
||||||
|
|
||||||
def test_a_reading_nobody_is_refreshing_reads_as_absent():
|
def test_a_reading_nobody_is_refreshing_reads_as_absent():
|
||||||
# dora-joy publishes every tick, so silence means it died — and a dead pad
|
# 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.
|
# node is indistinguishable from an unplugged pad as far as the arm cares.
|
||||||
joy = JoyInput(ttl_s=0.2)
|
joy = JoyInput(ttl_s=0.2)
|
||||||
|
joy.accept(stick(), 100.0)
|
||||||
joy.accept(stick(axes=[0.5]), 100.0)
|
joy.accept(stick(axes=[0.5]), 100.0)
|
||||||
|
|
||||||
assert joy.read(100.5) is ABSENT
|
assert joy.read(100.5) is ABSENT
|
||||||
|
|
||||||
|
|
||||||
|
# --- the pad this node is actually holding ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_is_measured_where_it_rests_and_the_tilt_is_subtracted():
|
||||||
|
# A stick that no longer centres is the ordinary kind of worn pad. Its rest
|
||||||
|
# becomes zero, so it asks for nothing until it is actually pushed.
|
||||||
|
joy = JoyInput(ttl_s=0.2)
|
||||||
|
tilted = axes_for(left_x=0.2, left_y=-0.15)
|
||||||
|
joy.accept(stick(axes=tilted), 100.0)
|
||||||
|
|
||||||
|
reading = joy.read(100.1)
|
||||||
|
|
||||||
|
assert reading.present is True
|
||||||
|
assert reading.axes[LAYOUT.left_x] == pytest.approx(0.0)
|
||||||
|
assert reading.axes[LAYOUT.left_y] == pytest.approx(0.0)
|
||||||
|
assert mapping.is_active(reading) is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_tilt_is_subtracted_rather_than_deadzoned_so_full_travel_remains():
|
||||||
|
joy = JoyInput(ttl_s=0.2)
|
||||||
|
joy.accept(stick(axes=axes_for(left_x=0.2)), 100.0)
|
||||||
|
joy.accept(stick(axes=axes_for(left_x=1.0)), 100.0)
|
||||||
|
|
||||||
|
assert joy.read(100.1).axes[LAYOUT.left_x] == pytest.approx(0.8)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_trigger_that_rests_at_zero_does_not_read_as_half_pressed():
|
||||||
|
# Some pads report a trigger as 0 until it first moves. Read as SDL's -1..1
|
||||||
|
# that is half travel, which would lift the arm on its own.
|
||||||
|
joy = JoyInput(ttl_s=0.2)
|
||||||
|
joy.accept(stick(axes=axes_for(right_trigger=0.0)), 100.0)
|
||||||
|
|
||||||
|
reading = joy.read(100.1)
|
||||||
|
|
||||||
|
assert mapping.trigger(reading.axes[LAYOUT.right_trigger]) == pytest.approx(0.0)
|
||||||
|
assert mapping.is_active(reading) is False
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_that_does_not_fit_the_layout_reads_as_absent():
|
||||||
|
# The bug this exists for: a trigger on an index the layout calls a stick
|
||||||
|
# rests at -1.0, which is full deflection — an untouched pad on the desk
|
||||||
|
# asking for full speed, and holding the lease so nothing can take it back.
|
||||||
|
joy = JoyInput(ttl_s=0.2)
|
||||||
|
wrong = list(axes_for())
|
||||||
|
wrong[LAYOUT.right_y] = -1.0
|
||||||
|
joy.accept(stick(axes=wrong), 100.0)
|
||||||
|
|
||||||
|
assert joy.read(100.1) is ABSENT
|
||||||
|
assert "right stick Y" in joy.calibration.mismatch
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_held_when_it_appears_is_refused_until_it_is_let_go():
|
||||||
|
# A stick held at startup and a wrongly numbered axis look identical in one
|
||||||
|
# reading, so the safe answer is the same for both: no arm until it rests.
|
||||||
|
joy = JoyInput(ttl_s=0.2)
|
||||||
|
joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.0)
|
||||||
|
|
||||||
|
assert joy.read(100.1) is ABSENT
|
||||||
|
|
||||||
|
joy.accept(stick(), 100.2)
|
||||||
|
|
||||||
|
assert joy.read(100.3).present is True
|
||||||
|
|
||||||
|
|
||||||
|
def test_rest_is_not_re_measured_while_the_pad_is_in_use():
|
||||||
|
# Re-measuring under a held stick would record that deflection as centre,
|
||||||
|
# and the arm would then move the moment the operator let go.
|
||||||
|
joy = JoyInput(ttl_s=0.2)
|
||||||
|
joy.accept(stick(), 100.0)
|
||||||
|
joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.1)
|
||||||
|
joy.accept(stick(axes=axes_for(left_y=-1.0)), 100.2)
|
||||||
|
|
||||||
|
assert joy.read(100.3).axes[LAYOUT.left_y] == pytest.approx(-1.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_that_goes_away_is_measured_again_when_it_comes_back():
|
||||||
|
# The second pad is not the first one, and may not rest where it did.
|
||||||
|
joy = JoyInput(ttl_s=0.2)
|
||||||
|
joy.accept(stick(axes=axes_for(left_x=0.2)), 100.0)
|
||||||
|
joy.accept(Sample(present=False), 100.1)
|
||||||
|
joy.accept(stick(axes=axes_for(left_x=-0.2)), 100.2)
|
||||||
|
|
||||||
|
assert joy.read(100.3).axes[LAYOUT.left_x] == pytest.approx(0.0)
|
||||||
|
|
||||||
|
|
||||||
def test_nothing_ever_arriving_reads_as_absent():
|
def test_nothing_ever_arriving_reads_as_absent():
|
||||||
# A dataflow with no `joy` wired at all, which is a browser-only console.
|
# 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
|
assert JoyInput(ttl_s=0.2).read(100.0) is ABSENT
|
||||||
|
|||||||
+88
-12
@@ -3,8 +3,8 @@
|
|||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
from dora_teleop import mapping
|
from dora_teleop import mapping
|
||||||
from dora_teleop.mapping import VX, VY, VZ, WX, WY, WZ
|
from dora_teleop.mapping import LAYOUT, VX, VY, VZ, WX, WY, WZ
|
||||||
from tests.conftest import stick
|
from tests.conftest import axes_for, buttons_for, stick
|
||||||
|
|
||||||
|
|
||||||
def test_a_stick_at_rest_asks_for_nothing(config):
|
def test_a_stick_at_rest_asks_for_nothing(config):
|
||||||
@@ -29,15 +29,15 @@ def test_pushing_the_stick_right_moves_the_arm_along_positive_y(config):
|
|||||||
|
|
||||||
|
|
||||||
def test_the_right_trigger_lifts_and_the_left_one_lowers(config):
|
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)
|
up = mapping.velocity(axes_for(right_trigger=1.0), [], config)
|
||||||
down = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, -1.0]).axes, [], config)
|
down = mapping.velocity(axes_for(left_trigger=1.0), [], config)
|
||||||
|
|
||||||
assert up[VZ] == pytest.approx(config.speed_mm_s)
|
assert up[VZ] == pytest.approx(config.speed_mm_s)
|
||||||
assert down[VZ] == pytest.approx(-config.speed_mm_s)
|
assert down[VZ] == pytest.approx(-config.speed_mm_s)
|
||||||
|
|
||||||
|
|
||||||
def test_both_triggers_together_cancel(config):
|
def test_both_triggers_together_cancel(config):
|
||||||
velocity = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, 1.0]).axes, [], config)
|
velocity = mapping.velocity(axes_for(left_trigger=1.0, right_trigger=1.0), [], config)
|
||||||
|
|
||||||
assert velocity[VZ] == 0.0
|
assert velocity[VZ] == 0.0
|
||||||
|
|
||||||
@@ -51,15 +51,15 @@ def test_an_untouched_trigger_does_not_read_as_half_pressed(config):
|
|||||||
|
|
||||||
|
|
||||||
def test_the_right_stick_turns_the_tool_in_degrees_per_second(config):
|
def test_the_right_stick_turns_the_tool_in_degrees_per_second(config):
|
||||||
velocity = mapping.velocity(stick(axes=[0, 0, -1, 1.0, -1.0, -1]).axes, [], config)
|
velocity = mapping.velocity(axes_for(right_x=1.0, right_y=-1.0), [], config)
|
||||||
|
|
||||||
assert velocity[WX] == pytest.approx(config.speed_deg_s)
|
assert velocity[WX] == pytest.approx(config.speed_deg_s)
|
||||||
assert velocity[WY] == pytest.approx(config.speed_deg_s)
|
assert velocity[WY] == pytest.approx(config.speed_deg_s)
|
||||||
|
|
||||||
|
|
||||||
def test_the_bumpers_yaw_in_opposite_directions(config):
|
def test_the_bumpers_yaw_in_opposite_directions(config):
|
||||||
right = mapping.velocity(stick().axes, [False] * 5 + [True], config)
|
right = mapping.velocity(stick().axes, buttons_for(yaw_right=True), config)
|
||||||
left = mapping.velocity(stick().axes, [False] * 4 + [True, False], config)
|
left = mapping.velocity(stick().axes, buttons_for(yaw_left=True), config)
|
||||||
|
|
||||||
assert right[WZ] == pytest.approx(config.speed_deg_s)
|
assert right[WZ] == pytest.approx(config.speed_deg_s)
|
||||||
assert left[WZ] == pytest.approx(-config.speed_deg_s)
|
assert left[WZ] == pytest.approx(-config.speed_deg_s)
|
||||||
@@ -68,7 +68,7 @@ def test_the_bumpers_yaw_in_opposite_directions(config):
|
|||||||
def test_several_axes_move_at_once(config):
|
def test_several_axes_move_at_once(config):
|
||||||
# The whole reason a pad beats the console's four-sector dial: the dial can
|
# The whole reason a pad beats the console's four-sector dial: the dial can
|
||||||
# only ever ask for one axis.
|
# only ever ask for one axis.
|
||||||
velocity = mapping.velocity(stick(axes=[1.0, -1.0, -1, 0, 0, 1.0]).axes, [], config)
|
velocity = mapping.velocity(axes_for(left_x=1.0, left_y=-1.0, right_trigger=1.0), [], config)
|
||||||
|
|
||||||
assert velocity[VX] > 0
|
assert velocity[VX] > 0
|
||||||
assert velocity[VY] > 0
|
assert velocity[VY] > 0
|
||||||
@@ -129,14 +129,90 @@ def test_a_nudged_stick_is_asking_for_the_arm():
|
|||||||
|
|
||||||
|
|
||||||
def test_a_held_trigger_is_asking_for_the_arm():
|
def test_a_held_trigger_is_asking_for_the_arm():
|
||||||
assert mapping.is_active(stick(axes=[0.0, 0.0, 1.0])) is True
|
assert mapping.is_active(stick(axes=axes_for(left_trigger=1.0))) is True
|
||||||
|
|
||||||
|
|
||||||
def test_a_held_button_is_asking_for_the_arm():
|
def test_a_held_button_is_asking_for_the_arm():
|
||||||
assert mapping.is_active(stick(buttons=[True])) is True
|
assert mapping.is_active(stick(buttons=buttons_for(grasp=True))) is True
|
||||||
|
|
||||||
|
|
||||||
def test_an_axis_the_layout_does_not_use_still_counts():
|
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
|
# 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.
|
# somebody presses it, whether or not this node maps it to anything.
|
||||||
assert mapping.is_active(stick(axes=[0.0, 0.0, -1.0, 0.0, 0.0, -1.0, 1.0])) is True
|
assert mapping.is_active(stick(axes=axes_for() + [1.0])) is True
|
||||||
|
|
||||||
|
|
||||||
|
# --- reading a pad that is not the layout's ideal one ---
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_resting_pad_measures_as_resting():
|
||||||
|
zeros = mapping.rest_zeros(axes_for())
|
||||||
|
|
||||||
|
assert zeros == axes_for()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_small_tilt_is_accepted_as_rest():
|
||||||
|
# A worn stick is an ordinary pad, not a broken one.
|
||||||
|
assert mapping.rest_zeros(axes_for(left_x=0.2, right_y=-0.2))
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_axis_resting_where_the_layout_says_it_cannot_is_refused():
|
||||||
|
wrong = axes_for()
|
||||||
|
wrong[LAYOUT.right_y] = -1.0
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match="right stick Y"):
|
||||||
|
mapping.rest_zeros(wrong)
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_refusal_names_the_index_and_what_it_read():
|
||||||
|
# It is read off a terminal by somebody deciding what to put in `Layout`.
|
||||||
|
wrong = axes_for()
|
||||||
|
wrong[LAYOUT.right_x] = 0.9
|
||||||
|
|
||||||
|
with pytest.raises(ValueError, match=r"axis 2 rests at \+0.900"):
|
||||||
|
mapping.rest_zeros(wrong)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_trigger_resting_at_either_end_is_accepted():
|
||||||
|
# -1 is what SDL reports; 0 is what some pads report until it first moves.
|
||||||
|
assert mapping.rest_zeros(axes_for(left_trigger=-1.0, right_trigger=0.0))
|
||||||
|
|
||||||
|
|
||||||
|
def test_an_axis_the_layout_does_not_use_still_has_to_rest():
|
||||||
|
# A d-pad reported as two extra axes counts towards `is_active`, so one
|
||||||
|
# stuck at an extreme would hold the arm as surely as a named one.
|
||||||
|
with pytest.raises(ValueError, match="axis 6"):
|
||||||
|
mapping.rest_zeros(axes_for() + [1.0])
|
||||||
|
|
||||||
|
|
||||||
|
def test_canonical_puts_a_tilted_stick_back_at_centre():
|
||||||
|
tilted = axes_for(left_x=0.2)
|
||||||
|
|
||||||
|
canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted))
|
||||||
|
|
||||||
|
assert canonical[LAYOUT.left_x] == pytest.approx(0.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_canonical_puts_a_trigger_back_at_its_own_rest():
|
||||||
|
zeroed = axes_for(left_trigger=0.0, right_trigger=0.0)
|
||||||
|
|
||||||
|
canonical = mapping.canonical(zeroed, mapping.rest_zeros(zeroed))
|
||||||
|
|
||||||
|
assert canonical[LAYOUT.left_trigger] == pytest.approx(mapping.TRIGGER_REST)
|
||||||
|
assert canonical[LAYOUT.right_trigger] == pytest.approx(mapping.TRIGGER_REST)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_trigger_measured_from_zero_still_reaches_full_travel():
|
||||||
|
zeros = mapping.rest_zeros(axes_for(right_trigger=0.0))
|
||||||
|
|
||||||
|
canonical = mapping.canonical(axes_for(right_trigger=1.0), zeros)
|
||||||
|
|
||||||
|
assert mapping.trigger(canonical[LAYOUT.right_trigger]) == pytest.approx(1.0)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_calibrated_pad_at_rest_asks_for_nothing(config):
|
||||||
|
tilted = axes_for(left_x=0.2, left_y=-0.15, right_trigger=0.0)
|
||||||
|
|
||||||
|
canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted))
|
||||||
|
|
||||||
|
assert mapping.velocity(canonical, [], config) == [0.0] * 6
|
||||||
|
|||||||
+39
-12
@@ -2,7 +2,7 @@
|
|||||||
|
|
||||||
from dora_teleop import mapping, probe
|
from dora_teleop import mapping, probe
|
||||||
from dora_teleop.joy import Sample
|
from dora_teleop.joy import Sample
|
||||||
from tests.conftest import stick
|
from tests.conftest import buttons_for, stick
|
||||||
|
|
||||||
|
|
||||||
def test_a_centred_stick_puts_the_marker_in_the_middle():
|
def test_a_centred_stick_puts_the_marker_in_the_middle():
|
||||||
@@ -28,7 +28,7 @@ def test_a_reading_beyond_full_deflection_stays_on_the_bar():
|
|||||||
def test_no_gamepad_says_so_and_says_the_arm_is_being_held_still(config):
|
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
|
# 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.
|
# must not imply the arm is waiting on something it is not.
|
||||||
drawn = probe.render(Sample(present=False), [0.0] * 6, config)
|
drawn = probe.render(Sample(present=False), Sample(present=False), [0.0] * 6, config)
|
||||||
|
|
||||||
assert "no gamepad" in drawn
|
assert "no gamepad" in drawn
|
||||||
assert "hold still" in drawn
|
assert "hold still" in drawn
|
||||||
@@ -39,30 +39,31 @@ def test_every_axis_the_pad_reports_is_shown_even_the_unused_ones(config):
|
|||||||
# for, so nothing may be hidden.
|
# for, so nothing may be hidden.
|
||||||
sample = Sample(axes=[0.0] * 8, buttons=[], present=True)
|
sample = Sample(axes=[0.0] * 8, buttons=[], present=True)
|
||||||
|
|
||||||
drawn = probe.render(sample, [0.0] * 6, config)
|
drawn = probe.render(sample, sample, [0.0] * 6, config)
|
||||||
|
|
||||||
assert "unused" in drawn
|
assert "unused" in drawn
|
||||||
assert "axes (8 reported)" in drawn
|
assert "axes (8 reported)" in drawn
|
||||||
|
|
||||||
|
|
||||||
def test_a_pressed_button_is_named(config):
|
def test_a_pressed_button_is_named(config):
|
||||||
sample = stick(buttons=[True])
|
sample = stick(buttons=buttons_for(grasp=True))
|
||||||
|
|
||||||
drawn = probe.render(sample, [0.0] * 6, config)
|
drawn = probe.render(sample, sample, [0.0] * 6, config)
|
||||||
|
|
||||||
assert "grasp" in drawn
|
assert "grasp" in drawn
|
||||||
|
|
||||||
|
|
||||||
def test_nothing_pressed_says_so_rather_than_showing_an_empty_line(config):
|
def test_nothing_pressed_says_so_rather_than_showing_an_empty_line(config):
|
||||||
drawn = probe.render(stick(), [0.0] * 6, config)
|
drawn = probe.render(stick(), stick(), [0.0] * 6, config)
|
||||||
|
|
||||||
assert "none pressed" in drawn
|
assert "none pressed" in drawn
|
||||||
|
|
||||||
|
|
||||||
def test_the_velocity_is_shown_in_the_units_the_arm_takes(config):
|
def test_the_velocity_is_shown_in_the_units_the_arm_takes(config):
|
||||||
velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config)
|
pushed = stick(axes=[0.0, -1.0])
|
||||||
|
velocity = mapping.velocity(pushed.axes, [], config)
|
||||||
|
|
||||||
drawn = probe.render(stick(axes=[0.0, -1.0]), velocity, config)
|
drawn = probe.render(pushed, pushed, velocity, config)
|
||||||
|
|
||||||
assert "mm/s" in drawn
|
assert "mm/s" in drawn
|
||||||
assert "deg/s" in drawn
|
assert "deg/s" in drawn
|
||||||
@@ -70,13 +71,14 @@ def test_the_velocity_is_shown_in_the_units_the_arm_takes(config):
|
|||||||
|
|
||||||
|
|
||||||
def test_a_still_arm_is_reported_as_still(config):
|
def test_a_still_arm_is_reported_as_still(config):
|
||||||
drawn = probe.render(stick(), [0.0] * 6, config)
|
drawn = probe.render(stick(), stick(), [0.0] * 6, config)
|
||||||
|
|
||||||
assert "told to hold still" in drawn
|
assert "told to hold still" in drawn
|
||||||
|
|
||||||
|
|
||||||
def test_a_moving_arm_is_reported_as_moving(config):
|
def test_a_moving_arm_is_reported_as_moving(config):
|
||||||
drawn = probe.render(stick(axes=[0.0, -1.0]), [10.0, 0, 0, 0, 0, 0], config)
|
pushed = stick(axes=[0.0, -1.0])
|
||||||
|
drawn = probe.render(pushed, pushed, [10.0, 0, 0, 0, 0, 0], config)
|
||||||
|
|
||||||
assert "moving: yes" in drawn
|
assert "moving: yes" in drawn
|
||||||
|
|
||||||
@@ -84,5 +86,30 @@ def test_a_moving_arm_is_reported_as_moving(config):
|
|||||||
def test_whether_the_pad_is_asking_for_the_arm_is_shown(config):
|
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
|
# 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.
|
# 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)
|
rest, pushed = stick(), stick(axes=[0.0, -1.0])
|
||||||
assert "asking for the arm: yes" in probe.render(stick(axes=[0.0, -1.0]), [0.0] * 6, config)
|
|
||||||
|
assert "asking for the arm: no" in probe.render(rest, rest, [0.0] * 6, config)
|
||||||
|
assert "asking for the arm: yes" in probe.render(pushed, pushed, [0.0] * 6, config)
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_pad_that_does_not_fit_the_layout_is_called_out(config):
|
||||||
|
# This is the tool for exactly that pad, so the reason goes on the screen
|
||||||
|
# rather than only in the node's log.
|
||||||
|
wrong = stick(axes=[0.0, 0.0, 0.0, -1.0, -1.0, -1.0])
|
||||||
|
mismatch = "axis 3 rests at -1.000, which is not where right stick Y sits"
|
||||||
|
|
||||||
|
drawn = probe.render(wrong, Sample(present=False), [0.0] * 6, config, mismatch)
|
||||||
|
|
||||||
|
assert "does not match the layout" in drawn
|
||||||
|
assert mismatch in drawn
|
||||||
|
assert "asking for the arm: no" in drawn
|
||||||
|
|
||||||
|
|
||||||
|
def test_the_raw_axes_are_still_shown_for_a_pad_that_does_not_fit(config):
|
||||||
|
# The numbers are the evidence: they are what `Layout` gets corrected from.
|
||||||
|
wrong = stick(axes=[0.0, 0.0, 0.0, -1.0, -1.0, -1.0])
|
||||||
|
|
||||||
|
drawn = probe.render(wrong, Sample(present=False), [0.0] * 6, config, "axis 3")
|
||||||
|
|
||||||
|
assert "-1.000" in drawn
|
||||||
|
assert "axes (6 reported)" in drawn
|
||||||
|
|||||||
Reference in New Issue
Block a user