modeld: add apex/exit 1s-path blend and crawl-speed time cap to turn planner

This commit is contained in:
Isaac Barham
2026-11-20 14:00:10 -05:00
parent adc286412a
commit be43e286e4
2 changed files with 121 additions and 61 deletions
@@ -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
+89 -48
View File
@@ -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