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