diff --git a/SConstruct b/SConstruct index 5e457bb2df..0e06df62f9 100644 --- a/SConstruct +++ b/SConstruct @@ -414,7 +414,6 @@ SConscript(['selfdrive/modeld/SConscript']) SConscript(['selfdrive/controls/lib/cluster/SConscript']) SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript']) -SConscript(['selfdrive/controls/lib/lead_mpc_lib/SConscript']) SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript']) SConscript(['selfdrive/boardd/SConscript']) diff --git a/release/files_common b/release/files_common index 94f2bb4ba7..2d3925ebc2 100644 --- a/release/files_common +++ b/release/files_common @@ -240,10 +240,8 @@ selfdrive/controls/lib/fcw.py selfdrive/controls/lib/cluster/* selfdrive/controls/lib/lateral_mpc_lib/.gitignore -selfdrive/controls/lib/lead_mpc_lib/.gitignore selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore selfdrive/controls/lib/lateral_mpc_lib/* -selfdrive/controls/lib/lead_mpc_lib/* selfdrive/controls/lib/longitudinal_mpc_lib/* selfdrive/hardware/__init__.py diff --git a/selfdrive/controls/lib/lead_mpc_lib/.gitignore b/selfdrive/controls/lib/lead_mpc_lib/.gitignore deleted file mode 100644 index 60533746d8..0000000000 --- a/selfdrive/controls/lib/lead_mpc_lib/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -acados_ocp_lead.json -c_generated_code/ diff --git a/selfdrive/controls/lib/lead_mpc_lib/SConscript b/selfdrive/controls/lib/lead_mpc_lib/SConscript deleted file mode 100644 index 75a4c7fd1a..0000000000 --- a/selfdrive/controls/lib/lead_mpc_lib/SConscript +++ /dev/null @@ -1,58 +0,0 @@ -Import('env', 'arch') - -gen = "c_generated_code" - -casadi_model = [ - f'{gen}/lead_model/lead_expl_ode_fun.c', - f'{gen}/lead_model/lead_expl_vde_forw.c', -] - -casadi_cost_y = [ - f'{gen}/lead_cost/lead_cost_y_fun.c', - f'{gen}/lead_cost/lead_cost_y_fun_jac_ut_xt.c', - f'{gen}/lead_cost/lead_cost_y_hess.c', -] - -casadi_cost_e = [ - f'{gen}/lead_cost/lead_cost_y_e_fun.c', - f'{gen}/lead_cost/lead_cost_y_e_fun_jac_ut_xt.c', - f'{gen}/lead_cost/lead_cost_y_e_hess.c', -] - -casadi_cost_0 = [ - f'{gen}/lead_cost/lead_cost_y_0_fun.c', - f'{gen}/lead_cost/lead_cost_y_0_fun_jac_ut_xt.c', - f'{gen}/lead_cost/lead_cost_y_0_hess.c', -] - -build_files = [f'{gen}/acados_solver_lead.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0 - -# extra generated files used to trigger a rebuild -generated_files = [ - f'{gen}/Makefile', - - f'{gen}/main_lead.c', - f'{gen}/acados_solver_lead.h', - - f'{gen}/lead_model/lead_expl_vde_adj.c', - - f'{gen}/lead_model/lead_model.h', - f'{gen}/lead_cost/lead_cost_y_fun.h', - f'{gen}/lead_cost/lead_cost_y_e_fun.h', - f'{gen}/lead_cost/lead_cost_y_0_fun.h', -] + build_files - -lenv = env.Clone() -lenv.Clean(generated_files, Dir(gen)) - -lenv.Command(generated_files, - ["lead_mpc.py"], - f"cd {Dir('.').abspath} && python lead_mpc.py") - -lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES") -lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES") -lenv["CCFLAGS"].append("-Wno-unused") -lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags") -lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lead", - build_files, - LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e']) diff --git a/selfdrive/controls/lib/lead_mpc_lib/__init__.py b/selfdrive/controls/lib/lead_mpc_lib/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py b/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py deleted file mode 100644 index de4f1b3276..0000000000 --- a/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py +++ /dev/null @@ -1,270 +0,0 @@ -#!/usr/bin/env python3 -import os -import math -import numpy as np - -from common.realtime import sec_since_boot -from common.numpy_fast import clip -from selfdrive.swaglog import cloudlog -from selfdrive.modeld.constants import T_IDXS -from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG, CONTROL_N -from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU - -from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver -from casadi import SX, vertcat, sqrt, exp - -LEAD_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) -EXPORT_DIR = os.path.join(LEAD_MPC_DIR, "c_generated_code") -JSON_FILE = "acados_ocp_lead.json" - -MPC_T = list(np.arange(0,1.,.2)) + list(np.arange(1.,10.6,.6)) -N = len(MPC_T) - 1 - - -def desired_follow_distance(v_ego, v_lead): - TR = 1.8 - G = 9.81 - return (v_ego * TR - (v_lead - v_ego) * TR + v_ego * v_ego / (2 * G) - v_lead * v_lead / (2 * G)) + 4.0 - - -def gen_lead_model(): - model = AcadosModel() - model.name = 'lead' - - # set up states & controls - x_ego = SX.sym('x_ego') - v_ego = SX.sym('v_ego') - a_ego = SX.sym('a_ego') - model.x = vertcat(x_ego, v_ego, a_ego) - - # controls - j_ego = SX.sym('j_ego') - model.u = vertcat(j_ego) - - # xdot - x_ego_dot = SX.sym('x_ego_dot') - v_ego_dot = SX.sym('v_ego_dot') - a_ego_dot = SX.sym('a_ego_dot') - model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot) - - # live parameters - x_lead = SX.sym('x_lead') - v_lead = SX.sym('v_lead') - model.p = vertcat(x_lead, v_lead) - - # dynamics model - f_expl = vertcat(v_ego, a_ego, j_ego) - model.f_impl_expr = model.xdot - f_expl - model.f_expl_expr = f_expl - return model - - -def gen_lead_mpc_solver(): - ocp = AcadosOcp() - ocp.model = gen_lead_model() - - Tf = np.array(MPC_T)[-1] - - # set dimensions - ocp.dims.N = N - - # set cost module - ocp.cost.cost_type = 'NONLINEAR_LS' - ocp.cost.cost_type_e = 'NONLINEAR_LS' - - QR = np.diag([0.0, 0.0, 0.0, 0.0]) - Q = np.diag([0.0, 0.0, 0.0]) - - ocp.cost.W = QR - ocp.cost.W_e = Q - - x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2] - j_ego = ocp.model.u[0] - - ocp.cost.yref = np.zeros((4, )) - ocp.cost.yref_e = np.zeros((3, )) - - x_lead, v_lead = ocp.model.p[0], ocp.model.p[1] - desired_dist = desired_follow_distance(v_ego, v_lead) - dist_err = (desired_dist - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1) - - # TODO hacky weights to keep behavior the same - ocp.model.cost_y_expr = vertcat(exp(.3 * dist_err) - 1., - ((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5), - a_ego * (.1 * v_ego + 1.0), - j_ego * (.1 * v_ego + 1.0)) - ocp.model.cost_y_expr_e = vertcat(exp(.3 * dist_err) - 1., - ((x_lead - x_ego) - (desired_dist)) / (0.05 * v_ego + 0.5), - a_ego * (.1 * v_ego + 1.0)) - ocp.parameter_values = np.array([0., .0]) - - # set constraints - ocp.constraints.constr_type = 'BGH' - ocp.constraints.idxbx = np.array([1,]) - ocp.constraints.lbx = np.array([0,]) - ocp.constraints.ubx = np.array([100.,]) - x0 = np.array([0.0, 0.0, 0.0]) - ocp.constraints.x0 = x0 - - - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' - ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' - ocp.solver_options.integrator_type = 'ERK' - ocp.solver_options.nlp_solver_type = 'SQP_RTI' - #ocp.solver_options.nlp_solver_tol_stat = 1e-3 - #ocp.solver_options.tol = 1e-3 - - ocp.solver_options.qp_solver_iter_max = 10 - #ocp.solver_options.qp_tol = 1e-3 - - # set prediction horizon - ocp.solver_options.tf = Tf - ocp.solver_options.shooting_nodes = np.array(MPC_T) - - ocp.code_export_directory = EXPORT_DIR - return ocp - - -class LeadMpc(): - def __init__(self, lead_id): - self.lead_id = lead_id - self.solver = AcadosOcpSolver('lead', N, EXPORT_DIR) - self.v_solution = [0.0 for i in range(N)] - self.a_solution = [0.0 for i in range(N)] - self.j_solution = [0.0 for i in range(N-1)] - yref = np.zeros((N+1,4)) - self.solver.cost_set_slice(0, N, "yref", yref[:N]) - self.solver.set(N, "yref", yref[N][:3]) - self.x_sol = np.zeros((N+1, 3)) - self.u_sol = np.zeros((N,1)) - self.lead_xv = np.zeros((N+1,2)) - self.reset() - self.set_weights() - - def reset(self): - for i in range(N+1): - self.solver.set(i, 'x', np.zeros(3)) - self.last_cloudlog_t = 0 - self.status = False - self.new_lead = False - self.prev_lead_status = False - self.crashing = False - self.prev_lead_x = 10 - self.solution_status = 0 - self.x0 = np.zeros(3) - - def set_weights(self): - W = np.diag([MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, - MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK]) - Ws = np.tile(W[None], reps=(N,1,1)) - self.solver.cost_set_slice(0, N, 'W', Ws, api='old') - #TODO hacky weights to keep behavior the same - self.solver.cost_set(N, 'W', (3./5.)*W[:3,:3]) - - def set_cur_state(self, v, a): - self.x0[1] = v - self.x0[2] = a - - def extrapolate_lead(self, x_lead, v_lead, a_lead_0, a_lead_tau): - dt =.2 - t = .0 - for i in range(N+1): - if i > 4: - dt = .6 - self.lead_xv[i, 0], self.lead_xv[i, 1] = x_lead, v_lead - a_lead = a_lead_0 * math.exp(-a_lead_tau * (t**2)/2.) - x_lead += v_lead * dt - v_lead += a_lead * dt - if v_lead < 0.0: - a_lead = 0.0 - v_lead = 0.0 - t += dt - - def init_with_sim(self, v_ego, lead_xv, a_lead_0): - a_ego = min(0.0, -2 * (v_ego - lead_xv[0,1]) * (v_ego - lead_xv[0,1]) / (2.0 * lead_xv[0,0] + 0.01) + a_lead_0) - dt =.2 - t = .0 - x_ego = 0.0 - for i in range(N+1): - if i > 4: - dt = .6 - v_ego += a_ego * dt - if v_ego <= 0.0: - v_ego = 0.0 - a_ego = 0.0 - x_ego += v_ego * dt - t += dt - self.solver.set(i, 'x', np.array([x_ego, v_ego, a_ego])) - - def update(self, carstate, radarstate, v_cruise): - self.crashing = False - v_ego = self.x0[1] - if self.lead_id == 0: - lead = radarstate.leadOne - else: - lead = radarstate.leadTwo - self.status = lead.status - if lead is not None and lead.status: - x_lead = lead.dRel - v_lead = max(0.0, lead.vLead) - a_lead = clip(lead.aLeadK, -5.0, 5.0) - - # MPC will not converge if immidiate crash is expected - # Clip lead distance to what is still possible to brake for - MIN_ACCEL = -3.5 - min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2) - if x_lead < min_x_lead: - x_lead = min_x_lead - self.crashing = True - - if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): - v_lead = 0.0 - a_lead = 0.0 - - self.a_lead_tau = lead.aLeadTau - self.new_lead = False - self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau) - if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: - self.init_with_sim(v_ego, self.lead_xv, a_lead) - self.new_lead = True - - self.prev_lead_status = True - self.prev_lead_x = x_lead - else: - self.prev_lead_status = False - # Fake a fast lead car, so mpc keeps running - x_lead = 50.0 - v_lead = v_ego + 10.0 - a_lead = 0.0 - self.a_lead_tau = _LEAD_ACCEL_TAU - self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau) - self.solver.constraints_set(0, "lbx", self.x0) - self.solver.constraints_set(0, "ubx", self.x0) - for i in range(N+1): - self.solver.set_param(i, self.lead_xv[i]) - - self.solution_status = self.solver.solve() - self.solver.fill_in_slice(0, N+1, 'x', self.x_sol) - self.solver.fill_in_slice(0, N, 'u', self.u_sol) - #self.solver.print_statistics() - - self.v_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T, list(self.x_sol[:,1])) - self.a_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T, list(self.x_sol[:,2])) - self.j_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T[:-1], list(self.u_sol[:,0])) - - # Reset if goes through lead car - self.crashing = self.crashing or np.sum(self.lead_xv[:,0] - self.x_sol[:,0] < 0) > 0 - - t = sec_since_boot() - if self.solution_status != 0: - if t > self.last_cloudlog_t + 5.0: - self.last_cloudlog_t = t - cloudlog.warning("Lead mpc %d reset, solution_status: %s" % ( - self.lead_id, self.solution_status)) - self.prev_lead_status = False - self.reset() - - -if __name__ == "__main__": - ocp = gen_lead_mpc_solver() - AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index 125d4a12be..70a3e72b1d 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -25,7 +25,15 @@ casadi_cost_0 = [ f'{gen}/long_cost/long_cost_y_0_hess.c', ] -build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0 +casadi_constraints = [ + f'{gen}/long_constraints/long_constr_h_fun.c', + f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c', + f'{gen}/long_constraints/long_constr_h_e_fun.c', + f'{gen}/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c', +] + +build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e + \ + casadi_cost_0 + casadi_constraints # extra generated files used to trigger a rebuild generated_files = [ @@ -37,6 +45,8 @@ generated_files = [ f'{gen}/long_model/long_expl_vde_adj.c', f'{gen}/long_model/long_model.h', + f'{gen}/long_constraints/long_h_constraint.h', + f'{gen}/long_constraints/long_h_e_constraint.h', f'{gen}/long_cost/long_cost_y_fun.h', f'{gen}/long_cost/long_cost_y_e_fun.h', f'{gen}/long_cost/long_cost_y_0_fun.h', diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index ac5f1096ac..21ef243c8e 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -1,23 +1,52 @@ #!/usr/bin/env python3 import os +import math import numpy as np -from common.numpy_fast import clip from common.realtime import sec_since_boot +from common.numpy_fast import clip from selfdrive.swaglog import cloudlog +from selfdrive.modeld.constants import T_IDXS as T_IDXS_LST from selfdrive.controls.lib.drive_helpers import LON_MPC_N as N -from selfdrive.modeld.constants import T_IDXS +from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver from casadi import SX, vertcat - - - LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") JSON_FILE = "acados_ocp_long.json" +SOURCES = ['lead0', 'lead1', 'cruise'] + +X_DIM = 3 +U_DIM = 1 +COST_E_DIM = 3 +COST_DIM = COST_E_DIM + 1 +MIN_ACCEL = -3.5 + +X_EGO_COST = 80. +X_EGO_E2E_COST = 100. +A_EGO_COST = .1 +J_EGO_COST = .2 +DANGER_ZONE_COST = 1e3 +CRASH_DISTANCE = 1.5 +LIMIT_COST = 1e6 +T_IDXS = np.array(T_IDXS_LST) + +T_REACT = 1.8 +MAX_BRAKE = 9.81 + + +def get_stopped_equivalence_factor(v_lead): + return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE) + +def get_safe_obstacle_distance(v_ego): + return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0 + +def desired_follow_distance(v_ego, v_lead): + return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) + def gen_long_model(): model = AcadosModel() @@ -39,6 +68,12 @@ def gen_long_model(): a_ego_dot = SX.sym('a_ego_dot') model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot) + # live parameters + x_obstacle = SX.sym('x_obstacle') + a_min = SX.sym('a_min') + a_max = SX.sym('a_max') + model.p = vertcat(a_min, a_max, x_obstacle) + # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) model.f_impl_expr = model.xdot - f_expl @@ -50,7 +85,7 @@ def gen_long_mpc_solver(): ocp = AcadosOcp() ocp.model = gen_long_model() - Tf = np.array(T_IDXS)[N] + Tf = T_IDXS[-1] # set dimensions ocp.dims.N = N @@ -59,8 +94,8 @@ def gen_long_mpc_solver(): ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS' - QR = np.diag([0.0, 0.0, 0.0, 0.0]) - Q = np.diag([0.0, 0.0, 0.0]) + QR = np.zeros((COST_DIM, COST_DIM)) + Q = np.zeros((COST_E_DIM, COST_E_DIM)) ocp.cost.W = QR ocp.cost.W_e = Q @@ -68,110 +103,245 @@ def gen_long_mpc_solver(): x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2] j_ego = ocp.model.u[0] - ocp.cost.yref = np.zeros((4, )) - ocp.cost.yref_e = np.zeros((3, )) - # TODO hacky weights to keep behavior the same - ocp.model.cost_y_expr = vertcat(x_ego, v_ego, a_ego, j_ego) - ocp.model.cost_y_expr_e = vertcat(x_ego, v_ego, a_ego) - - # set constraints - ocp.constraints.constr_type = 'BGH' - ocp.constraints.idxbx = np.array([0, 1,2]) - ocp.constraints.lbx = np.array([0., 0, -1.2]) - ocp.constraints.ubx = np.array([10000, 100., 1.2]) - ocp.constraints.Jsbx = np.eye(3) - x0 = np.array([0.0, 0.0, 0.0]) + a_min, a_max = ocp.model.p[0], ocp.model.p[1] + x_obstacle = ocp.model.p[2] + + ocp.cost.yref = np.zeros((COST_DIM, )) + ocp.cost.yref_e = np.zeros((COST_E_DIM, )) + + desired_dist_comfort = get_safe_obstacle_distance(v_ego) + desired_dist_danger = (7/8) * get_safe_obstacle_distance(v_ego) + + costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.), + x_ego, + a_ego * (v_ego + 10.), + j_ego * (v_ego + 10.)] + ocp.model.cost_y_expr = vertcat(*costs) + ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) + + constraints = vertcat((v_ego), + (a_ego - a_min), + (a_max - a_ego), + ((x_obstacle - x_ego) - (desired_dist_danger)) / (v_ego + 10.), + 0.0) + ocp.model.con_h_expr = constraints + ocp.model.con_h_expr_e = constraints + + x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 + ocp.parameter_values = np.array([-1.2, 1.2, 0.0]) - l2_penalty = 1.0 + # These constraints put hard limits on speed and + # acceleration as well as giving an assymetrical + # cost on approaching a lead. We only use lower + # bounds with an L2 cost. l1_penalty = 0.0 - weights = np.array([0.0, 1e4, 1e4]) - ocp.cost.Zl = l2_penalty * weights - ocp.cost.Zu = l2_penalty * weights + l2_penalty = 1.0 + weights = np.array([1e6, 1e6, 1e6, 0.0, 0.]) ocp.cost.zl = l1_penalty * weights - ocp.cost.zu = l1_penalty * weights + ocp.cost.Zl = l2_penalty * weights + ocp.cost.Zu = 0.0 * weights + ocp.cost.zu = 0.0 * weights + + ocp.constraints.lh = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) + ocp.constraints.lh_e = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) + ocp.constraints.uh = np.array([1e3, 1e3, 1e3, 1e4, 1e4]) + ocp.constraints.uh_e = np.array([1e3, 1e3, 1e3, 1e6, 1e6]) + ocp.constraints.idxsh = np.array([0,1,2,3,4]) + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.nlp_solver_type = 'SQP_RTI' - ocp.solver_options.qp_solver_iter_max = 2 + + ocp.solver_options.qp_solver_iter_max = 3 # set prediction horizon ocp.solver_options.tf = Tf - ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1] + ocp.solver_options.shooting_nodes = T_IDXS ocp.code_export_directory = EXPORT_DIR return ocp class LongitudinalMpc(): - def __init__(self): - self.solver = AcadosOcpSolver('long', N, EXPORT_DIR) - self.x_sol = np.zeros((N+1, 3)) - self.u_sol = np.zeros((N, 1)) - self.set_weights() - - self.v_solution = [0.0 for i in range(len(T_IDXS))] - self.a_solution = [0.0 for i in range(len(T_IDXS))] - self.j_solution = [0.0 for i in range(len(T_IDXS)-1)] - self.yref = np.zeros((N+1, 4)) - self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) - self.solver.cost_set(N, "yref", self.yref[N][:3]) - self.T_IDXS = np.array(T_IDXS[:N+1]) - self.min_a = -1.2 - self.max_a = 1.2 - self.mins = np.tile(np.array([0.0, 0.0, self.min_a])[None], reps=(N-1,1)) - self.maxs = np.tile(np.array([0.0, 100.0, self.max_a])[None], reps=(N-1,1)) - self.x0 = np.zeros(3) + def __init__(self, e2e=False): + self.e2e = e2e self.reset() + self.accel_limit_arr = np.zeros((N+1, 2)) + self.accel_limit_arr[:,0] = -1.2 + self.accel_limit_arr[:,1] = 1.2 + self.source = SOURCES[2] def reset(self): + self.solver = AcadosOcpSolver('long', N, EXPORT_DIR) + self.v_solution = [0.0 for i in range(N+1)] + self.a_solution = [0.0 for i in range(N+1)] + self.j_solution = [0.0 for i in range(N)] + self.yref = np.zeros((N+1, COST_DIM)) + self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) + self.solver.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,3)) + for i in range(N+1): + self.solver.set(i, 'x', np.zeros(X_DIM)) self.last_cloudlog_t = 0 - self.status = True + self.status = False + self.new_lead = False + self.prev_lead_status = False + self.crashing = False + self.prev_lead_x = 10 self.solution_status = 0 - for i in range(N+1): - self.solver.set(i, 'x', self.x0) + self.x0 = np.zeros(X_DIM) + self.set_weights() def set_weights(self): - W = np.diag([0.0, 1.0, 0.0, 50.0]) + if self.e2e: + self.set_weights_for_xva_policy() + else: + self.set_weights_for_lead_policy() + + def set_weights_for_lead_policy(self): + W = np.diag([X_EGO_COST, 0.0, A_EGO_COST, J_EGO_COST]) Ws = np.tile(W[None], reps=(N,1,1)) self.solver.cost_set_slice(0, N, 'W', Ws, api='old') #TODO hacky weights to keep behavior the same - self.solver.cost_set(N, 'W', (3/20.)*W[:3,:3]) + self.solver.cost_set(N, 'W', (3./5.)*np.copy(W[:COST_E_DIM, :COST_E_DIM])) - def set_accel_limits(self, min_a, max_a): - self.min_a = min_a - self.max_a = max_a - self.mins[:,2] = self.min_a - self.maxs[:,2] = self.max_a - self.solver.constraints_set_slice(1, N, "lbx", self.mins, api='old') - self.solver.constraints_set_slice(1, N, "ubx", self.maxs, api='old') + Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST, 0.]) + Zls = np.tile(Zl[None], reps=(N+1,1,1)) + self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old') + + def set_weights_for_xva_policy(self): + W = np.diag([0.0, X_EGO_E2E_COST, 0., J_EGO_COST]) + Ws = np.tile(W[None], reps=(N,1,1)) + self.solver.cost_set_slice(0, N, 'W', Ws, api='old') + self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM])) + + Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0, 0.]) + Zls = np.tile(Zl[None], reps=(N+1,1,1)) + self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old') def set_cur_state(self, v, a): - self.x0[1] = v - self.x0[2] = a + if abs(self.x0[1] - v) > 1.: + self.x0[1] = v + self.x0[2] = a + for i in range(0, N+1): + self.solver.set(i, 'x', self.x0) + else: + self.x0[1] = v + self.x0[2] = a + + def extrapolate_lead(self, x_lead, v_lead, a_lead_0, a_lead_tau): + lead_xv = np.zeros((N+1,2)) + lead_xv[0, 0], lead_xv[0, 1] = x_lead, v_lead + for i in range(1, N+1): + dt = T_IDXS[i] - T_IDXS[i-1] + a_lead = a_lead_0 * math.exp(-a_lead_tau * (T_IDXS[i]**2)/2.) + x_lead += v_lead * dt + v_lead += a_lead * dt + if v_lead < 0.0: + a_lead = 0.0 + v_lead = 0.0 + lead_xv[i, 0], lead_xv[i, 1] = x_lead, v_lead + return lead_xv + + def process_lead(self, lead): + v_ego = self.x0[1] + if lead is not None and lead.status: + x_lead = lead.dRel + v_lead = max(0.0, lead.vLead) + a_lead = clip(lead.aLeadK, -10.0, 5.0) + + # MPC will not converge if immidiate 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) / (-MIN_ACCEL * 2) + if x_lead < min_x_lead: + x_lead = min_x_lead + self.crashing = True + + if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): + v_lead = 0.0 + a_lead = 0.0 + + self.a_lead_tau = lead.aLeadTau + self.new_lead = False + lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau) + if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: + self.new_lead = True + + self.prev_lead_status = True + self.prev_lead_x = x_lead + else: + self.prev_lead_status = False + # 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 + self.a_lead_tau = _LEAD_ACCEL_TAU + lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau) + return lead_xv + + def set_accel_limits(self, min_a, max_a): + self.cruise_min_a = min_a + self.cruise_max_a = max_a + + def update(self, carstate, radarstate, v_cruise): + v_ego = self.x0[1] + self.crashing = False + 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) + self.accel_limit_arr[:,0] = np.interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL]) + self.accel_limit_arr[:,1] = self.cruise_max_a + + # To consider 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 + # TODO find cleaner way to write hacky fake cruise obstacle + cruise_lower_bound = v_ego - (1.0 + ((v_ego + 15)/60) * T_IDXS) + cruise_upper_bound = v_ego + (.7 + .7*T_IDXS) + v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), + cruise_lower_bound, + cruise_upper_bound) + cruise_obstacle = T_IDXS*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])] + x_obstacle = np.min(x_obstacles, axis=1) + self.params = np.concatenate([self.accel_limit_arr, + x_obstacle[:,None]], axis=1) + self.run() + self.crashing = self.crashing or np.sum(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) > 0 + + + def update_with_xva(self, x, v, a): + self.yref[:,1] = x + self.solver.cost_set_slice(0, N, "yref", self.yref[:N], api='old') + self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) + self.accel_limit_arr[:,0] = -10. + self.accel_limit_arr[:,1] = 10. + x_obstacle = 1e5*np.ones((N+1)) + self.params = np.concatenate([self.accel_limit_arr, + x_obstacle[:,None]], axis=1) + self.run() + + + def run(self): + for i in range(N+1): + self.solver.set_param(i, self.params[i]) self.solver.constraints_set(0, "lbx", self.x0) self.solver.constraints_set(0, "ubx", self.x0) - - def update(self, carstate, model, v_cruise): - v_cruise_clipped = clip(v_cruise, self.x0[1] - 10., self.x0[1] + 10.0) - position = v_cruise_clipped * self.T_IDXS - speed = v_cruise_clipped * np.ones(N+1) - accel = np.zeros(N+1) - self.update_with_xva(position, speed, accel) - - def update_with_xva(self, position, speed, accel): - self.yref[:,0] = position - self.yref[:,1] = speed - self.yref[:,2] = accel - self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) - self.solver.cost_set(N, "yref", self.yref[N][:3]) - self.solution_status = self.solver.solve() self.solver.fill_in_slice(0, N+1, 'x', self.x_sol) self.solver.fill_in_slice(0, N, 'u', self.u_sol) - #self.solver.print_statistics() self.v_solution = list(self.x_sol[:,1]) self.a_solution = list(self.x_sol[:,2]) @@ -181,7 +351,9 @@ class LongitudinalMpc(): if self.solution_status != 0: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t - cloudlog.warning(f'Longitudinal model mpc reset, solution status: {self.solution_status}') + cloudlog.warning("Long mpc reset, solution_status: %s" % ( + self.solution_status)) + self.prev_lead_status = False self.reset() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 339b9cf781..ba87b35dae 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -11,7 +11,6 @@ from selfdrive.modeld.constants import T_IDXS from selfdrive.config import Conversions as CV from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.longcontrol import LongCtrlState -from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import LeadMpc from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N from selfdrive.swaglog import cloudlog @@ -47,17 +46,13 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class Planner(): def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - self.mpcs = {} - self.mpcs['lead0'] = LeadMpc(0) - self.mpcs['lead1'] = LeadMpc(1) - self.mpcs['cruise'] = LongitudinalMpc() + self.mpc = LongitudinalMpc() self.fcw = False self.fcw_checker = FCWChecker() self.v_desired = init_v self.a_desired = init_a - self.longitudinalPlanSource = 'cruise' self.alpha = np.exp(-DT_MDL/2.0) self.lead_0 = log.ModelDataV2.LeadDataV3.new_message() self.lead_1 = log.ModelDataV2.LeadDataV3.new_message() @@ -100,23 +95,18 @@ class Planner(): # 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.mpcs['cruise'].set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) - next_a = np.inf - for key in self.mpcs: - self.mpcs[key].set_cur_state(self.v_desired, self.a_desired) - self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise) - if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a: # picks slowest solution from accel in ~0.2 seconds - self.longitudinalPlanSource = key - self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N] - self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N] - self.j_desired_trajectory = self.mpcs[key].j_solution[:CONTROL_N] - next_a = self.mpcs[key].a_solution[5] + self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) + self.mpc.set_cur_state(self.v_desired, self.a_desired) + self.mpc.update(sm['carState'], sm['radarState'], v_cruise) + self.v_desired_trajectory = self.mpc.v_solution[:CONTROL_N] + self.a_desired_trajectory = self.mpc.a_solution[:CONTROL_N] + self.j_desired_trajectory = self.mpc.j_solution[:CONTROL_N] # determine fcw - if self.mpcs['lead0'].new_lead: + if self.mpc.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker - self.fcw = self.fcw_checker.update(self.mpcs['lead0'].x_sol[:,2], cur_time, + self.fcw = self.fcw_checker.update(self.mpc.x_sol[:,2], cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, @@ -143,8 +133,8 @@ class Planner(): longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory] longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory] - longitudinalPlan.hasLead = self.mpcs['lead0'].status - longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource + longitudinalPlan.hasLead = self.mpc.prev_lead_status + longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 2c26d84a81..5b3fa65228 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -2,10 +2,10 @@ import unittest import numpy as np -from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance +from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver -def run_following_distance_simulation(v_lead, t_end=150.0): +def run_following_distance_simulation(v_lead, t_end=100.0): man = Maneuver( '', duration=t_end, @@ -29,7 +29,7 @@ class TestFollowingDistance(unittest.TestCase): simulation_steady_state = run_following_distance_simulation(v_lead) correct_steady_state = desired_follow_distance(v_lead, v_lead) - self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.2) + self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .3)) if __name__ == "__main__": diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 8d30f40ed5..c433f81d7a 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -9,9 +9,9 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver # TODO: make new FCW tests maneuvers = [ Maneuver( - 'approach stopped car at 30m/s', + 'approach stopped car at 20m/s', duration=20., - initial_speed=30., + initial_speed=25., lead_relevancy=True, initial_distance_lead=120., speed_lead_values=[30., 0.], @@ -22,7 +22,7 @@ maneuvers = [ duration=20., initial_speed=20., lead_relevancy=True, - initial_distance_lead=60., + initial_distance_lead=90., speed_lead_values=[20., 0.], breakpoints=[0., 1.], ), @@ -54,22 +54,26 @@ maneuvers = [ breakpoints=[0., 15., 21.66], ), Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2', + 'steady state following a car at 20m/s, then lead decel to 0mph at 4m/s^2', duration=40., initial_speed=20., lead_relevancy=True, initial_distance_lead=35., speed_lead_values=[20., 20., 0.], - breakpoints=[0., 15., 19.], + prob_lead_values=[0., 1., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[2., 2.01, 7.01], ), Maneuver( "approach stopped car at 20m/s", duration=30., initial_speed=20., lead_relevancy=True, - initial_distance_lead=50., - speed_lead_values=[0., 0.], - breakpoints=[1., 11.], + initial_distance_lead=120., + speed_lead_values=[0.0, 0., 0.], + prob_lead_values=[0.0, 0., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[0.0, 2., 2.01], ), Maneuver( "approach slower cut-in car at 20m/s", @@ -92,6 +96,16 @@ maneuvers = [ breakpoints=[1., 11.], only_radar=True, ), + Maneuver( + "NaN recovery", + duration=30., + initial_speed=15., + lead_relevancy=True, + initial_distance_lead=60., + speed_lead_values=[0., 0., 0.0], + breakpoints=[1., 1.01, 11.], + cruise_values=[float("nan"), 15., 15.], + ), ] @@ -118,7 +132,7 @@ def run_maneuver_worker(k): man = maneuvers[k] print(man.title) valid, _ = man.evaluate() - self.assertTrue(valid) + self.assertTrue(valid, msg=man.title) return run diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3165f7fc07..1d995bad7e 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8c733450bb28bcdb11d6b9991c8784e1f720f7b2 \ No newline at end of file +2282e3f208438237fe61d7bf636d6ad6b507c571 \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 660823a1d9..6da32679cd 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -23,7 +23,7 @@ PROCS = { "selfdrive.controls.controlsd": 50.0, "./loggerd": 45.0, "./locationd": 9.1, - "selfdrive.controls.plannerd": 33.0, + "selfdrive.controls.plannerd": 27.0, "./_ui": 15.0, "selfdrive.locationd.paramsd": 9.1, "./camerad": 7.07, @@ -55,7 +55,7 @@ if TICI: "selfdrive.controls.controlsd": 28.0, "./camerad": 31.0, "./_ui": 21.0, - "selfdrive.controls.plannerd": 15.0, + "selfdrive.controls.plannerd": 14.0, "selfdrive.locationd.paramsd": 5.0, "./_dmonitoringmodeld": 10.0, "selfdrive.thermald.thermald": 1.5,