add the axis mapping
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.
This commit is contained in:
@@ -0,0 +1,110 @@
|
||||
"""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
|
||||
Reference in New Issue
Block a user