mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-09 20:33:43 +08:00
modeld_v2: FSD-timed turns - the plan places the corner, FSD's own turns set when and how hard
Behind the turn switch (TurnInHold, now "FSD-timed turns"), it replaces the
learned Light Speed 2 correction (8d8b83482), which only scaled how much the
model steers and could not move when.
From qlogs of routes 85-b1, FSD's 21 turns of 60-120 deg follow a speed rule:
peak curvature 0.190 - 0.018 v (held out within 0.010 1/m; the driver's own
turns sit at 1.05x, openpilot's at 0.93x and 1.7 m early), with a curvature
shape over time around the half-heading point (median of 23 FSD turns). In a
turn the plan shows (>= 45 deg with the cue, 60 without, under 9 m/s, for
0.5 s), the command is that rule's peak at the car's speed, taken 8% low for
the first road test, with the shape stretched so it adds up to the turn's
heading (what is turned plus what the plan still shows, only revised down past
the middle), placed at the plan's half-heading point and looked up lat_action_t
ahead. Near a stop the timing counts 3 m/s, so the wheel winds in while pulling
away as FSD does; past the middle it uses the speed there. It blends in over
0.5 s, hands back over 1 s, and drops a turn the plan stops showing. Outside
it the floor and the slow-turn path cap still apply.
Replayed on 26 FSD turns of routes 9e-a4 (Light Speed 2 in shadow), each
route scored with the template and rule refit on the others: peak vs FSD's
0.78 -> 0.90 [IQR 0.78-1.12], turn-in +0.20 s -> +0.00 s, exit -0.20 s ->
+0.15 s, RMS before the middle 0.0225 -> 0.0198, after it 0.0089 -> 0.0089.
Replayed on the fine-tune's crawl turns (route af) it asks 0.65-0.76x of the
full lock the car took. Open-loop replays only; not driven.
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
Isaac Barham
@@ -125,10 +125,10 @@ class ModelsLayoutMici(NavScroller):
|
||||
"while the model still sets the cruising speed. Turns off the accel boost. " +
|
||||
"Needs openpilot longitudinal. Not available with the Default model."))
|
||||
|
||||
self.path_steering_toggle = BigParamControl(tr("turn correction (alpha)"), "TurnInHold",
|
||||
description=tr("In slow turns, corrects the model's steering toward the line FSD and you take, with a correction learned for " +
|
||||
"that model from your drives. It can only move steering within the range the model's own plan allows. A model " +
|
||||
"without a learned correction is only kept from turning tighter than its planned line at your speed. " +
|
||||
self.path_steering_toggle = BigParamControl(tr("FSD-timed turns (alpha)"), "TurnInHold",
|
||||
description=tr("In turns the model's plan shows (under about 20 mph), steers when and how hard FSD does: the model says where " +
|
||||
"the corner is and how far it turns, FSD's own turns set when to start and the peak for your speed, a little " +
|
||||
"under FSD for now. Outside those turns, a slow turn is kept from going tighter than the model's planned line. " +
|
||||
"Not available with the Default model."))
|
||||
|
||||
self.map_turn_toggle = BigParamControl(tr("signal navigation (experimental)"), "MapboxNextTurnEnabled",
|
||||
|
||||
@@ -0,0 +1,119 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
# FSD-timed turns: the model's plan says where the corner is and how much it turns; FSD's own turns say when to start
|
||||
# turning and how hard. From qlogs of routes 85-b1 (curvature from the wheel angle), FSD's 21 turns of 60-120 deg:
|
||||
# peak curvature = 0.190 - 0.018 v (held out, within 0.010 1/m; the driver's own turns sit at 1.05x of it)
|
||||
# and its curvature over time around the half-heading point follows TEMPLATE (median of 23 FSD turns >= 45 deg,
|
||||
# normalized to its peak): rising from ~3 s before, peaking just after, gone ~3 s after.
|
||||
# openpilot's turns (mostly Light Speed 2) turned in 1.7 m earlier than that and peaked 0.93x.
|
||||
# A turn's profile here is the rule's peak at the car's speed, the template stretched in time so the turn adds up to
|
||||
# the heading the plan (plus what is already turned) says it needs, placed at the plan's half-heading point.
|
||||
TAU = np.arange(-5.0, 5.01, 0.1) # s from the half-heading point
|
||||
TEMPLATE = np.array([
|
||||
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.002, 0.006, 0.008, 0.008, 0.008, 0.008, 0.014, 0.027, 0.043,
|
||||
0.046, 0.048, 0.051, 0.056, 0.072, 0.089, 0.109, 0.133, 0.172, 0.212, 0.261, 0.309, 0.359, 0.414, 0.454, 0.488, 0.523,
|
||||
0.575, 0.644, 0.683, 0.733, 0.784, 0.824, 0.871, 0.889, 0.915, 0.928, 0.940, 0.954, 0.956, 0.966, 0.972, 0.979, 0.985,
|
||||
0.986, 0.989, 0.978, 0.973, 0.956, 0.929, 0.894, 0.857, 0.814, 0.779, 0.738, 0.696, 0.649, 0.607, 0.581, 0.552, 0.509,
|
||||
0.465, 0.445, 0.397, 0.345, 0.315, 0.288, 0.260, 0.230, 0.198, 0.167, 0.139, 0.115, 0.099, 0.085, 0.072, 0.062, 0.058,
|
||||
0.054, 0.045, 0.038, 0.029, 0.022, 0.021, 0.017, 0.013, 0.010, 0.009, 0.009, 0.008, 0.000, 0.000, 0.000, 0.000])
|
||||
TEMPLATE /= TEMPLATE.max()
|
||||
TEMPLATE_WIDTH = float(np.trapezoid(TEMPLATE, TAU)) # s: the integral of curvature/peak over time
|
||||
RULE_INTERCEPT, RULE_SLOPE = 0.190, -0.018 # 1/m, 1/m per m/s
|
||||
RULE_SPEEDS = (3.0, 8.0) # m/s: the FSD turns it was fitted on; below, its crawl value, above, its lateral accel
|
||||
# First road test: a quarter of FSD's turns replayed 20%+ over FSD's own peak (corners the speed rule can't see), and
|
||||
# 10-20% too much already felt like far too much on the road, so the rule is taken 8% low: an under-turn can be helped.
|
||||
PEAK_SCALE = 0.92
|
||||
MIN_TURN = np.radians(45.) # heading change that makes it a turn (as the labels)
|
||||
MIN_TURN_NO_CUE = np.radians(60.)
|
||||
MAX_SPEED = 9.0 # m/s
|
||||
CONFIRM_FRAMES = 10 # the plan must show the turn for 0.5 s before it is taken over
|
||||
BLEND_IN_S, BLEND_OUT_S = 0.5, 1.0
|
||||
END_LEVEL = 0.05 # past the half-heading point, hand back once the profile is this low
|
||||
PHANTOM = np.radians(15.) # before the half-heading point, a turn the plan no longer shows is dropped
|
||||
TIMING_SPEED = 3.0 # m/s: below it the time to the half-heading point is counted at this speed, so the wheel winds in
|
||||
# while pulling away from a stop the way FSD does, instead of waiting until the car is moving
|
||||
MODE_PROFILE, MODE_MAX = 'profile', 'max' # the profile alone, or whichever of action and profile turns more
|
||||
|
||||
|
||||
def rule_peak(v_ego: float) -> float:
|
||||
v = float(np.clip(v_ego, *RULE_SPEEDS))
|
||||
peak = RULE_INTERCEPT + RULE_SLOPE * v
|
||||
if v_ego > RULE_SPEEDS[1]: # past the fitted speeds, hold its lateral acceleration rather than its curvature
|
||||
peak *= (RULE_SPEEDS[1] / v_ego) ** 2
|
||||
return PEAK_SCALE * peak
|
||||
|
||||
|
||||
def _width(v_ego: float, total: float) -> float:
|
||||
return float(np.clip(total / (max(v_ego, 1.0) * rule_peak(v_ego)), 1.5, 8.0)) # s, so the profile adds up to the turn
|
||||
|
||||
|
||||
def profile(tau: float, v_ego: float, total: float) -> float:
|
||||
"""Curvature toward the turn tau s from its half-heading point, for a turn of `total` rad at v_ego."""
|
||||
return rule_peak(v_ego) * float(np.interp(tau * TEMPLATE_WIDTH / _width(v_ego, total), TAU, TEMPLATE, left=0., right=0.))
|
||||
|
||||
|
||||
class FsdTurnProfile:
|
||||
def __init__(self, mode: str = MODE_PROFILE):
|
||||
self.mode = mode
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.active, self.direction, self.confirm = False, 0., 0
|
||||
self.done = self.total = self.since_mid = 0.
|
||||
self.past_mid = False
|
||||
self.v_mid = 0.
|
||||
self.weight = 0.
|
||||
self.last = 0.
|
||||
|
||||
def update(self, plan_x: np.ndarray, plan_y: np.ndarray, plan_yaw: np.ndarray, yaw_rate: float, v_ego: float,
|
||||
dt: float, action: float, turn_sign: float = 0., lead: float = 0.55) -> float:
|
||||
"""The steering command: the action, or a turn's FSD-timed profile while one is being taken (blended at the
|
||||
ends). Curvature, yaw and yaw rate share the plan's sign convention; turn_sign is the held turn cue (0 = none).
|
||||
lead is lat_action_t: the template is the curvature the car had, so the command looks it up that much ahead."""
|
||||
remaining = float(plan_yaw[-1]) # heading the plan still turns over its horizon
|
||||
if not self.active:
|
||||
cued = turn_sign != 0. and np.sign(turn_sign) == np.sign(remaining)
|
||||
wanted = abs(remaining) >= (MIN_TURN if cued else MIN_TURN_NO_CUE) and v_ego < MAX_SPEED
|
||||
self.confirm = self.confirm + 1 if wanted and (self.confirm == 0 or np.sign(remaining) == self.direction) else 0
|
||||
self.direction = float(np.sign(remaining)) if wanted else 0.
|
||||
if self.confirm >= CONFIRM_FRAMES:
|
||||
self.active, self.done, self.total, self.past_mid, self.since_mid = True, 0., abs(remaining), False, 0.
|
||||
self.weight = max(self.weight - dt / BLEND_OUT_S, 0.)
|
||||
command = self.weight * self.last + (1. - self.weight) * action
|
||||
return float(command)
|
||||
|
||||
self.done += self.direction * yaw_rate * dt
|
||||
estimate = max(self.done + self.direction * remaining, self.done)
|
||||
if self.past_mid: # past the half-heading point the plan's remaining heading starts to include the road beyond the
|
||||
estimate = min(estimate, self.total) # corner, so the turn's size can only be revised down
|
||||
self.total += 0.2 * (estimate - self.total)
|
||||
half = self.total / 2.
|
||||
if not self.past_mid and self.done >= half:
|
||||
self.past_mid, self.v_mid = True, v_ego
|
||||
if self.past_mid:
|
||||
# the template's time is distance from the half-heading point over the speed there; cars speed up out of a turn
|
||||
self.since_mid += max(v_ego, 0.) * dt
|
||||
tau = self.since_mid / max(self.v_mid, TIMING_SPEED)
|
||||
else:
|
||||
arc = np.concatenate([[0.], np.cumsum(np.hypot(np.diff(plan_x), np.diff(plan_y)))])
|
||||
heading = self.done + self.direction * np.asarray(plan_yaw)
|
||||
reach = np.flatnonzero(heading >= half)
|
||||
to_mid = float(np.interp(half, heading[:reach[0] + 1], arc[:reach[0] + 1])) if len(reach) and reach[0] > 0 else (
|
||||
0. if len(reach) else float(arc[-1]))
|
||||
tau = -to_mid / max(v_ego, TIMING_SPEED)
|
||||
level = profile(tau + lead, self.v_mid if self.past_mid else v_ego, self.total)
|
||||
self.last = self.direction * level
|
||||
if self.mode == MODE_MAX: # the action keeps its own timing where it leads; the profile lifts it to FSD's turn
|
||||
self.last = self.direction * max(level, self.direction * action)
|
||||
self.weight = min(self.weight + dt / BLEND_IN_S, 1.)
|
||||
ended = self.past_mid and level < END_LEVEL * rule_peak(self.v_mid)
|
||||
phantom = not self.past_mid and self.direction * remaining < PHANTOM and self.done < PHANTOM
|
||||
if ended or phantom or v_ego > MAX_SPEED + 2.:
|
||||
self.active, self.confirm, self.direction = False, 0, 0.
|
||||
return float(self.weight * self.last + (1. - self.weight) * action)
|
||||
@@ -53,7 +53,7 @@ from openpilot.sunnypilot.modeld_v2.steering_smoothing import lat_smooth_seconds
|
||||
from openpilot.sunnypilot.modeld_v2.turn_desire_refresh import TurnDesireRefresh, desire_input
|
||||
from openpilot.sunnypilot.modeld_v2.turn_plan_floor import apply_turn_plan_floor
|
||||
from openpilot.sunnypilot.modeld_v2 import path_steering
|
||||
from openpilot.sunnypilot.modeld_v2.turn_correction import apply_turn_correction, correction_for
|
||||
from openpilot.sunnypilot.modeld_v2.fsd_turn_profile import FsdTurnProfile
|
||||
from openpilot.sunnypilot.modeld_v2.plan_long import PlanLong
|
||||
from openpilot.sunnypilot.modeld_v2.navigation_input import (NavigationInputBuffer, NavigationStatePublisher, SignalRouteGuard,
|
||||
destination_route_context, navigation_capability)
|
||||
@@ -107,7 +107,7 @@ class ModelState(ModelStateBase):
|
||||
model_bundle = get_active_bundle(chestnut=chestnut)
|
||||
self.generation = model_bundle.generation if model_bundle is not None else None
|
||||
self.model_name = model_bundle.displayName if model_bundle is not None else ''
|
||||
self.turn_correction = correction_for(model_bundle.ref) if model_bundle is not None else None
|
||||
self.fsd_turn = FsdTurnProfile()
|
||||
overrides = {override.key: override.value for override in model_bundle.overrides} if model_bundle else {}
|
||||
|
||||
self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
|
||||
@@ -270,25 +270,25 @@ class ModelState(ModelStateBase):
|
||||
desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
|
||||
self.action_curvature, self.turn_plan_curvature = desired_curvature, 0.
|
||||
plan = model_output['plan'][0]
|
||||
if path_steering_enabled and self.turn_correction is not None:
|
||||
# this model has a learned turn correction toward the FSD/driver line; it replaces the floor and the path cap
|
||||
if turn_sign:
|
||||
# the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor
|
||||
self.turn_plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2],
|
||||
self.constants.T_IDXS, v_ego, lat_action_t)
|
||||
path = path_steering.path_curvature(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1],
|
||||
plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[0, Plan.ORIENTATION_RATE][2],
|
||||
v_ego, a_ego, lat_action_t)
|
||||
desired_curvature = apply_turn_correction(self.turn_correction, desired_curvature, self.turn_plan_curvature, path, v_ego)
|
||||
else:
|
||||
if turn_sign:
|
||||
# the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor
|
||||
self.turn_plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2],
|
||||
self.constants.T_IDXS, v_ego, lat_action_t)
|
||||
desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign)
|
||||
if path_steering_enabled and path_steering.path_weight(v_ego) > 0.:
|
||||
desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign)
|
||||
if path_steering_enabled:
|
||||
if path_steering.path_weight(v_ego) > 0.:
|
||||
path = path_steering.path_curvature(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1],
|
||||
plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[0, Plan.ORIENTATION_RATE][2],
|
||||
v_ego, a_ego, lat_action_t)
|
||||
desired_curvature = path_steering.apply_path_steering(desired_curvature, path, v_ego)
|
||||
# in a turn the plan shows, steer FSD's turn profile: the plan says where the corner is and how much it turns,
|
||||
# FSD's turns say when to start and how hard (fsd_turn_profile.py); outside one this returns the command above
|
||||
yaw_rate = float(model_output['pose'][0, 5]) if 'pose' in model_output else 0.
|
||||
desired_curvature = self.fsd_turn.update(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1],
|
||||
plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], yaw_rate, v_ego, DT_MDL,
|
||||
desired_curvature, turn_sign, lead=lat_action_t)
|
||||
else:
|
||||
self.fsd_turn.reset()
|
||||
if blendful is not None:
|
||||
desired_curvature += blendful(desired_curvature)
|
||||
|
||||
|
||||
@@ -0,0 +1,88 @@
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from openpilot.sunnypilot.modeld_v2 import fsd_turn_profile as ftp
|
||||
|
||||
T = np.linspace(0., 10., 33) ** 2 / 10. # like ModelConstants.T_IDXS
|
||||
DT = 0.05
|
||||
RIGHT_90 = float(np.radians(90.))
|
||||
|
||||
|
||||
def corner_plan(distance_to_corner, turn, v):
|
||||
"""A plan driving straight distance_to_corner metres, then turning `turn` rad over an arc of radius 8 m."""
|
||||
s = v * T
|
||||
radius = 8.
|
||||
arc = abs(turn) * radius
|
||||
yaw = np.sign(turn) * np.clip((s - distance_to_corner) / radius, 0., abs(turn))
|
||||
x = np.concatenate([[0.], np.cumsum(np.cos(yaw[1:]) * np.diff(s))])
|
||||
y = np.concatenate([[0.], np.cumsum(np.sin(yaw[1:]) * np.diff(s))])
|
||||
return x, y, yaw, arc
|
||||
|
||||
|
||||
def drive(turn=RIGHT_90, v=5., start=40., cue=1., steps=400, action=None):
|
||||
"""Toy closed loop: the car takes each command 0.55 s later (the lead the controller looks ahead by), the plan shows
|
||||
the corner where it still is, and the model's action steers what the turn still needs over the next 8 m."""
|
||||
controller = ftp.FsdTurnProfile()
|
||||
heading, travelled = 0., 0.
|
||||
commands, delay = [], round(0.55 / DT)
|
||||
for _ in range(steps):
|
||||
remaining = turn - heading
|
||||
x, y, yaw, _ = corner_plan(max(start - travelled, 0.), remaining, v)
|
||||
model = action if action is not None else (float(np.clip(remaining / 8., -0.2, 0.2)) if travelled > start - 5. else 0.)
|
||||
applied = commands[-delay] if len(commands) >= delay else 0.
|
||||
commands.append(controller.update(x, y, yaw, applied * v, v, DT, model, cue))
|
||||
heading += applied * v * DT
|
||||
travelled += v * DT
|
||||
return np.asarray(commands), heading, controller
|
||||
|
||||
|
||||
def test_a_cued_right_turn_follows_the_speed_rule_and_completes():
|
||||
commands, heading, controller = drive()
|
||||
assert np.max(commands) == pytest.approx(ftp.rule_peak(5.), rel=0.1)
|
||||
assert np.degrees(heading) == pytest.approx(90., abs=8.)
|
||||
assert not controller.active and abs(commands[-1]) < 0.01
|
||||
|
||||
|
||||
def test_left_turns_mirror_right_turns():
|
||||
right, _, _ = drive(turn=np.radians(90.), cue=1.)
|
||||
left, _, _ = drive(turn=np.radians(-90.), cue=-1.)
|
||||
assert np.allclose(left, -right, atol=1e-6)
|
||||
|
||||
|
||||
def test_no_cue_needs_a_bigger_turn():
|
||||
commands, _, controller = drive(turn=np.radians(50.), cue=0., action=0.)
|
||||
assert np.max(np.abs(commands)) == 0.
|
||||
commands, _, _ = drive(turn=np.radians(90.), cue=0.)
|
||||
assert np.max(commands) > 0.05
|
||||
|
||||
|
||||
def test_road_speed_is_left_to_the_model():
|
||||
commands, _, _ = drive(v=12., action=0.01)
|
||||
assert np.allclose(commands, 0.01)
|
||||
|
||||
|
||||
def test_a_turn_the_plan_stops_showing_is_dropped_back_to_the_action():
|
||||
controller = ftp.FsdTurnProfile()
|
||||
x, y, yaw, _ = corner_plan(30., np.radians(90.), 5.)
|
||||
for _ in range(20):
|
||||
controller.update(x, y, yaw, 0., 5., DT, 0.02, 1.)
|
||||
assert controller.active
|
||||
straight = (5. * T, np.zeros_like(T), np.zeros_like(T))
|
||||
for _ in range(40):
|
||||
command = controller.update(*straight, 0., 5., DT, 0.02, 1.)
|
||||
assert not controller.active and command == pytest.approx(0.02, abs=1e-3)
|
||||
|
||||
|
||||
def test_the_peak_rule_holds_lateral_accel_past_its_speeds():
|
||||
assert ftp.rule_peak(1.) == ftp.rule_peak(3.)
|
||||
assert ftp.rule_peak(10.) * 10. ** 2 == pytest.approx(ftp.rule_peak(8.) * 8. ** 2)
|
||||
assert ftp.rule_peak(3.) == pytest.approx(ftp.PEAK_SCALE * (0.190 - 0.018 * 3.))
|
||||
|
||||
|
||||
def test_pulling_away_from_a_stop_winds_in_before_the_car_is_moving():
|
||||
controller = ftp.FsdTurnProfile()
|
||||
x, y, yaw, _ = corner_plan(4., np.radians(90.), 3.)
|
||||
command = 0.
|
||||
for _ in range(40): # stopped 4 m from the corner with the turn cued
|
||||
command = controller.update(x, y, yaw, 0., 0.2, DT, 0., 1.)
|
||||
assert command > 0.03
|
||||
@@ -1,35 +0,0 @@
|
||||
import pytest
|
||||
|
||||
from openpilot.sunnypilot.modeld_v2 import turn_correction as tc
|
||||
|
||||
C = tc.TurnCorrection('test', v_full=6., v_end=10., theta_path=0.1, theta_time=0.8)
|
||||
|
||||
|
||||
def test_no_change_at_road_speed_and_full_below_v_full():
|
||||
assert tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 12.) == 0.05
|
||||
full = tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 3.)
|
||||
assert full == pytest.approx(0.05 + 0.1 * 0.07 + 0.8 * 0.05)
|
||||
half = tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 8.)
|
||||
assert half == pytest.approx(0.05 + 0.5 * (full - 0.05))
|
||||
|
||||
|
||||
def test_it_stays_within_the_models_own_range():
|
||||
# the plan's time axis says far more than both the action and the line at the car's speed: bounded at path + 10%
|
||||
assert tc.apply_turn_correction(C, 0.10, 0.40, 0.14, 3.) == pytest.approx(0.14 + 0.1 * 0.04)
|
||||
# a turn the action and the line both say is straight is not invented from the time axis
|
||||
assert tc.apply_turn_correction(C, 0.0, 0.2, 0.0, 3.) == 0.0
|
||||
# nor reversed: action and line right, the time axis left
|
||||
assert tc.apply_turn_correction(C, 0.1, -0.2, 0.1, 3.) == pytest.approx(0.1)
|
||||
|
||||
|
||||
def test_it_can_lower_an_overcommitted_action_toward_the_line():
|
||||
lower = tc.TurnCorrection('test', 4.5, 7., theta_path=0.9, theta_time=0.)
|
||||
assert tc.apply_turn_correction(lower, -0.21, -0.21, -0.14, 3.) == pytest.approx(-0.21 + 0.9 * 0.07)
|
||||
|
||||
|
||||
def test_lookup_by_ref_and_local_prefix(monkeypatch):
|
||||
monkeypatch.setattr(tc, 'CORRECTIONS', {'abc': C, 'local-ff_turns_600-': C})
|
||||
assert tc.correction_for('abc') is C
|
||||
assert tc.correction_for('local-ff_turns_600-0123456789abcdef') is C
|
||||
assert tc.correction_for('local-other-0123') is None
|
||||
assert tc.correction_for('') is None
|
||||
@@ -1,56 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
# Learned turn correction: per model, a small correction on top of its action toward the line FSD and the driver
|
||||
# took, fitted by openpilot.distill's supervised/fit_turn_correction.py on open-loop frames of driver/FSD turns and
|
||||
# closed-loop frames of the user's engaged drives (where the driver or FSD was steering, or a turn openpilot steered
|
||||
# with no push-back), scored leave-one-route-out. It replaces the 0.75 turn plan floor and the path cap for a model
|
||||
# that has one. With
|
||||
# time = the plan's curvature at action_t on its time axis (get_curvature_from_plan)
|
||||
# path = the plan's heading at the arc length the car covers in action_t (path_steering.path_curvature)
|
||||
# command = action + s(v) * (theta_path * (path - action) + theta_time * (time - action)), s = 1 below v_full and 0
|
||||
# from v_end, and the command stays within the range action..path spans (plus 10%), so it can only move the action
|
||||
# toward the model's own line, never invent or reverse a turn.
|
||||
MARGIN = 0.1
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TurnCorrection:
|
||||
name: str
|
||||
v_full: float
|
||||
v_end: float
|
||||
theta_path: float
|
||||
theta_time: float
|
||||
|
||||
|
||||
# bundle ref (or a local build's ref prefix ending in '-') -> correction
|
||||
CORRECTIONS: dict[str, TurnCorrection] = {
|
||||
# Nov 9 2026, closed-loop frames of routes 98, 9a, 9e, 9f, b0 (26 turns, 3640 frames). Held out by route, mean
|
||||
# |command - driven| 1-3 m/s 0.039 -> 0.021 (0.75 floor 0.029), 3-4.5 m/s 0.028 -> 0.014, 4.5-6 m/s 0.017 -> 0.008,
|
||||
# 6-9 m/s 0.006 -> 0.004; peak command / peak driven per turn 0.74 (floor 0.83) -> 1.02 [10-90%: 0.84-1.26].
|
||||
'b92fc4828664f8767bce5e92f1630ed319cbe509': TurnCorrection('Light Speed 2', v_full=6.0, v_end=10.0,
|
||||
theta_path=0.0554, theta_time=0.8708),
|
||||
}
|
||||
|
||||
|
||||
def correction_for(ref: str) -> TurnCorrection | None:
|
||||
if ref in CORRECTIONS:
|
||||
return CORRECTIONS[ref]
|
||||
return next((c for key, c in CORRECTIONS.items() if key.endswith('-') and ref.startswith(key)), None)
|
||||
|
||||
|
||||
def apply_turn_correction(correction: TurnCorrection, action: float, time: float, path: float, v_ego: float) -> float:
|
||||
weight = float(np.clip((correction.v_end - v_ego) / (correction.v_end - correction.v_full), 0., 1.))
|
||||
if weight == 0.:
|
||||
return action
|
||||
raw = action + weight * (correction.theta_path * (path - action) + correction.theta_time * (time - action))
|
||||
lo, hi = min(action, path), max(action, path)
|
||||
margin = MARGIN * (hi - lo)
|
||||
return float(np.clip(raw, lo - margin, hi + margin))
|
||||
Reference in New Issue
Block a user