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.

    These are what an Xbox One S pad reports over Bluetooth, swept one control
    at a time: sticks on 0-3, both triggers last, and buttons with gaps at 2 and
    5. 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. `dora-teleop-probe` is how you check.

    **The axes can be checked at runtime and the buttons cannot.**
    :func:`rest_zeros` refuses a pad whose resting axes say this numbering is
    wrong, because an axis at rest has a value to compare. An unpressed button
    reads the same on every pad, so a wrong button index is silent — it was
    found here by `clear` toggling freedrive and `yaw_left` firing the E-STOP.
    The probe, and pressing each button once, is the only check there is.
    """

    left_x: int = 0
    left_y: int = 1
    right_x: int = 2
    right_y: int = 3
    left_trigger: int = 4
    right_trigger: int = 5

    grasp: int = 0  # A
    release: int = 1  # B
    clear: int = 3  # X
    freedrive: int = 4  # Y
    yaw_left: int = 6  # LB
    yaw_right: int = 7  # RB
    estop: int = 11  # 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

# Where an untouched control sits once :func:`canonical` has had it: a stick at
# centre, a trigger at the bottom of its travel.
STICK_REST = 0.0
TRIGGER_REST = -1.0

# How far an axis may rest from those and still be believed. Wide enough for a
# worn stick's tilt and for a pad that reports a trigger it has never felt as 0,
# and far short of the full deflection a wrongly numbered axis shows.
REST_TOLERANCE = 0.25


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, so `TRIGGER_REST` is
    where an untouched one sits. A trigger that has never moved sometimes reads
    0 instead, which this function alone cannot tell from half pressed —
    :func:`canonical` is what fixes that, by measuring the pad's own rest before
    anything reaches here.
    """
    return max(0.0, min(1.0, (value - TRIGGER_REST) / (1.0 - TRIGGER_REST)))


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 axis_names(layout: Layout = LAYOUT) -> dict[int, str]:
    """Name each axis index the layout uses."""
    return {
        layout.left_x: "left stick X",
        layout.left_y: "left stick Y",
        layout.left_trigger: "left trigger",
        layout.right_x: "right stick X",
        layout.right_y: "right stick Y",
        layout.right_trigger: "right trigger",
    }


def rest_zeros(axes: list[float], layout: Layout = LAYOUT) -> list[float]:
    """Return where each axis of an untouched pad sits, to measure deflection from.

    An untouched pad is the one moment this node can check `layout` against the
    thing in front of it: a stick rests near centre and a trigger near the
    bottom of its travel, so an axis resting anywhere else means the numbering
    is wrong, or a control is being held, or the pad is broken. All three look
    the same from here and all three raise, because all three would otherwise
    drive the arm from a reading nobody made — a trigger read as a stick asks
    for full speed while sitting untouched on the desk.

    Parameters
    ----------
    axes
        One reading, taken while nobody is touching the pad.
    layout
        Which index is which control.

    Returns
    -------
    list of float
        The resting value of each axis, in the order they were reported.

    Raises
    ------
    ValueError
        If any axis rests where the layout says it cannot, naming the index,
        what the layout calls it, and what it read.
    """
    names = axis_names(layout)
    triggers = (layout.left_trigger, layout.right_trigger)
    for index, value in enumerate(axes):
        if index in triggers:
            # Either end is a resting trigger: -1 is what SDL reports, and 0 is
            # what some pads report until the trigger first moves.
            if abs(value - TRIGGER_REST) <= REST_TOLERANCE or abs(value - STICK_REST) <= REST_TOLERANCE:
                continue
        elif abs(value - STICK_REST) <= REST_TOLERANCE:
            continue
        name = names.get(index, "an axis the layout does not use")
        raise ValueError(f"axis {index} rests at {value:+.3f}, which is not where {name} sits")
    return list(axes)


def canonical(axes: list[float], zeros: list[float], layout: Layout = LAYOUT) -> list[float]:
    """Return this pad's axes as the layout's ideal pad would have reported them.

    Sticks come back resting at `STICK_REST` and triggers at `TRIGGER_REST`,
    whatever this particular pad does when untouched, so everything downstream
    is written once for a pad that behaves. A stick that no longer centres, and
    a trigger that reads 0 until it first moves, both stop being special cases
    here rather than in every function that reads one.

    Deflection is measured from `zeros` rather than from the ideal, which is the
    whole point: the tilt is subtracted, not deadzoned away, so the travel that
    is left still reaches full speed.
    """
    triggers = (layout.left_trigger, layout.right_trigger)
    out = []
    for index, value in enumerate(axes):
        zero = zeros[index] if index < len(zeros) else STICK_REST
        if index in triggers:
            travel = (value - zero) / (1.0 - zero) if zero < 1.0 else 0.0
            out.append(TRIGGER_REST + (1.0 - TRIGGER_REST) * max(0.0, min(1.0, travel)))
        else:
            out.append(max(-1.0, min(1.0, value - zero)))
    return out


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 ring sectors 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)