From bab01197aedbb0079aaccb1c2bb8102314ea85e7 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 21 Feb 2022 12:16:42 +0100 Subject: [PATCH 01/20] acados_ocp_solver_pyx.pyx: implement get_stats for timings and ints --- .../acados_template/acados_ocp_solver_pyx.pyx | 68 ++++++++++++++++++- 1 file changed, 67 insertions(+), 1 deletion(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 9bcf5bb46a1913..f7fe2e98fbfae6 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -194,7 +194,73 @@ 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': + # TODO: segfaults. why? dynamics_get works.. + # print(f"acados get_stats(): calling getter with field {field}") + NotImplementedError("TODO!") + 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, min_size) + + elif field_ == 'qp_iter': + NotImplementedError("TODO!") + # 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 cnp.ndarray[cnp.int_t, ndim=1] out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64) + # acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out.data) + 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): + print(f"get_stat_matrix: n, m {n, m}") + out_mat = np.zeros((m, n), order='F', 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): From 00739175b99f9cdc1c8188d1f05fc09a23db516b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 21 Feb 2022 12:23:40 +0100 Subject: [PATCH 02/20] long_mpc: use acados timers --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f9f9c31bb48207..884850b3fe8846 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -215,7 +215,11 @@ def reset(self): 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() @@ -355,9 +359,11 @@ 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]) for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') @@ -370,6 +376,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 From b7581d5335fefadecc2658099ec0cef4cf8086c1 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Mon, 21 Feb 2022 18:15:21 +0100 Subject: [PATCH 03/20] acados_ocp_solver_pyx.pyx: fix dynamics_get --- pyextra/acados_template/acados_ocp_solver_pyx.pyx | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index f7fe2e98fbfae6..2faaffd028dc87 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -234,15 +234,16 @@ cdef class AcadosOcpSolverFast: return self.__get_stat_matrix(field, stat_n, min_size) elif field_ == 'qp_iter': - NotImplementedError("TODO!") - # 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, :] + 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 cnp.ndarray[cnp.int_t, ndim=1] out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64) # acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out.data) @@ -427,7 +428,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) From 5e26f6d57f7e3dae4ecfdad1a34c51a9d3a03866 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 22 Feb 2022 11:11:24 +0100 Subject: [PATCH 04/20] acados_ocp_solver_pyx.pyx: get statistics --- pyextra/acados_template/acados_ocp_solver_pyx.pyx | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 2faaffd028dc87..3d75ec9742b283 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -224,14 +224,11 @@ cdef class AcadosOcpSolverFast: return self.__get_stat_double(field) elif field_ == 'statistics': - # TODO: segfaults. why? dynamics_get works.. - # print(f"acados get_stats(): calling getter with field {field}") - NotImplementedError("TODO!") 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, min_size) + 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") @@ -245,8 +242,6 @@ cdef class AcadosOcpSolverFast: def __get_stat_int(self, field): - # cdef cnp.ndarray[cnp.int_t, ndim=1] out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64) - # acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out.data) cdef int out acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, &out) return out @@ -257,13 +252,11 @@ cdef class AcadosOcpSolverFast: return out def __get_stat_matrix(self, field, n, m): - print(f"get_stat_matrix: n, m {n, m}") - out_mat = np.zeros((m, n), order='F', dtype=np.float64) + 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): """ Returns the cost value of the current solution. From b7288d6d9ea8e32adfb48f1c49b816b25e9a1849 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 22 Feb 2022 11:43:15 +0100 Subject: [PATCH 05/20] use acados_ocp_solver_pyx.pyx from commaai/cython2 branch --- pyextra/acados_template/acados_ocp_solver_pyx.pyx | 7 +++---- selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py | 2 +- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 3d75ec9742b283..b981cdc98f5715 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -51,9 +51,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 +67,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 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 884850b3fe8846..944f66f5213406 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -197,7 +197,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) From 2a8407a451b1dd07ea80a11145cf2edfeebc2383 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 22 Feb 2022 15:03:21 +0100 Subject: [PATCH 06/20] acados_ocp_solver_pyx.pyx: implement store_iterate --- .../acados_template/acados_ocp_solver_pyx.pyx | 30 ++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index b981cdc98f5715..6e7f80f09194a6 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 @@ -177,7 +179,33 @@ 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): From 2c2902f854e98ab6487687a6d3fe5fcbcc801ba5 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 22 Feb 2022 17:39:19 +0100 Subject: [PATCH 07/20] acados_ocp_solver_pyx.pyx: implement get_residuals --- .../acados_template/acados_ocp_solver_pyx.pyx | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 6e7f80f09194a6..f0440d4528c752 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -304,7 +304,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 From 969bfa6dd5dc5b0a87fe7ef5bcfc62767f6b8bd7 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Feb 2022 13:55:45 +0100 Subject: [PATCH 08/20] acados_ocp_solver_pyx.pyx: fix set() for empty fields --- pyextra/acados_template/acados_ocp_solver_pyx.pyx | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index f0440d4528c752..29861802aa1474 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -358,12 +358,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.\ @@ -378,16 +377,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_): From d915ede9128b5a0265489fe51322c85175326141 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Feb 2022 13:56:16 +0100 Subject: [PATCH 09/20] acados_ocp_solver_pyx.pyx: load_iterate --- pyextra/acados_template/acados_ocp_solver_pyx.pyx | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index 29861802aa1474..a9c0d23de43fa6 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -212,7 +212,15 @@ cdef class AcadosOcpSolverFast: """ 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_): From 89cea344157437307c824ba49c9e33d3f51b6364 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 22 Feb 2022 19:48:02 +0100 Subject: [PATCH 10/20] cython acados: add print_statistics --- pyextra/acados_template/acados_ocp_solver_pyx.pyx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index a9c0d23de43fa6..cab033a7ad4b95 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -169,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): From 988b7ed4859386e7075d5642996c0cc91aea63db Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Feb 2022 12:34:48 +0100 Subject: [PATCH 11/20] test_following_distance: fix typo --- selfdrive/controls/tests/test_following_distance.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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) From a51939371814b99ff41d5eb3ab61613dec01832e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Feb 2022 11:18:20 +0100 Subject: [PATCH 12/20] test_longitudinal: unique names for test maneuvers --- selfdrive/test/longitudinal_maneuvers/test_longitudinal.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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, From 2f3ed6ecf2eb4e88e11f7cd28f3387012b77cd36 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 22 Feb 2022 16:59:56 +0100 Subject: [PATCH 13/20] longitudinal MPC: comments for evaluation --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 944f66f5213406..f46e11d3627632 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -365,6 +365,9 @@ def run(self): 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}") + for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') for i in range(N): From c41d91604e9c5d0813edabff2cf588853acf41bd Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Feb 2022 13:26:07 +0100 Subject: [PATCH 14/20] longitudinal MPC: add comments to eval acados residuals --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f46e11d3627632..98d6dafd26533a 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -367,6 +367,9 @@ def run(self): # 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') From a1982548fae97c2d838e355ad8c622ab98eeedfb Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 18 Jan 2022 17:31:59 +0100 Subject: [PATCH 15/20] long_mpc: use qp_solver_cond_N = 1 --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 98d6dafd26533a..e8d86e8b56cd2f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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. From d83e3bcfa392e703c1962884d18ae4e97d30b668 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 1 Feb 2022 15:49:29 +0100 Subject: [PATCH 16/20] long MPC: comments, simplify set_cur_state --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index e8d86e8b56cd2f..d51c86539007fc 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 @@ 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 @@ -236,6 +236,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, @@ -261,14 +262,12 @@ def set_weights_for_xva_policy(self): 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): From 10e79219b93fe3b2119e66cc9a571721053ad434 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Feb 2022 10:57:15 +0100 Subject: [PATCH 17/20] update acados version in build script --- third_party/acados/build.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh index 9216032e98d028..888a067a483569 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 From fada3322b07e68a8b5f97b6c293191b1be22ca94 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Feb 2022 15:09:47 +0100 Subject: [PATCH 18/20] longitudinal mpc: weigh a_change in 1 place only --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d51c86539007fc..e91decb3fd1814 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -34,7 +34,7 @@ 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]) From 7e6072a254791e4106a15ecbf94c16f40d54b459 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Fri, 25 Feb 2022 11:43:21 -0800 Subject: [PATCH 19/20] update ref --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 9982949b20223e..92181648ebfad1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -67c8f283858998b75ac28879e1350a589a968e5d \ No newline at end of file +fada3322b07e68a8b5f97b6c293191b1be22ca94 \ No newline at end of file From f46cb3a12ea09e51a82e7dc6e9ac768275aae4e2 Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Fri, 25 Feb 2022 13:46:54 -0800 Subject: [PATCH 20/20] Update ref --- selfdrive/test/process_replay/ref_commit | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 92181648ebfad1..5619bafff0b57b 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -fada3322b07e68a8b5f97b6c293191b1be22ca94 \ No newline at end of file +7e6072a254791e4106a15ecbf94c16f40d54b459 \ No newline at end of file