Johnny Fernandes
85 lines
2.8 KiB
Python
85 lines
2.8 KiB
Python
"""Differential-drive kinematics and the (vx, vy) → wheel-speed map."""
|
|
|
|
import math
|
|
|
|
import pytest
|
|
|
|
from herding.world.diffdrive import (
|
|
heading_speed_to_wheels, kinematics_step, velocity_to_wheels,
|
|
)
|
|
|
|
|
|
WHEEL_R = 0.038
|
|
WHEEL_B = 0.28
|
|
MAX_OMEGA = 70.0
|
|
MAX_LINEAR = WHEEL_R * MAX_OMEGA
|
|
DT = 0.016
|
|
|
|
|
|
def test_kinematics_zero_input_is_identity():
|
|
x, y, h = kinematics_step(1.0, 2.0, 0.5, 0.0, 0.0, WHEEL_R, WHEEL_B, DT)
|
|
assert (x, y, h) == (1.0, 2.0, 0.5)
|
|
|
|
|
|
def test_kinematics_forward_motion():
|
|
# Equal wheel speeds → pure translation along the heading.
|
|
x, y, h = kinematics_step(0.0, 0.0, 0.0, 10.0, 10.0, WHEEL_R, WHEEL_B, DT)
|
|
assert h == 0.0
|
|
assert math.isclose(x, 10.0 * WHEEL_R * DT)
|
|
assert y == 0.0
|
|
|
|
|
|
def test_kinematics_pure_rotation():
|
|
# Opposite wheel speeds → pure rotation, position unchanged.
|
|
x, y, h = kinematics_step(0.0, 0.0, 0.0, -5.0, 5.0, WHEEL_R, WHEEL_B, DT)
|
|
assert (x, y) == (0.0, 0.0)
|
|
assert h > 0.0
|
|
|
|
|
|
def test_kinematics_heading_wrapped_to_pi():
|
|
_, _, h = kinematics_step(0.0, 0.0, math.pi - 0.01, 100.0, -100.0,
|
|
WHEEL_R, WHEEL_B, DT)
|
|
assert -math.pi <= h <= math.pi
|
|
|
|
|
|
def test_velocity_to_wheels_zero_velocity():
|
|
left, right = velocity_to_wheels(0.0, 0.0, 0.0,
|
|
MAX_LINEAR, WHEEL_R, MAX_OMEGA)
|
|
assert (left, right) == (0.0, 0.0)
|
|
|
|
|
|
def test_velocity_to_wheels_aligned_forward():
|
|
# Target straight ahead → equal positive wheel speeds.
|
|
left, right = velocity_to_wheels(1.0, 0.0, 0.0,
|
|
MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=4.0)
|
|
assert math.isclose(left, right, abs_tol=1e-6)
|
|
assert left > 0.0
|
|
|
|
|
|
def test_velocity_to_wheels_perpendicular_target_spins():
|
|
# Target 90° from heading → forward speed ≈ 0, wheels equal-and-opposite.
|
|
left, right = velocity_to_wheels(0.0, 1.0, 0.0,
|
|
MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=4.0)
|
|
assert left + right == pytest.approx(0.0, abs=1e-6)
|
|
assert right > 0.0 # turning CCW (left of heading is +y for h=0)
|
|
|
|
|
|
def test_velocity_to_wheels_clamped_to_max_omega():
|
|
# Far overshoot — both wheel commands clamped at ±MAX_OMEGA.
|
|
left, right = velocity_to_wheels(-1.0, 0.0, 0.0,
|
|
MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=20.0)
|
|
assert -MAX_OMEGA <= left <= MAX_OMEGA
|
|
assert -MAX_OMEGA <= right <= MAX_OMEGA
|
|
|
|
|
|
def test_heading_speed_to_wheels_aligned():
|
|
left, right = heading_speed_to_wheels(0.0, 10.0, 0.0, MAX_OMEGA)
|
|
assert math.isclose(left, right, abs_tol=1e-6)
|
|
assert left > 0.0
|
|
|
|
|
|
def test_heading_speed_to_wheels_reverse_target_forwards_zero():
|
|
left, right = heading_speed_to_wheels(math.pi, 10.0, 0.0, MAX_OMEGA)
|
|
# cos(π) clamped at 0 → no forward; pure rotation.
|
|
assert left + right == pytest.approx(0.0, abs=1e-6)
|