diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 9bcf5bb46a1913..cab033a7ad4b95 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -44,6 +44,8 @@ cimport acados_solver cimport numpy as cnp import os +import json +from datetime import datetime import numpy as np @@ -51,9 +53,8 @@ cdef class AcadosOcpSolverFast: """ Class to interact with the acados ocp solver C object. - :param acados_ocp: type AcadosOcp - description of the OCP for acados - :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json - :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs + :param model_name: + :param N: """ cdef acados_solver.nlp_solver_capsule *capsule @@ -68,7 +69,7 @@ cdef class AcadosOcpSolverFast: cdef int N cdef bint solver_created - def __cinit__(self, str model_name, int N, str code_export_dir): + def __cinit__(self, str model_name, int N): self.model_name = model_name self.N = N @@ -168,7 +169,7 @@ cdef class AcadosOcpSolverFast: - qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution - qp_res_comp: residual wrt complementarity conditions of the last QP solution """ - raise NotImplementedError() + acados_solver.acados_print_stats(self.capsule) def store_iterate(self, filename='', overwrite=False): @@ -178,14 +179,48 @@ cdef class AcadosOcpSolverFast: :param filename: if not set, use model_name + timestamp + '.json' :param overwrite: if false and filename exists add timestamp to filename """ - raise NotImplementedError() + if filename == '': + filename += self.model_name + '_' + 'iterate' + '.json' + + if not overwrite: + # append timestamp + if os.path.isfile(filename): + filename = filename[:-5] + filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json' + + # get iterate: + solution = dict() + + for i in range(self.N+1): + solution['x_'+str(i)] = self.get(i,'x') + solution['u_'+str(i)] = self.get(i,'u') + solution['z_'+str(i)] = self.get(i,'z') + solution['lam_'+str(i)] = self.get(i,'lam') + solution['t_'+str(i)] = self.get(i, 't') + solution['sl_'+str(i)] = self.get(i, 'sl') + solution['su_'+str(i)] = self.get(i, 'su') + for i in range(self.N): + solution['pi_'+str(i)] = self.get(i,'pi') + + # save + with open(filename, 'w') as f: + json.dump(solution, f, default=lambda x: x.tolist(), indent=4, sort_keys=True) + print("stored current iterate in ", os.path.join(os.getcwd(), filename)) def load_iterate(self, filename): """ Loads the iterate stored in json file with filename into the ocp solver. """ - raise NotImplementedError() + if not os.path.isfile(filename): + raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename)) + + with open(filename, 'r') as f: + solution = json.load(f) + + for key in solution.keys(): + (field, stage) = key.split('_') + self.set(int(stage), field, np.array(solution[key])) def get_stats(self, field_): @@ -194,7 +229,67 @@ cdef class AcadosOcpSolverFast: :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter'] """ - raise NotImplementedError() + fields = ['time_tot', # total cpu time previous call + 'time_lin', # cpu time for linearization + 'time_sim', # cpu time for integrator + 'time_sim_ad', # cpu time for integrator contribution of external function calls + 'time_sim_la', # cpu time for integrator contribution of linear algebra + 'time_qp', # cpu time qp solution + 'time_qp_solver_call', # cpu time inside qp solver (without converting the QP) + 'time_qp_xcond', + 'time_glob', # cpu time globalization + 'time_reg', # cpu time regularization + 'sqp_iter', # number of SQP iterations + 'qp_iter', # vector of QP iterations for last SQP call + 'statistics', # table with info about last iteration + 'stat_m', + 'stat_n',] + + field = field_ + field = field.encode('utf-8') + + if (field_ not in fields): + raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\ + \n Possible values are {}. Exiting.'.format(fields, fields)) + + if field_ in ['sqp_iter', 'stat_m', 'stat_n']: + return self.__get_stat_int(field) + + elif field_.startswith('time'): + return self.__get_stat_double(field) + + elif field_ == 'statistics': + sqp_iter = self.get_stats("sqp_iter") + stat_m = self.get_stats("stat_m") + stat_n = self.get_stats("stat_n") + min_size = min([stat_m, sqp_iter+1]) + return self.__get_stat_matrix(field, stat_n+1, min_size) + + elif field_ == 'qp_iter': + NotImplementedError("TODO! Cython not aware if SQP or SQP_RTI") + full_stats = self.get_stats('statistics') + if self.acados_ocp.solver_options.nlp_solver_type == 'SQP': + out = full_stats[6, :] + elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI': + out = full_stats[2, :] + else: + NotImplementedError("TODO!") + + + def __get_stat_int(self, field): + cdef int out + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &out) + return out + + def __get_stat_double(self, field): + cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((1,)) + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out.data) + return out + + def __get_stat_matrix(self, field, n, m): + cdef cnp.ndarray[cnp.float64_t, ndim=2] out_mat = np.ascontiguousarray(np.zeros((n, m)), dtype=np.float64) + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_mat.data) + return out_mat def get_cost(self): @@ -217,7 +312,32 @@ cdef class AcadosOcpSolverFast: """ Returns an array of the form [res_stat, res_eq, res_ineq, res_comp]. """ - raise NotImplementedError() + # TODO: check if RTI, only eval then + # if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI': + # compute residuals if RTI + acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out) + + # create output array + cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64)) + cdef double double_value + + field = "res_stat".encode('utf-8') + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &double_value) + out[0] = double_value + + field = "res_eq".encode('utf-8') + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &double_value) + out[1] = double_value + + field = "res_ineq".encode('utf-8') + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &double_value) + out[2] = double_value + + field = "res_comp".encode('utf-8') + acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &double_value) + out[3] = double_value + + return out # Note: this function should not be used anymore, better use cost_set, constraints_set @@ -246,12 +366,11 @@ cdef class AcadosOcpSolverFast: field = field_.encode('utf-8') - cdef double[::1] value + cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64) # treat parameters separately if field_ == 'p': - value = np.ascontiguousarray(value_, dtype=np.double) - assert acados_solver.acados_update_params(self.capsule, stage, &value[0], value.shape[0]) == 0 + assert acados_solver.acados_update_params(self.capsule, stage, value.data, value.shape[0]) == 0 else: if field_ not in constraints_fields + cost_fields + out_fields: raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ @@ -266,16 +385,15 @@ cdef class AcadosOcpSolverFast: msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0]) raise Exception(msg) - value = np.ascontiguousarray(value_, dtype=np.double) if field_ in constraints_fields: acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, - self.nlp_dims, self.nlp_in, stage, field, &value[0]) + self.nlp_dims, self.nlp_in, stage, field, value.data) elif field_ in cost_fields: acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, - self.nlp_dims, self.nlp_in, stage, field, &value[0]) + self.nlp_dims, self.nlp_in, stage, field, value.data) elif field_ in out_fields: acados_solver_common.ocp_nlp_out_set(self.nlp_config, - self.nlp_dims, self.nlp_out, stage, field, &value[0]) + self.nlp_dims, self.nlp_out, stage, field, value.data) def cost_set(self, int stage, str field_, value_): @@ -361,7 +479,7 @@ cdef class AcadosOcpSolverFast: acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0]) # create output data - out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64) + cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F') # call getter acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, out.data) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index e4a73bf97d7d73..1b4d5283a8200c 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -117,7 +117,7 @@ def gen_lat_mpc_solver(): class LateralMpc(): def __init__(self, x0=np.zeros(X_DIM)): - self.solver = AcadosOcpSolverFast('lat', N, EXPORT_DIR) + self.solver = AcadosOcpSolverFast('lat', N) self.reset(x0) def reset(self, x0=np.zeros(X_DIM)): diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f9f9c31bb48207..bdc46c73574fca 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -24,10 +24,10 @@ X_DIM = 3 U_DIM = 1 -PARAM_DIM= 4 +PARAM_DIM = 4 COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 -CONSTR_DIM = 4 +CONSTR_DIM = 5 X_EGO_OBSTACLE_COST = 3. X_EGO_COST = 0. @@ -147,7 +147,8 @@ def gen_long_mpc_solver(): constraints = vertcat(v_ego, (a_ego - a_min), (a_max - a_ego), - ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.)) + ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.), + a_ego) ocp.model.con_h_expr = constraints ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM)) @@ -155,16 +156,23 @@ def gen_long_mpc_solver(): ocp.constraints.x0 = x0 ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0]) - # We put all constraint cost weights to 0 and only set them at runtime + # We initialize all L2 slack penalties initialized with 0 and only set them at runtime cost_weights = np.zeros(CONSTR_DIM) - ocp.cost.zl = cost_weights ocp.cost.Zl = cost_weights ocp.cost.Zu = cost_weights - ocp.cost.zu = cost_weights ocp.constraints.lh = np.zeros(CONSTR_DIM) ocp.constraints.lh_e = np.zeros(CONSTR_DIM) - ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM) + + # L1 penalty on positive a_ego to avoid creeping, all other L1 penalties are 0 + ocp.cost.zl = cost_weights + uh = 1e4*np.ones(CONSTR_DIM) + uh[-1] = 0.0 + zu = np.zeros(CONSTR_DIM) + zu[-1] = 1e-0 + ocp.cost.zu = zu + + ocp.constraints.uh = uh ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM) ocp.constraints.idxsh = np.arange(CONSTR_DIM) @@ -176,11 +184,12 @@ def gen_long_mpc_solver(): 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_cond_N = N//4 + ocp.solver_options.qp_solver_cond_N = 1 # More iterations take too much time and less lead to inaccurate convergence in # some situations. Ideally we would run just 1 iteration to ensure fixed runtime. ocp.solver_options.qp_solver_iter_max = 10 + ocp.solver_options.qp_tol = 1e-3 # set prediction horizon ocp.solver_options.tf = Tf @@ -197,7 +206,7 @@ def __init__(self, e2e=False): self.source = SOURCES[2] def reset(self): - self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR) + self.solver = AcadosOcpSolverFast('long', N) self.v_solution = np.zeros(N+1) self.a_solution = np.zeros(N+1) self.prev_a = np.array(self.a_solution) @@ -211,11 +220,17 @@ def reset(self): self.params = np.zeros((N+1, PARAM_DIM)) for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) + for i in range(N): + self.solver.set(i, 'u', self.u_sol[i,:]) self.last_cloudlog_t = 0 self.status = False self.crash_cnt = 0.0 self.solution_status = 0 + # timers self.solve_time = 0.0 + self.time_qp_solution = 0.0 + self.time_linearization = 0.0 + self.time_integrator = 0.0 self.x0 = np.zeros(X_DIM) self.set_weights() @@ -232,6 +247,7 @@ 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])) for i in range(N): + # 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]) self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, @@ -239,7 +255,7 @@ def set_weights_for_lead_policy(self, prev_accel_constraint=True): 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([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST, 0.0]) for i in range(N): self.solver.cost_set(i, 'Zl', Zl) @@ -252,19 +268,17 @@ def set_weights_for_xva_policy(self): 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]) + Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0, 0.0]) for i in range(N): self.solver.cost_set(i, 'Zl', Zl) def set_cur_state(self, v, a): - if abs(self.x0[1] - v) > 2.: - self.x0[1] = v - self.x0[2] = 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 for i in range(0, N+1): self.solver.set(i, 'x', self.x0) - else: - self.x0[1] = v - self.x0[2] = a @staticmethod def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): @@ -355,9 +369,17 @@ def run(self): self.solver.constraints_set(0, "lbx", self.x0) self.solver.constraints_set(0, "ubx", self.x0) - t = sec_since_boot() self.solution_status = self.solver.solve() - self.solve_time = sec_since_boot() - t + self.solve_time = float(self.solver.get_stats('time_tot')[0]) + self.time_qp_solution = float(self.solver.get_stats('time_qp')[0]) + 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') @@ -370,6 +392,7 @@ def run(self): self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution) + t = sec_since_boot() if self.solution_status != 0: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index a0110a4dab9609..ebe47767393a4a 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -21,7 +21,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0): class TestFollowingDistance(unittest.TestCase): - def test_following_distanc(self): + def test_following_distance(self): for speed in np.arange(0, 40, 5): print(f'Testing {speed} m/s') v_lead = float(speed) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 60aaeb724d5f98..698877dd3ad29e 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -9,7 +9,7 @@ # TODO: make new FCW tests maneuvers = [ Maneuver( - 'approach stopped car at 20m/s', + 'approach stopped car at 20m/s, initial distance: 120m', duration=20., initial_speed=25., lead_relevancy=True, @@ -18,7 +18,7 @@ breakpoints=[0., 1.], ), Maneuver( - 'approach stopped car at 20m/s', + 'approach stopped car at 20m/s, initial distance 90m', duration=20., initial_speed=20., lead_relevancy=True, @@ -65,7 +65,7 @@ breakpoints=[2., 2.01, 8.8], ), Maneuver( - "approach stopped car at 20m/s", + "approach stopped car at 20m/s, with prob_lead_values", duration=30., initial_speed=20., lead_relevancy=True,