"""Tests for trajectory.py. Run: python -m pytest -q""" from __future__ import annotations import numpy as np import pytest from trajectory import (cubic, quintic, sample, trapezoid, trapezoid_duration, trapezoid_for_duration) def test_cubic_boundary_conditions_and_peak() -> None: q0, qf, T = 0.2, 1.4, 0.8 pos, vel, _ = cubic(q0, qf, T, np.array([0.0, T / 2, T])) assert pos[0] == pytest.approx(q0) and pos[-1] == pytest.approx(qf) assert vel[0] == pytest.approx(0.0, abs=1e-12) and vel[-1] == pytest.approx(0.0, abs=1e-12) assert vel[1] == pytest.approx(1.5 * (qf - q0) / T) # peak at mid-time def test_cubic_with_end_velocities() -> None: _, vel, _ = cubic(0.0, 1.0, 2.0, np.array([0.0, 2.0]), v0=0.3, vf=-0.2) np.testing.assert_allclose(vel, [0.3, -0.2], atol=1e-12) def test_cubic_acceleration_jumps_at_start() -> None: _, _, acc = cubic(0.0, 1.0, 1.0, np.array([-1e-6, 0.0])) assert acc[0] == 0.0 and acc[1] == pytest.approx(6.0) # 6 dq / T^2 step def test_quintic_boundary_conditions_and_peaks() -> None: q0, qf, T = -0.5, 0.7, 1.2 t = np.linspace(0.0, T, 20001) pos, vel, acc = quintic(q0, qf, T, t) assert pos[0] == pytest.approx(q0) and pos[-1] == pytest.approx(qf) for arr in (vel, acc): assert arr[0] == pytest.approx(0.0, abs=1e-9) and arr[-1] == pytest.approx(0.0, abs=1e-9) dq = qf - q0 assert vel.max() == pytest.approx(1.875 * dq / T, rel=1e-6) assert np.abs(acc).max() == pytest.approx(10.0 / np.sqrt(3.0) * dq / T**2, rel=1e-4) def test_velocity_is_derivative_of_position() -> None: t = np.linspace(0.0, 1.0, 100001) for pos, vel, _ in (quintic(0.0, 1.0, 1.0, t), cubic(0.0, 1.0, 1.0, t), trapezoid(0.0, 1.0, 1.5, 4.5, t)): np.testing.assert_allclose(np.gradient(pos, t)[1:-1], vel[1:-1], atol=1e-3) def test_holds_outside_time_range() -> None: pos, vel, acc = quintic(0.0, 1.0, 1.0, np.array([-0.5, 1.5])) np.testing.assert_allclose(pos, [0.0, 1.0]) np.testing.assert_allclose(vel, [0.0, 0.0]) np.testing.assert_allclose(acc, [0.0, 0.0]) def test_trapezoid_with_cruise() -> None: T, ta, vp = trapezoid_duration(1.0, v_max=1.5, a_max=4.5) assert (T, ta, vp) == pytest.approx((1.0, 1.0 / 3.0, 1.5)) t = np.linspace(0.0, T, 30001) pos, vel, acc = trapezoid(0.0, 1.0, 1.5, 4.5, t) assert pos[-1] == pytest.approx(1.0) assert np.trapezoid(vel, t) == pytest.approx(1.0, rel=1e-6) assert vel.max() == pytest.approx(1.5) assert set(np.unique(np.round(acc, 9))) <= {-4.5, 0.0, 4.5} def test_trapezoid_triangular_case_and_reverse_direction() -> None: T, ta, vp = trapezoid_duration(-0.1, v_max=2.0, a_max=4.0) assert vp == pytest.approx(np.sqrt(0.1 * 4.0)) and vp < 2.0 assert T == pytest.approx(2.0 * ta) t = np.linspace(0.0, T, 10001) pos, vel, _ = trapezoid(0.3, 0.2, 2.0, 4.0, t) assert pos[-1] == pytest.approx(0.2) assert vel.min() == pytest.approx(-vp, rel=1e-3) and vel.max() <= 0.0 def test_trapezoid_for_duration_gives_requested_time() -> None: v, a = trapezoid_for_duration(0.0, 2.0, 1.6) T, _, _ = trapezoid_duration(2.0, v, a) assert T == pytest.approx(1.6) def test_sample_ends_at_goal() -> None: pts = sample(lambda t: quintic(0.0, 0.8, 0.5, t), 0.5, rate_hz=30.0) assert pts[0] == pytest.approx(0.0) and pts[-1] == pytest.approx(0.8) assert len(pts) == 16 assert np.all(np.diff(pts) >= 0.0)