diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3f9d8245b..9408132c5 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -35,7 +35,7 @@ X_EGO_OBSTACLE_COST = 3. X_EGO_COST = 0. V_EGO_COST = 0. A_EGO_COST = 0. -J_EGO_COST = 5.0 +J_EGO_COST = 5. A_CHANGE_COST = 200. DANGER_ZONE_COST = 100. CRASH_DISTANCE = .25 @@ -43,7 +43,6 @@ LEAD_DANGER_FACTOR = 0.75 LIMIT_COST = 1e6 ACADOS_SOLVER_TYPE = 'SQP_RTI' - # Fewer timestamps don't hurt performance and lead to # much better convergence of the MPC with low iterations N = 12 @@ -57,6 +56,7 @@ 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): if personality==log.LongitudinalPersonality.relaxed: @@ -85,20 +85,12 @@ def get_stopped_equivalence_factor(v_lead): def get_safe_obstacle_distance(v_ego, t_follow): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE -def 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 gen_long_model(): model = AcadosModel() model.name = MODEL_NAME - # set up states & controls - x_ego = SX.sym('x_ego') - v_ego = SX.sym('v_ego') - a_ego = SX.sym('a_ego') + # states + x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_ego'), SX.sym('a_ego') model.x = vertcat(x_ego, v_ego, a_ego) # controls @@ -126,7 +118,6 @@ def gen_long_model(): model.f_expl_expr = f_expl return model - def gen_long_ocp(): ocp = AcadosOcp() ocp.model = gen_long_model() @@ -222,30 +213,31 @@ def gen_long_ocp(): class LongitudinalMpc: - def __init__(self, mode='acc', dt=DT_MDL): - self.mode = mode + def __init__(self, dt=DT_MDL): self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() self.source = SOURCES[2] def reset(self): - # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.solver.reset() - # self.solver.options_set('print_level', 2) + + self.x_sol = np.zeros((N+1, X_DIM)) + self.u_sol = np.zeros((N, 1)) self.v_solution = np.zeros(N+1) self.a_solution = np.zeros(N+1) - self.prev_a = np.array(self.a_solution) self.j_solution = np.zeros(N) + self.prev_a = np.array(self.a_solution) self.yref = np.zeros((N+1, COST_DIM)) + for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) - self.x_sol = np.zeros((N+1, X_DIM)) - self.u_sol = np.zeros((N,1)) + self.params = np.zeros((N+1, PARAM_DIM)) for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) + self.last_cloudlog_t = 0 self.status = False self.crash_cnt = 0.0 @@ -276,16 +268,9 @@ class LongitudinalMpc: def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): jerk_factor = get_jerk_factor(personality) - if self.mode == 'acc': - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] - 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, DANGER_ZONE_COST] - else: - raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') + a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 + cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] self.set_cost_weights(cost_weights, constraint_cost_weights) def set_cur_state(self, v, a): @@ -320,14 +305,14 @@ class LongitudinalMpc: # 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) + min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead) * (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 = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -341,56 +326,28 @@ class LongitudinalMpc: 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]) - self.params[:,0] = ACCEL_MIN - self.params[:,1] = ACCEL_MAX + # 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) - # Update in ACC mode or ACC/e2e blend - if self.mode == 'acc': - self.params[:,5] = LEAD_DANGER_FACTOR + x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) + self.source = SOURCES[np.argmin(x_obstacles[0])] - # 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]) - self.source = SOURCES[np.argmin(x_obstacles[0])] - - # These are not used in ACC mode - x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0 - - elif self.mode == 'blended': - self.params[:,5] = 1.0 - - x_obstacles = np.column_stack([lead_0_obstacle, - lead_1_obstacle]) - cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] - xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) - x = np.cumsum(np.insert(xforward, 0, x[0])) - - x_and_cruise = np.column_stack([x, cruise_target]) - x = np.min(x_and_cruise, axis=1) - - self.source = 'e2e' if x_and_cruise[1,0] < x_and_cruise[1,1] else 'cruise' - - else: - raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update') - - self.yref[:,1] = x - self.yref[:,2] = v - self.yref[:,3] = a - self.yref[:,5] = j + self.yref[:,:] = 0.0 for i in range(N): self.solver.set(i, "yref", self.yref[i]) self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) + self.params[:,0] = ACCEL_MIN + self.params[:,1] = ACCEL_MAX self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.prev_a) self.params[:,4] = t_follow + self.params[:,5] = LEAD_DANGER_FACTOR self.run() if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and @@ -399,18 +356,7 @@ class LongitudinalMpc: else: self.crash_cnt = 0 - # Check if it got within lead comfort range - # TODO This should be done cleaner - if self.mode == 'blended': - if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow))- self.x_sol[:,0] < 0.0): - self.source = 'lead0' - if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow))- self.x_sol[:,0] < 0.0) and \ - (lead_1_obstacle[0] - lead_0_obstacle[0]): - self.source = 'lead1' - def run(self): - # t0 = time.monotonic() - # reset = 0 for i in range(N+1): self.solver.set(i, 'p', self.params[i]) self.solver.constraints_set(0, "lbx", self.x0) @@ -422,13 +368,6 @@ class LongitudinalMpc: self.time_linearization = float(self.solver.get_stats('time_lin')[0]) self.time_integrator = float(self.solver.get_stats('time_sim')[0]) - # qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific - # print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, \ - # integrator {self.time_integrator:.2e}, qp_iter {qp_iter}") - # res = self.solver.get_residuals() - # print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}") - # self.solver.print_statistics() - for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') for i in range(N): @@ -446,12 +385,8 @@ class LongitudinalMpc: self.last_cloudlog_t = t cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}") self.reset() - # reset = 1 - # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(time.monotonic() - t0):.2e} qp {self.time_qp_solution:.2e}, \ - # lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}") if __name__ == "__main__": ocp = gen_long_ocp() AcadosOcpSolver.generate(ocp, json_file=JSON_FILE) - # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 34fc85f8a..ad84ecf24 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -9,13 +9,12 @@ from openpilot.common.filter_simple import FirstOrderFilter 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 LongitudinalMpc, SOURCES 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 -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.] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -26,14 +25,12 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5 _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] - def get_max_accel(v_ego): return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) 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 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 @@ -52,8 +49,6 @@ 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 @@ -67,7 +62,6 @@ class LongitudinalPlanner: 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 def parse_model(model_msg): @@ -90,8 +84,6 @@ class LongitudinalPlanner: return x, v, a, j, throttle_prob def update(self, sm): - mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' - if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) else: @@ -113,12 +105,9 @@ class LongitudinalPlanner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - if mode == 'acc': - accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] - steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) - else: - accel_clip = [ACCEL_MIN, ACCEL_MAX] + accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] + steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg + accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) if reset_state: self.v_desired_filter.x = v_ego @@ -127,7 +116,7 @@ class LongitudinalPlanner: # 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 @@ -141,7 +130,7 @@ class LongitudinalPlanner: 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.mpc.update(sm['radarState'], v_cruise, 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) @@ -163,12 +152,13 @@ class LongitudinalPlanner: output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if mode == 'acc': + if (output_a_target_e2e < output_a_target_mpc) and sm['selfdriveState'].experimentalMode: + output_a_target = output_a_target_e2e + self.output_should_stop = output_should_stop_e2e + self.mpc.source = SOURCES[3] + else: 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 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) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 0fd543dd6..8f66d89bf 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -4,10 +4,15 @@ from parameterized import parameterized_class from cereal import log -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver +def 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 run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0): man = Maneuver( '',