tests/test_motion_profile.py

"""Tests for fpgm.motion.profile.trapezoidal_profile -- pure scalar math, data-free.

No URDF, no robot: every check here is against the closed-form trapezoidal
schedule on the shared parameter ``s`` alone.
"""

from __future__ import annotations

import numpy as np
import pytest

from fpgm.motion.profile import ProfileConfig, trapezoidal_profile

CFG = ProfileConfig()  # a_max=0.5, omega_max=0.8, alpha_max=2.0, rate_hz=15.0


class TestGridSnapping:
    @pytest.mark.parametrize(
        "length_m,angle_rad,v_max",
        [
            (0.3, 0.0, 0.15),
            (0.0, 1.2, 0.15),
            (0.05, 0.3, 0.2),
            (1.5, 0.0, 0.08),
            (0.4, 2.5, 0.25),
        ],
    )
    def test_timestamps_are_exact_grid_multiples(self, length_m, angle_rad, v_max):
        """np.diff(timestamps) is exactly 1/15 everywhere -- constructed as
        k/15 for integer k, never by accumulating += dt, so there is zero
        float drift for segments to concatenate against.
        """
        profile = trapezoidal_profile(length_m, angle_rad, v_max, CFG)
        diffs = np.diff(profile.timestamps)
        assert np.all(diffs == pytest.approx(1.0 / CFG.rate_hz, abs=0.0, rel=1e-12))
        # Constructed directly from the tick index (arange(n+1) * dt) rather
        # than accumulated, so this must be exact -- matching the same
        # dt=1/rate_hz*index expression bit-for-bit, not merely close.
        dt = 1.0 / CFG.rate_hz
        expected = np.arange(profile.timestamps.shape[0], dtype=np.float64) * dt
        assert np.array_equal(profile.timestamps, expected)


class TestBoundaryAndMonotonicity:
    @pytest.mark.parametrize(
        "length_m,angle_rad,v_max",
        [
            (0.3, 0.0, 0.15),
            (0.0, 1.2, 0.15),
            (0.05, 0.3, 0.2),
            (1.5, 0.0, 0.08),
            (0.4, 2.5, 0.25),
            (0.02, 0.0, 0.15),  # short -> triangular
        ],
    )
    def test_s_boundaries_monotone_and_derivative_bounds(self, length_m, angle_rad, v_max):
        profile = trapezoidal_profile(length_m, angle_rad, v_max, CFG)

        assert profile.s[0] == 0.0
        assert profile.s[-1] == 1.0
        assert np.all(np.diff(profile.s) >= -1e-12)  # monotone non-decreasing
        assert np.all(profile.s_dot <= profile.s_dot_max + 1e-12)
        assert np.all(profile.s_dot >= -1e-12)
        assert np.all(np.abs(profile.s_ddot) <= profile.s_ddot_max + 1e-12)
        # Point-to-point: starts and stops at rest.
        assert profile.s_dot[0] == 0.0
        assert profile.s_dot[-1] == 0.0


class TestTriangularVsTrapezoidal:
    def test_short_segment_is_triangular(self):
        """A short move relative to a_max/v_max never reaches cruise speed."""
        profile = trapezoidal_profile(0.02, 0.0, 0.15, CFG)
        assert profile.is_triangular

    def test_long_segment_is_trapezoidal(self):
        """A long move reaches and holds cruise speed."""
        profile = trapezoidal_profile(1.5, 0.0, 0.15, CFG)
        assert not profile.is_triangular

    def test_branch_boundary_matches_closed_form_condition(self):
        """s_dot_max**2 / s_ddot_max >= 1 is exactly the triangular condition
        (see the module docstring's derivation): pick lengths straddling the
        threshold length L* where v_max**2/(a_max*L*) == 1.
        """
        v_max, a_max = 0.15, CFG.a_max
        # s_dot_max = v_max/L, s_ddot_max = a_max/L -> ratio = v_max**2/(a_max*L)
        l_star = v_max**2 / a_max
        just_short = trapezoidal_profile(l_star * 0.9, 0.0, v_max, CFG)
        just_long = trapezoidal_profile(l_star * 1.1, 0.0, v_max, CFG)
        assert just_short.is_triangular
        assert not just_long.is_triangular


class TestDegenerateCases:
    def test_zero_length_segment_single_tick_no_nan(self):
        """Zero translation and zero rotation: no division by zero, one grid tick."""
        profile = trapezoidal_profile(0.0, 0.0, 0.15, CFG)
        assert profile.n_ticks == 1
        assert not np.any(np.isnan(profile.s))
        assert not np.any(np.isnan(profile.s_dot))
        assert not np.any(np.isnan(profile.s_ddot))
        assert profile.s[-1] == 1.0

    def test_pure_rotation_no_nan(self):
        """L=0, Theta>0: the v_max/L and a_max/L terms must be skipped, not
        evaluated as inf/nan.
        """
        profile = trapezoidal_profile(0.0, 1.0, 0.15, CFG)
        assert not np.any(np.isnan(profile.s))
        assert not np.any(np.isnan(profile.s_dot))
        assert not np.any(np.isnan(profile.s_ddot))
        assert profile.s_dot_max > 0.0
        assert profile.s[0] == 0.0 and profile.s[-1] == 1.0

    def test_pure_translation_no_nan(self):
        """Theta=0, L>0: symmetric to the pure-rotation case."""
        profile = trapezoidal_profile(0.5, 0.0, 0.15, CFG)
        assert not np.any(np.isnan(profile.s))
        assert not np.any(np.isnan(profile.s_dot))
        assert not np.any(np.isnan(profile.s_ddot))
        assert profile.s_dot_max > 0.0
        assert profile.s[0] == 0.0 and profile.s[-1] == 1.0


class TestMinTicksForcesSlowerNeverFaster:
    def test_forcing_more_ticks_only_stretches(self):
        """min_ticks forces a segment to take longer (the retiming mechanism
        TrajectoryPlanner reuses) -- never shorter, and never violates the
        original limits (see the module docstring's stretch argument).
        """
        base = trapezoidal_profile(0.3, 0.0, 0.15, CFG)
        stretched = trapezoidal_profile(0.3, 0.0, 0.15, CFG, min_ticks=base.n_ticks * 3)
        assert stretched.n_ticks == base.n_ticks * 3
        assert stretched.duration > base.duration
        assert stretched.s_dot_max <= base.s_dot_max + 1e-12
        assert stretched.s_ddot_max <= base.s_ddot_max + 1e-12
        assert np.all(stretched.s_dot <= CFG.omega_max / 1.0 + 1)  # sanity: finite, not exploded