Skip to content

longitudinal: move cruise speed limit out of the MPC #38232

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
haraschax wants to merge 1 commit into
base: master
Choose a base branch
from
+24 −16
Open

longitudinal: move cruise speed limit out of the MPC #38232

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion openpilot/selfdrive/car/tests/test_cruise_speed.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ def test_cruise_speed(self):
cruise_speed = float(self.speed)

simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality)
assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s'
assert simulation_steady_state == pytest.approx(cruise_speed, abs=1.5), f'Did not reach {self.speed} m/s'


# TODO: test pcmCruise
Expand Down
17 changes: 3 additions & 14 deletions openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")

LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1, LongitudinalPlanSource.cruise)
MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)

X_DIM = 3
U_DIM = 1
Expand Down Expand Up @@ -55,8 +55,6 @@
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
MIN_X_LEAD_FACTOR = 0.5

def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
Expand Down Expand Up @@ -313,9 +311,8 @@ def process_lead(self, lead):
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv

def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
def update(self, radarstate, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(personality)
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status

lead_xv_0 = self.process_lead(radarstate.leadOne)
Expand All @@ -327,15 +324,7 @@ def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.s
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])

# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
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 * CRUISE_MAX_ACCEL * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)

x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]

self.yref[:,:] = 0.0
Expand Down
21 changes: 20 additions & 1 deletion openpilot/selfdrive/controls/lib/longitudinal_planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,10 @@
ALLOW_THROTTLE_THRESHOLD = 0.4
MIN_ALLOW_THROTTLE_SPEED = 2.5

# Cruise speed limit (moved out of the MPC)
CRUISE_MAX_ACCEL = 1.6
CRUISE_JERK = 0.5 # m/s^3, max rate of change of cruise limit acceleration

# Lookup table for turns
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
Expand Down Expand Up @@ -55,6 +59,7 @@ 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.a_cruise = 0.0
self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX]
self.output_a_target = 0.0
self.output_should_stop = False
Expand Down Expand Up @@ -130,7 +135,7 @@ def update(self, sm):

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, personality=sm['selfdriveState'].personality)
self.mpc.update(sm['radarState'], 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)
Expand All @@ -149,6 +154,20 @@ def update(self, sm):
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)

# Apply cruise speed limit outside the MPC
v_err = v_cruise - v_ego
if force_slow_decel:
a_cruise_target = ACCEL_MIN
elif v_err > 0:
a_cruise_target = CRUISE_MAX_ACCEL * np.clip(v_err / (CRUISE_MAX_ACCEL**2 / (2 * CRUISE_JERK)), 0, 1)
else:
a_cruise_target = 0.0
self.a_cruise = float(np.clip(a_cruise_target, self.a_cruise - CRUISE_JERK * self.dt, self.a_cruise + CRUISE_JERK * self.dt))
if self.a_cruise < output_a_target_mpc:
output_a_target_mpc = self.a_cruise
self.mpc.source = LongitudinalPlanSource.cruise

output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop

Expand Down
Loading