model

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):

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,

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,