From 4509316877fa502220b0068ccd6f89e8a4b759e5 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 26 Aug 2026 16:17:04 -0500 Subject: [PATCH] model --- .../tests/test_longitudinal_planner.py | 33 +++++++++++++++++++ selfdrive/modeld/compile_modeld.py | 10 ++++-- selfdrive/modeld/modeld.py | 10 +++--- 3 files changed, 46 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 1d50955e1..a78967023 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2990,6 +2990,39 @@ def test_modeld_action_uses_current_action_head_scaling_for_v15(monkeypatch): assert not action.shouldStop +def test_modeld_action_uses_current_action_head_scaling_for_v16(monkeypatch): + monkeypatch.setenv("DEBUG", "0") + fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx") + fake_commonmodel.DrivingModelFrame = object + fake_commonmodel.CLContext = object + monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel) + + from openpilot.selfdrive.modeld import modeld + + prev_action = log.ModelDataV2.Action.new_message() + prev_action.desiredCurvature = 0.05 + prev_action.desiredAcceleration = -0.2 + toggles = SimpleNamespace(vEgoStopping=0.42) + + action = modeld.get_action_from_model( + {"action": np.array([[12.0, -0.8]], dtype=np.float32)}, + prev_action, + lat_action_t=0.2, + long_action_t=0.73, + v_ego=5.0, + mlsim=True, + is_v9=False, + is_v14=False, + is_v15=False, + starpilot_toggles=toggles, + is_v16=True, + ) + + assert action.desiredCurvature == pytest.approx(modeld.smooth_value(0.48, prev_action.desiredCurvature, modeld.LAT_SMOOTH_SECONDS)) + assert action.desiredAcceleration < -0.2 + assert not action.shouldStop + + def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero(): class FakePM: def __init__(self): diff --git a/selfdrive/modeld/compile_modeld.py b/selfdrive/modeld/compile_modeld.py index 4dacdcd15..ea52dbb63 100644 --- a/selfdrive/modeld/compile_modeld.py +++ b/selfdrive/modeld/compile_modeld.py @@ -204,13 +204,14 @@ def _packed_policy_shapes(input_shapes, include_prev_feature=False): shapes[key] = tuple(shape) if include_prev_feature: features_shape = input_shapes["features_buffer"] - shapes["prev_feat"] = (features_shape[0], features_shape[2]) + shapes["prev_feat"] = (features_shape[0], math.prod(features_shape[2:])) return shapes, [math.prod(shape) for shape in shapes.values()] def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device): queues, npy = make_warp_input_queues(vision_input_shapes, frame_skip, device) features_shape = policy_input_shapes["features_buffer"] + feature_dim = math.prod(features_shape[2:]) desire_key = _detect_desire_key(policy_input_shapes) desire_shape = policy_input_shapes[desire_key] packed_shapes, packed_sizes = _packed_policy_shapes(policy_input_shapes) @@ -224,7 +225,7 @@ def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip }) queues.update({ "feat_q": Tensor( - np.zeros((frame_skip * (features_shape[1] - 1) + 1, features_shape[0], features_shape[2]), dtype=np.float32), + np.zeros((frame_skip * (features_shape[1] - 1) + 1, features_shape[0], feature_dim), dtype=np.float32), device=device, ).contiguous().realize(), "desire_q": Tensor( @@ -239,6 +240,7 @@ def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip def make_supercombo_input_queues(input_shapes, frame_skip, device): queues, npy = make_warp_input_queues(input_shapes, frame_skip, device) features_shape = input_shapes["features_buffer"] + feature_dim = math.prod(features_shape[2:]) desire_key = _detect_desire_key(input_shapes) desire_shape = input_shapes[desire_key] packed_shapes, packed_sizes = _packed_policy_shapes(input_shapes, include_prev_feature=True) @@ -252,7 +254,7 @@ def make_supercombo_input_queues(input_shapes, frame_skip, device): }) queues.update({ "feat_q": Tensor( - np.zeros((frame_skip * features_shape[1], features_shape[0], features_shape[2]), dtype=np.float32), + np.zeros((frame_skip * features_shape[1], features_shape[0], feature_dim), dtype=np.float32), device=device, ).contiguous().realize(), "desire_q": Tensor( @@ -335,6 +337,7 @@ def make_run_split_policy(vision_runner, policy_runners, metadata, policy_order, vision_output = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast("float32") new_feature = vision_output[:, vision_features_slice].reshape(1, -1).unsqueeze(0) features_buffer = shift_and_sample(feat_q, new_feature, sample_skip_fn) + features_buffer = features_buffer.reshape(policy_metadata["input_shapes"]["features_buffer"]) policy_inputs = { "features_buffer": features_buffer, @@ -388,6 +391,7 @@ def make_run_supercombo(model_runner, metadata, frame_skip, image_history_pipeli features_buffer = shift_and_sample( feat_q, previous_feature.reshape(1, 1, -1), sample_skip_fn, ) + features_buffer = features_buffer.reshape(input_shapes["features_buffer"]) model_inputs = { road_key: img, wide_key: big_img, diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 29989956c..20889f267 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -277,10 +277,11 @@ def _close_tinygrad_disk_cache_connection() -> None: def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool, is_v9: bool, is_v14: bool, is_v15: bool, starpilot_toggles, - lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS) -> log.ModelDataV2.Action: - if is_v14 or is_v15: + lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS, + is_v16: bool = False) -> log.ModelDataV2.Action: + if is_v14 or is_v15 or is_v16: desired_curv_unscaled, desired_accel = model_output['action'][0] - if is_v15: + if is_v15 or is_v16: desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2 else: desired_curvature = float(desired_curv_unscaled) / 100.0 @@ -464,6 +465,7 @@ class ModelState: self.is_v9 = self.policy_generation == "v9" self.is_v14 = self.policy_generation == "v14" self.is_v15 = self.policy_generation == "v15" + self.is_v16 = self.policy_generation == "v16" self.mlsim = is_tinygrad_model_version(self.policy_generation) if write_model_version: params.put("ModelVersion", self.policy_generation) @@ -953,7 +955,7 @@ def main(demo=False): lat_action_t, long_action_t, v_ego, model.mlsim, model.is_v9, model.is_v14, model.is_v15, starpilot_toggles, - lat_smooth_seconds, long_smooth_seconds, + lat_smooth_seconds, long_smooth_seconds, is_v16=model.is_v16, ) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action,