diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_turn_planner.py b/openpilot/sunnypilot/modeld_v2/tests/test_turn_planner.py index 2526905333..82fb2cc12b 100644 --- a/openpilot/sunnypilot/modeld_v2/tests/test_turn_planner.py +++ b/openpilot/sunnypilot/modeld_v2/tests/test_turn_planner.py @@ -55,14 +55,14 @@ class TestTurnPlanner(OpenpilotTestCase): assert out == k def test_sign_preservation_and_magnitude_bound_under_jitter(self): - """Output must never oppose model_curv or exceed |model_curv|, even after entry jitter.""" + """Output must never oppose model_curv or exceed APEX_PATH_BOOST_CAP * |model_curv|.""" tp = TurnPlanner(wheelbase=2.89) # Reproduces a5 seg31 Turn 1: brief negative jitter then sharp positive right turn ks = [-0.015] * 15 + [0.016, 0.05, 0.11, 0.14, 0.17, 0.18, 0.14, 0.08, 0.02, -0.01] plan_right = make_plan(0.15, 3.0) for k in ks: out = tp.update(plan_right, v_ego=3.0, actual_curv=k * 0.7, active=True, is_rhd=False, model_curv=k) - assert abs(out) <= abs(k) + 1e-9 + assert abs(out) <= abs(k) * 1.35 + 1e-9 if k > 0: assert out >= 0.0 elif k < 0: @@ -81,7 +81,7 @@ class TestTurnPlanner(OpenpilotTestCase): for i in range(1, 20): k = max(0.0, 0.14 * (1.0 - i / 15.0)) out = tp.update(make_plan(k, 4.0), v_ego=4.0, actual_curv=k + 0.01, active=True, is_rhd=False, model_curv=k) - assert abs(out - k) < 1e-9 + assert abs(out - k) < 1e-3 def test_standstill_launch_and_mid_turn_pause(self): tp = TurnPlanner(wheelbase=2.89) @@ -105,10 +105,10 @@ class TestTurnPlanner(OpenpilotTestCase): def test_wheelbase_and_rhd_scaling(self): tp_short = TurnPlanner(wheelbase=2.4) tp_long = TurnPlanner(wheelbase=3.4) - plan_r = make_plan(0.14, 3.5) + plan_r = make_plan(0.14, 4.2) for _ in range(25): - tp_short.update(plan_r, 3.5, 0.0, True, False, 0.02) - tp_long.update(plan_r, 3.5, 0.0, True, False, 0.02) + tp_short.update(plan_r, 4.2, 0.0, True, False, 0.02) + tp_long.update(plan_r, 4.2, 0.0, True, False, 0.02) assert tp_long.shift_dist > tp_short.shift_dist * 1.3 # RHD gives larger shift on left turns (k < 0) than right turns (k > 0) @@ -178,14 +178,33 @@ class TestTurnPlanner(OpenpilotTestCase): assert plan_ovr < 0.0 # stays inside the oncoming yellow line def test_rolling_speed_taper(self): - """Shift must taper smoothly down at rolling turn speeds (8-13.5 mph) and be 0 at 14+ mph.""" + """Shift is strong at low speed, light at rolling speed, 0 at 19+ mph, and time-capped at crawl.""" + tp_crawl = TurnPlanner(wheelbase=2.89) tp_low = TurnPlanner(wheelbase=2.89) - tp_mid = TurnPlanner(wheelbase=2.89) + tp_roll = TurnPlanner(wheelbase=2.89) tp_fast = TurnPlanner(wheelbase=2.89) for _ in range(30): - tp_low.update(make_plan(0.14, 3.0), v_ego=3.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02) - tp_mid.update(make_plan(0.14, 4.75), v_ego=4.75, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02) - tp_fast.update(make_plan(0.14, 6.5), v_ego=6.5, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02) - assert tp_low.shift_dist > 1.5 - assert 0.5 < tp_mid.shift_dist < 1.1 + tp_crawl.update(make_plan(0.14, 1.8), v_ego=1.8, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02) + tp_low.update(make_plan(0.14, 3.5), v_ego=3.5, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02) + tp_roll.update(make_plan(0.14, 6.0), v_ego=6.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02) + tp_fast.update(make_plan(0.14, 9.0), v_ego=9.0, actual_curv=0.0, active=True, is_rhd=False, model_curv=0.02) + assert tp_crawl.shift_dist <= 1.8 * 0.48 + 0.05 # time-capped so crawl starts don't stall + assert tp_low.shift_dist > 1.3 + assert 0.3 < tp_roll.shift_dist < 0.7 assert tp_fast.shift_dist == 0.0 + + def test_apex_1s_path_boost_and_exit_unwind(self): + """Once past entry, blends with 1s path to boost under-steering apex and unwind hanging exit.""" + tp = TurnPlanner(wheelbase=2.89) + # Drive past entry (psi > 30 deg) + for _ in range(35): + tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10) + # Apex (psi ~ 48 deg): action under-steers at 0.10 while 1s path says 0.14 -> boosted cleanly above 0.10 + apex_out = tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.10) + assert apex_out > 0.115 + # Advance past 60 deg into late-turn exit + for _ in range(20): + tp.update(make_plan(0.14, 4.0), v_ego=4.0, actual_curv=0.12, active=True, is_rhd=False, model_curv=0.14) + # Exit hang: action still stuck at 0.20 while 1s path already unwound to 0.10 -> pulls down cleanly + exit_out = tp.update(make_plan(0.10, 4.0), v_ego=4.0, actual_curv=0.15, active=True, is_rhd=False, model_curv=0.20) + assert exit_out < 0.15 diff --git a/openpilot/sunnypilot/modeld_v2/turn_planner.py b/openpilot/sunnypilot/modeld_v2/turn_planner.py index d35210b619..7f4f0a3150 100644 --- a/openpilot/sunnypilot/modeld_v2/turn_planner.py +++ b/openpilot/sunnypilot/modeld_v2/turn_planner.py @@ -9,36 +9,48 @@ from collections import deque import numpy as np from openpilot.common.realtime import DT_MDL -from openpilot.sunnypilot.modeld_v2.constants import Plan +from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan -# Rear-axle protected turn-in shaping parameters. -# Models operating on cropped forward-looking views plan front/camera-centric -# trajectories and initiate tight turns early, causing the rear axle to off-track -# inside the front path by ~L^2 / (2R) and clip the inside curb. +# Two-stage rear-axle protected turn planner: +# Stage 1 (Entry): wheelbase-scaled spatial delay on curvature buildup so the +# inside rear tire clears the corner curb instead of off-tracking over it. +# Capped in time (MAX_SHIFT_TIME) so crawl-speed starts never feel stalled. +# Stage 2 (Apex & Exit): once past initial corner entry, smoothly blends +# action curvature with the model's own 1-second path curvature so tight +# apexes don't under-steer/go wide and exits unwind cleanly. SHIFT_WHEELBASE_RATIO = 0.58 # fraction of wheelbase to delay turn-in (~1.68 m for 2.89 m WB) WHEELBASE_CLIP = (2.0, 4.0) # m -OPPOSITE_SIDE_WEIGHT = 0.75 # slightly lighter shift for non-curb-side turns +OPPOSITE_SIDE_WEIGHT = 0.70 # lighter shift for non-curb-side turns +MAX_SHIFT_TIME = 0.48 # s, caps entry delay at crawl speeds (2-5 mph) -# Taper shift down where lateral accel/jerk limits (3.0/v^2, 5.0/v^2) leave less -# curvature headroom for a tighter late-apex recovery (full <=7.8 mph, off >=13.4 mph). -SPEED_TAPER = (3.5, 6.0) # m/s (~7.8-13.4 mph) -CURV_GATE = (0.04, 0.09) # 1/m (R=25 m -> R=11 m): ignores gentle curves, full on tight turns -PHASE_RELEASE_DEG = (8.0, 25.0) # deg of turn heading completed: release early for snappy apex turn-in +TURN_SPEED_TAPER = (7.5, 10.5) # m/s (~17-23.5 mph): overall active window +SHIFT_SPEED_BP = (3.5, 6.0, 8.5) # m/s (~7.8, 13.4, 19.0 mph) +SHIFT_SPEED_V = (1.0, 0.35, 0.0) # full from stop, light on rolling turns, 0 above 19 mph +CURV_GATE = (0.04, 0.09) # 1/m (R=25 m -> R=11 m): ignores gentle curves -SHIFT_BUILD_TAU = 0.15 # s, smooths shift ramp-in -SHIFT_RELEASE_TAU = 0.08 # s, releases shift crisply once turn is underway -BUFFER_MAX_DIST = 4.5 # m, distance history retained for spatial lookup +ENTRY_PHASE_RELEASE_DEG = (8.0, 22.0) # deg: release entry shift crisply once rotating +APEX_BLEND_PHASE_DEG = (10.0, 25.0) # deg: ramp in 1s-path blend through apex & exit +APEX_PATH_BLEND_MAX = 0.65 # max weight on 1s path curvature during apex/exit +APEX_PATH_BOOST_CAP = 1.35 # max ratio vs |model_curv| for safety +PATH_AVG_HORIZON_S = 1.0 # s, horizon for average path curvature + +SHIFT_BUILD_TAU = 0.15 # s +SHIFT_RELEASE_TAU = 0.07 # s +APEX_BLEND_TAU = 0.12 # s +BUFFER_MAX_DIST = 4.5 # m MIN_MOVE_SPEED = 0.3 # m/s -MID_TURN_CURV_THRESH = 0.02 # 1/m, threshold to recognize a pause mid-turn -PLAN_S_RANGE = (1.0, 18.0) # m, lookahead window for upcoming turn peak curvature -PLAN_MIN_SEG_DS = 0.8 # m, minimum 2-step arc length to suppress knot noise +MID_TURN_CURV_THRESH = 0.02 # 1/m +PLAN_S_RANGE = (1.0, 18.0) # m +PLAN_MIN_SEG_DS = 0.8 # m + +T_IDXS = np.array(ModelConstants.T_IDXS, dtype=np.float32) -def compute_plan_peak_curv(model_output: dict[str, np.ndarray]) -> float: - """Estimate peak upcoming curvature within PLAN_S_RANGE from model_output['plan'].""" +def compute_plan_metrics(model_output: dict[str, np.ndarray]) -> tuple[float, float]: + """Return (peak_upcoming_curv, avg_1s_path_curv) from model_output['plan'].""" plan_arr = model_output.get('plan') if plan_arr is None or plan_arr.ndim < 3 or plan_arr.shape[1] < 3: - return 0.0 + return 0.0, 0.0 plan = plan_arr[0] pos = plan[:, Plan.POSITION] @@ -59,22 +71,30 @@ def compute_plan_peak_curv(model_output: dict[str, np.ndarray]) -> float: k = float((yaws[i + 2] - yaws[i]) / seg_ds) if abs(k) > abs(peak_k): peak_k = k - return float(np.clip(peak_k, -0.25, 0.25)) + peak_k = float(np.clip(peak_k, -0.25, 0.25)) + + if len(yaws) == len(T_IDXS): + s_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, s)) + yaw_1s = float(np.interp(PATH_AVG_HORIZON_S, T_IDXS, yaws)) + path_k_1s = float(np.clip(yaw_1s / s_1s, -0.25, 0.25)) if s_1s >= PLAN_MIN_SEG_DS else 0.0 + else: + path_k_1s = 0.0 + + return peak_k, path_k_1s + + +def compute_plan_peak_curv(model_output: dict[str, np.ndarray]) -> float: + return compute_plan_metrics(model_output)[0] class TurnPlanner: """ - Asymmetric spatial turn-in shaper that protects the inside rear wheel during tight turns. + Two-stage rear-axle protected turn planner for tight low-speed turns. - During turn entry of sharp low-speed turns, delays curvature buildup by a - wheelbase-proportional distance so the rear axle advances deeper into the - intersection before curving in. - - Asymmetric: only delays curvature buildup; unwinding (exit) and sign reversals - pass through with zero lag. - - Phase-aware: smoothly releases the spatial shift as accumulated turn heading - passes the corner apex, allowing crisp destination-lane alignment. - - Strictly bounded: output always shares the sign of model_curv and never - exceeds |model_curv|. + Stage 1 (Entry): delays curvature buildup by a wheelbase-proportional distance + (time-capped at MAX_SHIFT_TIME) so the rear axle clears the inside curb. + Stage 2 (Apex & Exit): blends action curvature with the model's 1-second path + curvature once past entry, preventing wide apexes and exit hang-ups. """ def __init__(self, wheelbase: float = 2.7, dt: float = DT_MDL): @@ -86,7 +106,9 @@ class TurnPlanner: self.turn_sign = 0.0 self.turn_psi = 0.0 self.shift_dist = 0.0 + self.apex_blend = 0.0 self.plan_peak_curv = 0.0 + self.path_curv_1s = 0.0 def reset(self) -> None: self.s = 0.0 @@ -95,7 +117,9 @@ class TurnPlanner: self.turn_sign = 0.0 self.turn_psi = 0.0 self.shift_dist = 0.0 + self.apex_blend = 0.0 self.plan_peak_curv = 0.0 + self.path_curv_1s = 0.0 def update(self, model_output: dict[str, np.ndarray], v_ego: float, actual_curv: float, active: bool, is_rhd: bool, model_curv: float, enabled: bool = True) -> float: @@ -110,6 +134,7 @@ class TurnPlanner: self.turn_psi = 0.0 self.turn_sign = 0.0 self.shift_dist = 0.0 + self.apex_blend = 0.0 self.buf_s.clear() self.buf_k.clear() self.buf_s.append(self.s) @@ -124,21 +149,23 @@ class TurnPlanner: self.buf_s.popleft() self.buf_k.popleft() - speed_w = float(np.interp(v_ego, SPEED_TAPER, [1.0, 0.0])) - if speed_w <= 0.0 and self.shift_dist < 1e-3: + turn_speed_w = float(np.interp(v_ego, TURN_SPEED_TAPER, [1.0, 0.0])) + if turn_speed_w <= 0.0 and self.shift_dist < 1e-3 and self.apex_blend < 1e-3: self.turn_psi = 0.0 self.turn_sign = 0.0 self.plan_peak_curv = 0.0 + self.path_curv_1s = 0.0 return model_curv - plan_peak = compute_plan_peak_curv(model_output) + plan_peak, path_k_1s = compute_plan_metrics(model_output) self.plan_peak_curv = plan_peak + self.path_curv_1s = path_k_1s tightness = max(abs(model_curv), abs(plan_peak)) curv_w = float(np.interp(tightness, CURV_GATE, [0.0, 1.0])) ref_k = model_curv if abs(model_curv) > 0.015 else plan_peak - if abs(ref_k) > 0.035 and speed_w > 0.05: + if abs(ref_k) > 0.035 and turn_speed_w > 0.05: new_sign = 1.0 if ref_k > 0.0 else -1.0 if new_sign != self.turn_sign: self.turn_sign = new_sign @@ -150,24 +177,38 @@ class TurnPlanner: self.turn_sign = 0.0 psi_deg = math.degrees(self.turn_psi) - phase_w = float(np.interp(psi_deg, PHASE_RELEASE_DEG, [1.0, 0.0])) - # In LHD (not is_rhd), right turns (curv > 0) hug the near-side curb; in RHD, left turns (curv < 0) do. + # Stage 1: Entry rear-axle spatial shift (time-capped so crawl-speed starts stay responsive) + shift_speed_w = float(np.interp(v_ego, SHIFT_SPEED_BP, SHIFT_SPEED_V)) + entry_phase_w = float(np.interp(psi_deg, ENTRY_PHASE_RELEASE_DEG, [1.0, 0.0])) is_curb_side = (ref_k < 0.0) if is_rhd else (ref_k > 0.0) side_w = 1.0 if is_curb_side else OPPOSITE_SIDE_WEIGHT - target_shift = SHIFT_WHEELBASE_RATIO * self.wheelbase * speed_w * curv_w * phase_w * side_w + raw_shift = SHIFT_WHEELBASE_RATIO * self.wheelbase * shift_speed_w * curv_w * entry_phase_w * side_w + target_shift = min(raw_shift, v_ego * MAX_SHIFT_TIME) tau = SHIFT_RELEASE_TAU if target_shift < self.shift_dist else SHIFT_BUILD_TAU - alpha = self.dt / (tau + self.dt) - self.shift_dist += (target_shift - self.shift_dist) * alpha + self.shift_dist += (target_shift - self.shift_dist) * (self.dt / (tau + self.dt)) - if self.shift_dist < 0.05 or len(self.buf_s) < 2: - return model_curv + out_k = model_curv + if self.shift_dist >= 0.05 and len(self.buf_s) >= 2: + k_delayed = float(np.interp(self.s - self.shift_dist, self.buf_s, self.buf_k)) + # Same sign as model_curv (or 0), never exceeds |model_curv|, zero lag on unwind. + if model_curv >= 0.0: + out_k = float(np.clip(k_delayed, 0.0, model_curv)) + else: + out_k = float(np.clip(k_delayed, model_curv, 0.0)) - k_delayed = float(np.interp(self.s - self.shift_dist, self.buf_s, self.buf_k)) + # Stage 2: Apex & Exit 1s-path blend (prevents wide apexes and exit hang-ups) + apex_phase_w = float(np.interp(psi_deg, APEX_BLEND_PHASE_DEG, [0.0, 1.0])) + target_blend = APEX_PATH_BLEND_MAX * turn_speed_w * curv_w * apex_phase_w + self.apex_blend += (target_blend - self.apex_blend) * (self.dt / (APEX_BLEND_TAU + self.dt)) - # Hard invariant: same sign as model_curv (or 0) and |out_k| <= |model_curv|. - # Holds back curvature buildup on entry, passes unwinding/exit through with zero lag. - if model_curv >= 0.0: - return float(np.clip(k_delayed, 0.0, model_curv)) - return float(np.clip(k_delayed, model_curv, 0.0)) + if self.apex_blend > 0.01 and out_k * path_k_1s > 0.0: + # Through the apex, boost when 1s path asks for tighter curvature than action (prevents wide turns). + # Only pull curvature down toward 1s path late in the turn (psi > 60 deg) to prevent exit hang-ups. + if abs(path_k_1s) > abs(out_k) or psi_deg > 60.0: + max_boosted = abs(model_curv) * APEX_PATH_BOOST_CAP + target_k = float(np.clip(path_k_1s, -max_boosted, max_boosted)) + out_k = (1.0 - self.apex_blend) * out_k + self.apex_blend * target_k + + return out_k