diff --git a/openpilot/common/pid.py b/openpilot/common/pid.py index f541baf05c..630b3b5361 100644 --- a/openpilot/common/pid.py +++ b/openpilot/common/pid.py @@ -1,6 +1,8 @@ import numpy as np from collections.abc import Sequence +from openpilot.common.scalar_clip import clip_float64 + Gain = int | float | tuple[Sequence[float], Sequence[float]] | list[list[float]] class PIDController: @@ -52,8 +54,8 @@ class PIDController: test_control = self.p + i + self.d + self.f i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit - self.i = np.clip(i, i_lowerbound, i_upperbound) + self.i = clip_float64(i, i_lowerbound, i_upperbound) control = self.p + self.i + self.d + self.f - self.control = np.clip(control, self.neg_limit, self.pos_limit) + self.control = clip_float64(control, self.neg_limit, self.pos_limit) return self.control diff --git a/openpilot/common/scalar_clip.py b/openpilot/common/scalar_clip.py index 9eb1da46c5..425ef7c284 100644 --- a/openpilot/common/scalar_clip.py +++ b/openpilot/common/scalar_clip.py @@ -14,3 +14,12 @@ def clip_scalar(value, lower, upper): lo < hi and scalar != lo and scalar != hi): return lo if scalar < lo else hi if scalar > hi else scalar return float(np.clip(value, lower, upper)) + + +def clip_float64(value, lower, upper): + """Preserve NumPy's scalar result type while avoiding its scalar dispatch.""" + if ((type(value) is float or type(value) is np.float64) and + (type(lower) is float or type(lower) is np.float64) and + (type(upper) is float or type(upper) is np.float64)): + return np.float64(clip_scalar(value, lower, upper)) + return np.clip(value, lower, upper) diff --git a/openpilot/common/tests/test_pid_scalar_clip.py b/openpilot/common/tests/test_pid_scalar_clip.py new file mode 100644 index 0000000000..7f77739817 --- /dev/null +++ b/openpilot/common/tests/test_pid_scalar_clip.py @@ -0,0 +1,71 @@ +import math +import random +import struct +import warnings + +import numpy as np +import pytest + +from openpilot.common import pid +from openpilot.common.scalar_clip import clip_float64 + + +def outcome(function, *values): + with warnings.catch_warnings(record=True) as seen, np.errstate(all='warn'): + warnings.simplefilter('always') + try: + result = function(*values) + array = np.asarray(result) + value = ('value', type(result), array.dtype.str, array.shape, array.tobytes()) + except Exception as error: + value = ('error', type(error), str(error)) + return value, [(item.category, str(item.message)) for item in seen] + + +def test_float64_clip_exact_bits_types_errors_and_warnings(): + bits = (0, 1, 1 << 63, (1 << 63) + 1, 0x7ff0000000000001, 0xfff0000000000001, + 0x7ff8000000004321, 0xfff8000000004321, 0x7ff0000000000000, 0xfff0000000000000) + values = [struct.unpack('>d', value.to_bytes(8, 'big'))[0] for value in bits] + randomizer = random.Random(20261024) + values += [struct.unpack('>d', randomizer.getrandbits(64).to_bytes(8, 'big'))[0] for _ in range(1500)] + bounds = ((-1.0, 1.0), (-0.0, 1.0), (-1.0, 0.0), (-1.0, -0.0), + (-0.0, 0.0), (0.0, -0.0), (1.0, 1.0), (1.0, -1.0), (-math.inf, math.inf), (math.nan, 1.0)) + for value in values: + for lower, upper in bounds: + for typed in (value, np.float64(value)): + assert outcome(clip_float64, typed, lower, upper) == outcome(np.clip, typed, lower, upper) + + +@pytest.mark.parametrize('value', [0, True, np.int64(1), np.float32(0.125), np.array(0.125), + np.array([0.125, 2.0]), np.array([1, 2]), None, 'bad', 1 + 2j]) +def test_other_domains_keep_original_numpy(value): + for lower, upper in ((0, 1), (-1.0, 1.0), (None, 1.0), (1, 0), ('x', 1)): + assert outcome(clip_float64, value, lower, upper) == outcome(np.clip, value, lower, upper) + + +def state(controller): + return tuple((type(value), np.asarray(value).dtype.str, np.asarray(value).tobytes()) + for value in (controller.p, controller.i, controller.d, controller.f, controller.control)) + + +@pytest.mark.parametrize('gains', [(0.3, 0.05, 0.0), (([0.0, 30.0], [0.2, 0.4]), ([0.0, 30.0], [0.05, 0.1]), 0.01)]) +@pytest.mark.parametrize('limits', [(-1.0, 1.0), (-0.0, 0.0), (0.0, -0.0), (-1e308, 1e308)]) +def test_actual_pid_trace_preserves_outputs_and_integrator_bits(monkeypatch, gains, limits): + baseline = pid.PIDController(*gains, neg_limit=limits[0], pos_limit=limits[1]) + candidate = pid.PIDController(*gains, neg_limit=limits[0], pos_limit=limits[1]) + randomizer = random.Random(20261024) + for index in range(600): + args = {'error': randomizer.uniform(-5.0, 5.0), 'error_rate': randomizer.uniform(-2.0, 2.0), + 'speed': randomizer.uniform(0.0, 40.0), 'feedforward': randomizer.uniform(-1.0, 1.0), + 'freeze_integrator': index % 7 == 0} + monkeypatch.setattr(pid, 'clip_float64', np.clip) + # Both run the actual PID update; only the original NumPy operation differs. + with warnings.catch_warnings(record=True) as original_warnings: + warnings.simplefilter('always') + baseline.update(**args) + monkeypatch.setattr(pid, 'clip_float64', clip_float64) + with warnings.catch_warnings(record=True) as candidate_warnings: + warnings.simplefilter('always') + candidate.update(**args) + assert state(candidate) == state(baseline) + assert [(w.category, str(w.message)) for w in candidate_warnings] == [(w.category, str(w.message)) for w in original_warnings] diff --git a/prebuilt.json b/prebuilt.json index 575c6661e0..de0725d0ca 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "0fdb25f5deda7ee4e6ae2fd8afbff774c5c2260bc9dce6afa5dc0e95186b2aaf", "schema_version": 1} +{"protected_inventory_sha256": "412c4b2720b3ed3cedd39ea021157f1870a96c0745189de872c9d8eef686d802", "schema_version": 1}