diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 9bcf5bb46..cab033a7a 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 e4a73bf97..1b4d5283a 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 f9f9c31bb..e91decb3f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -24,7 +24,7 @@ SOURCES = ['lead0', 'lead1', 'cruise'] 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 @@ -34,7 +34,7 @@ X_EGO_COST = 0. V_EGO_COST = 0. A_EGO_COST = 0. J_EGO_COST = 5.0 -A_CHANGE_COST = .5 +A_CHANGE_COST = 200. DANGER_ZONE_COST = 100. CRASH_DISTANCE = .5 LIMIT_COST = 1e6 @@ -136,7 +136,7 @@ def gen_long_mpc_solver(): x_ego, v_ego, a_ego, - 20*(a_ego - prev_a), + a_ego - prev_a, j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) @@ -176,7 +176,7 @@ 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. @@ -197,7 +197,7 @@ class LongitudinalMpc: 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) @@ -215,7 +215,11 @@ class LongitudinalMpc: 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 +236,7 @@ class LongitudinalMpc: 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, @@ -257,14 +262,12 @@ class LongitudinalMpc: 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 +358,17 @@ class LongitudinalMpc: 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 +381,7 @@ class LongitudinalMpc: 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 a0110a4da..ebe477673 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 60aaeb724..698877dd3 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -9,7 +9,7 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver # 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 @@ maneuvers = [ 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 @@ maneuvers = [ 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, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 9982949b2..5619bafff 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -67c8f283858998b75ac28879e1350a589a968e5d \ No newline at end of file +7e6072a254791e4106a15ecbf94c16f40d54b459 \ No newline at end of file diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh index 9216032e9..888a067a4 100755 --- a/third_party/acados/build.sh +++ b/third_party/acados/build.sh @@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then fi cd acados_repo git fetch -git checkout 79e9e3e76f2751198858adf382c97837833ad31f +git checkout 92b85c61f7358a1b08b7cd30aeb9d32ad15942e8 git submodule update --recursive --init # build