From 29fe152bd366f17c83ddc9b9286bd52a9039c7d2 Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Thu, 4 Sep 2025 19:26:59 -0700 Subject: [PATCH] modeld_v2: desire rename and add many parts from thneed modeld (#1197) * Add model metadata lookup and update desire handling * Bump selector version to 10 * meh * Refactor shape mode parameters for desire handling in test buffer logic * loop more models * Refactor buffer handling for temporal inputs and streamline desire updates * Refactor lateral control input handling and remove unused code --- sunnypilot/modeld_v2/modeld.py | 48 ++++---- .../tests/test_buffer_logic_inspect.py | 109 ++++++++++-------- sunnypilot/models/helpers.py | 2 +- 3 files changed, 86 insertions(+), 73 deletions(-) diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index bf89bc98d..0fd45940d 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -27,7 +27,7 @@ from openpilot.sunnypilot.modeld.modeld_base import ModelStateBase from openpilot.sunnypilot.models.helpers import get_active_bundle from openpilot.sunnypilot.models.runners.helpers import get_model_runner -PROCESS_NAME = "selfdrive.modeld.modeld" +PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad" class FrameMeta: @@ -77,42 +77,47 @@ class ModelState(ModelStateBase): self.numpy_inputs[key] = np.zeros(shape, dtype=np.float32) # Temporal input: shape is [batch, history, features] if len(shape) == 3 and shape[1] > 1: - buffer_history_len = max(100, (shape[1] * 4 if shape[1] < 100 else shape[1])) # Allow for higher history buffers in the future + buffer_history_len = shape[1] * 4 if shape[1] < 99 else shape[1] # Allow for higher history buffers in the future feature_len = shape[2] - self.temporal_buffers[key] = np.zeros((1, buffer_history_len, feature_len), dtype=np.float32) features_buffer_shape = self.model_runner.input_shapes.get('features_buffer') if shape[1] in (24, 25) and features_buffer_shape is not None and features_buffer_shape[1] == 24: # 20Hz + buffer_history_len = (features_buffer_shape[1] + 1) * 4 step = int(-buffer_history_len / shape[1]) self.temporal_idxs_map[key] = np.arange(step, step * (shape[1] + 1), step)[::-1] elif shape[1] == 25: # Split skip = buffer_history_len // shape[1] self.temporal_idxs_map[key] = np.arange(buffer_history_len)[-1 - (skip * (shape[1] - 1))::skip] - elif shape[1] == buffer_history_len: # non20hz - self.temporal_idxs_map[key] = np.arange(buffer_history_len) + elif shape[1] >= 99: # non20hz + self.temporal_idxs_map[key] = np.arange(shape[1]) + self.temporal_buffers[key] = np.zeros((1, buffer_history_len, feature_len), dtype=np.float32) @property def mlsim(self) -> bool: return bool(self.generation is not None and self.generation >= 11) + @property + def desire_key(self) -> str: + return next(key for key in self.numpy_inputs if key.startswith('desire')) + def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray], inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None: # Model decides when action is completed, so desire input is just a pulse triggered on rising edge - inputs['desire'][0] = 0 - new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) - self.prev_desire[:] = inputs['desire'] - self.temporal_buffers['desire'][0,:-1] = self.temporal_buffers['desire'][0,1:] - self.temporal_buffers['desire'][0,-1] = new_desire + inputs[self.desire_key][0] = 0 + new_desire = np.where(inputs[self.desire_key] - self.prev_desire > .99, inputs[self.desire_key], 0) + self.prev_desire[:] = inputs[self.desire_key] + self.temporal_buffers[self.desire_key][0,:-1] = self.temporal_buffers[self.desire_key][0,1:] + self.temporal_buffers[self.desire_key][0,-1] = new_desire # Roll buffer and assign based on desire.shape[1] value - if self.temporal_buffers['desire'].shape[1] > self.numpy_inputs['desire'].shape[1]: - skip = self.temporal_buffers['desire'].shape[1] // self.numpy_inputs['desire'].shape[1] - self.numpy_inputs['desire'][:] = ( - self.temporal_buffers['desire'][0].reshape(self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], skip, -1).max(axis=2)) + if self.temporal_buffers[self.desire_key].shape[1] > self.numpy_inputs[self.desire_key].shape[1]: + skip = self.temporal_buffers[self.desire_key].shape[1] // self.numpy_inputs[self.desire_key].shape[1] + self.numpy_inputs[self.desire_key][:] = (self.temporal_buffers[self.desire_key][0].reshape( + self.numpy_inputs[self.desire_key].shape[0], self.numpy_inputs[self.desire_key].shape[1], skip, -1).max(axis=2)) else: - self.numpy_inputs['desire'][:] = self.temporal_buffers['desire'][0, self.temporal_idxs_map['desire']] + self.numpy_inputs[self.desire_key][:] = self.temporal_buffers[self.desire_key][0, self.temporal_idxs_map[self.desire_key]] for key in self.numpy_inputs: - if key in inputs and key not in ['desire']: + if key in inputs and key not in [self.desire_key]: self.numpy_inputs[key][:] = inputs[key] imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.model_runner.vision_input_names} @@ -156,10 +161,11 @@ class ModelState(ModelStateBase): desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) desired_curvature = get_curvature_from_output(model_output, v_ego, lat_action_t, self.mlsim) - if v_ego > self.MIN_LAT_CONTROL_SPEED: - desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS) - else: - desired_curvature = prev_action.desiredCurvature + if self.generation is not None and self.generation >= 10: # smooth curvature for post FOF models + if v_ego > self.MIN_LAT_CONTROL_SPEED: + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS) + else: + desired_curvature = prev_action.desiredCurvature return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop)) @@ -306,7 +312,7 @@ def main(demo=False): bufs = {name: buf_extra if 'big' in name else buf_main for name in model.model_runner.vision_input_names} transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.model_runner.vision_input_names} inputs:dict[str, np.ndarray] = { - 'desire': vec_desire, + model.desire_key: vec_desire, 'traffic_convention': traffic_convention, } diff --git a/sunnypilot/modeld_v2/tests/test_buffer_logic_inspect.py b/sunnypilot/modeld_v2/tests/test_buffer_logic_inspect.py index 8a0cfd97c..f664db31b 100644 --- a/sunnypilot/modeld_v2/tests/test_buffer_logic_inspect.py +++ b/sunnypilot/modeld_v2/tests/test_buffer_logic_inspect.py @@ -8,12 +8,16 @@ import openpilot.sunnypilot.modeld_v2.modeld as modeld_module ModelState = modeld_module.ModelState - # These are the shapes extracted/loaded from the model onnx SHAPE_MODE_PARAMS = [ - ({'desire': (1, 25, 8), 'features_buffer': (1, 25, 512), 'prev_desired_curv': (1, 25, 1)}, 'split'), - ({'desire': (1, 25, 8), 'features_buffer': (1, 24, 512), 'prev_desired_curv': (1, 25, 1)}, '20hz'), - ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), 'prev_desired_curv': (1, 100, 1)}, 'non20hz'), + ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), "nav_features": (1, 256), "nav_instructions": (1, 150)}, 'non20hz'), # Optimus Prime + ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), "lat_planner_state": (1, 4),}, 'non20hz'), # farmville + ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), "lateral_control_params": (1, 2), "prev_desired_curv": (1, 100, 1)}, 'non20hz'), # wd40 + ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), 'prev_desired_curv': (1, 100, 1), "lateral_control_params": (1, 2),}, 'non20hz'), # NTS + ({'desire': (1, 25, 8), 'features_buffer': (1, 24, 512)}, '20hz'), # NPR + ({'desire': (1, 100, 8), 'features_buffer': (1, 99, 512), 'prev_desired_curv': (1, 100, 1), "lateral_control_params": (1, 2),}, 'non20hz'), # NTS + ({'desire': (1, 25, 8), 'features_buffer': (1, 25, 512)}, 'split'), # Steam Powered v2 + ({'desire_pulse': (1, 25, 8), 'features_buffer': (1, 25, 512)}, 'split'), # desire rename ] @@ -95,9 +99,7 @@ def get_expected_indices(shape, constants, mode, key=None): idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1] return idxs elif mode == 'non20hz': - if key and shape[1] == constants.FULL_HISTORY_BUFFER_LEN: - return np.arange(constants.FULL_HISTORY_BUFFER_LEN) - return None + return np.arange(shape[1]) return None @@ -108,6 +110,8 @@ def test_buffer_shapes_and_indices(shapes, mode, apply_patches): for key in shapes: buf = state.temporal_buffers.get(key, None) idxs = state.temporal_idxs_map.get(key, None) + if buf is None: + continue # not all shapes are 3D, and the non-3D ones are not buffered # Buffer shape logic if mode == 'split': expected_shape = (1, constants.FULL_HISTORY_BUFFER_LEN, shapes[key][2]) @@ -116,10 +120,7 @@ def test_buffer_shapes_and_indices(shapes, mode, apply_patches): expected_shape = (1, constants.FULL_HISTORY_BUFFER_LEN, shapes[key][2]) expected_idxs = get_expected_indices(shapes[key], constants, '20hz', key) elif mode == 'non20hz': - if key == 'features_buffer': - expected_shape = (1, shapes[key][1]*4, shapes[key][2]) - else: - expected_shape = (1, shapes[key][1], shapes[key][2]) + expected_shape = (1, shapes[key][1], shapes[key][2]) expected_idxs = get_expected_indices(shapes[key], constants, 'non20hz', key) assert buf is not None, f"{key}: buffer not found" @@ -130,10 +131,10 @@ def test_buffer_shapes_and_indices(shapes, mode, apply_patches): assert idxs is None or idxs.size == 0, f"{key}: buffer idxs should be None or empty" -def legacy_buffer_update(buf, new_val, mode, key, constants, idxs): +def legacy_buffer_update(buf, new_val, mode, key, constants, idxs, input_shape, prev_desire=None): # This is what we compare the new dynamic logic to, to ensure it does the same thing if mode == 'split': - if key == 'desire': + if key == 'desire' or key.startswith('desire'): buf[0,:-1] = buf[0,1:] buf[0,-1] = new_val return buf.reshape((1, constants.INPUT_HISTORY_BUFFER_LEN, constants.TEMPORAL_SKIP, -1)).max(axis=2) @@ -173,15 +174,22 @@ def legacy_buffer_update(buf, new_val, mode, key, constants, idxs): return legacy_buf[idxs] elif mode == 'non20hz': if key == 'desire': - length = new_val.shape[0] - buf[0,:-1,:length] = buf[0,1:,:length] - buf[0,-1,:length] = new_val[:length] + desire_len = constants.DESIRE_LEN + if prev_desire is None: + prev_desire = np.zeros(desire_len, dtype=np.float32) + # Set first element to zero + new_val = new_val.copy() + new_val[0] = 0 + # Shift buffer by desire len + buf[0][:-desire_len] = buf[0][desire_len:] + # Only insert new desire if rising edge + buf[0][-desire_len:] = np.where(new_val - prev_desire > 0.99, new_val, 0) + prev_desire[:] = new_val return buf[0] elif key == 'features_buffer': - feature_len = new_val.shape[0] - buf[0,:-1,:feature_len] = buf[0,1:,:feature_len] - buf[0,-1,:feature_len] = new_val[:feature_len] - return buf[0] + buf[0, :-1] = buf[0, 1:] + buf[0, -1] = new_val + return buf[0, -input_shape[1]:] # (99, 512) elif key == 'prev_desired_curv': length = new_val.shape[0] buf[0,:-length,0] = buf[0,length:,0] @@ -191,32 +199,18 @@ def legacy_buffer_update(buf, new_val, mode, key, constants, idxs): def dynamic_buffer_update(state, key, new_val, mode): - if key == 'desire': - state.temporal_buffers['desire'][0,:-1] = state.temporal_buffers['desire'][0,1:] - state.temporal_buffers['desire'][0,-1] = new_val - if state.temporal_buffers['desire'].shape[1] > state.numpy_inputs['desire'].shape[1]: - skip = state.temporal_buffers['desire'].shape[1] // state.numpy_inputs['desire'].shape[1] - return state.temporal_buffers['desire'][0].reshape( - state.numpy_inputs['desire'].shape[0], state.numpy_inputs['desire'].shape[1], skip, -1 - ).max(axis=2) - else: - return state.temporal_buffers['desire'][0, state.temporal_idxs_map['desire']] - - inputs = {'desire': np.zeros((1, state.constants.DESIRE_LEN), dtype=np.float32)} - for k, tb in state.temporal_buffers.items(): - if k in state.temporal_idxs_map: - continue - buf_len = tb.shape[1] - if k in state.numpy_inputs: - out_len = state.numpy_inputs[k].shape[1] - if out_len <= buf_len: - state.temporal_idxs_map[k] = np.arange(buf_len)[-out_len:] - else: - state.temporal_idxs_map[k] = np.arange(buf_len) - else: - state.temporal_idxs_map[k] = np.arange(buf_len) + if key == 'desire' or key.startswith('desire'): + inputs = {k: np.zeros(v[2], dtype=np.float32) if len(v) == 3 else np.zeros(v[1], dtype=np.float32) + for k, v in state.model_runner.input_shapes.items() if k != key} + inputs[key] = new_val.copy() + # ModelState.run expects desire as a pulse, so we zero the first element. + inputs[key][0] = 0 + state.run({}, {}, inputs, prepare_only=False) + return state.numpy_inputs[key] if key == 'features_buffer': + inputs = {k: np.zeros(v[2], dtype=np.float32) if len(v) == 3 else np.zeros(v[1], dtype=np.float32) + for k, v in state.model_runner.input_shapes.items() if k != 'features_buffer'} def run_model_stub(): return { 'hidden_state': np.asarray(new_val, dtype=np.float32).reshape(1, -1), @@ -226,6 +220,8 @@ def dynamic_buffer_update(state, key, new_val, mode): return state.numpy_inputs['features_buffer'][0] if key == 'prev_desired_curv': + inputs = {k: np.zeros(v[2], dtype=np.float32) if len(v) == 3 else np.zeros(v[1], dtype=np.float32) + for k, v in state.model_runner.input_shapes.items() if k != 'prev_desired_curv'} def run_model_stub(): return { 'hidden_state': np.zeros((1, state.constants.FEATURE_LEN), dtype=np.float32), @@ -241,16 +237,27 @@ def dynamic_buffer_update(state, key, new_val, mode): @pytest.mark.parametrize("key", ["desire", "features_buffer", "prev_desired_curv"]) def test_buffer_update_equivalence(shapes, mode, key, apply_patches): state = ModelState(None) + if key == "desire": + desire_keys = [k for k in shapes.keys() if k.startswith('desire')] + if desire_keys: + actual_key = desire_keys[0] # Use the first (and likely only) desire key + else: + actual_key = key + + if actual_key not in state.numpy_inputs: + pytest.skip() + constants = DummyModelRunner(shapes).constants - buf = state.temporal_buffers.get(key, None) - idxs = state.temporal_idxs_map.get(key, None) - input_shape = shapes[key] + buf = state.temporal_buffers.get(actual_key, None) + idxs = state.temporal_idxs_map.get(actual_key, None) + input_shape = shapes[actual_key] + prev_desire = np.zeros(constants.DESIRE_LEN, dtype=np.float32) if key == 'desire' else None + for step in range(20): # multiple steps to ensure history is built up new_val = np.full((input_shape[2],), step, dtype=np.float32) - expected = legacy_buffer_update(buf, new_val, mode, key, constants, idxs) - actual = dynamic_buffer_update(state, key, new_val, mode) - # Model returns the reduced numpy_inputs history, compare the last n entries so the test is checking the same slices. + expected = legacy_buffer_update(buf, new_val, mode, actual_key, constants, idxs, input_shape, prev_desire) + actual = dynamic_buffer_update(state, actual_key, new_val, mode) if expected is not None and actual is not None and expected.shape != actual.shape: if expected.ndim == 2 and actual.ndim == 2 and expected.shape[1] == actual.shape[1]: expected = expected[-actual.shape[0]:] - assert np.allclose(actual, expected), f"{mode} {key}: dynamic buffer update does not match legacy logic" + assert np.allclose(actual, expected), f"{mode} {actual_key}: dynamic buffer update does not match legacy logic" diff --git a/sunnypilot/models/helpers.py b/sunnypilot/models/helpers.py index 79241cd83..ecf0a39b7 100644 --- a/sunnypilot/models/helpers.py +++ b/sunnypilot/models/helpers.py @@ -19,7 +19,7 @@ from openpilot.system.hardware.hw import Paths from pathlib import Path # see the README.md for more details on the model selector versioning -CURRENT_SELECTOR_VERSION = 9 +CURRENT_SELECTOR_VERSION = 10 REQUIRED_MIN_SELECTOR_VERSION = 9 USE_ONNX = os.getenv('USE_ONNX', PC)