From 770ad5ceac5c6261bf8dcfbefc4731a5a655c978 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 11 Sep 2025 21:20:31 -0700 Subject: [PATCH 1/6] replace lead mpc --- .../controls/lib/longitudinal_planner.py | 172 +++++++++++++----- 1 file changed, 122 insertions(+), 50 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 34fc85f8a55c6c..d43f99c0b3ea32 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2,6 +2,7 @@ import math import numpy as np +from cereal import log import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.constants import CV @@ -9,23 +10,67 @@ from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU -LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] +A_CRUISE_MIN_VALS = [-1.2,] +A_CRUISE_MIN_BP = [0., ] + CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.4 MIN_ALLOW_THROTTLE_SPEED = 2.5 +#FCW_IDXS = ModelConstants.T_IDXS < 5.0 +#T_DIFFS = np.diff(T_IDXS, prepend=[0.]) +COMFORT_BRAKE = 2.5 +STOP_DISTANCE = 6.0 + + # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] +T_IDXS = np.array(ModelConstants.T_IDXS) +FCW_IDXS = T_IDXS < 5.0 +T_DIFFS = np.diff(T_IDXS, prepend=[0.]) + + +def get_jerk_factor(personality=log.LongitudinalPersonality.standard): + if personality==log.LongitudinalPersonality.relaxed: + return 1.0 + elif personality==log.LongitudinalPersonality.standard: + return 1.0 + elif personality==log.LongitudinalPersonality.aggressive: + return 0.5 + else: + raise NotImplementedError("Longitudinal personality not supported") + + +def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): + if personality==log.LongitudinalPersonality.relaxed: + return 1.75 + elif personality==log.LongitudinalPersonality.standard: + return 1.45 + elif personality==log.LongitudinalPersonality.aggressive: + return 1.25 + else: + raise NotImplementedError("Longitudinal personality not supported") + +def get_stopped_equivalence_factor(v_lead): + return (v_lead**2) / (2 * COMFORT_BRAKE) + +def get_safe_obstacle_distance(v_ego, t_follow): + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE + +def get_desired_follow_distance(v_ego, v_lead, t_follow=None): + if t_follow is None: + t_follow = get_T_FOLLOW() + return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead) + def get_max_accel(v_ego): return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) @@ -33,6 +78,34 @@ def get_max_accel(v_ego): def get_coast_accel(pitch): return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py +def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): + a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) + v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8) + x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj) + lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) + return lead_xv + +def process_lead(v_ego, lead): + if lead is not None and lead.status: + x_lead = lead.dRel + v_lead = lead.vLead + a_lead = lead.aLeadK + a_lead_tau = lead.aLeadTau + else: + # Fake a fast lead car, so mpc can keep running in the same mode + x_lead = 50.0 + v_lead = v_ego + 10.0 + a_lead = 0.0 + a_lead_tau = _LEAD_ACCEL_TAU + + # MPC will not converge if immediate crash is expected + # Clip lead distance to what is still possible to brake for + min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) + x_lead = np.clip(x_lead, min_x_lead, 1e8) + v_lead = np.clip(v_lead, 0.0, 1e8) + a_lead = np.clip(a_lead, -10., 5.) + lead_xv = extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) + return lead_xv def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ @@ -51,9 +124,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP - self.mpc = LongitudinalMpc(dt=dt) # TODO remove mpc modes when TR released - self.mpc.mode = 'acc' self.fcw = False self.dt = dt self.allow_throttle = True @@ -71,23 +142,11 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): @staticmethod def parse_model(model_msg): - if (len(model_msg.position.x) == ModelConstants.IDX_N and - len(model_msg.velocity.x) == ModelConstants.IDX_N and - len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) - j = np.zeros(len(T_IDXS_MPC)) - else: - x = np.zeros(len(T_IDXS_MPC)) - v = np.zeros(len(T_IDXS_MPC)) - a = np.zeros(len(T_IDXS_MPC)) - j = np.zeros(len(T_IDXS_MPC)) if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1: throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1] else: throttle_prob = 1.0 - return x, v, a, j, throttle_prob + return throttle_prob def update(self, sm): mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' @@ -127,7 +186,7 @@ def update(self, sm): # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2']) + throttle_prob = self.parse_model(sm['modelV2']) # Don't clip at low speeds since throttle_prob doesn't account for creep self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED @@ -139,42 +198,61 @@ def update(self, sm): if force_slow_decel: v_cruise = 0.0 - self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) - self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) - - self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) - self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) - self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone - self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill - if self.fcw: - cloudlog.info("FCW triggered") + #self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill + #if self.fcw: + # cloudlog.info("FCW triggered") # Interpolate 0.05 seconds and save as starting point for next iteration - a_prev = self.a_desired - self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) - self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 + #a_prev = self.a_desired + #self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) + #self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 action_t = self.CP.longitudinalActuatorDelay + DT_MDL - output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) - output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_should_stop_e2e = sm['modelV2'].action.shouldStop + out_accels = {} + cruise_min = np.interp(v_ego, A_CRUISE_MIN_BP, A_CRUISE_MIN_VALS) + cruise_max = np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) + cruise_accel = 0.5*(v_cruise - v_ego) + cruise_accel = np.clip(cruise_accel, cruise_min, cruise_max) + out_accels['cruise'] = (cruise_accel, False) - if mode == 'acc': - output_a_target = output_a_target_mpc - self.output_should_stop = output_should_stop_mpc - else: - output_a_target = min(output_a_target_mpc, output_a_target_e2e) - self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc + + lead_xv_0 = process_lead(v_ego, sm['radarState'].leadOne) + #lead_xv_1 = process_lead(v_ego, sm['radarState'].leadTwo) + + + get_safe_obstacle_distance(v_ego, get_T_FOLLOW()) + + ## To estimate a safe distance from a moving lead, we calculate how much stopping + ## distance that lead needs as a minimum. We can add that to the current distance + ## and then treat that as a stopped car/obstacle at this new distance. + #lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) + #lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + + desired_follow_distance = get_desired_follow_distance(v_ego, lead_xv_0[0,1], get_T_FOLLOW()) + true_follow_distance = lead_xv_0[0,0] + + follow_distance_error = true_follow_distance - desired_follow_distance + lead_accel = np.clip(0.2 * follow_distance_error, -3.5, 2.0) + + out_accels['lead'] = (lead_accel, False) + + + if mode == 'blended': + out_accels['model']= (sm['modelV2'].action.desiredAcceleration, sm['modelV2'].action.shouldStop) + + + # smoothing opn the clip #TODO do better? for idx in range(2): accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) - self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) self.prev_accel_clip = accel_clip + output_a_target = np.min([x for x, _ in out_accels.values()]) + self.output_should_stop = np.all([x for _, x in out_accels.values()]) + self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) + def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') @@ -182,15 +260,9 @@ def publish(self, sm, pm): longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] - longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] - longitudinalPlan.solverExecutionTime = self.mpc.solve_time - - longitudinalPlan.speeds = self.v_desired_trajectory.tolist() - longitudinalPlan.accels = self.a_desired_trajectory.tolist() - longitudinalPlan.jerks = self.j_desired_trajectory.tolist() longitudinalPlan.hasLead = sm['radarState'].leadOne.status - longitudinalPlan.longitudinalPlanSource = self.mpc.source + #longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw longitudinalPlan.aTarget = float(self.output_a_target) From 9ff718c273340dbe00b1e52a9eeec93c6da9dbe7 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 11 Sep 2025 21:43:17 -0700 Subject: [PATCH 2/6] litn --- selfdrive/controls/lib/longitudinal_planner.py | 11 +++-------- selfdrive/test/process_replay/migration.py | 2 +- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index d43f99c0b3ea32..55a87a88c1c11c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -10,9 +10,8 @@ from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET -from openpilot.common.swaglog import cloudlog +#from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] @@ -20,7 +19,6 @@ A_CRUISE_MIN_VALS = [-1.2,] A_CRUISE_MIN_BP = [0., ] -CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.4 MIN_ALLOW_THROTTLE_SPEED = 2.5 @@ -135,9 +133,6 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.output_a_target = 0.0 self.output_should_stop = False - self.v_desired_trajectory = np.zeros(CONTROL_N) - self.a_desired_trajectory = np.zeros(CONTROL_N) - self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 @staticmethod @@ -170,7 +165,7 @@ def update(self, sm): reset_state = reset_state or not v_cruise_initialized # No change cost when user is controlling the speed, or when standstill - prev_accel_constraint = not (reset_state or sm['carState'].standstill) + #prev_accel_constraint = not (reset_state or sm['carState'].standstill) if mode == 'acc': accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] @@ -209,7 +204,7 @@ def update(self, sm): #self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) #self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - action_t = self.CP.longitudinalActuatorDelay + DT_MDL + #action_t = self.CP.longitudinalActuatorDelay + DT_MDL out_accels = {} cruise_min = np.interp(v_ego, A_CRUISE_MIN_BP, A_CRUISE_MIN_VALS) cruise_max = np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 33b363cfd9496c..14de5356757215 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -109,7 +109,7 @@ def migrate_longitudinalPlan(msgs): if msg.which() != 'longitudinalPlan': continue new_msg = msg.as_builder() - a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, CONTROL_N_T_IDX) + a_target, should_stop = 0.0, False #get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, CONTROL_N_T_IDX) new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = float(a_target), bool(should_stop) ops.append((index, new_msg.as_reader())) return ops, [], [] From 2dd68405d6446d5f80f11234a8b08a9ec21c80ae Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 11 Sep 2025 21:47:17 -0700 Subject: [PATCH 3/6] litn --- selfdrive/test/process_replay/migration.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 14de5356757215..e9fc2900ee2f3f 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -13,7 +13,6 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan, CONTROL_N_T_IDX from openpilot.system.manager.process_config import managed_processes from openpilot.tools.lib.logreader import LogIterable From 48d35e4fa561fd0b8655d12d3f12c8b89b837a99 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 12 Sep 2025 13:46:14 -0700 Subject: [PATCH 4/6] does something --- selfdrive/controls/lib/longitudinal_planner.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 55a87a88c1c11c..19b347ca5b2989 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -13,6 +13,7 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET #from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +from openpilot.selfdrive.controls.lib.drive_helpers import smooth_value A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] @@ -129,7 +130,6 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) - self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] self.output_a_target = 0.0 self.output_should_stop = False @@ -177,7 +177,7 @@ def update(self, sm): if reset_state: self.v_desired_filter.x = v_ego # Clip aEgo to cruise limits to prevent large accelerations when becoming active - self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1]) + self.output_a_target = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1]) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) @@ -210,6 +210,7 @@ def update(self, sm): cruise_max = np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) cruise_accel = 0.5*(v_cruise - v_ego) cruise_accel = np.clip(cruise_accel, cruise_min, cruise_max) + cruise_accel = smooth_value(cruise_accel, self.output_a_target, 1.0) out_accels['cruise'] = (cruise_accel, False) @@ -218,8 +219,6 @@ def update(self, sm): #lead_xv_1 = process_lead(v_ego, sm['radarState'].leadTwo) - get_safe_obstacle_distance(v_ego, get_T_FOLLOW()) - ## To estimate a safe distance from a moving lead, we calculate how much stopping ## distance that lead needs as a minimum. We can add that to the current distance ## and then treat that as a stopped car/obstacle at this new distance. @@ -230,7 +229,9 @@ def update(self, sm): true_follow_distance = lead_xv_0[0,0] follow_distance_error = true_follow_distance - desired_follow_distance - lead_accel = np.clip(0.2 * follow_distance_error, -3.5, 2.0) + follow_distance_cost_signed = (follow_distance_error / (v_ego + 10))**2 * np.sign(follow_distance_error) + lead_accel = np.clip(2*follow_distance_cost_signed, -3.5, 2.0) + lead_accel = smooth_value(lead_accel, self.output_a_target, 0.5) out_accels['lead'] = (lead_accel, False) @@ -238,12 +239,6 @@ def update(self, sm): if mode == 'blended': out_accels['model']= (sm['modelV2'].action.desiredAcceleration, sm['modelV2'].action.shouldStop) - - # smoothing opn the clip #TODO do better? - for idx in range(2): - accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) - self.prev_accel_clip = accel_clip - output_a_target = np.min([x for x, _ in out_accels.values()]) self.output_should_stop = np.all([x for _, x in out_accels.values()]) self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) From 43a5d3e14046b3cd2bb129c7ee701a442ab2632a Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 12 Sep 2025 15:30:33 -0700 Subject: [PATCH 5/6] fix follow distance test --- selfdrive/controls/lib/longitudinal_planner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 19b347ca5b2989..4709d05306de9d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -225,11 +225,11 @@ def update(self, sm): #lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) #lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) - desired_follow_distance = get_desired_follow_distance(v_ego, lead_xv_0[0,1], get_T_FOLLOW()) + desired_follow_distance = get_desired_follow_distance(v_ego, lead_xv_0[0,1], get_T_FOLLOW(sm['selfdriveState'].personality)) true_follow_distance = lead_xv_0[0,0] follow_distance_error = true_follow_distance - desired_follow_distance - follow_distance_cost_signed = (follow_distance_error / (v_ego + 10))**2 * np.sign(follow_distance_error) + follow_distance_cost_signed = (follow_distance_error / (v_ego + 1))**2 * np.sign(follow_distance_error) lead_accel = np.clip(2*follow_distance_cost_signed, -3.5, 2.0) lead_accel = smooth_value(lead_accel, self.output_a_target, 0.5) From 49cbb0bd56553287cba634088283d3b06339baf7 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 12 Sep 2025 17:55:44 -0700 Subject: [PATCH 6/6] all tests pass --- .../controls/lib/longitudinal_planner.py | 60 ++++++++++++------- 1 file changed, 40 insertions(+), 20 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4709d05306de9d..c7190405866f08 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -19,6 +19,8 @@ A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] A_CRUISE_MIN_VALS = [-1.2,] A_CRUISE_MIN_BP = [0., ] +ACC_ACCEL_MAX = 2.0 +ACCEL_MIN = -3.5 ALLOW_THROTTLE_THRESHOLD = 0.4 MIN_ALLOW_THROTTLE_SPEED = 2.5 @@ -33,9 +35,11 @@ _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] + T_IDXS = np.array(ModelConstants.T_IDXS) FCW_IDXS = T_IDXS < 5.0 -T_DIFFS = np.diff(T_IDXS, prepend=[0.]) +T_IDXS_LEAD = np.arange(0., 2.5, 0.1) +T_DIFFS_LEAD = np.diff(T_IDXS_LEAD, prepend=[0.]) def get_jerk_factor(personality=log.LongitudinalPersonality.standard): @@ -78,9 +82,9 @@ def get_coast_accel(pitch): return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): - a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) - v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8) - x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj) + a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS_LEAD**2)/2.) + v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS_LEAD * a_lead_traj), 0.0, 1e8) + x_lead_traj = x_lead + np.cumsum(T_DIFFS_LEAD * v_lead_traj) lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv @@ -106,6 +110,21 @@ def process_lead(v_ego, lead): lead_xv = extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv +def process_ego(v_ego, a_ego): + x_lead = 0.0 + v_lead = v_ego + a_lead = a_ego + a_lead_tau = _LEAD_ACCEL_TAU + + # MPC will not converge if immediate crash is expected + # Clip lead distance to what is still possible to brake for + min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) + x_lead = np.clip(x_lead, min_x_lead, 1e8) + v_lead = np.clip(v_lead, 0.0, 1e8) + a_lead = np.clip(a_lead, -10., 5.) + lead_xv = extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) + return lead_xv + def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ This function returns a limited long acceleration allowed, depending on the existing lateral acceleration @@ -215,33 +234,34 @@ def update(self, sm): - lead_xv_0 = process_lead(v_ego, sm['radarState'].leadOne) - #lead_xv_1 = process_lead(v_ego, sm['radarState'].leadTwo) - + lead_info = {'lead0': sm['radarState'].leadOne, 'lead1': sm['radarState'].leadTwo} + ego_xv = process_ego(v_ego, sm['carState'].aEgo) + for key in lead_info.keys(): + lead_xv = process_lead(v_ego, lead_info[key]) + - ## To estimate a safe distance from a moving lead, we calculate how much stopping - ## distance that lead needs as a minimum. We can add that to the current distance - ## and then treat that as a stopped car/obstacle at this new distance. - #lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) - #lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + ## To estimate a safe distance from a moving lead, we calculate how much stopping + ## distance that lead needs as a minimum. We can add that to the current distance + ## and then treat that as a stopped car/obstacle at this new distance. + desired_follow_distance = np.array([get_desired_follow_distance(v_ego, lead_xv[0,1], get_T_FOLLOW(sm['selfdriveState'].personality)) for v, vl in zip(ego_xv, lead_xv)]) - desired_follow_distance = get_desired_follow_distance(v_ego, lead_xv_0[0,1], get_T_FOLLOW(sm['selfdriveState'].personality)) - true_follow_distance = lead_xv_0[0,0] + error_weights = np.linspace(1.0, 0.0, len(desired_follow_distance)) + error_weights = error_weights / np.sum(error_weights) - follow_distance_error = true_follow_distance - desired_follow_distance - follow_distance_cost_signed = (follow_distance_error / (v_ego + 1))**2 * np.sign(follow_distance_error) - lead_accel = np.clip(2*follow_distance_cost_signed, -3.5, 2.0) - lead_accel = smooth_value(lead_accel, self.output_a_target, 0.5) + follow_distance_error = np.sum(error_weights*(lead_xv[:,0] - desired_follow_distance)) - out_accels['lead'] = (lead_accel, False) + follow_distance_cost_signed = (follow_distance_error / (v_ego + 1))**2 * np.sign(follow_distance_error) + lead_accel = np.clip(2*follow_distance_cost_signed, ACCEL_MIN, ACCEL_MAX) + lead_accel = smooth_value(lead_accel, self.output_a_target, 0.5) + out_accels[key] = (lead_accel, False) if mode == 'blended': out_accels['model']= (sm['modelV2'].action.desiredAcceleration, sm['modelV2'].action.shouldStop) output_a_target = np.min([x for x, _ in out_accels.values()]) self.output_should_stop = np.all([x for _, x in out_accels.values()]) - self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) + self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan')