frontier_code/tests/test_frontier.py

"""Tests for the Frontier F01 control lab.

Run from frontier_code/:
    python -m unittest discover -s tests -p "test_*.py" -v
Test your own starter implementation instead of the reference:
    MANUAL_IMPL=starters python -m unittest discover -s tests -p "test_*.py" -v
(PowerShell: $env:MANUAL_IMPL="starters"; python -m unittest discover -s tests -p "test_*.py" -v)
"""

import importlib
import math
import os
import unittest

PACKAGE = os.environ.get("MANUAL_IMPL", "solutions")
lab = importlib.import_module(f"{PACKAGE}.control_lab")

MASS = 1.5


class PIDTests(unittest.TestCase):
    def test_proportional_only_output(self):
        pid = lab.PID(kp=2.0, ki=0.0, kd=0.0)
        self.assertAlmostEqual(pid.update(10.0, 7.0, 0.01), 6.0)

    def test_no_derivative_kick_on_setpoint_jump(self):
        # Measurement is steady; only the target jumps. Derivative-on-measurement
        # must contribute nothing, so the output change is purely P + I.
        pid = lab.PID(kp=1.0, ki=0.0, kd=50.0)
        pid.update(0.0, 5.0, 0.01)
        out = pid.update(10.0, 5.0, 0.01)
        self.assertAlmostEqual(out, 5.0)

    def test_derivative_opposes_rising_measurement(self):
        pid = lab.PID(kp=0.0, ki=0.0, kd=2.0)
        pid.update(0.0, 0.0, 0.1)
        self.assertAlmostEqual(pid.update(0.0, 0.5, 0.1), -10.0)

    def test_output_is_clamped(self):
        pid = lab.PID(kp=100.0, ki=0.0, kd=0.0, out_min=-1.0, out_max=1.0)
        self.assertEqual(pid.update(10.0, 0.0, 0.01), 1.0)
        self.assertEqual(pid.update(-10.0, 0.0, 0.01), -1.0)

    def test_integral_frozen_while_saturated_in_error_direction(self):
        pid = lab.PID(kp=10.0, ki=1.0, kd=0.0, out_min=0.0, out_max=1.0)
        for _ in range(100):
            pid.update(10.0, 0.0, 0.01)
        self.assertEqual(pid.integral, 0.0)

    def test_rejects_nonpositive_dt(self):
        with self.assertRaises(ValueError):
            lab.PID(1.0, 0.0, 0.0).update(1.0, 0.0, 0.0)


class HoverTests(unittest.TestCase):
    def test_p_only_steady_state_error_matches_theory(self):
        # At rest, thrust must equal weight: kp * e = m g, so e = m g / kp.
        for kp in (5.0, 10.0):
            s = lab.simulate_hover(kp, 0.0, 3.0, mass=MASS, duration=30.0)
            m = lab.step_metrics(s, 10.0)
            self.assertAlmostEqual(m.steady_state_error, MASS * lab.G / kp, delta=0.01)

    def test_feedforward_removes_error_when_mass_is_known(self):
        s = lab.simulate_hover(4.0, 0.0, 3.0, mass=MASS, feedforward_mass=MASS, duration=15.0)
        self.assertLess(abs(lab.step_metrics(s, 10.0).steady_state_error), 0.005)

    def test_unknown_payload_leaves_error_without_integral(self):
        s = lab.simulate_hover(4.0, 0.0, 3.0, mass=MASS, feedforward_mass=MASS, payload=0.3, duration=15.0)
        self.assertAlmostEqual(lab.step_metrics(s, 10.0).steady_state_error, 0.3 * lab.G / 4.0, delta=0.01)

    def test_integral_removes_payload_error(self):
        s = lab.simulate_hover(4.0, 1.0, 3.0, mass=MASS, feedforward_mass=MASS, payload=0.3, duration=20.0)
        self.assertLess(abs(lab.step_metrics(s, 10.0).steady_state_error), 0.02)

    def test_thrust_stays_within_motor_limits(self):
        s = lab.simulate_hover(50.0, 5.0, 1.0, mass=MASS, feedforward_mass=MASS, setpoint=40.0, duration=10.0)
        t_max = 2.0 * MASS * lab.G
        self.assertTrue(all(-1e-9 <= x.thrust <= t_max + 1e-9 for x in s))

    def test_anti_windup_reduces_overshoot(self):
        kwargs = dict(mass=MASS, feedforward_mass=MASS, setpoint=50.0, duration=30.0)
        with_aw = lab.step_metrics(lab.simulate_hover(6.0, 2.0, 4.0, anti_windup=True, **kwargs), 50.0)
        without = lab.step_metrics(lab.simulate_hover(6.0, 2.0, 4.0, anti_windup=False, **kwargs), 50.0)
        self.assertLess(with_aw.overshoot_pct, 20.0)
        self.assertGreater(without.overshoot_pct, 3.0 * with_aw.overshoot_pct)

    def test_noise_is_reproducible_with_seed(self):
        a = lab.simulate_hover(4.0, 1.0, 3.0, feedforward_mass=MASS, noise_std=0.05, seed=7, duration=2.0)
        b = lab.simulate_hover(4.0, 1.0, 3.0, feedforward_mass=MASS, noise_std=0.05, seed=7, duration=2.0)
        self.assertEqual([x.z for x in a], [x.z for x in b])

    def test_finite_difference_amplifies_noise(self):
        # Noise of 1 cm on position becomes ~1.4 m/s of noise in a 0.01 s
        # backward difference: sqrt(2) * sigma / dt.
        import random
        rng = random.Random(1)
        dt, sigma = 0.01, 0.01
        noisy = [rng.gauss(0.0, sigma) for _ in range(20000)]
        d = lab.finite_difference(noisy, dt)[1:]
        std = math.sqrt(sum(v * v for v in d) / len(d))
        self.assertAlmostEqual(std, math.sqrt(2.0) * sigma / dt, delta=0.05)


class CarTests(unittest.TestCase):
    def test_constant_steer_traces_circle_of_radius_L_over_tan_delta(self):
        wheelbase, steer = 2.9, math.radians(5.0)
        radius = wheelbase / math.tan(steer)
        s = lab.CarState(0.0, 0.0, 0.0, 0.0)
        for _ in range(3000):
            s = lab.bicycle_step(s, 5.0, steer, wheelbase, 0.01)
            self.assertAlmostEqual(math.hypot(s.x, s.y - radius), radius, delta=0.1)

    def test_pure_pursuit_curvature_formula(self):
        # A car at the origin heading +x; target on the circle of radius R that is
        # tangent to the heading. Steering must reproduce that circle.
        wheelbase, radius = 2.9, 25.0
        theta = 0.4  # angle travelled along the circle to reach the target
        target = (radius * math.sin(theta), radius - radius * math.cos(theta))
        delta = lab.pure_pursuit_steer(lab.CarState(0.0, 0.0, 0.0, 0.0), target, wheelbase)
        self.assertAlmostEqual(wheelbase / math.tan(delta), radius, places=9)

    def test_converges_to_straight_line(self):
        _, err = lab.simulate_pure_pursuit(lab.straight_path(), start=lab.CarState(0.0, 2.0, 0.0, 0.0),
                                           lookahead=8.0, duration=20.0)
        self.assertLess(err[-1], 0.01)

    def test_longer_lookahead_converges_slower_but_cuts_more(self):
        def first_settled(errors):
            return next(i for i, e in enumerate(errors) if all(x < 0.1 for x in errors[i:]))
        start = lab.CarState(0.0, 2.0, 0.0, 0.0)
        _, short = lab.simulate_pure_pursuit(lab.straight_path(), start=start, lookahead=4.0)
        _, long = lab.simulate_pure_pursuit(lab.straight_path(), start=start, lookahead=16.0)
        self.assertLess(first_settled(short), first_settled(long))
        _, lc_short = lab.simulate_pure_pursuit(lab.lane_change_path(), start=lab.CarState(0, 0, 0, 0),
                                                lookahead=4.0, duration=18.0)
        _, lc_long = lab.simulate_pure_pursuit(lab.lane_change_path(), start=lab.CarState(0, 0, 0, 0),
                                               lookahead=16.0, duration=18.0)
        self.assertLess(max(lc_short), max(lc_long))

    def test_tracks_a_looping_path(self):
        # Regression: a global nearest-point search jumps to the second lap.
        states, err = lab.simulate_pure_pursuit(lab.circle_path(30.0, turns=1.5),
                                                start=lab.CarState(0.0, 0.0, 0.0, 0.0),
                                                lookahead=8.0, duration=25.0)
        self.assertLess(max(err), 0.1)

    def test_slow_steering_plus_short_lookahead_rings(self):
        start = lab.CarState(0.0, 2.0, 0.0, 0.0)
        states, _ = lab.simulate_pure_pursuit(lab.straight_path(600.0), start=start, lookahead=7.0,
                                              speed=20.0, max_steer_rate=0.5, duration=12.0)
        ys = [s.y for s in states]
        crossings = sum(1 for a, b in zip(ys, ys[1:]) if a * b < 0)
        self.assertGreater(crossings, 5)


if __name__ == "__main__":
    unittest.main()