cristhian
7a0847893c
An Xbox One S pad over Bluetooth numbers its controls differently from the indices this node shipped with: the right stick is on axes 2 and 3, the triggers on 4 and 5, and the buttons carry gaps at 2 and 5. Against the old layout an untouched pad reported a resting trigger as full right-stick deflection, which asked for 30 deg/s of pitch while lying on the desk, held the lease for ever so GAMEPAD_IDLE_RELEASE_S never fired, and moved a real arm into a controller error. X toggled freedrive and LB was the E-STOP, so the resulting latch could not be released from the pad at all. Layout now carries the swept indices. Each pad is also measured the first time it is seen at rest: a stick must rest near centre and a trigger near the bottom of its travel, and a pad that does not fit reads as absent and is named in the log rather than driving an arm from a numbering nobody checked. The measured rest becomes each axis's zero, so a worn stick's tilt is subtracted rather than deadzoned away and a trigger that reads 0 until it first moves stops reading as half pressed. Rest is never re-measured while the pad is in use, because doing that under a held stick would record the deflection as centre and the arm would move when the operator let go. The probe drives from the same calibration, so what it prints is still what the arm would get, and it shows the mismatch. Tests name their controls through axes_for and buttons_for instead of numbering them; the positional lists are why a wrong index passed a green suite.
219 lines
7.6 KiB
Python
219 lines
7.6 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 LAYOUT, VX, VY, VZ, WX, WY, WZ
|
|
from tests.conftest import axes_for, buttons_for, 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(axes_for(right_trigger=1.0), [], config)
|
|
down = mapping.velocity(axes_for(left_trigger=1.0), [], 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(axes_for(left_trigger=1.0, right_trigger=1.0), [], 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(axes_for(right_x=1.0, right_y=-1.0), [], 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, buttons_for(yaw_right=True), config)
|
|
left = mapping.velocity(stick().axes, buttons_for(yaw_left=True), 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(axes_for(left_x=1.0, left_y=-1.0, right_trigger=1.0), [], 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=axes_for(left_trigger=1.0))) is True
|
|
|
|
|
|
def test_a_held_button_is_asking_for_the_arm():
|
|
assert mapping.is_active(stick(buttons=buttons_for(grasp=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=axes_for() + [1.0])) is True
|
|
|
|
|
|
# --- reading a pad that is not the layout's ideal one ---
|
|
|
|
|
|
def test_a_resting_pad_measures_as_resting():
|
|
zeros = mapping.rest_zeros(axes_for())
|
|
|
|
assert zeros == axes_for()
|
|
|
|
|
|
def test_a_small_tilt_is_accepted_as_rest():
|
|
# A worn stick is an ordinary pad, not a broken one.
|
|
assert mapping.rest_zeros(axes_for(left_x=0.2, right_y=-0.2))
|
|
|
|
|
|
def test_an_axis_resting_where_the_layout_says_it_cannot_is_refused():
|
|
wrong = axes_for()
|
|
wrong[LAYOUT.right_y] = -1.0
|
|
|
|
with pytest.raises(ValueError, match="right stick Y"):
|
|
mapping.rest_zeros(wrong)
|
|
|
|
|
|
def test_the_refusal_names_the_index_and_what_it_read():
|
|
# It is read off a terminal by somebody deciding what to put in `Layout`.
|
|
wrong = axes_for()
|
|
wrong[LAYOUT.right_x] = 0.9
|
|
|
|
with pytest.raises(ValueError, match=r"axis 2 rests at \+0.900"):
|
|
mapping.rest_zeros(wrong)
|
|
|
|
|
|
def test_a_trigger_resting_at_either_end_is_accepted():
|
|
# -1 is what SDL reports; 0 is what some pads report until it first moves.
|
|
assert mapping.rest_zeros(axes_for(left_trigger=-1.0, right_trigger=0.0))
|
|
|
|
|
|
def test_an_axis_the_layout_does_not_use_still_has_to_rest():
|
|
# A d-pad reported as two extra axes counts towards `is_active`, so one
|
|
# stuck at an extreme would hold the arm as surely as a named one.
|
|
with pytest.raises(ValueError, match="axis 6"):
|
|
mapping.rest_zeros(axes_for() + [1.0])
|
|
|
|
|
|
def test_canonical_puts_a_tilted_stick_back_at_centre():
|
|
tilted = axes_for(left_x=0.2)
|
|
|
|
canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted))
|
|
|
|
assert canonical[LAYOUT.left_x] == pytest.approx(0.0)
|
|
|
|
|
|
def test_canonical_puts_a_trigger_back_at_its_own_rest():
|
|
zeroed = axes_for(left_trigger=0.0, right_trigger=0.0)
|
|
|
|
canonical = mapping.canonical(zeroed, mapping.rest_zeros(zeroed))
|
|
|
|
assert canonical[LAYOUT.left_trigger] == pytest.approx(mapping.TRIGGER_REST)
|
|
assert canonical[LAYOUT.right_trigger] == pytest.approx(mapping.TRIGGER_REST)
|
|
|
|
|
|
def test_a_trigger_measured_from_zero_still_reaches_full_travel():
|
|
zeros = mapping.rest_zeros(axes_for(right_trigger=0.0))
|
|
|
|
canonical = mapping.canonical(axes_for(right_trigger=1.0), zeros)
|
|
|
|
assert mapping.trigger(canonical[LAYOUT.right_trigger]) == pytest.approx(1.0)
|
|
|
|
|
|
def test_a_calibrated_pad_at_rest_asks_for_nothing(config):
|
|
tilted = axes_for(left_x=0.2, left_y=-0.15, right_trigger=0.0)
|
|
|
|
canonical = mapping.canonical(tilted, mapping.rest_zeros(tilted))
|
|
|
|
assert mapping.velocity(canonical, [], config) == [0.0] * 6
|