liria/dora-teleop
4
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

read the gamepad from dora-joy and publish twist and command

Browse Source 
The pad moves to dora-joy, so this node keeps no hardware dependency and the
pad becomes reusable. It arrives on `joy` as a plain reading; `JoyInput.read`
treats silence past JOY_TTL_S as absence, because that node dying looks like
silence rather than like an unplugged pad and the arm cannot tell them apart.

Both control sources are now normalised to a [-1, 1] six-vector before anything
is scaled, and the scaling happens in one function. The web path used to pass a
velocity straight through without going near mapping, which is why the console
had to speak the arm's units. It no longer does.

`twist` (SI) and `command` (robot-agnostic verbs) are the standard ports. `jog`
and `request` still carry the same intent in dora-ulite6's units and service
names so the two repositories can move independently; LEGACY_REQUEST is the
only place that knows what that node calls a verb. Wire one or the other, never
both. `joint_jog` is specified in the manifest and deliberately not built.

Schemas are declared rather than inferred: pyarrow reads a null `value` as its
own type, so an inferred `command` would change its port's type between one
verb and the next.

Three bugs found while moving this code, each with a test:

- `Sample.active` compared raw axes against a threshold, but SDL rests a
  trigger at -1.0, so every connected pad read as in use for ever: the pad took
  the lease on plug-in and GAMEPAD_IDLE_RELEASE_S never fired.
- The e-stop latch was never consulted when deciding a velocity, so Back
  stopped the arm for one tick and the next stick nudge drove it again. Nothing
  on the pad could clear it either. It is now checked above the lease, and X
  releases it and clears the controller's error, as the console's RESET does.
- Buttons were read before the arbiter granted the lease, so the first press
  after the pad had been idle was swallowed: not owned yet when read, and no
  longer a rising edge by the next tick.
This commit is contained in:
cristhian
2026-09-16 12:18:51 -05:00
parent 885b7a569f
commit d336f8d499
21 changed files with 1165 additions and 433 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dora_teleop/config.py
+22 -19
View File
@@ -20,31 +20,27 @@ def env_float(name: str, default: float) -> float:
raise ValueError(f"{name} must be a number, not {value!r}") from None
def env_int(name: str, default: int) -> int:
"""Read a whole number, naming the variable if it cannot be read as one."""
value = os.environ.get(name, "").strip()
if not value:
return default
try:
return int(value)
except ValueError:
raise ValueError(f"{name} must be a whole number, not {value!r}") from None
@dataclass
class DeadmanConfig:
"""How long a source may go quiet before the arm is told to stop.
Three layers, outermost last. This node re-sends the current velocity every
tick; `jog_ttl_s` is how long it will keep doing so without hearing from the
source; and behind both sits the arm node's own `VELOCITY_COMMAND_TTL_S`,
which the controller enforces whether or not this process is alive.
Four layers, outermost last. `joy_ttl_s` is how long a pad reading stays
worth acting on; this node re-sends the current velocity every tick;
`jog_ttl_s` is how long it will keep doing so without hearing from the
source; and behind all of them sits the arm node's own
`VELOCITY_COMMAND_TTL_S`, which the controller enforces whether or not this
process is alive.
Keep them in that order — ours shorter than the controller's — or the
Keep them in that order — each shorter than the one behind it — or a
backstop fires before the thing it is backing up.
"""
jog_ttl_s: float = 0.4
# How long the `joy` stream may go quiet before the pad reads as gone. The
# pad lives in another node now, and that node dying looks like silence
# rather than like an unplugged pad, so silence has to mean the same thing.
# `dora-joy` publishes every tick, so it means nothing else.
joy_ttl_s: float = 0.2
# How long a gamepad may sit untouched before it gives the arm back, so the
# browser can take over without anyone having to press anything.
gamepad_idle_release_s: float = 2.0
@@ -57,6 +53,7 @@ class DeadmanConfig:
"""Build the timings from the environment."""
return cls(
jog_ttl_s=env_float("JOG_TTL_S", 0.4),
joy_ttl_s=env_float("JOY_TTL_S", 0.2),
gamepad_idle_release_s=env_float("GAMEPAD_IDLE_RELEASE_S", 2.0),
state_poll_s=env_float("STATE_POLL_S", 0.5),
)
@@ -77,7 +74,6 @@ class Config:
# How much of the curve is cubic: 0 is linear, 1 is fully cubic. Fine
# positioning happens near centre, and expo is what buys resolution there.
expo: float
joystick_index: int
deadman: DeadmanConfig = field(default_factory=DeadmanConfig)
@classmethod
@@ -89,11 +85,18 @@ class Config:
expo = env_float("EXPO", 0.4)
if not 0.0 <= expo <= 1.0:
raise ValueError(f"EXPO must be between 0 and 1, not {expo}")
deadman = DeadmanConfig.from_env()
# The layers have to fire outermost last, and each one is a backstop for
# the one inside it. Behind both of these sits the arm node's own
# VELOCITY_COMMAND_TTL_S, which no setting here can see.
if deadman.joy_ttl_s >= deadman.jog_ttl_s:
raise ValueError(
f"JOY_TTL_S must be below JOG_TTL_S ({deadman.jog_ttl_s}), not {deadman.joy_ttl_s}"
)
return cls(
speed_mm_s=env_float("SPEED_MM_S", 60.0),
speed_deg_s=env_float("SPEED_DEG_S", 30.0),
deadzone=deadzone,
expo=expo,
joystick_index=env_int("JOYSTICK_INDEX", 0),
deadman=DeadmanConfig.from_env(),
deadman=deadman,
)
dora_teleop/gamepad.py
-135
View File
@@ -1,135 +0,0 @@
"""The gamepad, and the thread that reads it.
**Nothing outside this file may touch pygame.** The rest of the node sees only
:class:`Sample`, which is a plain immutable reading — that is what lets every
other module be tested without a pad plugged in, and what keeps SDL's quirks
from leaking into the arm logic.
The reader is a thread because SDL wants to be pumped at its own cadence and
because a pad being unplugged should be noticed within milliseconds rather than
on whenever the dataflow next ticks. It never publishes anything; it fills a
slot, and the event loop drains it.
"""
import os
import threading
import time
from dataclasses import dataclass, field
from dora_teleop.log import log
@dataclass(frozen=True)
class Sample:
"""One reading of the pad.
``present`` is the one that matters for safety: a pad that has been
unplugged reads as absent rather than as centred, so the difference between
"the operator let go" and "the operator's pad fell off the desk" is not lost.
"""
axes: list[float] = field(default_factory=list)
buttons: list[bool] = field(default_factory=list)
present: bool = False
@property
def active(self) -> bool:
"""Whether anything is being touched at all.
Deliberately raw — no deadzone. This decides whether the pad is asking
for the arm, and :mod:`dora_teleop.mapping` decides how far to move it.
A separate, tighter threshold here would mean a stick could claim the
arm and then not move it.
"""
return self.present and (any(abs(a) > 0.3 for a in self.axes) or any(self.buttons))
ABSENT = Sample()
class Gamepad:
"""Reads one pad on a thread and keeps the latest reading available."""
def __init__(self, index: int, poll_interval_s: float = 0.01):
self.index = index
self.poll_interval_s = poll_interval_s
self._lock = threading.Lock()
self._sample = ABSENT
self._stopping = threading.Event()
self._thread = threading.Thread(target=self._run, name="teleop-gamepad", daemon=True)
def start(self) -> None:
"""Start reading. Never blocks, and never fails for want of a pad."""
self._thread.start()
def read(self) -> Sample:
"""Return the most recent reading."""
with self._lock:
return self._sample
def shutdown(self) -> None:
"""Stop reading and let the thread finish."""
self._stopping.set()
self._thread.join(timeout=1.0)
def _run(self) -> None:
"""Open the pad, read it, and keep trying if it is not there.
A missing pad is an ordinary state, not an error: the browser is a whole
control path of its own, and the node has no business refusing to start
because nobody has plugged a controller in yet.
"""
# Two subsystems, not all of them. `pygame.init()` also opens audio,
# which on a headless robot means a screenful of ALSA errors in the
# dataflow's stderr for a device this node never uses. The display is
# needed despite there being no window: SDL refreshes joystick state
# during the event pump, and the pump needs the video subsystem.
os.environ.setdefault("SDL_VIDEODRIVER", "dummy")
os.environ.setdefault("SDL_AUDIODRIVER", "dummy")
import pygame
pygame.display.init()
pygame.joystick.init()
joystick = None
while not self._stopping.is_set():
pygame.event.pump()
if joystick is None:
joystick = self._open(pygame)
if joystick is None:
self._store(ABSENT)
self._stopping.wait(1.0)
continue
try:
sample = Sample(
axes=[joystick.get_axis(i) for i in range(joystick.get_numaxes())],
buttons=[bool(joystick.get_button(i)) for i in range(joystick.get_numbuttons())],
present=True,
)
except Exception as error: # noqa: BLE001 — an unplugged pad raises from inside SDL
log(f"the gamepad went away ({error})")
joystick = None
self._store(ABSENT)
continue
self._store(sample)
self._stopping.wait(self.poll_interval_s)
def _open(self, pygame):
"""Open the configured pad, or return None if it is not there."""
pygame.joystick.quit()
pygame.joystick.init()
if pygame.joystick.get_count() <= self.index:
return None
joystick = pygame.joystick.Joystick(self.index)
joystick.init()
log(f"gamepad {self.index}: {joystick.get_name()}")
return joystick
def _store(self, sample: Sample) -> None:
with self._lock:
self._sample = sample
def now() -> float:
"""Return the clock the deadman runs on: monotonic, so it cannot step backwards."""
return time.monotonic()
dora_teleop/joy.py
+83
View File
@@ -0,0 +1,83 @@
"""The gamepad, as it arrives from `dora-joy`.
The pad itself lives in another node now. What reaches this one is a plain
reading on the `joy` input, and this module is the seam the loop reads it
through: :class:`Sample` is what the rest of the node sees, exactly as it did
when the pad was read in-process.
**The safety-critical part is :meth:`JoyInput.read`.** Moving the pad into
another process added a way for it to go away that did not exist before — that
process dying — and from here that looks like silence rather than like an
unplugged pad. So silence past `JOY_TTL_S` reads as *absent*, which is the same
answer, because a reading nobody is refreshing is not evidence of anything.
`dora-joy` publishes on every tick whether or not the pad moved, so silence
means one thing only.
"""
import time
from dataclasses import dataclass, field
@dataclass(frozen=True)
class Sample:
"""One reading of the pad.
``present`` is the one that matters for safety: a pad that has been
unplugged reads as absent rather than as centred, so the difference between
"the operator let go" and "the operator's pad fell off the desk" is not lost.
"""
axes: list[float] = field(default_factory=list)
buttons: list[bool] = field(default_factory=list)
present: bool = False
ABSENT = Sample()
def decode(event: dict) -> Sample:
"""Read one `joy` message into a reading.
A message this node cannot make sense of reads as absent rather than as
centred, for the same reason an unplugged pad does.
"""
value = event["value"]
if not len(value):
return ABSENT
row = value[0].as_py()
return Sample(
axes=list(row.get("axes") or []),
buttons=[bool(down) for down in row.get("buttons") or []],
present=bool(row.get("present")),
)
class JoyInput:
"""The latest reading, and how long ago it arrived."""
def __init__(self, ttl_s: float):
self.ttl_s = ttl_s
self._sample = ABSENT
self._seen_at = 0.0
def accept(self, sample: Sample, moment: float) -> None:
"""Take one reading off the wire."""
self._sample = sample
self._seen_at = moment
def read(self, moment: float) -> Sample:
"""Return the current reading, or absence if nobody is still sending one.
A stale reading is not a reading. `dora-joy` publishes every tick, so
silence means it died, and a dead pad node is indistinguishable from an
unplugged pad as far as the arm is concerned — which is the only safe
way to read it.
"""
if moment - self._seen_at > self.ttl_s:
return ABSENT
return self._sample
def now() -> float:
"""Return the clock the deadman runs on: monotonic, so it cannot step backwards."""
return time.monotonic()
dora_teleop/main.py
+141 -60
View File
@@ -2,9 +2,11 @@
The loop owns the one decision this node exists to make — whether a velocity
reaches the arm this tick, and from whom. Everything else is delegated:
:mod:`dora_teleop.gamepad` owns the hardware, :mod:`dora_teleop.mapping` turns a
reading into a velocity, :mod:`dora_teleop.arbiter` says who is allowed to, and
:mod:`dora_teleop.ui` speaks the console's language.
:mod:`dora_teleop.joy` takes the pad off the wire, :mod:`dora_teleop.mapping`
turns a reading into a velocity, :mod:`dora_teleop.arbiter` says who is allowed
to, and :mod:`dora_teleop.ui` speaks the console's language. **No hardware is
touched here at all** — the pad is `dora-joy`, the arm is `dora-ulite6` and the
browser is the WebSocket bridge.
**The safety-critical part is `_drive`.** An arm moving under a velocity command
keeps moving until told otherwise, so the question is never "should it start"
@@ -20,15 +22,37 @@ import time
import pyarrow as pa
from dora import Node
from dora_teleop import mapping, ui
from dora_teleop import joy, mapping, message, ui
from dora_teleop.arbiter import GAMEPAD, WEB, Arbiter
from dora_teleop.config import Config
from dora_teleop.gamepad import Gamepad, now
from dora_teleop.joy import JoyInput, now
from dora_teleop.log import log
from dora_teleop.mapping import LAYOUT, pressed
REQUEST_ID = "request_id"
STOP_VELOCITY = [0.0] * 6
# Still, in the normalised units both sources speak.
STOP_AXES = [0.0] * 6
# The verbs this node publishes on `command`. Robot-agnostic on purpose: a
# consumer that grows a two-finger gripper where the last one had a vacuum
# should need no change here, and neither should this node.
GRIPPER_CLOSE = "gripper_close"
GRIPPER_OPEN = "gripper_open"
STOP = "stop"
HOME = "home"
CLEAR = "clear"
FREEDRIVE = "freedrive"
# The same verbs as `dora-ulite6` names them, for the deprecated `request` port.
# This table is the whole of what used to be spread through the loop, and it is
# what a robot speaking the standard lets us delete.
LEGACY_REQUEST = {
GRIPPER_CLOSE: {"service": "grasp"},
GRIPPER_OPEN: {"service": "release"},
STOP: {"service": "stop"},
CLEAR: {"service": "clear"},
HOME: {"service": "home", "wait": False},
}
class Teleop:
@@ -37,10 +61,12 @@ class Teleop:
def __init__(self, config: Config):
self.config = config
self.arbiter = Arbiter(config.deadman.gamepad_idle_release_s)
self.joy = JoyInput(config.deadman.joy_ttl_s)
self.view = ui.ArmView()
# The velocity the browser last asked for, and when. A jog is a stream;
# what matters is whether the stream is still arriving.
self.web_velocity = STOP_VELOCITY
# What the browser last asked for, and when. A jog is a stream; what
# matters is whether the stream is still arriving. Normalised, like the
# pad's — the console does not know the robot's units.
self.web_axes = STOP_AXES
self.web_seen_at = 0.0
self.clients = 0
# Whether the arm is currently being driven by us. Used to send the stop
@@ -58,7 +84,7 @@ class Teleop:
def main() -> None:
"""Connect to the dataflow, start reading the pad, and run until dora stops us."""
"""Connect to the dataflow and run until dora stops us."""
args = _parse_args()
config = Config.from_env()
@@ -66,8 +92,6 @@ def main() -> None:
# dora spawns it, the id from the dataflow wins.
node = Node(args.name)
state = Teleop(config)
gamepad = Gamepad(config.joystick_index)
gamepad.start()
try:
for event in node:
@@ -75,14 +99,13 @@ def main() -> None:
raise RuntimeError(event["error"])
if event["type"] != "INPUT":
continue
_handle(node, state, gamepad, event)
_handle(node, state, event)
finally:
# A motion lives in the arm's controller, not in this process. Leaving
# without zeroing the velocity leaves it travelling until the
# controller's own expiry catches up, which is the one moment nobody is
# watching for it.
_send_jog(node, STOP_VELOCITY)
gamepad.shutdown()
_send_motion(node, state, STOP_AXES)
def _parse_args() -> argparse.Namespace:
@@ -96,11 +119,13 @@ def _parse_args() -> argparse.Namespace:
return parser.parse_args()
def _handle(node: Node, state: Teleop, gamepad: Gamepad, event: dict) -> None:
def _handle(node: Node, state: Teleop, event: dict) -> None:
"""Route one input to whatever deals with it."""
event_id = event["id"]
if event_id == "tick":
_tick(node, state, gamepad)
_tick(node, state)
elif event_id == "joy":
state.joy.accept(joy.decode(event), now())
elif event_id == "ui_command":
_accept_ui_command(node, state, event)
elif event_id == "clients":
@@ -122,21 +147,30 @@ def _handle(node: Node, state: Teleop, gamepad: Gamepad, event: dict) -> None:
# --- the tick, where the arm is actually driven ---
def _tick(node: Node, state: Teleop, gamepad: Gamepad) -> None:
def _tick(node: Node, state: Teleop) -> None:
"""Decide what the arm should be doing, and say so."""
moment = now()
sample = gamepad.read()
_read_buttons(node, state, sample)
# Absent covers three things now: no pad plugged in, a pad unplugged
# mid-jog, and `dora-joy` no longer sending. The arm cannot tell them apart
# and does not need to.
sample = state.joy.read(moment)
# Who owns the arm is settled before anything they pressed is acted on.
# The other order drops the first press after the pad has been idle: the
# lease is not held yet when the buttons are read, and by the next tick the
# button is held rather than newly down, so the edge is gone for good.
#
# A pad taking the arm the moment a stick moves is what keeps an operator
# from having to reach for a browser to use the thing in their hands.
if sample.active and state.arbiter.owner is None:
active = mapping.is_active(sample)
if active and state.arbiter.owner is None:
state.arbiter.take(GAMEPAD, moment)
if sample.active:
if active:
state.arbiter.note_activity(GAMEPAD, moment)
state.arbiter.expire_idle_gamepad(moment)
_read_buttons(node, state, sample)
_drive(node, state, sample, moment)
_poll_state(node, state, moment)
_publish(node, state)
@@ -149,46 +183,51 @@ def _drive(node: Node, state: Teleop, sample, moment: float) -> None:
stream of jogs as one continuous one, and the re-send exists so the
controller's own expiry never fires mid-motion.
"""
velocity = _wanted_velocity(state, sample, moment)
axes = _wanted_axes(state, sample, moment)
if mapping.is_moving(velocity):
if mapping.is_moving(axes):
state.jogging = True
_send_jog(node, velocity)
_send_motion(node, state, axes)
return
if state.jogging:
# Once, not every tick. Repeating it would keep the arm node believing a
# jog is live and hold `robot_state` at `moving` forever.
state.jogging = False
_send_jog(node, STOP_VELOCITY)
_send_motion(node, state, STOP_AXES)
def _wanted_velocity(state: Teleop, sample, moment: float) -> list[float]:
"""Return what the arm should be doing, which is usually nothing.
def _wanted_axes(state: Teleop, sample, moment: float) -> list[float]:
"""Return what the robot should be doing, normalised, which is usually nothing.
Every path that is not an owner actively asking for motion returns a stop.
That is deliberate: the default has to be "still", so that a case nobody
thought of fails by halting rather than by carrying on.
"""
# First, and above the lease. A stop somebody had to reach for is not undone
# by the next thumb movement: it holds until it is deliberately cleared.
if state.view.estop:
return STOP_AXES
owner = state.arbiter.owner
if owner == GAMEPAD:
# An unplugged pad is not a centred pad. Stopping here rather than
# waiting out the TTL is the difference between a dropped controller
# costing 10 ms of travel and costing 400.
if not sample.present:
return STOP_VELOCITY
return mapping.velocity(sample.axes, sample.buttons, state.config)
return STOP_AXES
return mapping.from_pad(sample.axes, sample.buttons, state.config)
if owner == WEB:
# The console refreshes its jog every 150 ms; the browser going quiet,
# or the tab closing, has to stop the arm without anything being sent.
if state.clients == 0:
return STOP_VELOCITY
return STOP_AXES
if moment - state.web_seen_at > state.config.deadman.jog_ttl_s:
return STOP_VELOCITY
return state.web_velocity
return STOP_AXES
return state.web_axes
return STOP_VELOCITY
return STOP_AXES
def _read_buttons(node: Node, state: Teleop, sample) -> None:
@@ -204,20 +243,23 @@ def _read_buttons(node: Node, state: Teleop, sample) -> None:
def tapped(index: int) -> bool:
return pressed(sample.buttons, index) and not pressed(previous, index)
# The stop is not gated on the lease. Somebody reaching for it is the one
# moment when asking who owns the arm is the wrong question.
# Neither the stop nor its release is gated on the lease. Somebody reaching
# for the stop is the one moment when asking who owns the arm is the wrong
# question, and a latch that cannot be released without first taking the arm
# back is a trap — an idle pad hands the lease away two seconds after the
# stop it just made. The console's RESET is ungated for the same reason.
if tapped(LAYOUT.estop):
_estop(node, state)
if tapped(LAYOUT.clear):
_clear(node, state)
if not state.arbiter.may_drive(GAMEPAD):
return
if tapped(LAYOUT.grasp):
_request(node, state, {"service": "grasp"})
_act(node, state, GRIPPER_CLOSE)
if tapped(LAYOUT.release):
_request(node, state, {"service": "release"})
if tapped(LAYOUT.clear):
_request(node, state, {"service": "clear"})
_act(node, state, GRIPPER_OPEN)
if tapped(LAYOUT.freedrive):
_request(node, state, {"service": "freedrive", "enabled": not state.view.freedrive_on})
_freedrive(node, state, not state.view.freedrive_on)
def _poll_state(node: Node, state: Teleop, moment: float) -> None:
@@ -259,11 +301,7 @@ def _accept_ui_command(node: Node, state: Teleop, event: dict) -> None:
_control(node, state, command, moment)
return
if command.type == "reset":
# Clearing the latch is this node's business; clearing the controller's
# error is the arm's. Both, in that order, or the console's RESET button
# leaves one of the two still saying stop.
state.view.estop = False
_request(node, state, {"service": "clear"})
_clear(node, state)
return
if not state.arbiter.may_drive(WEB):
@@ -271,18 +309,18 @@ def _accept_ui_command(node: Node, state: Teleop, event: dict) -> None:
return
if command.type == "jog":
state.web_velocity = command.velocity
state.web_axes = mapping.from_console(command.axes)
state.web_seen_at = moment
state.arbiter.note_activity(WEB, moment)
elif command.type == "jog_stop":
state.web_velocity = STOP_VELOCITY
state.web_axes = STOP_AXES
state.web_seen_at = moment
elif command.type == "home":
_request(node, state, {"service": "home", "wait": False})
_act(node, state, HOME)
elif command.type == "gripper":
_request(node, state, {"service": "grasp" if command.enabled else "release"})
_act(node, state, GRIPPER_CLOSE if command.enabled else GRIPPER_OPEN)
elif command.type == "freedrive":
_request(node, state, {"service": "freedrive", "enabled": command.enabled})
_freedrive(node, state, command.enabled)
elif command.type == "move":
_request(node, state, {"service": "move_to", "wait": False, **command.row})
@@ -291,7 +329,7 @@ def _control(node: Node, state: Teleop, command, moment: float) -> None:
"""Hand the arm to the console, or take it back."""
if command.action == "release":
state.arbiter.release(WEB)
state.web_velocity = STOP_VELOCITY
state.web_axes = STOP_AXES
return
if not state.arbiter.take(WEB, moment, preempt=command.preempt):
_post_ui(node, ui.error(f"the {state.arbiter.owner} has the arm"))
@@ -303,7 +341,7 @@ def _accept_clients(state: Teleop, event: dict) -> None:
state.clients = int(row.get("connected", 0))
if state.clients == 0:
state.arbiter.release(WEB)
state.web_velocity = STOP_VELOCITY
state.web_axes = STOP_AXES
# --- what the arm says back ---
@@ -336,14 +374,57 @@ def _estop(node: Node, state: Teleop) -> None:
log("e-stop")
state.view.estop = True
state.jogging = False
state.web_velocity = STOP_VELOCITY
_send_jog(node, STOP_VELOCITY)
_request(node, state, {"service": "stop"})
state.web_axes = STOP_AXES
_send_motion(node, state, STOP_AXES)
_act(node, state, STOP)
def _send_jog(node: Node, velocity: list[float]) -> None:
"""Send one velocity to the arm. There is no answer to a jog, ever."""
node.send_output("jog", pa.array([{"velocity": [float(v) for v in velocity]}]))
def _clear(node: Node, state: Teleop) -> None:
"""Release the latch and clear the controller's error, in that order.
Both, or one of the two is still saying stop: the latch is this node's
business and the error is the arm's. The console's RESET and the pad's
`clear` button are the same act and go through here, so a pad-only
dataflow is not left with a latch nothing can release.
"""
log("cleared")
state.view.estop = False
_act(node, state, CLEAR)
def _send_motion(node: Node, state: Teleop, axes: list[float]) -> None:
"""Send one motion command, on the standard port and the deprecated one.
There is no answer to either, ever — a jog is a stream that expires on the
consumer's own TTL, so the sender keeps sending and nobody waits.
`twist` is the standard: SI, framed, what a robot node should take. `jog`
is `dora-ulite6`'s mm/s and deg/s, published alongside until that node
learns the standard. Both carry the same intent, so wiring either is safe;
wiring both would drive the arm twice.
"""
config = state.config
node.send_output("twist", message.pack_twist(mapping.twist(axes, config)))
node.send_output("jog", message.pack_jog(mapping.scale(axes, config)))
def _act(node: Node, state: Teleop, action: str, value: float | None = None) -> None:
"""Ask for one discrete thing, on the standard port and the deprecated one.
The verb is robot-agnostic; `LEGACY_REQUEST` is the only place that knows
what `dora-ulite6` calls it. A robot speaking the standard needs no entry
there, which is how that table eventually goes away.
"""
node.send_output("command", message.pack_command(action, value))
row = LEGACY_REQUEST.get(action)
if row is not None:
_request(node, state, dict(row))
def _freedrive(node: Node, state: Teleop, enabled: bool) -> None:
"""Toggle freedrive. Its argument rides as the verb's value."""
_act(node, state, FREEDRIVE, value=float(enabled))
_request(node, state, {"service": "freedrive", "enabled": enabled})
def _request(node: Node, state: Teleop, row: dict) -> None:
dora_teleop/mapping.py
+116 -20
View File
@@ -1,19 +1,34 @@
"""Turning stick deflection into a velocity the arm understands.
"""Turning a control input into motion, in one place for every source.
Pure functions, no state and no hardware, because this is the part that has to
be right and the part that is cheapest to check. Everything here is in the
arm node's units: `[vx, vy, vz, wx, wy, wz]`, mm/s and deg/s, in the robot's
**base** frame — the same frame `tcp_pose` reports.
be right and the part that is cheapest to check.
Everything passes through a **normalised** six-vector first: `[-1, 1]` per
degree of freedom, `[x, y, z, roll, pitch, yaw]`, in the robot's **base**
frame. A gamepad reaches it through :func:`from_pad`, which shapes away a worn
stick; the browser sends it already clean. Only then is it scaled, and that
happens exactly once — which is what lets the same node drive an arm in mm/s
and one in m/s without either source knowing the difference.
Two scalings exist. :func:`twist` is the standard one, SI, and is what a robot
should consume. :func:`velocity` is mm/s and deg/s for `dora-ulite6`'s `jog`
port, kept while that node learns the standard.
The screen convention comes from the old web console, so the two agree: stick
up is +X, stick right is +Y.
"""
import math
from dataclasses import dataclass
# The arm takes six numbers; these name them.
# Six degrees of freedom; these name their places in the vector.
VX, VY, VZ, WX, WY, WZ = range(6)
# Operator-facing settings are in mm/s and deg/s because that is what a person
# reading a dataflow can judge — 60 mm/s means something, 0.06 m/s less so. The
# wire is SI. These convert between the two, and are the only place that does.
MM_PER_M = 1000.0
@dataclass(frozen=True)
class Layout:
@@ -43,6 +58,17 @@ class Layout:
LAYOUT = Layout()
# The frame a velocity is expressed in. Base is what `tcp_pose` reports and what
# the console's arrows mean; tool is reserved for a consumer that wants it.
BASE_FRAME = "base"
TOOL_FRAME = "tool"
# How far a control must travel before the pad counts as being used. Raw, with
# no deadzone: this decides whether the pad is asking for the arm, and
# `velocity` decides how far to move it. A tighter threshold here than there
# would mean a stick could claim the arm and then not move it.
ACTIVITY = 0.3
def deadzone(value: float, width: float) -> float:
"""Ignore travel near centre, and rescale what is left to the full range.
@@ -89,13 +115,16 @@ def pressed(buttons: list[bool], index: int) -> bool:
return bool(buttons[index]) if index < len(buttons) else False
def velocity(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
"""Map one gamepad reading to a base-frame velocity.
def from_pad(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
"""Map one gamepad reading to the normalised six-vector.
Translation comes from the left stick and the triggers, rotation from the
right stick and the bumpers. Several axes move at once on purpose: a stick
is analog and blended, which is exactly what the console's four-sector dial
pad could not do.
is analog and blended, which is exactly what a four-sector dial pad cannot do.
The deadzone and the response curve are applied here and nowhere else,
because they exist to correct a physical stick. A browser has no worn
centre, so :func:`from_console` does not use them.
"""
def shaped(value: float) -> float:
@@ -104,20 +133,87 @@ def velocity(axes: list[float], buttons: list[bool], config, layout: Layout = LA
out = [0.0] * 6
# Up on the stick is negative in SDL, and up should send the arm away from
# the operator, along +X.
out[VX] = shaped(-axis(axes, layout.left_y)) * config.speed_mm_s
out[VY] = shaped(axis(axes, layout.left_x)) * config.speed_mm_s
lift = trigger(axis(axes, layout.right_trigger)) - trigger(axis(axes, layout.left_trigger))
out[VZ] = shaped(lift) * config.speed_mm_s
out[WX] = shaped(axis(axes, layout.right_x)) * config.speed_deg_s
out[WY] = shaped(-axis(axes, layout.right_y)) * config.speed_deg_s
yaw = pressed(buttons, layout.yaw_right) - pressed(buttons, layout.yaw_left)
out[WZ] = yaw * config.speed_deg_s
out[VX] = shaped(-axis(axes, layout.left_y))
out[VY] = shaped(axis(axes, layout.left_x))
out[VZ] = shaped(trigger(axis(axes, layout.right_trigger)) - trigger(axis(axes, layout.left_trigger)))
out[WX] = shaped(axis(axes, layout.right_x))
out[WY] = shaped(-axis(axes, layout.right_y))
out[WZ] = float(pressed(buttons, layout.yaw_right) - pressed(buttons, layout.yaw_left))
return out
def from_console(values) -> list[float]:
"""Read the normalised six-vector a browser sent, clamped to what it may ask for.
The console speaks the same units as a stick — a fraction of full speed per
axis — so that how fast anything goes is decided once, here, rather than
once per source. A page that has never heard of the robot cannot get its
units wrong.
"""
if not isinstance(values, list) or len(values) != 6:
return [0.0] * 6
out = []
for value in values:
if not isinstance(value, (int, float)):
return [0.0] * 6
out.append(max(-1.0, min(1.0, float(value))))
return out
def velocity(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
"""Map one gamepad reading straight to `dora-ulite6`'s mm/s and deg/s.
A convenience over `scale(from_pad(...))`, kept because the probe and the
tests read better for it.
"""
return scale(from_pad(axes, buttons, config, layout), config)
def scale(normalised: list[float], config) -> list[float]:
"""Scale the normalised six-vector to mm/s and deg/s, base frame.
`dora-ulite6`'s `jog` units. Deprecated in favour of :func:`twist`, and kept
only while that node learns to take the standard one.
"""
out = [value * config.speed_mm_s for value in normalised[:3]]
out += [value * config.speed_deg_s for value in normalised[3:]]
return out
def twist(normalised: list[float], config, frame: str = BASE_FRAME) -> dict:
"""Scale the normalised six-vector to a Twist, in SI.
Metres per second and radians per second, which is what a robot node should
consume: unambiguous, and the same numbers a UR or MoveIt would take. A
consumer wanting other units converts them itself, at one known boundary.
"""
linear = [value * config.speed_mm_s / MM_PER_M for value in normalised[:3]]
angular = [math.radians(value * config.speed_deg_s) for value in normalised[3:]]
return {"linear": linear, "angular": angular, "frame": frame}
def is_active(sample, layout: Layout = LAYOUT) -> bool:
"""Whether anything on the pad is being touched at all.
A trigger is not a stick. SDL rests a trigger at **-1.0**, so comparing its
raw value against a threshold reports every connected pad as being used,
for ever — which means the pad claims the arm the moment it is plugged in
and `GAMEPAD_IDLE_RELEASE_S` never fires. Triggers go through
:func:`trigger` here for the same reason they do in :func:`velocity`.
Axes the layout does not name are still counted: a pad that reports its
d-pad as two extra axes is asking for the arm when somebody presses it.
"""
if not sample.present:
return False
if any(sample.buttons):
return True
triggers = (layout.left_trigger, layout.right_trigger)
if any(trigger(axis(sample.axes, index)) > ACTIVITY for index in triggers):
return True
return any(abs(value) > ACTIVITY for index, value in enumerate(sample.axes) if index not in triggers)
def is_moving(velocity_vector: list[float]) -> bool:
"""Whether this velocity would actually drive the arm."""
return any(velocity_vector)
dora_teleop/message.py
+46
View File
@@ -0,0 +1,46 @@
"""The wire shapes this node publishes.
Declared, never inferred. pyarrow reads `{"value": None}` as `null` and
`{"value": 1.0}` as `double`, so an inferred `command` would change the type of
its port depending on which verb was pressed — `stop` and `freedrive` would not
agree. The same hazard that makes an unplugged gamepad retype a `joy` port.
dora 1.0 has no standard type for either of these, so these structs and the
manifest that describes them are the whole contract a consumer gets.
"""
import pyarrow as pa
# A velocity, in SI. Metres per second and radians per second, in the frame
# named by `frame` — the units a robot node should consume, and the same ones a
# UR or MoveIt would take.
TWIST = pa.struct(
[
("linear", pa.list_(pa.float64())),
("angular", pa.list_(pa.float64())),
("frame", pa.string()),
]
)
# One discrete thing to do. `action` is a robot-agnostic verb; `value` carries
# its argument where it has one and is null otherwise.
COMMAND = pa.struct([("action", pa.string()), ("value", pa.float64())])
# `dora-ulite6`'s velocity, in mm/s and deg/s. Deprecated along with the `jog`
# port it goes out on.
JOG = pa.struct([("velocity", pa.list_(pa.float64()))])
def pack_twist(twist: dict) -> pa.Array:
"""Build one `twist` message."""
return pa.array([twist], type=TWIST)
def pack_command(action: str, value: float | None) -> pa.Array:
"""Build one `command` message."""
return pa.array([{"action": action, "value": value}], type=COMMAND)
def pack_jog(velocity: list[float]) -> pa.Array:
"""Build one deprecated `jog` message."""
return pa.array([{"velocity": [float(v) for v in velocity]}], type=JOG)
dora_teleop/probe.py
+48 -29
View File
@@ -1,24 +1,40 @@
"""Show what the pad is doing, with no dataflow and no arm.
"""Show what the pad is doing to the arm, without an arm.
`dora-teleop-probe` opens the gamepad and prints, live: the raw axes SDL
reports, the buttons it sees pressed, and the velocity this node would send.
`dora-teleop-probe` attaches to a running dataflow as a dynamic node, reads the
same `joy` stream this node reads, and prints: the raw axes, the buttons, and the
velocity this node would send. **It cannot move anything.** It publishes no
output and never talks to the arm; it only reads.
It exists because the axis and button indices in :mod:`dora_teleop.mapping` are
SDL's documented layout rather than an observed one, and a pad that disagrees is
otherwise discovered by watching a real arm move the wrong way. It goes through
:class:`~dora_teleop.gamepad.Gamepad` and :func:`~dora_teleop.mapping.velocity`
— the same code the node runs — so what it prints is what the arm would get.
the same :func:`~dora_teleop.mapping.velocity` the node runs, on the same
readings, so what it prints is what the arm would get — keep it that way, or it
stops being evidence.
Nothing here can move anything. There is no dora node and no connection to the
arm; this only reads.
Because it reads `joy` rather than a pad, the dataflow has to declare it:
```yaml
- id: teleop-probe
path: dynamic
inputs:
joy: joy/joy
```
`dataflow-gamepad.yml` in this repo already does. Start that dataflow under a
coordinator (`dora start`, not `dora run`), then run `dora-teleop-probe` in
another terminal.
For a pad whose layout is not known at all — which index is which — use
`dora-joy-probe` from the `dora-joy` repo, which needs no dataflow.
"""
import argparse
import time
from dora_teleop import mapping
from dora import Node
from dora_teleop import joy, mapping
from dora_teleop.config import Config
from dora_teleop.gamepad import Gamepad
from dora_teleop.mapping import LAYOUT
# What each velocity component is, in the order the arm takes them.
@@ -79,10 +95,10 @@ def render(sample, velocity: list[float], config: Config) -> str:
if not sample.present:
lines += [
" no gamepad found.",
" no gamepad.",
"",
f" looking for joystick index {config.joystick_index};",
" plug one in and this will pick it up.",
" `dora-joy` is reporting no pad, or has stopped reporting at all.",
" Either way the arm is being told to hold still.",
]
return "\n".join(lines)
@@ -102,30 +118,33 @@ def render(sample, velocity: list[float], config: Config) -> str:
lines.append(f" {name} {value:+8.2f} {units:<6} {meaning:<14}{marker}".rstrip())
moving = "yes" if mapping.is_moving(velocity) else "no — the arm would be told to hold still"
lines += ["", f" moving: {moving}"]
lines += [
"",
f" moving: {moving}",
f" asking for the arm: {'yes' if mapping.is_active(sample) else 'no'}",
]
return "\n".join(lines)
def main() -> None:
"""Open the pad and print what it is doing until interrupted."""
parser = argparse.ArgumentParser(description="Show what the gamepad is doing. Moves nothing.")
parser.add_argument("--hz", type=float, default=10.0, help="Redraw rate. Defaults to 10.")
"""Attach to a running dataflow and print what the pad is doing."""
parser = argparse.ArgumentParser(description="Show what the gamepad would do to the arm. Moves nothing.")
parser.add_argument(
"--name",
default="teleop-probe",
help="The id this probe is declared under in the dataflow. Defaults to teleop-probe.",
)
args = parser.parse_args()
config = Config.from_env()
gamepad = Gamepad(config.joystick_index)
gamepad.start()
node = Node(args.name)
try:
while True:
sample = gamepad.read()
velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6
print(CLEAR + render(sample, velocity, config), flush=True)
time.sleep(1.0 / args.hz)
except KeyboardInterrupt:
print()
finally:
gamepad.shutdown()
for event in node:
if event["type"] != "INPUT" or event["id"] != "joy":
continue
sample = joy.decode(event)
velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6
print(CLEAR + render(sample, velocity, config), flush=True)
if __name__ == "__main__":
dora_teleop/ui.py
+17 -9
View File
@@ -49,7 +49,10 @@ class Command:
"""One thing the console asked for."""
type: str
velocity: list[float] = field(default_factory=list)
# Normalised, `[-1, 1]` per degree of freedom — a fraction of full speed,
# exactly what a stick reports. Not a velocity: the console does not know
# the robot's units and must not have to.
axes: list[float] = field(default_factory=list)
action: str = ""
preempt: bool = False
enabled: bool = False
@@ -70,10 +73,10 @@ def command_of(payload: dict) -> Command | None:
return None
if kind == "jog":
velocity = payload.get("velocity") or []
if len(velocity) != 6 or not all(isinstance(v, (int, float)) for v in velocity):
axes = payload.get("axes") or []
if len(axes) != 6 or not all(isinstance(v, (int, float)) for v in axes):
return None
return Command(type="jog", velocity=[float(v) for v in velocity])
return Command(type="jog", axes=[float(v) for v in axes])
if kind in ("jog_stop", "estop", "reset", "home"):
return Command(type=kind)
if kind == "control":
@@ -83,7 +86,7 @@ def command_of(payload: dict) -> Command | None:
preempt=bool(payload.get("preempt", False)),
)
if kind == "gripper":
return Command(type="gripper", enabled=bool(payload.get("vacuum", False)))
return Command(type="gripper", enabled=bool(payload.get("closed", False)))
if kind == "freedrive":
return Command(type="freedrive", enabled=bool(payload.get("enabled", False)))
if kind == "move":
@@ -146,9 +149,10 @@ def arm_state(view: ArmView, jogging: bool) -> str:
def snapshot(view: ArmView, owner: str | None, jogging: bool, timestamp: float) -> dict:
"""Build the telemetry object the console reads.
Every key here is read by name in `app.js`. `vacuum` is the console's name
for the mounted tool whatever it actually is, kept because renaming it would
mean editing the page for no gain.
Every key here is read by name in `app.js`, so this and the page change
together. The names are the robot-agnostic ones: `gripper` rather than
`vacuum`, because a page that says vacuum only fits a robot with one on it,
and `units` so a page can label a pose without guessing.
"""
return {
"type": "telemetry",
@@ -158,10 +162,14 @@ def snapshot(view: ArmView, owner: str | None, jogging: bool, timestamp: float)
"arm_state": arm_state(view, jogging),
"owner": owner or "none",
"estop": view.estop,
"vacuum": view.tool_engaged,
"gripper": view.tool_engaged,
"freedrive": view.freedrive_on,
"position": dict(zip(POSE_KEYS, view.pose, strict=False)) if view.pose else None,
"joints": list(view.joints),
# What `position` is in, so the page labels it rather than assuming. The
# telemetry ports carry whatever the robot publishes; only the command
# path is normalised.
"units": {"position": "mm", "rotation": "deg"},
}