cristhian
c9cb4b8c9e
Pure functions, no state and no hardware, because this is the part that has to be right and the cheapest to check. Analog and blended: how fast the arm moves depends on how far the stick is pushed, and several axes move at once. That is the whole reason a pad beats the web console's four-sector dial pad, which can only ask for one axis at a time at one fixed speed. The deadzone rescales what is left of the travel rather than just clipping it, so the arm starts from zero just outside the band instead of jumping to a fraction of full speed the instant the stick leaves it. Expo bends the response because fine positioning happens near centre, where a linear stick gives almost no resolution. Up on the stick sends the arm along +X, matching the console's dial, so the two agree about which way is which.
124 lines
4.2 KiB
Python
124 lines
4.2 KiB
Python
"""Turning stick deflection into a velocity the arm understands.
|
|
|
|
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.
|
|
|
|
The screen convention comes from the old web console, so the two agree: stick
|
|
up is +X, stick right is +Y.
|
|
"""
|
|
|
|
from dataclasses import dataclass
|
|
|
|
# The arm takes six numbers; these name them.
|
|
VX, VY, VZ, WX, WY, WZ = range(6)
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Layout:
|
|
"""Which axis and button index does what, in SDL's numbering.
|
|
|
|
The defaults are the layout SDL reports for an Xbox-style pad, which is also
|
|
what it reports for most PlayStation-style ones. A pad that disagrees is a
|
|
change here rather than a new environment variable: an index nobody can
|
|
verify from the outside is worse than a line of code somebody can read.
|
|
"""
|
|
|
|
left_x: int = 0
|
|
left_y: int = 1
|
|
left_trigger: int = 2
|
|
right_x: int = 3
|
|
right_y: int = 4
|
|
right_trigger: int = 5
|
|
|
|
yaw_left: int = 4 # LB
|
|
yaw_right: int = 5 # RB
|
|
grasp: int = 0 # A
|
|
release: int = 1 # B
|
|
clear: int = 2 # X
|
|
freedrive: int = 3 # Y
|
|
estop: int = 6 # Back
|
|
|
|
|
|
LAYOUT = Layout()
|
|
|
|
|
|
def deadzone(value: float, width: float) -> float:
|
|
"""Ignore travel near centre, and rescale what is left to the full range.
|
|
|
|
Rescaling matters: without it the arm jumps to `width` of full speed the
|
|
instant the stick leaves the dead band, which feels like a broken control
|
|
rather than a gentle one.
|
|
"""
|
|
if width <= 0.0:
|
|
return value
|
|
size = abs(value)
|
|
if size < width:
|
|
return 0.0
|
|
scaled = (size - width) / (1.0 - width)
|
|
return scaled if value > 0 else -scaled
|
|
|
|
|
|
def expo(value: float, factor: float) -> float:
|
|
"""Bend the response so small deflections move the arm slowly.
|
|
|
|
Fine positioning happens near centre, and a linear stick gives it almost no
|
|
resolution there. `factor` is how much of the curve is cubic: 0 is linear.
|
|
"""
|
|
return (1.0 - factor) * value + factor * value**3
|
|
|
|
|
|
def trigger(value: float) -> float:
|
|
"""Return a trigger's travel as 0 at rest and 1 fully pressed.
|
|
|
|
SDL reports a trigger from -1 to 1, unlike a stick, and a trigger that has
|
|
never been touched sometimes reads 0 instead of -1 until it first moves.
|
|
Clamping keeps that from reading as half pressed.
|
|
"""
|
|
return max(0.0, min(1.0, (value + 1.0) / 2.0))
|
|
|
|
|
|
def axis(values: list[float], index: int) -> float:
|
|
"""Read one axis, treating a pad that does not report it as centred."""
|
|
return values[index] if index < len(values) else 0.0
|
|
|
|
|
|
def pressed(buttons: list[bool], index: int) -> bool:
|
|
"""Read one button, treating a pad that does not report it as unpressed."""
|
|
return bool(buttons[index]) if index < len(buttons) else False
|
|
|
|
|
|
def velocity(axes: list[float], buttons: list[bool], config, layout: Layout = LAYOUT) -> list[float]:
|
|
"""Map one gamepad reading to a base-frame velocity.
|
|
|
|
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.
|
|
"""
|
|
|
|
def shaped(value: float) -> float:
|
|
return expo(deadzone(value, config.deadzone), config.expo)
|
|
|
|
out = [0.0] * 6
|
|
# Up on the stick is negative in SDL, and up should send the arm away from
|
|
# the operator, along +X.
|
|
out[VX] = shaped(-axis(axes, layout.left_y)) * 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
|
|
return out
|
|
|
|
|
|
def is_moving(velocity_vector: list[float]) -> bool:
|
|
"""Whether this velocity would actually drive the arm."""
|
|
return any(velocity_vector)
|