Optimize scalar PID clipping

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

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)

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]

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "0fdb25f5deda7ee4e6ae2fd8afbff774c5c2260bc9dce6afa5dc0e95186b2aaf", "schema_version": 1}
+{"protected_inventory_sha256": "412c4b2720b3ed3cedd39ea021157f1870a96c0745189de872c9d8eef686d802", "schema_version": 1}