diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 51983dd95..74c4eaa41 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -285,7 +285,7 @@ class LongitudinalPlanner: @property def mlsim(self): - return self.generation in ("v8", "v10", "v11", "v12", "v13") + return self.generation in ("v8", "v10", "v11", "v12", "v13", "v14") def get_mpc_mode(self) -> str: if not self.mlsim: diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 5166a81cd..185e11b2a 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -105,7 +105,7 @@ def make_toggles(model_version: str = "v11"): ) -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version): v_ego = 18.0 desired_accel = -1.0 @@ -126,7 +126,7 @@ def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version): assert planner.output_a_target < comfort_min_accel -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_uses_close_raw_lead_when_tracking_lead_is_debounced(model_version): v_ego = 5.0 @@ -151,7 +151,7 @@ def test_acc_mode_uses_close_raw_lead_when_tracking_lead_is_debounced(model_vers ) -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_matches_no_lead_baseline_for_far_vision_only_lead_without_tracking(model_version): v_ego = 29.0 @@ -313,7 +313,7 @@ def test_vision_slow_stopped_lead_cap_ignores_far_high_speed_stop_candidate(): assert planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.45) is None -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version): v_ego = 21.535 @@ -337,7 +337,7 @@ def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version): assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.45 -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_dynamic_t_follow_stays_near_base_for_far_highway_lead(model_version): v_ego = 29.26 @@ -359,7 +359,7 @@ def test_dynamic_t_follow_stays_near_base_for_far_highway_lead(model_version): assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02) -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_dynamic_t_follow_releases_toward_base_after_lead_opens(model_version): v_ego = 21.535 @@ -387,7 +387,7 @@ def test_dynamic_t_follow_releases_toward_base_after_lead_opens(model_version): assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02) -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_version): approach_v_ego = 21.535 close_v_ego = 21.435 @@ -424,7 +424,7 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers assert planner_close.output_a_target < planner_approach.output_a_target - 0.25 -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_version): v_ego = 23.23 @@ -459,7 +459,7 @@ def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_ver assert planner_with_lead.output_a_target < -0.2 -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_tracking(model_version): v_ego = 21.48 @@ -500,7 +500,7 @@ def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_track assert lead_outputs[-1] < no_lead_outputs[-1] - 0.15 -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_pretracking_near_stopped_vision_lead_does_not_relax_when_confidence_is_midrange(model_version): v_ego = 20.35 @@ -534,7 +534,7 @@ def test_acc_mode_pretracking_near_stopped_vision_lead_does_not_relax_when_confi assert planner_with_lead.output_a_target < -0.45 -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_tracked_pace_matched_lead_caps_positive_catchup(model_version): v_ego = 28.7 @@ -568,7 +568,7 @@ def test_acc_mode_tracked_pace_matched_lead_caps_positive_catchup(model_version) assert planner_with_lead.output_a_target < 0.08 -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(model_version): v_ego = 3.8 @@ -591,7 +591,7 @@ def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap( assert planner.output_a_target < -1.0 -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version): v_ego = 0.0 @@ -615,7 +615,7 @@ def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_ve assert planner.output_a_target < 0.1 -@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"]) +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version): far_v_ego = 29.26 far_v_cruise = 32.22 @@ -690,6 +690,7 @@ def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch): v_ego=5.0, mlsim=True, is_v9=True, + is_v14=False, starpilot_toggles=toggles, ) @@ -701,6 +702,37 @@ def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch): assert action.shouldStop +def test_modeld_action_uses_direct_action_head_for_v14(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=True, + starpilot_toggles=toggles, + ) + + assert action.desiredCurvature == pytest.approx(0.12) + assert action.desiredAcceleration < -0.2 + assert not action.shouldStop + + def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): v_ego = 10.0 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index c1ce8e86f..6805a1a27 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -99,7 +99,23 @@ def _canonical_model_id(model_id: str) -> str: 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, starpilot_toggles) -> log.ModelDataV2.Action: + lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool, + is_v9: bool, is_v14: bool, starpilot_toggles) -> log.ModelDataV2.Action: + if is_v14: + desired_curv_unscaled, desired_accel = model_output['action'][0] + desired_curvature = float(desired_curv_unscaled) / 100.0 + should_stop = (v_ego < 0.3 and desired_accel < 0.1) + + desired_accel = smooth_value(float(desired_accel), prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) + if v_ego > MIN_LAT_CONTROL_SPEED: + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, 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)) + plan = model_output['plan'][0] if 'planplus' in model_output: recovery_power = getattr(starpilot_toggles, "recovery_power", 1.0) @@ -158,6 +174,8 @@ class ModelState: numpy_inputs['features_buffer'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) if 'action_t' in input_shapes: numpy_inputs['action_t'] = np.zeros(input_shapes['action_t'], dtype=np.float32) + if 'prev_action' in input_shapes: + numpy_inputs['prev_action'] = np.zeros(input_shapes['prev_action'], dtype=np.float32) # Optional inputs for non-v11 (and some v10/v9 variants) # Lateral control params @@ -285,8 +303,9 @@ class ModelState: self.is_v10 = (self.policy_generation == "v10") self.is_v12 = (self.policy_generation == "v12") self.is_v13 = (self.policy_generation == "v13") + self.is_v14 = (self.policy_generation == "v14") self.is_v9 = (self.policy_generation == "v9") - self.mlsim = (self.policy_generation in ("v8", "v10", "v11", "v12", "v13")) + self.mlsim = (self.policy_generation in ("v8", "v10", "v11", "v12", "v13", "v14")) self.policy_has_plan = 'plan' in self.policy_output_slices self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names} @@ -311,7 +330,7 @@ class ModelState: self.off_policy_output: np.ndarray | None = None off_policy_metadata = None - if self.policy_generation in ("v12", "v13") or OFF_POLICY_METADATA_PATH.is_file() or OFF_POLICY_PKL_PATH.is_file(): + if self.policy_generation in ("v12", "v13", "v14") or OFF_POLICY_METADATA_PATH.is_file() or OFF_POLICY_PKL_PATH.is_file(): resolved_off_policy_meta = ensure_artifact(OFF_POLICY_METADATA_PATH, "driving_off_policy_metadata.pkl", optional=True) if resolved_off_policy_meta is not None: with open(resolved_off_policy_meta, 'rb') as f: @@ -386,6 +405,11 @@ class ModelState: if self.off_policy_enabled and 'action_t' in self.off_policy_numpy_inputs: self.off_policy_numpy_inputs['action_t'][:] = inputs['action_t'] + if 'prev_action' in self.numpy_inputs: + self.numpy_inputs['prev_action'][:] = inputs['prev_action'] + if self.off_policy_enabled and 'prev_action' in self.off_policy_numpy_inputs: + self.off_policy_numpy_inputs['prev_action'][:] = inputs['prev_action'] + if 'lateral_control_params' in self.numpy_inputs: self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params'] if self.off_policy_enabled and 'lateral_control_params' in self.off_policy_numpy_inputs: @@ -425,14 +449,14 @@ class ModelState: self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :] if self.prev_desired_curv_key is not None: - # v9/v10/v11/v12/v13 models expect zeros for prev_desired_curv(s); others use history - if self.is_v9 or self.is_v10 or self.is_v11 or self.is_v12 or self.is_v13: + # v9/v10/v11/v12/v13/v14 models expect zeros for prev_desired_curv(s); others use history + if self.is_v9 or self.is_v10 or self.is_v11 or self.is_v12 or self.is_v13 or self.is_v14: self.numpy_inputs[self.prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs] else: self.numpy_inputs[self.prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs] if self.off_policy_enabled and self.off_policy_prev_desired_curv_key is not None: - if self.is_v9 or self.is_v12 or self.is_v13: + if self.is_v9 or self.is_v12 or self.is_v13 or self.is_v14: self.off_policy_numpy_inputs[self.off_policy_prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs] else: self.off_policy_numpy_inputs[self.off_policy_prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs] @@ -611,6 +635,11 @@ def main(demo=False): } if 'action_t' in model.numpy_inputs or (model.off_policy_enabled and 'action_t' in model.off_policy_numpy_inputs): inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32) + if 'prev_action' in model.numpy_inputs or (model.off_policy_enabled and 'prev_action' in model.off_policy_numpy_inputs): + inputs['prev_action'] = np.array([ + prev_action.desiredCurvature * max(1.0, v_ego) ** 2, + prev_action.desiredAcceleration, + ], dtype=np.float32) # Include optional inputs only if the loaded model expects them if 'lateral_control_params' in model.numpy_inputs: inputs['lateral_control_params'] = lateral_control_params @@ -630,7 +659,7 @@ def main(demo=False): model_output, prev_action, lat_action_t, long_action_t, - v_ego, model.mlsim, model.is_v9, starpilot_toggles, + v_ego, model.mlsim, model.is_v9, model.is_v14, starpilot_toggles, ) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, diff --git a/selfdrive/ui/layouts/settings/starpilot/driving_model.py b/selfdrive/ui/layouts/settings/starpilot/driving_model.py index 6f83762e6..cd96cca0c 100644 --- a/selfdrive/ui/layouts/settings/starpilot/driving_model.py +++ b/selfdrive/ui/layouts/settings/starpilot/driving_model.py @@ -723,7 +723,7 @@ class StarPilotDrivingModelLayout(_SettingsPage): f"{key}_driving_vision_metadata.pkl", ] - if version in {"v12", "v13"}: + if version in {"v12", "v13", "v14"}: files.extend( [ f"{key}_driving_off_policy_tinygrad.pkl", diff --git a/selfdrive/ui/mici/layouts/settings/driving_model.py b/selfdrive/ui/mici/layouts/settings/driving_model.py index 1d46f9b8d..f6cde94d0 100644 --- a/selfdrive/ui/mici/layouts/settings/driving_model.py +++ b/selfdrive/ui/mici/layouts/settings/driving_model.py @@ -725,7 +725,7 @@ class DrivingModelBigButton(BigButton): f"{key}_driving_vision_metadata.pkl", ] - if version in {"v12", "v13"}: + if version in {"v12", "v13", "v14"}: files.extend([ f"{key}_driving_off_policy_tinygrad.pkl", f"{key}_driving_off_policy_metadata.pkl", diff --git a/starpilot/assets/model_manager.py b/starpilot/assets/model_manager.py index 13eb58b12..5214e577a 100644 --- a/starpilot/assets/model_manager.py +++ b/starpilot/assets/model_manager.py @@ -18,7 +18,7 @@ from openpilot.starpilot.common.starpilot_utilities import delete_file from openpilot.starpilot.common.starpilot_variables import MODELS_PATH MANIFEST_CANDIDATES = ("v21",) -TINYGRAD_VERSIONS = {"v8", "v9", "v10", "v11", "v12", "v13"} +TINYGRAD_VERSIONS = {"v8", "v9", "v10", "v11", "v12", "v13", "v14"} DEFAULT_MODEL_KEY = "sc2" MODEL_KEY_CANONICAL_MAP = { "sc": DEFAULT_MODEL_KEY, @@ -192,7 +192,7 @@ class ModelManager: f"{model_key}_driving_vision_metadata.pkl", ] - if model_version in {"v12", "v13"}: + if model_version in {"v12", "v13", "v14"}: filenames += [ f"{model_key}_driving_off_policy_tinygrad.pkl", f"{model_key}_driving_off_policy_metadata.pkl", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 1329f4ae3..6e0b95579 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -983,7 +983,7 @@ class StarPilotVariables: if isinstance(toggle.model_version, bytes): toggle.model_version = toggle.model_version.decode("utf-8", "ignore") toggle.classic_model = toggle.model_version in {"v1", "v2", "v3", "v4"} - toggle.tinygrad_model = toggle.model_version in {"v8", "v9", "v10", "v11", "v12", "v13"} + toggle.tinygrad_model = toggle.model_version in {"v8", "v9", "v10", "v11", "v12", "v13", "v14"} toggle.tomb_raider = toggle.model == "space-lab" toggle.model_ui = self.get_value("ModelUI") diff --git a/starpilot/system/the_pond/the_pond.py b/starpilot/system/the_pond/the_pond.py index 7d0f0c52f..1fdd43fe6 100644 --- a/starpilot/system/the_pond/the_pond.py +++ b/starpilot/system/the_pond/the_pond.py @@ -4339,14 +4339,14 @@ def setup(app): if f"{model_key}.thneed" in on_disk_files: return True - if model_version in ("v8", "v9", "v10", "v11", "v12", "v13"): + if model_version in ("v8", "v9", "v10", "v11", "v12", "v13", "v14"): required_files = { f"{model_key}_driving_policy_tinygrad.pkl", f"{model_key}_driving_vision_tinygrad.pkl", f"{model_key}_driving_policy_metadata.pkl", f"{model_key}_driving_vision_metadata.pkl", } - if model_version in ("v12", "v13"): + if model_version in ("v12", "v13", "v14"): required_files |= { f"{model_key}_driving_off_policy_tinygrad.pkl", f"{model_key}_driving_off_policy_metadata.pkl",