From d07d6ae7b7a1f13160e2940a8e2720834cd2d35d Mon Sep 17 00:00:00 2001 From: Prabhaav Pillai Date: Tue, 25 Aug 2026 18:01:37 -0400 Subject: [PATCH] fix hem --- .../controls/lib/longitudinal_planner.py | 1 + .../controls/tests/test_starpilot_planner.py | 4 +- .../common/assets/device_settings_layout.json | 4 +- .../controls/lib/hybrid_experimental_mode.py | 131 ++++++--- starpilot/controls/starpilot_card.py | 3 + starpilot/controls/starpilot_planner.py | 55 ---- .../tests/test_hybrid_experimental_mode.py | 251 ++++++++++++++---- tools/replay/mode_sim.py | 2 +- 8 files changed, 294 insertions(+), 157 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index bd5d639f4..5a7999057 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2312,6 +2312,7 @@ class LongitudinalPlanner: model_v2=sm['modelV2'], a_chill=output_a_target_mpc, a_exp=output_a_target_e2e, + t_follow=effective_t_follow, ) output_should_stop = output_should_stop_mpc or output_should_stop_e2e elif tinygrad_model and self.mode != 'acc' and self.generation != 'v9': diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index 1735ec943..0dade2728 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -175,7 +175,7 @@ def test_lateral_resume_delay_ignores_signal_cycles_that_never_slow_enough(monke planner.shutdown() -def test_hybrid_mode_runs_continuous_controller(monkeypatch): +def test_hybrid_mode_keeps_cem_detector_warm_with_modes_off(monkeypatch): planner = make_planner(monkeypatch) monkeypatch.setattr(planner.starpilot_cem, "update", lambda *args, **kwargs: None) @@ -184,8 +184,6 @@ def test_hybrid_mode_runs_continuous_controller(monkeypatch): planner.update(0.0, False, make_sm(planner, frame=1, v_ego=20.0, left_blinker=False), toggles) - assert planner.hybrid_controller is not None - assert planner.hybrid_acceleration != 0.0 assert planner.starpilot_ccm.experimental_mode is False assert planner.starpilot_cem.experimental_mode is False finally: diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index e81439856..c0d968111 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -2031,8 +2031,8 @@ }, { "key": "HybridExperimental", - "label": "Continuous Hybrid Control", - "description": "Fuse the crisp throttle response of classical ACC with the model's early, natural vision braking using a single continuous controller. Classical ACC always provides the hard safety distance, while the model's vision trajectory is blended in for early stops. Unconfirmed vision braking is capped to a gentle coast, and vision is never allowed to under-brake.", + "label": "Hybrid Experimental Mode", + "description": "Fuse the throttle response of chill ACC with the model's early, natural vision braking using a single continuous controller. Chill ACC always provides the hard safety distance, while the E2E trajectory is blended in for early stops. Unconfirmed vision braking is capped to a gentle coast, and vision is never allowed to under-brake.", "picker_description": "Continuously blends Chill ACC with the model's vision braking.", "data_type": "bool", "ui_type": "toggle", diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index 40475639d..1a4eb777d 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -25,17 +25,18 @@ def smooth_max(a: float, b: float, k: float = 6.0) -> float: class HybridExperimentalMode: """ Fuses Chill Mode (radar/lead tracking) and Experimental Mode (vision/stop signs/lights): - 1. Detects vision stopping intent from trajectory v_min and distance horizon. + 1. Detects vision stopping intent from trajectory drop and horizon endpoints. 2. Calculates kinematically required stopping deceleration (-v^2 / 2d). - 3. Clamps Chill positive throttle during stop events to prevent brake fighting. - 4. Holds 0 m/s at standstills to prevent creep. + 3. Clamps Chill positive throttle during stop events to prevent brake fighting/dilution. + 4. Holds brake at standstills to prevent creep, but releases immediately on green lights or gas tap. 5. Falls back safely to Chill if lead vehicle distance is compromised. - 6. Slew-rate limits acceleration to respect vehicle jerk limits. + 6. Slew-rate limits acceleration to respect vehicle jerk limits, with emergency bypass. """ - # Base physical actuator jerk limits (m/s^3) + # Physical actuator jerk limits (m/s^3) BASE_MAX_JERK_BRAKE = 3.5 BASE_MAX_JERK_ACCEL = 5.5 + EMERGENCY_JERK_BRAKE = 14.0 # Base safety floor parameters BASE_T_FOLLOW = 1.45 @@ -55,8 +56,9 @@ class HybridExperimentalMode: self.jerk_factor = 1.0 self._update_profile_limits(self.t_follow, self.jerk_factor) - def reset(self, a: float = 0.0): - self.prev_a_target = float(a) + def reset(self, a_ego: float = 0.0): + """Seed target with actual vehicle acceleration on engagement to prevent torque bumps.""" + self.prev_a_target = float(a_ego) if np.isfinite(a_ego) else 0.0 self.exp_authority = 0.5 def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float, t_follow=None, jerk_factor=None): @@ -84,7 +86,10 @@ class HybridExperimentalMode: traj_v = getattr(velocity, "x", None) if velocity is not None else None if traj_v is None or len(traj_v) == 0: return np.array([v_ego], dtype=float) - return np.asarray(traj_v, dtype=float) + traj_v = np.asarray(traj_v, dtype=float) + if not np.all(np.isfinite(traj_v)): + return np.array([v_ego], dtype=float) + return traj_v @staticmethod def _get_model_trajectory_x(model_v2) -> np.ndarray: @@ -92,7 +97,10 @@ class HybridExperimentalMode: traj_x = getattr(position, "x", None) if position is not None else None if traj_x is None or len(traj_x) == 0: return np.array([], dtype=float) - return np.asarray(traj_x, dtype=float) + traj_x = np.asarray(traj_x, dtype=float) + if not np.all(np.isfinite(traj_x)): + return np.array([], dtype=float) + return traj_x def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp, t_follow=None, jerk_factor=None): @@ -101,6 +109,11 @@ class HybridExperimentalMode: (jerk_factor is not None and abs(jerk_factor - self.jerk_factor) > 1e-4): self._update_profile_limits(t_follow, jerk_factor) + if not np.isfinite(a_chill): + a_chill = float(self.prev_a_target) + if not np.isfinite(a_exp): + a_exp = a_chill + lead_status = bool(getattr(lead_one, "status", False)) lead_d_rel = float(getattr(lead_one, "dRel", 150.0)) @@ -108,53 +121,80 @@ class HybridExperimentalMode: traj_v = self._get_model_trajectory_v(model_v2, v_ego) traj_x = self._get_model_trajectory_x(model_v2) + v_min = float(np.min(traj_v)) min_idx = int(np.argmin(traj_v)) - v_min = float(traj_v[min_idx]) - d_min = float(traj_x[min_idx]) if len(traj_x) > min_idx else 100.0 + v_horizon = float(traj_v[-1]) if len(traj_v) > 0 else v_ego v_ref = max(v_ego, 2.0) - # Detect drop anywhere along trajectory (stop sign / red light profile) + # Detect deceleration profile or low-speed stop line target speed_drop_ratio = max(0.0, (v_ego - v_min) / v_ref) - is_stopping_profile = sigmoid(1.5 - v_min, k=4.0, x0=0.0) * sigmoid(v_ego, k=3.0, x0=1.0) - model_decel_strength = max(0.0, -a_exp / 2.5) + stop_target_active = sigmoid(1.2 - v_horizon, k=4.0, x0=0.0) + model_decel_strength = max(0.0, -a_exp / 2.0) - raw_vision_metric = max(speed_drop_ratio, is_stopping_profile, model_decel_strength) + raw_vision_metric = max(speed_drop_ratio, stop_target_active, model_decel_strength) w_vision = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0)) - # Calculate actual kinematic braking required to stop at the detected stop line - if v_min < 1.2 and v_ego > 1.0 and d_min > 0.5: - # Kinematic decel: -v^2 / (2 * d) with 2.0m stop line cushion - d_stop_effective = max(d_min - 2.0, 1.5) - a_kinematic_stop = - (v_ego ** 2) / (2.0 * d_stop_effective) + # Kinematic stopping calculation when approaching a stop line + if v_min < 1.2 and v_horizon < 2.0 and v_ego > 0.1 and len(traj_x) > min_idx and traj_x[min_idx] > 0.2: + d_min = float(traj_x[min_idx]) + d_stop_effective = max(d_min - 1.5, 2.0) + a_kinematic_stop = float(np.clip(- (v_ego ** 2) / (2.0 * d_stop_effective), -3.5, 0.0)) + if d_stop_effective < 6.0 and v_ego < 3.0: + a_kinematic_stop = min(a_kinematic_stop, -0.6) a_exp_effective = min(a_exp, a_kinematic_stop) + elif v_horizon < 1.0 and v_ego > 0.05: + # Final roll-in: enforce negative acceleration to complete stop + a_exp_effective = min(a_exp, -0.5) else: a_exp_effective = a_exp - # Dynamic Exp Authority: scales directly to 100% when vision intent is high base_auth = float(np.clip(0.5 + (0.35 * self.HYBRID_EXP_BIAS), 0.0, 1.0)) alpha_exp = lerp(base_auth, 1.0, w_vision) self.exp_authority = alpha_exp # 2. ACCELERATION FUSION (Throttle vs Braking Regimes) - # Throttle Regime: Snappy pickup on open roads - a_throttle_optimal = smooth_max(a_chill, a_exp, k=4.0) + # Throttle Regime: Snappy pickup on open roads with clean cruise setpoint clamp + a_throttle_raw = smooth_max(a_chill, a_exp, k=4.0) + if v_ego >= v_cruise: + a_throttle_optimal = min(a_throttle_raw, a_chill) + else: + overshoot_risk = sigmoid(v_ego - v_cruise, k=4.0, x0=-1.0) + a_throttle_capped = min(a_throttle_raw, max(0.0, a_chill)) + a_throttle_optimal = lerp(a_throttle_raw, a_throttle_capped, overshoot_risk) + a_throttle_conservative = smooth_min(a_chill, a_exp, k=4.0) a_throttle_fused = lerp(a_throttle_optimal, a_throttle_conservative, w_vision) - # Braking Regime: CLAMP Chill to <= 0 so cruise throttle cannot fight the stop! - a_chill_brake = min(a_chill, 0.0) - a_brake_fused = lerp(a_chill_brake, a_exp_effective, alpha_exp) + # Braking Regime: Never dilute Exp stop braking with Chill's 0.0 m/s^2 + if a_exp_effective < 0.0: + a_chill_brake = min(a_chill, 0.0) + a_brake_fused = min(a_exp_effective, a_chill_brake) if a_chill_brake < a_exp_effective \ + else lerp(a_exp_effective, a_chill_brake, 1.0 - alpha_exp) + else: + a_brake_fused = min(a_chill, a_exp_effective) + + # Regime Selection: If vision sees a stop or braking is requested, lock out positive throttle + is_braking_phase = (w_vision > 0.3) or (a_exp_effective < -0.2) or (a_chill < -0.2) + if is_braking_phase: + w_accel = 0.0 + else: + phase_metric = smooth_min(a_chill, a_exp, k=4.0) + w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1) * (1.0 - w_vision) - # Regime Selection: If vision sees a stop (w_vision -> 1), FORCE braking regime (w_accel -> 0) - phase_metric = smooth_min(a_chill, a_exp, k=4.0) - w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1) * (1.0 - w_vision) a_fused = lerp(a_brake_fused, a_throttle_fused, w_accel) - # 3. STANDSTILL ANCHOR (Prevent creeping at 0 mph) + # 3. STANDSTILL ANCHOR (Hold at 0 mph, release cleanly on green departure or gas tap) is_stopped = sigmoid(0.4 - v_ego, k=8.0, x0=0.0) - is_min_stopped = sigmoid(0.8 - v_min, k=4.0, x0=0.0) - standstill_weight = is_stopped * is_min_stopped - a_anchored = lerp(a_fused, smooth_min(a_fused, 0.0, k=8.0), standstill_weight) + is_staying_stopped = sigmoid(0.5 - v_horizon, k=6.0, x0=0.0) + lead_departing = lead_status and (getattr(lead_one, "vLead", 0.0) > 0.5) + vision_departing = (v_horizon > 0.5) and (a_exp > 0.1) + driver_departing = (a_chill > 0.4) and (not lead_status or lead_d_rel > 10.0) + # A real trajectory still predicting a stop line (red light / stop sign) keeps + # the anchor engaged: cruise creep must not release the brake at a stop it can't see. + model_stop_predicted = len(traj_v) > 1 and v_horizon < 0.5 + departing = (lead_departing or vision_departing or driver_departing) and not model_stop_predicted + standstill_weight = (0.0 if departing else 1.0) * is_stopped * is_staying_stopped + a_anchored = lerp(a_fused, smooth_min(a_fused, -0.5, k=6.0), standstill_weight) # 4. SAFETY BARRIER (Lead Vehicle Proximity Check) d_static_effective = self.D_STATIC_SAFE + max(0.0, 1.5 * (1.0 - (v_ego / 4.0))) @@ -163,12 +203,25 @@ class HybridExperimentalMode: distance_ratio = (lead_d_rel - d_static_effective) / max(d_safe - d_static_effective, 1.0) lead_safety_risk = sigmoid(1.0 - distance_ratio, k=5.0, x0=0.0) * float(lead_status) - a_emergency_brake = smooth_min(a_anchored, a_chill, k=6.0) - a_safe = lerp(a_anchored, a_emergency_brake, lead_safety_risk) + # Enforce hard ceiling when lead is within safety envelope or Chill is braking for lead + lead_safety_active = lead_status and (lead_d_rel < d_safe or a_chill < 0.0) + if lead_safety_active and a_chill < a_anchored: + a_safe = min(a_anchored, a_chill) + else: + a_safe = a_anchored + + # 5. ASYMMETRIC DIRECTIONAL SLEW FILTER (Limit Jerk) + da = a_safe - self.prev_a_target + if da >= 0.0: + jerk_limit = self.MAX_JERK_ACCEL + else: + # Bypass comfort brake rate for lead collision or committed vision-stop emergencies. + is_lead_emergency = (lead_safety_risk > 0.5) and (a_chill < -1.5) + is_vision_emergency = (w_vision > 0.7) and (a_exp < -2.0) + jerk_limit = self.EMERGENCY_JERK_BRAKE if (is_lead_emergency or is_vision_emergency) else self.MAX_JERK_BRAKE - # 5. ASYMMETRIC SLEW FILTER (Limit Jerk) - jerk_limit = self.MAX_JERK_ACCEL if a_safe >= self.prev_a_target else self.MAX_JERK_BRAKE max_delta = jerk_limit * self.DT - - self.prev_a_target = float(np.clip(a_safe, self.prev_a_target - max_delta, self.prev_a_target + max_delta)) + a_out = float(np.clip(a_safe, self.prev_a_target - max_delta, self.prev_a_target + max_delta)) + if np.isfinite(a_out): + self.prev_a_target = a_out return self.prev_a_target \ No newline at end of file diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 0d24a6401..b454b7d68 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -123,6 +123,9 @@ class StarPilotCard: if getattr(starpilot_toggles, "safe_mode", False): return + if getattr(starpilot_toggles, "hybrid_experimental_mode", False): + return + if starpilot_toggles.conditional_experimental_mode: current_status = self.params_memory.get_int("CEStatus", default=CEStatus["OFF"]) override_value = next_manual_ce_status(current_status, sm["selfdriveState"].experimentalMode) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index b9a4c0a4f..1278add36 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -24,7 +24,6 @@ from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode -from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode from openpilot.starpilot.controls.lib.starpilot_acceleration import StarPilotAcceleration from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents from openpilot.starpilot.controls.lib.starpilot_following import StarPilotFollowing @@ -69,7 +68,6 @@ class StarPilotPlanner: self.starpilot_acceleration = StarPilotAcceleration(self) self.starpilot_cem = ConditionalExperimentalMode(self) self.starpilot_ccm = ConditionalChillMode(self, self.starpilot_cem) - self.hybrid_controller = HybridExperimentalMode() self.starpilot_events = StarPilotEvents(self, error_log, ThemeManager) self.starpilot_following = StarPilotFollowing(self) self.starpilot_vcruise = StarPilotVCruise(self) @@ -102,7 +100,6 @@ class StarPilotPlanner: self.road_curvature = 0 self.time_to_curve = 0 self.v_cruise = 0 - self.hybrid_acceleration = 0.0 self.gps_position = None @@ -224,27 +221,9 @@ class StarPilotPlanner: conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled if conditional_tracking_active and bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)): - # Continuous Hybrid Experimental Control: instead of a binary CEM/CCM mode - # switch, fuse the classical Chill ACC target with the model's E2E - # trajectory through the HybridExperimentalMode controller. CEM's detector - # stays warm so red-light/stop-sign scene state (redLight, forcing_stop) - # remains accurate. Experimental Mode stays off so the MPC keeps producing - # the crisp classical a_chill that the hybrid is built on. self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise) self.starpilot_ccm.experimental_mode = False self.starpilot_cem.experimental_mode = False - self.hybrid_controller.set_tuning( - getattr(starpilot_toggles, "hybrid_exp_bias", 0.0), - getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0), - ) - self.hybrid_acceleration = self.hybrid_controller.update( - v_ego=v_ego, - v_cruise=v_cruise, - lead_one=self.lead_one, - model_v2=sm["modelV2"], - a_chill=self._get_chill_accel(v_ego, v_cruise), - a_exp=self._get_vision_exp_accel(sm), - ) elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)): # Keep CEM's filters warm in AOL so engagement can inherit the current scene. self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise) @@ -321,40 +300,6 @@ class StarPilotPlanner: self.tracking_lead_filter.update(following_lead) return self.tracking_lead_filter.x >= THRESHOLD - def _get_vision_exp_accel(self, sm): - """Model's raw predicted acceleration for the E2E (Experimental) channel.""" - try: - accel_x = sm["modelV2"].acceleration.x - if len(accel_x) > 0: - return float(accel_x[0]) - except (AttributeError, IndexError, TypeError): - pass - try: - return float(sm["modelV2"].action.desiredAcceleration) - except (AttributeError, TypeError, ValueError): - return 0.0 - - def _get_chill_accel(self, v_ego, v_cruise): - """Cruise/lead tracking acceleration for the Chill (ACC) channel.""" - max_accel = float(getattr(self.starpilot_acceleration, "max_accel", 0.0) or 0.0) - min_accel = float(getattr(self.starpilot_acceleration, "min_accel", 0.0) or 0.0) - if max_accel <= 0.0 and min_accel == 0.0: - max_accel, min_accel = 1.5, -2.0 - - a_chill = float(np.clip((v_cruise - v_ego) / 2.0, min_accel, max_accel)) - lead = getattr(self, "lead_one", None) - if lead is not None and bool(getattr(lead, "status", False)): - d_rel = float(getattr(lead, "dRel", float("inf"))) - v_lead = float(getattr(lead, "vLead", v_ego)) - t_follow = float(getattr(self.starpilot_following, "t_follow", 1.45) or 1.45) - desired_gap = max(v_ego * t_follow, 4.0) - if d_rel < desired_gap: - gap_deficit = max(desired_gap - d_rel, 0.0) - closing = max(0.0, v_ego - v_lead) - lead_decel = -min(closing / 2.0 + gap_deficit / 4.0, 3.0) - a_chill = min(a_chill, lead_decel) - return float(np.clip(a_chill, min_accel, max_accel)) - def publish(self, theme_updated, sm, pm, starpilot_toggles, serialized_toggles=""): starpilot_plan_send = messaging.new_message("starpilotPlan") starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"]) diff --git a/starpilot/controls/tests/test_hybrid_experimental_mode.py b/starpilot/controls/tests/test_hybrid_experimental_mode.py index 18434a280..36c4d39db 100644 --- a/starpilot/controls/tests/test_hybrid_experimental_mode.py +++ b/starpilot/controls/tests/test_hybrid_experimental_mode.py @@ -1,24 +1,27 @@ +#!/usr/bin/env python3 import numpy as np import pytest from openpilot.starpilot.controls.lib.hybrid_experimental_mode import ( HybridExperimentalMode, + lerp, sigmoid, - soft_max, - soft_min, + smooth_max, + smooth_min, ) class FakeLead: def __init__(self, status=False, d_rel=150.0, v_lead=0.0): self.status = status - self.dRel = d_rel - self.vLead = v_lead + self.dRel = float(d_rel) + self.vLead = float(v_lead) class FakeModel: - def __init__(self, velocity=None): - self.velocity = type("Velocity", (), {"x": velocity})() + def __init__(self, velocity=None, position=None): + self.velocity = type("Velocity", (), {"x": list(velocity)})() if velocity is not None else None + self.position = type("Position", (), {"x": list(position)})() if position is not None else None def make_controller(prev=0.0): @@ -29,81 +32,147 @@ def make_controller(prev=0.0): def run(controller, *, v_ego=20.0, v_cruise=30.0, lead=None, model=None, a_chill=0.5, a_exp=0.8, frames=80): lead = lead if lead is not None else FakeLead() - model = model if model is not None else FakeModel(velocity=[v_ego] * 20) + if model is None: + model = FakeModel(velocity=[v_ego] * 33, position=list(np.linspace(0.0, 100.0, 33))) result = 0.0 for _ in range(frames): result = controller.update(v_ego, v_cruise, lead, model, a_chill, a_exp) return result - def test_soft_operators_are_continuous_and_bounded(): for value in (-5.0, -0.1, 0.0, 0.1, 5.0): assert 0.0 < sigmoid(value) < 1.0 - assert soft_max(1.0, 2.0) == pytest.approx(2.0, abs=1e-2) - assert soft_min(1.0, 2.0) == pytest.approx(1.0, abs=1e-2) - + assert smooth_max(1.0, 2.0) == pytest.approx(2.0, abs=1e-2) + assert smooth_min(1.0, 2.0) == pytest.approx(1.0, abs=1e-2) + assert lerp(10.0, 20.0, 0.5) == pytest.approx(15.0, abs=1e-3) def test_throttle_fusion_leans_toward_snappier_target(): controller = make_controller() - # No lead, no vision stop: the continuous fusion routes toward the more - # confident throttle (exp 0.8) without a mode switch. a = run(controller, a_chill=0.5, a_exp=0.8) - assert a > 0.75 - assert a < 0.85 + assert 0.70 < a < 0.85 -def test_vision_stop_horizon_grants_full_braking(): +def test_throttle_fusion_conservative_when_chill_is_more_eager(): controller = make_controller() - model = FakeModel(velocity=np.linspace(20.0, 0.1, 20)) - # The model trajectory decays to a terminal stop, so w_vision -> 1 and the - # early vision braking curve takes over. - a = run(controller, lead=FakeLead(status=False), model=model, a_chill=-0.5, a_exp=-2.0) - assert a <= -1.5 + a = run(controller, a_chill=1.2, a_exp=0.6) + assert a > 1.0 -def test_phantom_brake_shield_caps_unconfirmed_vision_braking(): +def test_throttle_fusion_smooth_cruise_speed_capping(): controller = make_controller() - # No lead and a flat model horizon: unconfirmed vision braking is clamped to - # the gentle coast limit instead of passing the raw E2E target through. - a = run(controller, lead=FakeLead(status=False), a_chill=0.0, a_exp=-2.0) - assert -0.6 <= a <= -0.5 + a = run(controller, v_ego=30.0, v_cruise=30.0, a_chill=-0.2, a_exp=1.0) + assert a <= 0.0, f"Must not accelerate past set cruise speed, got {a}" + +def test_green_light_departure_from_standstill(): + controller = make_controller(prev=-0.5) + traj_v = np.linspace(0.0, 12.0, 33) + traj_x = np.linspace(0.0, 45.0, 33) + model = FakeModel(velocity=traj_v, position=traj_x) + + a = run(controller, v_ego=0.0, v_cruise=20.0, model=model, a_chill=1.0, a_exp=1.5, frames=40) + assert a > 1.0, f"Vehicle should depart promptly on green light, got {a}" -def test_cbf_safety_floor_prevents_under_braking_near_close_lead(): +def test_standstill_hold_at_red_light_without_lead(): + controller = make_controller(prev=0.0) + traj_v = np.zeros(33) + traj_x = np.zeros(33) + model = FakeModel(velocity=traj_v, position=traj_x) + + a = run(controller, v_ego=0.0, v_cruise=25.0, lead=FakeLead(status=False), + model=model, a_chill=0.2, a_exp=-1.0, frames=40) + assert a <= -0.4, f"Standstill brake must hold even when Chill cruise wants to go, got {a}" + +def test_red_light_high_speed_approach_braking(): + controller = make_controller(prev=0.0) + traj_v = np.linspace(25.0, 0.0, 33) + traj_x = np.linspace(0.0, 60.0, 33) + model = FakeModel(velocity=traj_v, position=traj_x) + + a = run(controller, v_ego=25.0, v_cruise=25.0, model=model, a_chill=0.5, a_exp=-2.0, frames=60) + assert a <= -2.0, f"Stopping deceleration should be fully honored without dilution, got {a}" + + +def test_red_light_low_speed_roll_prevent_dilution(): + controller = make_controller(prev=-0.5) + traj_v = np.linspace(0.8, 0.0, 33) + traj_x = np.linspace(0.0, 3.0, 33) + model = FakeModel(velocity=traj_v, position=traj_x) + + a = run(controller, v_ego=0.8, v_cruise=20.0, model=model, a_chill=0.8, a_exp=-0.6, frames=30) + assert a <= -0.55, f"Kinematic stop clamp must enforce stopping bite at low speeds, got {a}" + + +def test_kinematic_stopping_does_not_blow_up_on_close_dmin(): + controller = make_controller(prev=0.0) + traj_v = np.zeros(33) + traj_x = np.ones(33) * 1.0 + model = FakeModel(velocity=traj_v, position=traj_x) + + a = controller.update(15.0, 20.0, FakeLead(), model, a_chill=0.0, a_exp=-1.0) + assert a >= -(controller.MAX_JERK_BRAKE * controller.DT + 1e-4) + + +def test_kinematic_stopping_graceful_on_missing_position(): + controller = make_controller(prev=0.0) + traj_v = np.linspace(10.0, 0.0, 33) + model = FakeModel(velocity=traj_v, position=[]) + + a = run(controller, v_ego=10.0, v_cruise=20.0, model=model, a_chill=0.5, a_exp=-2.0, frames=40) + assert a <= -1.8 + +def test_radar_lead_safety_barrier_overrides_vision_throttle(): controller = make_controller() - # Vision wants throttle but a close lead means the Control Barrier Function - # smoothly forces the target toward the Chill safety floor. lead = FakeLead(status=True, d_rel=5.0, v_lead=0.0) - a = run(controller, lead=lead, a_chill=-1.0, a_exp=0.5) - assert a == pytest.approx(-1.0, abs=1e-2) + model = FakeModel(velocity=[15.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) + + a = run(controller, v_ego=15.0, v_cruise=20.0, lead=lead, model=model, a_chill=-2.5, a_exp=1.0, frames=40) + assert a == pytest.approx(-2.5, abs=0.1), f"Safety barrier must enforce radar braking floor, got {a}" -def test_cbf_is_inactive_when_lead_is_far(): +def test_radar_lead_safety_barrier_inactive_when_gap_is_safe(): controller = make_controller() lead = FakeLead(status=True, d_rel=80.0, v_lead=25.0) - a = run(controller, lead=lead, a_chill=0.5, a_exp=0.8) - assert a > 0.7 + model = FakeModel(velocity=[25.0] * 33, position=list(np.linspace(0.0, 150.0, 33))) + + a = run(controller, v_ego=25.0, v_cruise=30.0, lead=lead, model=model, a_chill=0.6, a_exp=0.9, frames=40) + assert a > 0.75, f"Lead barrier should remain transparent when headway is safe, got {a}" -def test_jerk_slew_limits_single_frame_step(): +def test_cut_in_emergency_braking_ramp_rate(): controller = make_controller(prev=0.0) - lead = FakeLead(status=False) - model = FakeModel(velocity=[20.0] * 20) - a = controller.update(20.0, 30.0, lead, model, 2.0, 2.0) - assert a == pytest.approx(controller.MAX_JERK * controller.DT, abs=1e-3) + cut_in_lead = FakeLead(status=True, d_rel=6.0, v_lead=10.0) + model = FakeModel(velocity=[20.0] * 33) + + for _ in range(8): + a = controller.update(20.0, 25.0, cut_in_lead, model, a_chill=-3.2, a_exp=0.0) + assert a <= -2.5, f"Emergency cut-in should ramp braking power quickly, got {a}" + +def test_asymmetric_jerk_slew_rates(): + controller_accel = make_controller(prev=0.0) + a_up = controller_accel.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 4.0, 4.0) + assert a_up == pytest.approx(controller_accel.MAX_JERK_ACCEL * controller_accel.DT, abs=1e-3) + + controller_brake = make_controller(prev=0.0) + a_down = controller_brake.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), -1.0, -1.0) + assert a_down == pytest.approx(-controller_brake.MAX_JERK_BRAKE * controller_brake.DT, abs=1e-3) -def test_reset_clears_integrator_state(): - controller = make_controller(prev=0.0) - lead = FakeLead(status=True, d_rel=20.0, v_lead=15.0) - model = FakeModel(velocity=[20.0] * 20) - controller.update(20.0, 30.0, lead, model, -1.0, -2.0) - assert controller.prev_a_target != 0.0 - controller.reset(0.0) - assert controller.prev_a_target == 0.0 +def test_directional_brake_release_rate(): + controller = make_controller(prev=-3.0) + a_rel = controller.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 0.0, 0.0) + expected = -3.0 + controller.MAX_JERK_ACCEL * controller.DT + assert a_rel == pytest.approx(expected, abs=1e-3) -def test_set_tuning_clamps_to_documented_ranges(): +def test_profile_limits_dynamic_update(): + controller = make_controller() + controller.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 0.5, 0.5, t_follow=1.1, jerk_factor=1.3) + assert controller.t_follow == pytest.approx(1.1, abs=1e-4) + assert controller.jerk_factor == pytest.approx(1.3, abs=1e-4) + assert controller.MAX_JERK_ACCEL == pytest.approx(5.5 * 1.3, abs=1e-3) + +def test_tuning_clamping(): controller = make_controller() controller.set_tuning(5.0, 9.0) assert controller.HYBRID_EXP_BIAS == 1.0 @@ -111,15 +180,83 @@ def test_set_tuning_clamps_to_documented_ranges(): controller.set_tuning(-5.0, -1.0) assert controller.HYBRID_EXP_BIAS == -1.0 assert controller.VISION_BRAKE_SENSITIVITY == 0.0 - controller.set_tuning(0.5, 1.5) - assert controller.HYBRID_EXP_BIAS == 0.5 - assert controller.VISION_BRAKE_SENSITIVITY == 1.5 -def test_missing_model_velocity_falls_back_to_ego_speed(): +def test_reset_seeds_active_acceleration(): + controller = make_controller(prev=0.0) + controller.reset(a_ego=-1.8) + assert controller.prev_a_target == -1.8 + + +def test_missing_or_corrupt_model_v2_fallbacks(): controller = make_controller() - model = FakeModel(velocity=None) - lead = FakeLead(status=False) - # Falls back to a constant-velocity horizon, so no vision braking is injected. - a = run(controller, lead=lead, model=model, a_chill=0.5, a_exp=0.2) - assert a > 0.4 + model_none = FakeModel(velocity=None, position=None) + a = run(controller, lead=FakeLead(status=False), model=model_none, a_chill=0.5, a_exp=0.2) + assert a > 0.35, "Controller should gracefully fallback when trajectory is None" + + model_empty = FakeModel(velocity=[], position=[]) + a = run(controller, lead=FakeLead(status=False), model=model_empty, a_chill=0.6, a_exp=0.7) + assert a > 0.55, "Controller should gracefully handle empty trajectory lists" + + +def test_nan_a_exp_falls_back_to_chill_channel(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) + a = run(controller, model=model, a_chill=0.5, a_exp=float("nan"), frames=10) + assert np.isfinite(a), f"NaN a_exp must never propagate, got {a}" + assert 0.4 <= a <= 0.6, f"NaN a_exp should degrade to the Chill channel, got {a}" + + +def test_inf_a_exp_does_not_propagate(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) + for a_exp in (float("inf"), float("-inf")): + controller.reset(0.0) + a = run(controller, model=model, a_chill=0.5, a_exp=a_exp, frames=10) + assert np.isfinite(a), f"Non-finite a_exp={a_exp} must not propagate, got {a}" + + +def test_nan_a_chill_falls_back_to_last_target(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) + a = run(controller, model=model, a_chill=float("nan"), a_exp=0.8, frames=10) + assert np.isfinite(a), f"NaN a_chill must never propagate, got {a}" + + +def test_nan_in_trajectory_does_not_propagate(): + controller = make_controller(prev=0.0) + v = [20.0] * 20 + [float("nan")] + [20.0] * 12 + model = FakeModel(velocity=v, position=list(np.linspace(0.0, 100.0, 33))) + a = run(controller, model=model, a_chill=0.5, a_exp=0.8, frames=10) + assert np.isfinite(a), f"NaN trajectory must not propagate, got {a}" + + +def test_reset_rejects_non_finite_seed(): + controller = make_controller(prev=-1.5) + controller.reset(float("nan")) + assert controller.prev_a_target == 0.0 + + +def test_standstill_holds_without_model_brake_at_predicted_stop(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=np.zeros(33), position=np.zeros(33)) + a = run(controller, v_ego=0.0, v_cruise=25.0, lead=FakeLead(status=False), + model=model, a_chill=0.8, a_exp=0.0, frames=40) + assert a <= -0.4, f"Anchor must hold at a predicted stop even with cruise creep, got {a}" + + +def test_green_light_departure_releases_anchor(): + controller = make_controller(prev=-0.5) + traj_v = np.linspace(0.0, 12.0, 33) + traj_x = np.linspace(0.0, 45.0, 33) + model = FakeModel(velocity=traj_v, position=traj_x) + a = run(controller, v_ego=0.0, v_cruise=20.0, lead=FakeLead(status=False), + model=model, a_chill=1.0, a_exp=1.5, frames=40) + assert a > 1.0, f"Green light departure must still release the anchor, got {a}" + + +def test_vision_stop_uses_emergency_brake_ramp(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=list(np.linspace(20.0, 0.2, 33)), position=list(np.linspace(0.0, 40.0, 33))) + out = [controller.update(20.0, 25.0, FakeLead(status=False), model, 0.0, -3.5) for _ in range(5)] + assert out[-1] <= -2.5, f"Vision stop should ramp braking fast, got {out}" \ No newline at end of file diff --git a/tools/replay/mode_sim.py b/tools/replay/mode_sim.py index 43e0b7f49..671fcb446 100644 --- a/tools/replay/mode_sim.py +++ b/tools/replay/mode_sim.py @@ -532,7 +532,7 @@ def run_simulation(grid, bufs, toggles): a_exp = 0.0 # Hybrid Experimental Mode continuous fusion - a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp) + a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp, t_follow=t_follow) authority = hybrid.exp_authority model_v0 = v_ego