diff --git a/sunnypilot/common/utils.py b/sunnypilot/common/utils.py new file mode 100644 index 0000000000..d790f7edb4 --- /dev/null +++ b/sunnypilot/common/utils.py @@ -0,0 +1,52 @@ +import numpy as np + +def compute_symmetric_slopes(x, y): + """ + Compute symmetric slopes for use in Hermite interpolation. + + Args: + x: Array of x coordinates + y: Array of y coordinates + + Returns: + Array of computed slopes at each point + """ + n = len(x) + m = np.zeros(n) + for i in range(n): + if i == 0: + m[i] = (y[i+1] - y[i]) / (x[i+1] - x[i]) + elif i == n-1: + m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1]) + else: + m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2 + return m + +def hermite_interpolate(x, xp, yp, slopes): + """ + Perform Hermite interpolation at point x given data points and slopes. + + Args: + x: Point at which to interpolate + xp: Array of x coordinates of data points + yp: Array of y coordinates of data points + slopes: Array of slopes at data points + + Returns: + Interpolated value at x + """ + x = np.clip(x, xp[0], xp[-1]) + idx = np.searchsorted(xp, x) - 1 + idx = np.clip(idx, 0, len(slopes) - 2) + + x0, x1 = xp[idx], xp[idx+1] + y0, y1 = yp[idx], yp[idx+1] + m0, m1 = slopes[idx], slopes[idx+1] + + t = (x - x0) / (x1 - x0) + h00 = 2*t**3 - 3*t**2 + 1 + h10 = t**3 - 2*t**2 + t + h01 = -2*t**3 + 3*t**2 + h11 = t**3 - t**2 + + return (h00 * y0) + (h10 * (x1 - x0) * m0) + (h01 * y1) + (h11 * (x1 - x0) * m1) \ No newline at end of file diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_personality_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_personality_controller.py index 911624ab46..af5ab29d23 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_personality_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_personality_controller.py @@ -1,5 +1,5 @@ from cereal import log -import numpy as np +from openpilot.sunnypilot.common.utils import compute_symmetric_slopes, hermite_interpolate class DynamicPersonalityController: """ @@ -10,35 +10,6 @@ class DynamicPersonalityController: def __init__(self): pass - def compute_symmetric_slopes(self, x, y): - n = len(x) - m = np.zeros(n) - for i in range(n): - if i == 0: - m[i] = (y[i+1] - y[i]) / (x[i+1] - x[i]) - elif i == n-1: - m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1]) - else: - m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2 - return m - - def hermite_interpolate(self, x, xp, yp, slopes): - x = np.clip(x, xp[0], xp[-1]) - idx = np.searchsorted(xp, x) - 1 - idx = np.clip(idx, 0, len(slopes) - 2) - - x0, x1 = xp[idx], xp[idx+1] - y0, y1 = yp[idx], yp[idx+1] - m0, m1 = slopes[idx], slopes[idx+1] - - t = (x - x0) / (x1 - x0) - h00 = 2*t**3 - 3*t**2 + 1 - h10 = t**3 - 2*t**2 + t - h01 = -2*t**3 + 3*t**2 - h11 = t**3 - t**2 - - return (h00 * y0) + (h10 * (x1 - x0) * m0) + (h01 * y1) + (h11 * (x1 - x0) * m1) - def get_dynamic_follow_distance(self, v_ego, personality=log.LongitudinalPersonality.standard): """ Calculate the dynamic follow distance based on current speed and personality. @@ -62,7 +33,7 @@ class DynamicPersonalityController: else: raise NotImplementedError("Dynamic personality not supported") - slopes = self.compute_symmetric_slopes(x_vel, y_dist) - result = float(self.hermite_interpolate(v_ego, x_vel, y_dist, slopes)) + slopes = compute_symmetric_slopes(x_vel, y_dist) + result = float(hermite_interpolate(v_ego, x_vel, y_dist, slopes)) return result diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_personality/tests/pytest_dpc.py b/sunnypilot/selfdrive/controls/lib/dynamic_personality/tests/pytest_dpc.py index f134194244..f3b22d4629 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_personality/tests/pytest_dpc.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_personality/tests/pytest_dpc.py @@ -3,6 +3,7 @@ import numpy as np from cereal import log from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_personality_controller import DynamicPersonalityController +from openpilot.sunnypilot.common.utils import compute_symmetric_slopes, hermite_interpolate class TestDynamicPersonalityController: @@ -25,7 +26,7 @@ class TestDynamicPersonalityController: expected_slopes[2] = ((y[3] - y[2]) / (x[3] - x[2]) + (y[2] - y[1]) / (x[2] - x[1])) / 2 # (3.0 + 2.0) / 2 = 2.5 expected_slopes[3] = (y[3] - y[2]) / (x[3] - x[2]) # 3.0 - computed_slopes = self.controller.compute_symmetric_slopes(x, y) + computed_slopes = compute_symmetric_slopes(x, y) np.testing.assert_allclose(computed_slopes, expected_slopes, rtol=1e-5) @@ -36,13 +37,13 @@ class TestDynamicPersonalityController: slopes = np.array([1.0, 0.0, -1.0]) # Test at data points - assert self.controller.hermite_interpolate(0.0, xp, yp, slopes) == pytest.approx(5.0) - assert self.controller.hermite_interpolate(10.0, xp, yp, slopes) == pytest.approx(15.0) - assert self.controller.hermite_interpolate(20.0, xp, yp, slopes) == pytest.approx(10.0) + assert hermite_interpolate(0.0, xp, yp, slopes) == pytest.approx(5.0) + assert hermite_interpolate(10.0, xp, yp, slopes) == pytest.approx(15.0) + assert hermite_interpolate(20.0, xp, yp, slopes) == pytest.approx(10.0) # Test interpolation - assert self.controller.hermite_interpolate(5.0, xp, yp, slopes) == pytest.approx(11.25) - assert self.controller.hermite_interpolate(15.0, xp, yp, slopes) == pytest.approx(13.75) + assert hermite_interpolate(5.0, xp, yp, slopes) == pytest.approx(11.25) + assert hermite_interpolate(15.0, xp, yp, slopes) == pytest.approx(13.75) def test_hermite_interpolate_clipping(self): """Test that hermite interpolation properly clips values outside the range""" @@ -51,10 +52,10 @@ class TestDynamicPersonalityController: slopes = np.array([1.0, 0.0, -1.0]) # Test clipping below minimum - assert self.controller.hermite_interpolate(-5.0, xp, yp, slopes) == pytest.approx(5.0) + assert hermite_interpolate(-5.0, xp, yp, slopes) == pytest.approx(5.0) # Test clipping above maximum - assert self.controller.hermite_interpolate(25.0, xp, yp, slopes) == pytest.approx(10.0) + assert hermite_interpolate(25.0, xp, yp, slopes) == pytest.approx(10.0) def test_get_dynamic_follow_distance_relaxed(self): """Test follow distance calculation for relaxed personality"""