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/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: