mirror of
https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
synced 2026-07-25 05:22:11 +08:00
IQ.Lvbs CI [bot]
81 lines
3.5 KiB
Python
81 lines
3.5 KiB
Python
import numpy as np
|
|
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
|
from openpilot.common.realtime import DT_CTRL, DT_MDL
|
|
|
|
MIN_SPEED = 1.0
|
|
CONTROL_N = 17
|
|
CAR_ROTATION_RADIUS = 0.0
|
|
# This is a turn radius smaller than most cars can achieve
|
|
MAX_CURVATURE = 0.4
|
|
MAX_VEL_ERR = 5.0 # m/s
|
|
|
|
MAX_LATERAL_JERK = 5.0 # m/s^3
|
|
MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2
|
|
MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE = 5.0 # m/s^2
|
|
DEFAULT_STOPPING_SPEED = 0.25 # m/s
|
|
|
|
|
|
def should_stop(v_ego: float, a_target: float, stopping_speed: float = DEFAULT_STOPPING_SPEED) -> bool:
|
|
return bool(v_ego < stopping_speed and a_target < 0.1)
|
|
|
|
|
|
def clamp(val, min_val, max_val):
|
|
clamped_val = float(np.clip(val, min_val, max_val))
|
|
return clamped_val, clamped_val != val
|
|
|
|
def smooth_value(val, prev_val, tau, dt=DT_MDL):
|
|
alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
|
|
return alpha * val + (1 - alpha) * prev_val
|
|
|
|
# "Model smoothing": when the policy's own predicted uncertainty (plan_stds) for the
|
|
# 1s-ahead lateral position spikes, temporarily lengthen the desiredCurvature smoothing
|
|
# time constant so a noisy/uncertain model output doesn't jerk the wheel.
|
|
MODEL_SMOOTHING_STD_LOW = 0.15 # m, plan y_std at 1s below which no extra smoothing is added
|
|
MODEL_SMOOTHING_STD_HIGH = 0.25 # m, plan y_std at 1s at/above which the full max_extra_seconds is added
|
|
MODEL_SMOOTHING_MAX_TOTAL_SEC = 0.60 # hard ceiling on base + dynamic lat smoothing seconds
|
|
|
|
def dynamic_lat_smooth_extra_seconds(y_std_1s: float, max_extra_seconds: float) -> float:
|
|
if max_extra_seconds <= 0.0:
|
|
return 0.0
|
|
return float(np.interp(y_std_1s, [MODEL_SMOOTHING_STD_LOW, MODEL_SMOOTHING_STD_HIGH], [0.0, max_extra_seconds]))
|
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature, roll, override=False) -> tuple[float, bool]:
|
|
# This function respects ISO lateral jerk and acceleration limits + a max curvature
|
|
v_ego = max(v_ego, MIN_SPEED)
|
|
max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
|
|
new_curvature = np.clip(new_curvature,
|
|
prev_curvature - max_curvature_rate * DT_CTRL,
|
|
prev_curvature + max_curvature_rate * DT_CTRL)
|
|
|
|
max_lat_accel_no_roll = MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE if override else MAX_LATERAL_ACCEL_NO_ROLL
|
|
roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
|
|
max_lat_accel = max_lat_accel_no_roll + roll_compensation
|
|
min_lat_accel = -max_lat_accel_no_roll + roll_compensation
|
|
new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
|
|
|
|
new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
|
return float(new_curvature), limited_accel or limited_max_curv
|
|
|
|
|
|
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, stopping_speed=DEFAULT_STOPPING_SPEED):
|
|
if len(speeds) == len(t_idxs):
|
|
v_now = speeds[0]
|
|
a_now = accels[0]
|
|
v_target = np.interp(action_t, t_idxs, speeds)
|
|
a_target = 2 * (v_target - v_now) / (action_t) - a_now
|
|
else:
|
|
v_now = 0.0
|
|
v_target = 0.0
|
|
a_target = 0.0
|
|
return a_target, should_stop(v_now, a_target, stopping_speed)
|
|
|
|
def curv_from_psis(psi_target, psi_rate, vego, action_t):
|
|
vego = np.clip(vego, MIN_SPEED, np.inf)
|
|
curv_from_psi = psi_target / (vego * action_t)
|
|
return 2*curv_from_psi - psi_rate / vego
|
|
|
|
def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
|
|
psi_target = np.interp(action_t, t_idxs, yaws)
|
|
psi_rate = yaw_rates[0]
|
|
return curv_from_psis(psi_target, psi_rate, vego, action_t)
|