From 77a442c4e84f8c7e228ffcbc6d6c388aa06eb4c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Wed, 12 Feb 2025 13:11:32 -0800 Subject: [PATCH] Long planner: allow negative accel constraint (#34565) * allow negative accel constraint * dont let MPC do clipping * Typo * whitespace * Fix tests * More cruise accel * rm print * ref commit --- .../lib/longitudinal_mpc_lib/long_mpc.py | 19 ++++----- .../controls/lib/longitudinal_planner.py | 39 ++++++++++--------- .../test/longitudinal_maneuvers/maneuver.py | 2 +- .../test/longitudinal_maneuvers/plant.py | 10 ++--- .../test_longitudinal.py | 7 +--- selfdrive/test/process_replay/ref_commit | 2 +- 6 files changed, 37 insertions(+), 42 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 579ee85fd4..3f9d8245bd 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -3,7 +3,7 @@ import os import time import numpy as np from cereal import log -from opendbc.car.interfaces import ACCEL_MIN +from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild @@ -55,6 +55,8 @@ FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 +CRUISE_MIN_ACCEL = -1.2 +CRUISE_MAX_ACCEL = 1.6 def get_jerk_factor(personality=log.LongitudinalPersonality.standard): if personality==log.LongitudinalPersonality.relaxed: @@ -281,7 +283,7 @@ class LongitudinalMpc: elif self.mode == 'blended': a_change_cost = 40.0 if prev_accel_constraint else 0 cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] else: raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') self.set_cost_weights(cost_weights, constraint_cost_weights) @@ -325,12 +327,6 @@ class LongitudinalMpc: lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def set_accel_limits(self, min_a, max_a): - # TODO this sets a max accel limit, but the minimum limit is only for cruise decel - # needs refactor - self.cruise_min_a = min_a - self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) v_ego = self.x0[1] @@ -346,8 +342,7 @@ class LongitudinalMpc: lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) self.params[:,0] = ACCEL_MIN - # negative accel constraint causes problems because negative speed is not allowed - self.params[:,1] = max(0.0, self.max_a) + self.params[:,1] = ACCEL_MAX # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': @@ -355,9 +350,9 @@ class LongitudinalMpc: # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. - v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) + v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * self.max_a * 1.05) + v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5560dfbb09..c7a2295a5f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,6 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s -A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -76,7 +75,10 @@ class LongitudinalPlanner: 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.v_model_error = 0.0 + 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) @@ -128,17 +130,16 @@ class LongitudinalPlanner: prev_accel_constraint = not (reset_state or sm['carState'].standstill) if self.mpc.mode == 'acc': - accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) + accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) else: - accel_limits = [ACCEL_MIN, ACCEL_MAX] - accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] + accel_clip = [ACCEL_MIN, ACCEL_MAX] 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_limits[0], accel_limits[1]) + self.a_desired = 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)) @@ -149,18 +150,14 @@ class LongitudinalPlanner: self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED if not self.allow_throttle: - clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) - clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) - accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp) + clipped_accel_coast = max(accel_coast, accel_clip[0]) + clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) + accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) if force_slow_decel: v_cruise = 0.0 - # clip limits, cannot init MPC outside of bounds - accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) - accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) - self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) 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) @@ -178,6 +175,15 @@ class LongitudinalPlanner: 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, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, + action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + + 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 + def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') @@ -196,11 +202,8 @@ class LongitudinalPlanner: longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw - action_t = self.CP.longitudinalActuatorDelay + DT_MDL - a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) - longitudinalPlan.aTarget = float(a_target) - longitudinalPlan.shouldStop = bool(should_stop) + longitudinalPlan.aTarget = float(self.output_a_target) + longitudinalPlan.shouldStop = bool(self.output_should_stop) longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = bool(self.allow_throttle) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 301f99dd56..dfd5b3e109 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -66,7 +66,7 @@ class Maneuver: print("Crashed!!!!") valid = False - if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: + if self.ensure_start and log['v_rel'] > 0 and log['acceleration'] < 1e-3: print('LongitudinalPlanner not starting!') valid = False diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 026c8ce22a..989b84dee3 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -30,8 +30,8 @@ class Plant: self.distance = 0. self.speed = speed + self.should_stop = False self.acceleration = 0.0 - self.speeds = [] # lead car self.lead_relevancy = lead_relevancy @@ -134,9 +134,9 @@ class Plant: 'liveParameters': lp.liveParameters, 'modelV2': model.modelV2} self.planner.update(sm) - self.speed = self.planner.v_desired_filter.x - self.acceleration = self.planner.a_desired - self.speeds = self.planner.v_desired_trajectory.tolist() + self.acceleration = self.planner.output_a_target + self.speed = self.speed + self.acceleration * self.ts + self.should_stop = self.planner.output_should_stop fcw = self.planner.fcw self.distance_lead = self.distance_lead + v_lead * self.ts @@ -168,7 +168,7 @@ class Plant: "distance": self.distance, "speed": self.speed, "acceleration": self.acceleration, - "speeds": self.speeds, + "should_stop": self.should_stop, "distance_lead": self.distance_lead, "fcw": fcw, } diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 4bc1ebba8f..ab1800b4fb 100644 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -150,10 +150,7 @@ def create_maneuvers(kwargs): enabled=False, **kwargs, ), - ] - if not kwargs['e2e']: - # allow_throttle won't trigger with e2e - maneuvers.append(Maneuver( + Maneuver( "slow to 5m/s with allow_throttle = False and pitch = +0.1", duration=30., initial_speed=20., @@ -164,7 +161,7 @@ def create_maneuvers(kwargs): breakpoints=[0.0, 2., 2.01], ensure_slowdown=True, **kwargs, - )) + )] if not kwargs['force_decel']: # controls relies on planner commanding to move for stock-ACC resume spamming maneuvers.append(Maneuver( diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3b5f5f3466..1cdd551dd0 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -95f1384edc0dc00959b0e804de1aaafc35d2f15f \ No newline at end of file +b8595cc8351043a7c49b41e7c36f93ffd2e3dc6d