diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 94efd7a875..eaecb63a6b 100644 --- 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 numpy as np from common.realtime import sec_since_boot -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from system.swaglog import cloudlog from selfdrive.modeld.constants import index_function from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU @@ -20,7 +20,7 @@ LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json") -SOURCES = ['lead0', 'lead1', 'cruise'] +SOURCES = ['lead0', 'lead1', 'cruise', 'e2e'] X_DIM = 3 U_DIM = 1 @@ -50,6 +50,7 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1 T_IDXS = np.array(T_IDXS_LST) T_DIFFS = np.diff(T_IDXS, prepend=[0.]) MIN_ACCEL = -3.5 +MAX_ACCEL = 2.0 T_FOLLOW = 1.45 COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 @@ -190,8 +191,8 @@ def gen_long_ocp(): class LongitudinalMpc: - def __init__(self, e2e=False): - self.e2e = e2e + def __init__(self, mode='acc'): + self.mode = mode self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() self.source = SOURCES[2] @@ -225,49 +226,42 @@ class LongitudinalMpc: self.x0 = np.zeros(X_DIM) self.set_weights() - def set_weights(self, prev_accel_constraint=True): - if self.e2e: - self.set_weights_for_xva_policy() - self.params[:,0] = -10. - self.params[:,1] = 10. - self.params[:,2] = 1e5 - else: - self.set_weights_for_lead_policy(prev_accel_constraint) - - def set_weights_for_lead_policy(self, prev_accel_constraint=True): - a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 - W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST])) + def set_cost_weights(self, cost_weights, constraint_cost_weights): + W = np.asfortranarray(np.diag(cost_weights)) for i in range(N): + # TODO don't hardcode A_CHANGE_COST idx # reduce the cost on (a-a_prev) later in the horizon. - W[4,4] = a_change_cost * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) + W[4,4] = cost_weights[4] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, # causing issues with the C interface. self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) # Set L2 slack cost on lower bound constraints - Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]) + Zl = np.array(constraint_cost_weights) for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights_for_xva_policy(self): - W = np.asfortranarray(np.diag([0., 0.2, 0.25, 1., 0.0, .1])) - for i in range(N): - self.solver.cost_set(i, 'W', W) - # Setting the slice without the copy make the array not contiguous, - # causing issues with the C interface. - self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) - - # Set L2 slack cost on lower bound constraints - Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]) - for i in range(N): - self.solver.cost_set(i, 'Zl', Zl) + def set_weights(self, prev_accel_constraint=True): + 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, a_change_cost, J_EGO_COST] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] + elif self.mode == 'blended': + cost_weights = [0., 1.0, 0.0, 0.0, 0.0, 1.0] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0] + elif self.mode == 'e2e': + cost_weights = [0., 0.2, 0.25, 1., 0.0, .1] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0] + else: + raise NotImplementedError(f'Planner mode {self.mode} not recognized') + self.set_cost_weights(cost_weights, constraint_cost_weights) def set_cur_state(self, v, a): v_prev = self.x0[1] self.x0[1] = v self.x0[2] = a - if abs(v_prev - v) > 2.: # probably only helps if v < v_prev + if abs(v_prev - v) > 2.: # probably only helps if v < v_prev for i in range(0, N+1): self.solver.set(i, 'x', self.x0) @@ -306,31 +300,62 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.cruise_max_a = max_a - def update(self, carstate, radarstate, v_cruise): + def update(self, carstate, radarstate, v_cruise, x, v, a, j): v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) - # set accel limits in params - self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL]) - self.params[:,1] = self.cruise_max_a - # 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]) - # 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_upper = v_ego + (T_IDXS * self.cruise_max_a * 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) + # Update in ACC mode or ACC/e2e blend + if self.mode == 'acc': + self.params[:,0] = MIN_ACCEL if self.status else self.cruise_min_a + self.params[:,1] = self.cruise_max_a + + # 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_upper = v_ego + (T_IDXS * self.cruise_max_a * 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) + 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[:,0] = MIN_ACCEL + self.params[:,1] = MAX_ACCEL + + x_obstacles = np.column_stack([lead_0_obstacle, + lead_1_obstacle]) + cruise_target = T_IDXS * v_cruise + 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' + + else: + raise NotImplementedError(f'Planner mode {self.mode} not recognized') + + self.yref[:,1] = x + self.yref[:,2] = v + self.yref[:,3] = a + self.yref[:,5] = j + for i in range(N): + self.solver.set(i, "yref", self.yref[i]) + self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] @@ -345,6 +370,10 @@ class LongitudinalMpc: self.crash_cnt = 0 def update_with_xva(self, x, v, a): + self.params[:,0] = -10. + self.params[:,1] = 10. + self.params[:,2] = 1e5 + # v, and a are in local frame, but x is wrt the x[0] position # In >90degree turns, x goes to 0 (and may even be -ve) # So, we use integral(v) + x[0] to obtain the forward-distance diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cf51136770..42ce2bd0ec 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -53,12 +53,31 @@ class Planner: self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) + self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5) 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 + def parse_model(self, model_msg): + if (len(model_msg.position.x) == 33 and + len(model_msg.velocity.x) == 33 and + len(model_msg.acceleration.x) == 33): + x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) + v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) + a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x) + # Uniform interp so gradient is less noisy + a_sparse = np.interp(self.t_uniform, T_IDXS, model_msg.acceleration.x) + j_sparse = np.gradient(a_sparse, self.t_uniform) + j = np.interp(T_IDXS_MPC, self.t_uniform, j_sparse) + 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)) + return x, v, a, j + def update(self, sm): v_ego = sm['carState'].vEgo @@ -95,7 +114,8 @@ class Planner: self.mpc.set_weights(prev_accel_constraint) 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['carState'], sm['radarState'], v_cruise) + x, v, a, j = self.parse_model(sm['modelV2']) + self.mpc.update(sm['carState'], sm['radarState'], v_cruise, x, v, a, j) self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 7a7a6ff6e3..8d02eb6b2f 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -203,6 +203,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic std::array pos_x_std, pos_y_std, pos_z_std; std::array vel_x, vel_y, vel_z; std::array rot_x, rot_y, rot_z; + std::array acc_x, acc_y, acc_z; std::array rot_rate_x, rot_rate_y, rot_rate_z; for(int i=0; i