dora-teleop/dora_teleop/mapping.py

"""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 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

# 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:
    """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()

# 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.

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