From 3c49d47d01e4e8d33eb97d0a32603dc67cca4f2f Mon Sep 17 00:00:00 2001 From: Prabhaav Pillai Date: Thu, 27 Aug 2026 16:58:54 -0400 Subject: [PATCH] hem rolled through sign temp exp test --- .../controls/lib/hybrid_experimental_mode.py | 139 +++--------------- 1 file changed, 18 insertions(+), 121 deletions(-) diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index 4ec775b99..1b37130d4 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -1,11 +1,9 @@ #!/usr/bin/env python3 -import time import math from openpilot.common.realtime import DT_MDL def lerp(a: float, b: float, t: float) -> float: - """Linear interpolation between a and b by weight t (0.0 to 1.0).""" return (1.0 - t) * a + t * b @@ -15,36 +13,30 @@ def clamp(val: float, low: float, high: float) -> float: class HybridExperimentalMode: """ - Hybrid Experimental Mode (HEM) - 1. Open Road: Follows Chill MPC cruise & radar headway. - 2. Slower Lead Approach: Blends smooth vision decel with MPC follow distance. - 3. Red Lights / Stop Signs: Latches pure vision braking with active zero-speed ramp. - 4. Rolling-Dip Immune: Decelerates all the way to 0 m/s even if model trajectory rises at low speeds. - 5. Standstill Hold & Departure: Locks mechanical brake hold until green light/lead departure/gas press. + TEMPORARY PASS-THROUGH: Pure Experimental Mode only. + Directly outputs experimental acceleration and experimental stop signals. """ - STANDSTILL_SPEED = 0.6 # m/s (~1.3 mph) - STOP_ENGAGE_DECEL = -0.15 # m/s^2 (threshold to enter stopping state) - MINOR_DIFF_THRESHOLD = 0.25 # m/s^2 + STANDSTILL_SPEED = 0.6 + STOP_ENGAGE_DECEL = -0.15 + MINOR_DIFF_THRESHOLD = 0.25 def __init__(self): self.DT = DT_MDL - self.w_vision = 0.0 + self.w_vision = 1.0 self.prev_a_target = 0.0 - self.last_exp_dominant = False + self.last_exp_dominant = True self.stopping_latched = False self.diag = {} self.record_diag = False - # Tunings self.HYBRID_EXP_BIAS = 0.0 self.VISION_BRAKE_SENSITIVITY = 1.0 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 math.isfinite(a_ego) else 0.0 - self.w_vision = 0.0 - self.last_exp_dominant = False + self.w_vision = 1.0 + self.last_exp_dominant = True self.stopping_latched = False self.diag = {} @@ -55,116 +47,21 @@ class HybridExperimentalMode: def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp, should_stop_exp=False, should_stop_chill=False, gas_pressed=False): - # 0. Sanitize inputs - if not math.isfinite(a_chill): - a_chill = self.prev_a_target - if not math.isfinite(a_exp): - a_exp = a_chill - - # 1. Trajectory Analysis - traj_v = getattr(getattr(model_v2, "velocity", None), "x", None) - has_full_trajectory = bool(traj_v and len(traj_v) >= 24 and all(math.isfinite(v) for v in traj_v)) - - if has_full_trajectory: - v_horizon = float(traj_v[-1]) - v_short = float(traj_v[23]) # ~4.0s lookahead - v_min = float(min(traj_v)) - else: - v_horizon = v_short = v_min = float(v_ego) - - lead_status = bool(getattr(lead_one, "status", False)) - lead_v = float(getattr(lead_one, "vLead", 0.0)) - lead_d = float(getattr(lead_one, "dRel", 150.0)) - - # 2. Standstill & Departure Detection - at_standstill = v_ego < self.STANDSTILL_SPEED - driver_override = bool(gas_pressed) - lead_departing = at_standstill and lead_status and (lead_v > 0.6) and ((lead_v - v_ego) > 0.4) - # Verified green light: sustained positive model accel and open lookahead - vision_departing = at_standstill and (v_horizon > 3.0) and (v_short > 1.5) and (a_exp > 0.25) - is_departing = lead_departing or vision_departing or driver_override - - # 3. Stopping Latch Management - # Trigger stopping latch on real vision decel, stop request, or low-speed stop line approach - wants_stopping = ( - (a_exp < (a_chill - 0.05) and a_exp < self.STOP_ENGAGE_DECEL) or - should_stop_exp or - (v_ego < 3.5 and v_min < 1.0 and a_exp < 0.0) - ) - - if driver_override or is_departing: - self.stopping_latched = False - elif wants_stopping: - self.stopping_latched = True - elif self.stopping_latched and not at_standstill: - # Only clear latch while rolling if the road clearly opens (e.g. light turns green mid-slowdown) - if a_exp > 0.40 and v_min > (v_ego * 0.95) and not should_stop_exp: - self.stopping_latched = False - - # 4. Output Arbitration & Regime Fusion - if self.stopping_latched: - self.last_exp_dominant = True - self.w_vision = 1.0 - - if a_exp < -0.20: - # Vision model is actively commanding strong decel - a_vision_brake = a_exp * self.VISION_BRAKE_SENSITIVITY - a_out = min(a_chill, a_vision_brake) - else: - # Rolling dip / low-speed completion: enforce smooth decel ramp to 0 m/s - # (Prevents coasting/surging when model trajectory bottoms out at ~3 mph) - a_stop_ramp = -clamp(v_ego * 0.75 + 0.35, 0.45, 1.20) - a_out = min(a_chill, a_stop_ramp) - - else: - # CRUISE / THROTTLE REGIME - self.last_exp_dominant = False - a_diff = a_exp - a_chill - - if a_diff < 0.0 and a_exp < 0.0: - # Transient mild deceleration - a_out = min(a_chill, a_exp) - self.w_vision = max(0.0, self.w_vision - 0.05) - elif abs(a_diff) <= self.MINOR_DIFF_THRESHOLD: - # Minor difference: smooth blend between Chill and Exp - blend_weight = abs(a_diff) / self.MINOR_DIFF_THRESHOLD - a_out = lerp(a_chill, a_exp, blend_weight * 0.5) - self.w_vision = max(0.0, self.w_vision - 0.05) - else: - # Open road acceleration governed by Chill MPC + optional exp bias - a_out = a_chill + max(0.0, a_diff) * self.HYBRID_EXP_BIAS - self.w_vision = max(0.0, self.w_vision - 0.05) - - # 5. Authoritative Standstill Handshake - # Bring to full mechanical stop hold when stopping is latched near standstill - standstill_intent = not is_departing and ( - should_stop_exp or - (self.stopping_latched and v_ego < 1.2) or - (v_ego < 0.5 and (v_horizon < 1.0 or a_exp < -0.05)) - ) - should_stop_fused = bool(should_stop_chill or standstill_intent) + # Sanitize exp acceleration fallback to previous target if invalid + a_out = float(a_exp) if math.isfinite(a_exp) else self.prev_a_target + should_stop_fused = bool(should_stop_exp) if self.record_diag: self.diag = { - "v_ego": v_ego, "v_cruise": v_cruise, - "a_chill": a_chill, "a_exp": a_exp, + "v_ego": v_ego, + "v_cruise": v_cruise, + "a_chill": a_chill, + "a_exp": a_exp, "should_stop_chill": should_stop_chill, "should_stop_exp": should_stop_exp, - "lead_status": lead_status, "lead_d_rel": lead_d, "lead_v_lead": lead_v, - "has_full_trajectory": has_full_trajectory, - "v_horizon": v_horizon, "v_short": v_short, "v_min": v_min, - "w_vision": self.w_vision, - "lead_departing": lead_departing, - "vision_departing": vision_departing, - "driver_override": driver_override, - "is_departing": is_departing, - "stopping_latched": self.stopping_latched, - "exp_dominant": self.last_exp_dominant, - "standstill_intent": standstill_intent, - "should_stop_fused": should_stop_fused, "a_out": a_out, - "regime": "pure_brake" if self.stopping_latched else "cruise", - "standstill": standstill_intent, + "should_stop_fused": should_stop_fused, + "regime": "pure_exp_passthrough", } self.prev_a_target = a_out