From 1801baba68c752ee424338cf56bd36ba3ccf1565 Mon Sep 17 00:00:00 2001 From: Jonathan Frey <31900285+FreyJo@users.noreply.github.com> Date: Thu, 3 Mar 2022 05:53:21 +0100 Subject: [PATCH] remove trivial terminal constraint and associated CasADi generated functions (#23898) * remove trivial terminal constraint and associated CasADi generated functions * Update refs Co-authored-by: Harald Schafer --- selfdrive/controls/lib/longitudinal_mpc_lib/SConscript | 3 --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 3 --- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 1 insertion(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index 4c43985d1f..b19401339c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -28,8 +28,6 @@ casadi_cost_0 = [ casadi_constraints = [ f'{gen}/long_constraints/long_constr_h_fun.c', f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c', - f'{gen}/long_constraints/long_constr_h_e_fun.c', - f'{gen}/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c', ] build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e + \ @@ -47,7 +45,6 @@ generated_files = [ f'{gen}/long_model/long_model.h', f'{gen}/long_constraints/long_h_constraint.h', - f'{gen}/long_constraints/long_h_e_constraint.h', f'{gen}/long_cost/long_cost_y_fun.h', f'{gen}/long_cost/long_cost_y_e_fun.h', f'{gen}/long_cost/long_cost_y_0_fun.h', diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index e91decb3fd..122c06ada9 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -149,7 +149,6 @@ def gen_long_mpc_solver(): (a_max - a_ego), ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.)) ocp.model.con_h_expr = constraints - ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM)) x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 @@ -163,9 +162,7 @@ def gen_long_mpc_solver(): 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) - ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM) ocp.constraints.idxsh = np.arange(CONSTR_DIM) # The HPIPM solver can give decent solutions even when it is stopped early diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 26fe9a59c2..e5a435b6d5 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -072ee2ba6bca1ea5943fef27b0cc764e40275568 \ No newline at end of file +5cd22804273530aaf69ebb9f5039e8d08ca44ef8 \ No newline at end of file