"""Tests for pid.py. Run: python -m pytest -q""" from __future__ import annotations import numpy as np import pytest from pid import PID, JointPlant, pd_gains, simulate, step_metrics DT = 0.001 J, B = 0.01, 0.1 def test_plant_reaches_terminal_speed() -> None: plant = JointPlant(inertia=J, damping=B) for _ in range(20000): # 2 s at 0.1 ms plant.step(0.05, 1e-4) assert plant.omega == pytest.approx(0.05 / B, rel=1e-3) def test_p_control_matches_second_order_overshoot() -> None: kp = 1.0 # zeta = b / (2 sqrt(kp J)) = 0.5 zeta = B / (2.0 * np.sqrt(kp * J)) expected = 100.0 * np.exp(-np.pi * zeta / np.sqrt(1.0 - zeta**2)) res = simulate(PID(kp=kp, ki=0.0, kd=0.0, dt=DT), JointPlant(J, B), 0.5, 2.0) assert step_metrics(res)["overshoot_pct"] == pytest.approx(expected, abs=1.0) def test_pd_critical_damping_has_no_overshoot() -> None: kp, kd = pd_gains(J, B, omega_n=20.0, zeta=1.0) assert (kp, kd) == pytest.approx((4.0, 0.3)) res = simulate(PID(kp=kp, ki=0.0, kd=kd, dt=DT), JointPlant(J, B), 0.5, 1.5) m = step_metrics(res) assert m["overshoot_pct"] < 0.5 assert abs(m["steady_state_error"]) < 1e-4 def test_p_control_has_load_offset_and_pid_removes_it() -> None: load = 0.05 res_p = simulate(PID(kp=4.0, ki=0.0, kd=0.3, dt=DT), JointPlant(J, B, tau_load=load), 0.5, 2.0) assert step_metrics(res_p)["steady_state_error"] == pytest.approx(load / 4.0, rel=0.02) res_pid = simulate(PID(kp=4.0, ki=20.0, kd=0.3, dt=DT), JointPlant(J, B, tau_load=load), 0.5, 2.0) assert abs(step_metrics(res_pid)["steady_state_error"]) < 1e-3 def test_output_respects_limits() -> None: c = PID(kp=4.0, ki=20.0, kd=0.3, dt=DT, u_min=-0.3, u_max=0.3) res = simulate(c, JointPlant(J, B, tau_load=0.05), 1.0, 3.0) assert np.all(np.abs(res.u) <= 0.3 + 1e-12) @pytest.mark.parametrize("mode", ["clamp", "backcalc"]) def test_anti_windup_cuts_overshoot(mode: str) -> None: def run(aw: str) -> dict[str, float]: c = PID(kp=4.0, ki=20.0, kd=0.3, dt=DT, u_min=-0.3, u_max=0.3, anti_windup=aw) return step_metrics(simulate(c, JointPlant(J, B, tau_load=0.05), 1.0, 3.0)) wound, protected = run("none"), run(mode) assert wound["overshoot_pct"] > 50.0 assert protected["overshoot_pct"] < 10.0 assert protected["settling_time"] < wound["settling_time"] def test_derivative_on_measurement_has_no_setpoint_kick() -> None: meas = PID(kp=2.0, ki=0.0, kd=0.5, dt=DT, derivative_on_measurement=True) err = PID(kp=2.0, ki=0.0, kd=0.5, dt=DT, derivative_on_measurement=False) for c in (meas, err): c.update(0.0, 0.0) # settle at zero u_meas = meas.update(1.0, 0.0) # setpoint steps to 1 u_err = err.update(1.0, 0.0) assert u_meas == pytest.approx(2.0) # only the P term assert u_err == pytest.approx(2.0 + 0.5 / DT) # D term spikes to 500 def test_derivative_filter_smooths_noise() -> None: def u_std(tau_d: float) -> float: c = PID(kp=4.0, ki=0.0, kd=0.3, dt=DT, tau_d=tau_d) res = simulate(c, JointPlant(J, B), 0.0, 1.0, measurement_noise=1e-3, seed=7) return float(np.std(res.u)) assert u_std(0.005) < 0.3 * u_std(0.0) def test_noise_is_reproducible_with_seed() -> None: a = simulate(PID(kp=4.0, ki=0.0, kd=0.3, dt=DT), JointPlant(J, B), 0.2, 0.5, measurement_noise=1e-3, seed=3) b = simulate(PID(kp=4.0, ki=0.0, kd=0.3, dt=DT), JointPlant(J, B), 0.2, 0.5, measurement_noise=1e-3, seed=3) np.testing.assert_array_equal(a.u, b.u) def test_reset_clears_state() -> None: c = PID(kp=1.0, ki=10.0, kd=0.1, dt=DT) for _ in range(100): c.update(1.0, 0.0) assert c.integral > 0.0 c.reset() assert c.integral == 0.0 assert c.update(1.0, 0.0) == pytest.approx(1.0 + 10.0 * DT) def test_feedforward_and_derivative_on_error_track_a_quintic() -> None: from trajectory import quintic load, T = 0.05, 0.5 def ref(t: float) -> float: return float(quintic(0.0, 0.5, T, np.array(t))[0]) def ff(t: float) -> float: _, v, a = quintic(0.0, 0.5, T, np.array(t)) return float(J * a + B * v) def max_error(on_measurement: bool, feedforward) -> float: c = PID(kp=4.0, ki=20.0, kd=0.3, dt=DT, derivative_on_measurement=on_measurement) c.integral = load # start in equilibrium against the load res = simulate(c, JointPlant(J, B, tau_load=load), ref, 1.5, feedforward=feedforward) return float(np.max(np.abs(res.theta - res.setpoint))) assert max_error(False, ff) < 1e-3 # 0.2 mrad in practice assert max_error(True, None) > 0.1 # D on measurement brakes the motion