diff --git a/phonelibs/acados/build.sh b/phonelibs/acados/build.sh index 2d5690a891..749ac8291d 100755 --- a/phonelibs/acados/build.sh +++ b/phonelibs/acados/build.sh @@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then fi cd acados_repo git fetch -git checkout 4bfbdd7915d188cc2f56da10236c780460ed30f0 +git checkout 43ba28e95062f9ac9b48facd3b45698d57666fa3 git submodule update --recursive --init # build diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py index 449c89da1f..4e80e12054 100644 --- a/pyextra/acados_template/acados_ocp_solver.py +++ b/pyextra/acados_template/acados_ocp_solver.py @@ -881,6 +881,15 @@ class AcadosOcpSolver: self.shared_lib.ocp_nlp_set.argtypes = \ [c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int + self.shared_lib.ocp_nlp_out_get_slice.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + + def solve(self): """ Solve the ocp with current input. @@ -893,24 +902,13 @@ class AcadosOcpSolver: def get_slice(self, start_stage_, end_stage_, field_): - """ - Get the last solution of the solver: - - :param start_stage: integer corresponding to shooting node that indicates start of slice - :param end_stage: integer corresponding to shooting node that indicates end of slice - :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',] - - .. note:: regarding lam, t: \n - the inequalities are internally organized in the following order: \n - [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n - lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, start_stage_, field_) + out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64) + self.fill_in_slice(start_stage_, end_stage_, field_, out) + return out - .. note:: pi: multipliers for dynamics equality constraints \n - lam: multipliers for inequalities \n - t: slack variables corresponding to evaluation of all inequalities (at the solution) \n - sl: slack variables of soft lower inequality constraints \n - su: slack variables of soft upper inequality constraints \n - """ + def fill_in_slice(self, start_stage_, end_stage_, field_, arr): out_fields = ['x', 'u', 'z', 'pi', 'lam', 't'] mem_fields = ['sl', 'su'] field = field_ @@ -931,30 +929,16 @@ class AcadosOcpSolver: if start_stage_ < 0 or end_stage_ > self.N + 1: raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N)) - self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ - [c_void_p, c_void_p, c_void_p, c_int, c_char_p] - self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int - dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ - self.nlp_dims, self.nlp_out, start_stage_, field) - - out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64) - out_data = cast(out.ctypes.data, POINTER(c_double)) + out_data = cast(arr.ctypes.data, POINTER(c_double)) if (field_ in out_fields): - self.shared_lib.ocp_nlp_out_get_slice.argtypes = \ - [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p] self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \ self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data) elif field_ in mem_fields: - self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ - [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \ self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data) - return out - - def get(self, stage_, field_): return self.get_slice(stage_, stage_ + 1, field_)[0] @@ -1255,13 +1239,10 @@ class AcadosOcpSolver: """ # cast value_ to avoid conversion issues field = field_.encode('utf-8') - dim = np.product(value_.shape[1:]) - - dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) - dims_data = cast(dims.ctypes.data, POINTER(c_int)) - - self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \ - self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) + if len(value_.shape) > 2: + dim = value_.shape[1]*value_.shape[2] + else: + dim = value_.shape[1] self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, \ self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field, @@ -1280,18 +1261,12 @@ class AcadosOcpSolver: :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi'] :param value: of appropriate size """ - # cast value_ to avoid conversion issues - if isinstance(value_, (float, int)): - value_ = np.array([value_]) field = field_.encode('utf-8') - dim = np.product(value_.shape[1:]) - - dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) - dims_data = cast(dims.ctypes.data, POINTER(c_int)) - - self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ - self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) + if len(value_.shape) > 2: + dim = value_.shape[1]*value_.shape[2] + else: + dim = value_.shape[1] self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \ self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field, diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py index e582f54f5f..04fed92669 100644 --- a/selfdrive/controls/lib/fcw.py +++ b/selfdrive/controls/lib/fcw.py @@ -44,8 +44,7 @@ class FCWChecker(): ttc = min(2 * x_lead / (math.sqrt(delta) + v_rel), max_ttc) return ttc - def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers): - mpc_solution_a = list(mpc_solution[0].a_ego) + def update(self, mpc_solution_a, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers): self.last_min_a = min(mpc_solution_a) self.v_lead_max = max(self.v_lead_max, v_lead) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index c084b34922..6b0290863d 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -11,7 +11,7 @@ from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code") JSON_FILE = "acados_ocp_lat.json" - +X_DIM = 6 def gen_lat_model(): model = AcadosModel() @@ -56,7 +56,6 @@ def gen_lat_mpc_solver(): ocp = AcadosOcp() ocp.model = gen_lat_model() - N = 16 Tf = np.array(T_IDXS)[N] # set dimensions @@ -109,13 +108,13 @@ def gen_lat_mpc_solver(): class LateralMpc(): - def __init__(self, x0=np.zeros(6)): + def __init__(self, x0=np.zeros(X_DIM)): self.solver = AcadosOcpSolver('lat', N, EXPORT_DIR) self.reset(x0) - def reset(self, x0=np.zeros(6)): - self.x_sol = np.zeros((N+1, 4)) - self.u_sol = np.zeros((N)) + def reset(self, x0=np.zeros(X_DIM)): + self.x_sol = np.zeros((N+1, X_DIM)) + self.u_sol = np.zeros((N, 1)) self.yref = np.zeros((N+1, 3)) self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) self.solver.cost_set(N, "yref", self.yref[N][:2]) @@ -124,7 +123,7 @@ class LateralMpc(): # Somehow needed for stable init for i in range(N+1): - self.solver.set(i, 'x', np.zeros(6)) + self.solver.set(i, 'x', np.zeros(X_DIM)) self.solver.constraints_set(0, "lbx", x0) self.solver.constraints_set(0, "ubx", x0) self.solver.solve() @@ -149,8 +148,8 @@ class LateralMpc(): self.solver.cost_set(N, "yref", self.yref[N][:2]) self.solution_status = self.solver.solve() - self.x_sol = self.solver.get_slice(0, N+1, 'x') - self.u_sol = self.solver.get_slice(0, N, 'u') + self.solver.fill_in_slice(0, N+1, 'x', self.x_sol) + self.solver.fill_in_slice(0, N, 'u', self.u_sol) self.cost = self.solver.get_cost() diff --git a/selfdrive/controls/lib/lead_mpc_lib/.gitignore b/selfdrive/controls/lib/lead_mpc_lib/.gitignore index f61a939cda..60533746d8 100644 --- a/selfdrive/controls/lib/lead_mpc_lib/.gitignore +++ b/selfdrive/controls/lib/lead_mpc_lib/.gitignore @@ -1,2 +1,2 @@ -generator -lib_qp/ +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 index 94deb5d6f5..75a4c7fd1a 100644 --- a/selfdrive/controls/lib/lead_mpc_lib/SConscript +++ b/selfdrive/controls/lib/lead_mpc_lib/SConscript @@ -1,48 +1,58 @@ Import('env', 'arch') +gen = "c_generated_code" -cpp_path = [ - "#phonelibs/acado/include", - "#phonelibs/acado/include/acado", - "#phonelibs/qpoases/INCLUDE", - "#phonelibs/qpoases/INCLUDE/EXTRAS", - "#phonelibs/qpoases/SRC/", - "#phonelibs/qpoases", - "lib_mpc_export", +casadi_model = [ + f'{gen}/lead_model/lead_expl_ode_fun.c', + f'{gen}/lead_model/lead_expl_vde_forw.c', ] -generated_c = [ - 'lib_mpc_export/acado_auxiliary_functions.c', - 'lib_mpc_export/acado_qpoases_interface.cpp', - 'lib_mpc_export/acado_integrator.c', - 'lib_mpc_export/acado_solver.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', ] -generated_h = [ - 'lib_mpc_export/acado_common.h', - 'lib_mpc_export/acado_auxiliary_functions.h', - 'lib_mpc_export/acado_qpoases_interface.hpp', +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', +] -interface_dir = Dir('lib_mpc_export') +build_files = [f'{gen}/acados_solver_lead.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0 -SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) +# extra generated files used to trigger a rebuild +generated_files = [ + f'{gen}/Makefile', -if GetOption('mpc_generate'): - generator_cpp = File('generator.cpp') + f'{gen}/main_lead.c', + f'{gen}/acados_solver_lead.h', - acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"), - File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"), - File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")] + f'{gen}/lead_model/lead_expl_vde_adj.c', - generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path, - CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"]) + 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 - cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}" - env.Command(generated_c + generated_h, generator, cmd) +lenv = env.Clone() +lenv.Clean(generated_files, Dir(gen)) +lenv.Command(generated_files, + ["lead_mpc.py"], + f"cd {Dir('.').abspath} && python lead_mpc.py") -mpc_files = ["longitudinal_mpc.c"] + generated_c -env.SharedLibrary('mpc0', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) -env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) +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/lead_mpc.py b/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py new file mode 100644 index 0000000000..bc25b31448 --- /dev/null +++ b/selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python3 +import os +import math +import numpy as np + +from common.realtime import sec_since_boot +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 RW(v_ego, v_l): + TR = 1.8 + G = 9.81 + return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G)) + + +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] + G = 9.81 + TR = 1.8 + desired_dist = (v_ego * TR + - (v_lead - v_ego) * TR + + v_ego*v_ego/(2*G) + - v_lead * v_lead / (2*G)) + dist_err = (desired_dist + 4.0 - (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 + 4.0)) / (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 + 4.0)) / (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, -(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): + 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 = lead.aLeadK + + 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 = 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/.gitignore b/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore index f61a939cda..6f09b1aa0c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore @@ -1,2 +1,2 @@ -generator -lib_qp/ +acados_ocp_long.json +c_generated_code/ diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index 19387d4ae1..125d4a12be 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -1,47 +1,58 @@ Import('env', 'arch') -cpp_path = [ - "#", - "#selfdrive", - "#phonelibs/acado/include", - "#phonelibs/acado/include/acado", - "#phonelibs/qpoases/INCLUDE", - "#phonelibs/qpoases/INCLUDE/EXTRAS", - "#phonelibs/qpoases/SRC/", - "#phonelibs/qpoases", - "lib_mpc_export", +gen = "c_generated_code" + +casadi_model = [ + f'{gen}/long_model/long_expl_ode_fun.c', + f'{gen}/long_model/long_expl_vde_forw.c', +] + +casadi_cost_y = [ + f'{gen}/long_cost/long_cost_y_fun.c', + f'{gen}/long_cost/long_cost_y_fun_jac_ut_xt.c', + f'{gen}/long_cost/long_cost_y_hess.c', ] -generated_c = [ - 'lib_mpc_export/acado_auxiliary_functions.c', - 'lib_mpc_export/acado_qpoases_interface.cpp', - 'lib_mpc_export/acado_integrator.c', - 'lib_mpc_export/acado_solver.c', +casadi_cost_e = [ + f'{gen}/long_cost/long_cost_y_e_fun.c', + f'{gen}/long_cost/long_cost_y_e_fun_jac_ut_xt.c', + f'{gen}/long_cost/long_cost_y_e_hess.c', ] -generated_h = [ - 'lib_mpc_export/acado_common.h', - 'lib_mpc_export/acado_auxiliary_functions.h', - 'lib_mpc_export/acado_qpoases_interface.hpp', +casadi_cost_0 = [ + f'{gen}/long_cost/long_cost_y_0_fun.c', + f'{gen}/long_cost/long_cost_y_0_fun_jac_ut_xt.c', + f'{gen}/long_cost/long_cost_y_0_hess.c', ] -interface_dir = Dir('lib_mpc_export') +build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0 -SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) +# extra generated files used to trigger a rebuild +generated_files = [ + f'{gen}/Makefile', -if GetOption('mpc_generate'): - generator_cpp = File('generator.cpp') + f'{gen}/main_long.c', + f'{gen}/acados_solver_long.h', - acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"), - File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"), - File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")] + f'{gen}/long_model/long_expl_vde_adj.c', - generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path, - CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"]) + f'{gen}/long_model/long_model.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', +] + build_files - cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}" - env.Command(generated_c + generated_h, generator, cmd) +lenv = env.Clone() +lenv.Clean(generated_files, Dir(gen)) +lenv.Command(generated_files, + ["long_mpc.py"], + f"cd {Dir('.').abspath} && python long_mpc.py") -mpc_files = ["longitudinal_mpc.c"] + generated_c -env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) +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_long", + build_files, + LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e']) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py new file mode 100644 index 0000000000..03f71bb32f --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +import os +import numpy as np + +from common.numpy_fast import clip +from common.realtime import sec_since_boot +from selfdrive.swaglog import cloudlog +from selfdrive.controls.lib.drive_helpers import LON_MPC_N as N +from selfdrive.modeld.constants import T_IDXS + +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" + + +def gen_long_model(): + model = AcadosModel() + model.name = 'long' + + # 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) + + # 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_long_mpc_solver(): + ocp = AcadosOcp() + ocp.model = gen_long_model() + + Tf = np.array(T_IDXS)[N] + + # 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, )) + # 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]) + ocp.constraints.x0 = x0 + + l2_penalty = 1.0 + l1_penalty = 0.0 + weights = np.array([0.0, 1e4, 1e4]) + ocp.cost.Zl = l2_penalty * weights + ocp.cost.Zu = l2_penalty * weights + ocp.cost.zl = l1_penalty * weights + ocp.cost.zu = l1_penalty * weights + + 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 + + # set prediction horizon + ocp.solver_options.tf = Tf + ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1] + + 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.ones((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) + self.reset() + + def reset(self): + self.last_cloudlog_t = 0 + self.status = True + self.solution_status = 0 + for i in range(N+1): + self.solver.set(i, 'x', self.x0) + + def set_weights(self): + W = np.diag([0.0, 1.0, 0.0, 50.0]) + 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]) + + 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') + + def set_cur_state(self, v, a): + self.x0[1] = v + self.x0[2] = a + 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) + self.yref[:,0] = v_cruise_clipped * self.T_IDXS # position + self.yref[:,1] = v_cruise_clipped * np.ones(N+1) # speed + 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]) + self.j_solution = list(self.u_sol[:,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(f'Longitudinal model mpc reset, solution status: {self.solution_status}') + self.reset() + + +if __name__ == "__main__": + ocp = gen_long_mpc_solver() + AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cab3e20aa3..30840d2951 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -11,8 +11,8 @@ 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 import LeadMpc -from selfdrive.controls.lib.long_mpc import LongitudinalMpc +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 @@ -64,6 +64,7 @@ class Planner(): self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) + self.j_desired_trajectory = np.zeros(CONTROL_N) def update(self, sm, CP): @@ -97,10 +98,9 @@ class Planner(): accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) # clip limits, cannot init MPC outside of bounds - accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired) - accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired) + 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) @@ -116,7 +116,7 @@ class Planner(): if self.mpcs['lead0'].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'].mpc_solution, cur_time, + self.fcw = self.fcw_checker.update(self.mpcs['lead0'].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, diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index aeaabcf024..e8701a42c0 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -5,13 +5,7 @@ import numpy as np from cereal import log import cereal.messaging as messaging from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.lead_mpc import LeadMpc - - -def RW(v_ego, v_l): - TR = 1.8 - G = 9.81 - return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G)) +from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import RW, LeadMpc class FakePubMaster(): @@ -36,8 +30,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0): # Setup CarState CS = messaging.new_message('carState') - CS.carState.vEgo = v_ego - CS.carState.aEgo = a_ego + CS.carState.vEgo = float(v_ego) + CS.carState.aEgo = float(a_ego) # Setup model packet radarstate = messaging.new_message('radarState') @@ -57,7 +51,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0): mpc.update(CS.carState, radarstate.radarState, 0) # Choose slowest of two solutions - v_ego, a_ego = mpc.mpc_solution.v_ego[5], mpc.mpc_solution.a_ego[5] + v_ego, a_ego = float(mpc.v_solution[5]), float(mpc.a_solution[5]) # Update state x_lead += v_lead * dt @@ -76,7 +70,7 @@ class TestFollowingDistance(unittest.TestCase): simulation_steady_state = run_following_distance_simulation(v_lead) correct_steady_state = RW(v_lead, v_lead) + 4.0 - self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.1) + self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.2) if __name__ == "__main__": diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 624d5c9657..59e6fb6035 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c967d67902bd36e82c8b9e70c5538475b448ac49 \ No newline at end of file +c66c5414f85cd98ad2dd7a83989e05990851da74 \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 770c43f383..ce0ffc4323 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": 26.0, + "selfdrive.controls.plannerd": 33.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": 12.0, + "selfdrive.controls.plannerd": 15.0, "selfdrive.locationd.paramsd": 5.0, "./_dmonitoringmodeld": 10.0, "selfdrive.thermald.thermald": 1.5, @@ -198,8 +198,8 @@ class TestOnroad(unittest.TestCase): ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s] self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}") self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") - result += f"'{s}' execution time: {min(ts)}'" - result += f"'{s}' avg execution time: {np.mean(ts)}" + result += f"'{s}' execution time: {min(ts)}\n" + result += f"'{s}' avg execution time: {np.mean(ts)}\n" print(result) def test_timings(self):