cristhian
d336f8d499
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.
143 lines
5.1 KiB
Python
143 lines
5.1 KiB
Python
"""Stick deflection to base-frame velocity: units, signs and the shaping curve."""
|
|
|
|
import pytest
|
|
|
|
from dora_teleop import mapping
|
|
from dora_teleop.mapping import VX, VY, VZ, WX, WY, WZ
|
|
from tests.conftest import stick
|
|
|
|
|
|
def test_a_stick_at_rest_asks_for_nothing(config):
|
|
velocity = mapping.velocity(stick().axes, [], config)
|
|
|
|
assert velocity == [0.0] * 6
|
|
assert not mapping.is_moving(velocity)
|
|
|
|
|
|
def test_pushing_the_stick_up_moves_the_arm_along_positive_x(config):
|
|
# Up is negative in SDL, and up should send the arm away from the operator.
|
|
velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config)
|
|
|
|
assert velocity[VX] == pytest.approx(config.speed_mm_s)
|
|
assert velocity[VY] == 0.0
|
|
|
|
|
|
def test_pushing_the_stick_right_moves_the_arm_along_positive_y(config):
|
|
velocity = mapping.velocity(stick(axes=[1.0, 0.0]).axes, [], config)
|
|
|
|
assert velocity[VY] == pytest.approx(config.speed_mm_s)
|
|
|
|
|
|
def test_the_right_trigger_lifts_and_the_left_one_lowers(config):
|
|
up = mapping.velocity(stick(axes=[0, 0, -1.0, 0, 0, 1.0]).axes, [], config)
|
|
down = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, -1.0]).axes, [], config)
|
|
|
|
assert up[VZ] == pytest.approx(config.speed_mm_s)
|
|
assert down[VZ] == pytest.approx(-config.speed_mm_s)
|
|
|
|
|
|
def test_both_triggers_together_cancel(config):
|
|
velocity = mapping.velocity(stick(axes=[0, 0, 1.0, 0, 0, 1.0]).axes, [], config)
|
|
|
|
assert velocity[VZ] == 0.0
|
|
|
|
|
|
def test_an_untouched_trigger_does_not_read_as_half_pressed(config):
|
|
# SDL reports a trigger from -1 to 1, and one that has never moved sometimes
|
|
# reads 0 until it does. Treating that as half travel would lift the arm.
|
|
assert mapping.trigger(-1.0) == 0.0
|
|
assert mapping.trigger(0.0) == pytest.approx(0.5)
|
|
assert mapping.trigger(1.0) == 1.0
|
|
|
|
|
|
def test_the_right_stick_turns_the_tool_in_degrees_per_second(config):
|
|
velocity = mapping.velocity(stick(axes=[0, 0, -1, 1.0, -1.0, -1]).axes, [], config)
|
|
|
|
assert velocity[WX] == pytest.approx(config.speed_deg_s)
|
|
assert velocity[WY] == pytest.approx(config.speed_deg_s)
|
|
|
|
|
|
def test_the_bumpers_yaw_in_opposite_directions(config):
|
|
right = mapping.velocity(stick().axes, [False] * 5 + [True], config)
|
|
left = mapping.velocity(stick().axes, [False] * 4 + [True, False], config)
|
|
|
|
assert right[WZ] == pytest.approx(config.speed_deg_s)
|
|
assert left[WZ] == pytest.approx(-config.speed_deg_s)
|
|
|
|
|
|
def test_several_axes_move_at_once(config):
|
|
# The whole reason a pad beats the console's four-sector dial: the dial can
|
|
# only ever ask for one axis.
|
|
velocity = mapping.velocity(stick(axes=[1.0, -1.0, -1, 0, 0, 1.0]).axes, [], config)
|
|
|
|
assert velocity[VX] > 0
|
|
assert velocity[VY] > 0
|
|
assert velocity[VZ] > 0
|
|
|
|
|
|
def test_drift_inside_the_deadzone_is_ignored(config):
|
|
# A worn stick that does not centre would otherwise drive the arm across the
|
|
# table with nobody touching it.
|
|
velocity = mapping.velocity(stick(axes=[config.deadzone / 2, 0.0]).axes, [], config)
|
|
|
|
assert velocity == [0.0] * 6
|
|
|
|
|
|
def test_motion_starts_from_zero_just_outside_the_deadzone(config):
|
|
# Without rescaling, the arm would jump to a fraction of full speed the
|
|
# instant the stick left the dead band.
|
|
just_outside = mapping.deadzone(config.deadzone + 1e-9, config.deadzone)
|
|
|
|
assert just_outside == pytest.approx(0.0, abs=1e-6)
|
|
|
|
|
|
def test_the_deadzone_still_reaches_full_speed_at_full_deflection(config):
|
|
assert mapping.deadzone(1.0, config.deadzone) == pytest.approx(1.0)
|
|
assert mapping.deadzone(-1.0, config.deadzone) == pytest.approx(-1.0)
|
|
|
|
|
|
def test_expo_slows_small_deflections_without_capping_large_ones():
|
|
assert mapping.expo(0.5, 0.0) == pytest.approx(0.5)
|
|
assert mapping.expo(0.5, 1.0) == pytest.approx(0.125)
|
|
assert mapping.expo(1.0, 1.0) == pytest.approx(1.0)
|
|
assert mapping.expo(-0.5, 1.0) == pytest.approx(-0.125)
|
|
|
|
|
|
def test_a_pad_reporting_fewer_axes_is_read_as_centred_not_as_an_error(config):
|
|
velocity = mapping.velocity([0.0, 0.0], [], config)
|
|
|
|
assert velocity[VX] == 0.0
|
|
assert velocity[WZ] == 0.0
|
|
|
|
|
|
# --- is the pad being used at all ---
|
|
|
|
|
|
def test_a_pad_at_rest_is_not_asking_for_the_arm():
|
|
# SDL rests a trigger at -1.0, so a raw threshold reports every connected
|
|
# pad as in use for ever: the pad claims the arm the moment it is plugged
|
|
# in and GAMEPAD_IDLE_RELEASE_S never fires.
|
|
assert mapping.is_active(stick()) is False
|
|
|
|
|
|
def test_an_absent_pad_is_not_asking_for_the_arm():
|
|
assert mapping.is_active(stick(present=False)) is False
|
|
|
|
|
|
def test_a_nudged_stick_is_asking_for_the_arm():
|
|
assert mapping.is_active(stick(axes=[0.5])) is True
|
|
|
|
|
|
def test_a_held_trigger_is_asking_for_the_arm():
|
|
assert mapping.is_active(stick(axes=[0.0, 0.0, 1.0])) is True
|
|
|
|
|
|
def test_a_held_button_is_asking_for_the_arm():
|
|
assert mapping.is_active(stick(buttons=[True])) is True
|
|
|
|
|
|
def test_an_axis_the_layout_does_not_use_still_counts():
|
|
# A pad reporting its d-pad as two extra axes is asking for the arm when
|
|
# somebody presses it, whether or not this node maps it to anything.
|
|
assert mapping.is_active(stick(axes=[0.0, 0.0, -1.0, 0.0, 0.0, -1.0, 1.0])) is True
|