diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index fa249765bc..ee68cc29cf 100644 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -28,6 +28,8 @@ 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 + \ diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json index 5ca5a14edf..6f17c5dbe5 100644 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json @@ -32,7 +32,12 @@ 2, 3 ], - "idxsh_e": [], + "idxsh_e": [ + 0, + 1, + 2, + 3 + ], "idxsphi": [], "idxsphi_e": [], "lbu": [], @@ -51,7 +56,12 @@ 0.0, 0.0 ], - "lh_e": [], + "lh_e": [ + 0.0, + 0.0, + 0.0, + 0.0 + ], "lphi": [], "lphi_e": [], "lsbu": [], @@ -65,7 +75,12 @@ 0.0, 0.0 ], - "lsh_e": [], + "lsh_e": [ + 0.0, + 0.0, + 0.0, + 0.0 + ], "lsphi": [], "lsphi_e": [], "ubu": [], @@ -84,7 +99,12 @@ 10000.0, 10000.0 ], - "uh_e": [], + "uh_e": [ + 10000.0, + 10000.0, + 10000.0, + 10000.0 + ], "uphi": [], "uphi_e": [], "usbu": [], @@ -98,7 +118,12 @@ 0.0, 0.0 ], - "ush_e": [], + "ush_e": [ + 0.0, + 0.0, + 0.0, + 0.0 + ], "usphi": [], "usphi_e": [] }, @@ -253,14 +278,24 @@ 0.0, 0.0 ], - "Zl_e": [], + "Zl_e": [ + 0.0, + 0.0, + 0.0, + 0.0 + ], "Zu": [ 0.0, 0.0, 0.0, 0.0 ], - "Zu_e": [], + "Zu_e": [ + 0.0, + 0.0, + 0.0, + 0.0 + ], "cost_ext_fun_type": "casadi", "cost_ext_fun_type_0": "casadi", "cost_ext_fun_type_e": "casadi", @@ -296,14 +331,24 @@ 0.0, 0.0 ], - "zl_e": [], + "zl_e": [ + 0.0, + 0.0, + 0.0, + 0.0 + ], "zu": [ 0.0, 0.0, 0.0, 0.0 ], - "zu_e": [] + "zu_e": [ + 0.0, + 0.0, + 0.0, + 0.0 + ] }, "cython_include_dirs": [ "/work/.venv-linux-arm64/lib/python3.12/site-packages/numpy/_core/include", @@ -319,21 +364,21 @@ "ng": 0, "ng_e": 0, "nh": 4, - "nh_e": 0, + "nh_e": 4, "np": 6, "nphi": 0, "nphi_e": 0, "nr": 0, "nr_e": 0, "ns": 4, - "ns_e": 0, + "ns_e": 4, "nsbu": 0, "nsbx": 0, "nsbx_e": 0, "nsg": 0, "nsg_e": 0, "nsh": 4, - "nsh_e": 0, + "nsh_e": 4, "nsphi": 0, "nsphi_e": 0, "nu": 1, @@ -346,7 +391,7 @@ "json_file": "/work/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json", "model": { "con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa", - "con_h_expr_e": null, + "con_h_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa", "con_phi_expr": null, "con_phi_expr_e": null, "con_r_expr": null, diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile index 1e33e89c7e..4e5d2445eb 100644 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile @@ -45,6 +45,8 @@ MODEL_OBJ := $(MODEL_SRC:.c=.o) OCP_SRC= OCP_SRC+= long_constraints/long_constr_h_fun_jac_uxt_zt.c OCP_SRC+= long_constraints/long_constr_h_fun.c +OCP_SRC+= long_constraints/long_constr_h_e_fun_jac_uxt_zt.c +OCP_SRC+= long_constraints/long_constr_h_e_fun.c OCP_SRC+= long_cost/long_cost_y_0_fun.c OCP_SRC+= long_cost/long_cost_y_0_fun_jac_ut_xt.c OCP_SRC+= long_cost/long_cost_y_0_hess.c diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so index 8f17377d2d..9175943d56 100755 Binary files a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so and b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so differ diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.c b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.c index bdeec91e94..9e83d9c4bc 100644 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.c +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.c @@ -313,6 +313,8 @@ void long_acados_create_3_create_and_set_functions(long_solver_capsule* capsule) } + MAP_CASADI_FNC(nl_constr_h_e_fun_jac, long_constr_h_e_fun_jac_uxt_zt); + MAP_CASADI_FNC(nl_constr_h_e_fun, long_constr_h_e_fun); // explicit ode @@ -487,6 +489,27 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N, free(zlumem); + // slacks terminal + double* zluemem = calloc(4*NSN, sizeof(double)); + double* Zl_e = zluemem+NSN*0; + double* Zu_e = zluemem+NSN*1; + double* zl_e = zluemem+NSN*2; + double* zu_e = zluemem+NSN*3; + + // change only the non-zero elements: + + + + + + + + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zl", Zl_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zu", Zu_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zl", zl_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zu", zu_e); + free(zluemem); /**** Constraints ****/ @@ -583,6 +606,23 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N, + // set up soft bounds for nonlinear constraints + int* idxsh_e = malloc(NSHN * sizeof(int)); + + idxsh_e[0] = 0; + idxsh_e[1] = 1; + idxsh_e[2] = 2; + idxsh_e[3] = 3; + double* lush_e = calloc(2*NSHN, sizeof(double)); + double* lsh_e = lush_e; + double* ush_e = lush_e + NSHN; + + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e); + free(idxsh_e); + free(lush_e); @@ -591,6 +631,24 @@ void long_acados_create_5_set_nlp_in(long_solver_capsule* capsule, const int N, + // set up nonlinear constraints for last stage + double* luh_e = calloc(2*NHN, sizeof(double)); + double* lh_e = luh_e; + double* uh_e = luh_e + NHN; + + + + uh_e[0] = 10000.0; + uh_e[1] = 10000.0; + uh_e[2] = 10000.0; + uh_e[3] = 10000.0; + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &capsule->nl_constr_h_e_fun_jac); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun", &capsule->nl_constr_h_e_fun); + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); + free(luh_e); } @@ -905,6 +963,9 @@ int long_acados_update_params(long_solver_capsule* capsule, int stage, double *p capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p); // constraints + capsule->nl_constr_h_e_fun_jac.set_param(&capsule->nl_constr_h_e_fun_jac, p); + capsule->nl_constr_h_e_fun.set_param(&capsule->nl_constr_h_e_fun, p); + } return solver_status; @@ -965,6 +1026,9 @@ int long_acados_update_params_sparse(long_solver_capsule * capsule, int stage, i capsule->cost_y_e_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_e_fun_jac_ut_xt, n_update, idx, p); capsule->cost_y_e_hess.set_param_sparse(&capsule->cost_y_e_hess, n_update, idx, p); // constraints + + capsule->nl_constr_h_e_fun_jac.set_param_sparse(&capsule->nl_constr_h_e_fun_jac, n_update, idx, p); + capsule->nl_constr_h_e_fun.set_param_sparse(&capsule->nl_constr_h_e_fun, n_update, idx, p); } @@ -1030,6 +1094,8 @@ int long_acados_free(long_solver_capsule* capsule) } free(capsule->nl_constr_h_fun_jac); free(capsule->nl_constr_h_fun); + external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac); + external_function_param_casadi_free(&capsule->nl_constr_h_e_fun); return 0; } diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h index 351d6ac8cc..372a8d0901 100644 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h @@ -48,12 +48,12 @@ #define LONG_NSH 4 #define LONG_NSG 0 #define LONG_NSPHI 0 -#define LONG_NSHN 0 +#define LONG_NSHN 4 #define LONG_NSGN 0 #define LONG_NSPHIN 0 #define LONG_NSBXN 0 #define LONG_NS 4 -#define LONG_NSN 0 +#define LONG_NSN 4 #define LONG_NG 0 #define LONG_NBXN 0 #define LONG_NGN 0 @@ -63,7 +63,7 @@ #define LONG_N 12 #define LONG_NH 4 #define LONG_NPHI 0 -#define LONG_NHN 0 +#define LONG_NHN 4 #define LONG_NPHIN 0 #define LONG_NR 0 @@ -122,6 +122,8 @@ typedef struct long_solver_capsule + external_function_param_casadi nl_constr_h_e_fun_jac; + external_function_param_casadi nl_constr_h_e_fun; } long_solver_capsule; diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so index 4ce69df785..a8be365c2d 100755 Binary files a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so and b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so differ diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_e_fun.c b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_e_fun.c new file mode 100644 index 0000000000..2098d64bd0 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_e_fun.c @@ -0,0 +1,185 @@ +/* This file was automatically generated by CasADi 3.6.7. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) long_constr_h_e_fun_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2}; +static const casadi_int casadi_s1[3] = {0, 0, 0}; +static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5}; +static const casadi_int casadi_s3[8] = {4, 1, 0, 4, 0, 1, 2, 3}; + +/* long_constr_h_e_fun:(i0[3],i1[],i2[],i3[6])->(o0[4]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a2, a3, a4; + a0=arg[0]? arg[0][1] : 0; + if (res[0]!=0) res[0][0]=a0; + a1=arg[0]? arg[0][2] : 0; + a2=arg[3]? arg[3][0] : 0; + a2=(a1-a2); + if (res[0]!=0) res[0][1]=a2; + a2=arg[3]? arg[3][1] : 0; + a2=(a2-a1); + if (res[0]!=0) res[0][2]=a2; + a2=arg[3]? arg[3][2] : 0; + a1=arg[0]? arg[0][0] : 0; + a2=(a2-a1); + a1=arg[3]? arg[3][5] : 0; + a3=casadi_sq(a0); + a4=5.; + a3=(a3/a4); + a4=arg[3]? arg[3][4] : 0; + a4=(a4*a0); + a3=(a3+a4); + a4=6.; + a3=(a3+a4); + a1=(a1*a3); + a2=(a2-a1); + a1=10.; + a0=(a0+a1); + a2=(a2/a0); + if (res[0]!=0) res[0][3]=a2; + return 0; +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void long_constr_h_e_fun_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void long_constr_h_e_fun_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void long_constr_h_e_fun_incref(void) { +} + +CASADI_SYMBOL_EXPORT void long_constr_h_e_fun_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int long_constr_h_e_fun_n_in(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_int long_constr_h_e_fun_n_out(void) { return 1;} + +CASADI_SYMBOL_EXPORT casadi_real long_constr_h_e_fun_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* long_constr_h_e_fun_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* long_constr_h_e_fun_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_e_fun_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s1; + case 3: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_e_fun_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s3; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4; + if (sz_res) *sz_res = 1; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*); + if (sz_res) *sz_res = 1*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c new file mode 100644 index 0000000000..2375c34329 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c @@ -0,0 +1,211 @@ +/* This file was automatically generated by CasADi 3.6.7. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) long_constr_h_e_fun_jac_uxt_zt_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2}; +static const casadi_int casadi_s1[3] = {0, 0, 0}; +static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5}; +static const casadi_int casadi_s3[8] = {4, 1, 0, 4, 0, 1, 2, 3}; +static const casadi_int casadi_s4[12] = {3, 4, 0, 1, 2, 3, 5, 1, 2, 2, 0, 1}; +static const casadi_int casadi_s5[3] = {4, 0, 0}; + +/* long_constr_h_e_fun_jac_uxt_zt:(i0[3],i1[],i2[],i3[6])->(o0[4],o1[3x4,5nz],o2[4x0]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a2, a3, a4, a5; + a0=arg[0]? arg[0][1] : 0; + if (res[0]!=0) res[0][0]=a0; + a1=arg[0]? arg[0][2] : 0; + a2=arg[3]? arg[3][0] : 0; + a2=(a1-a2); + if (res[0]!=0) res[0][1]=a2; + a2=arg[3]? arg[3][1] : 0; + a2=(a2-a1); + if (res[0]!=0) res[0][2]=a2; + a2=arg[3]? arg[3][2] : 0; + a1=arg[0]? arg[0][0] : 0; + a2=(a2-a1); + a1=arg[3]? arg[3][5] : 0; + a3=casadi_sq(a0); + a4=5.; + a3=(a3/a4); + a4=arg[3]? arg[3][4] : 0; + a5=(a4*a0); + a3=(a3+a5); + a5=6.; + a3=(a3+a5); + a3=(a1*a3); + a2=(a2-a3); + a3=10.; + a3=(a0+a3); + a2=(a2/a3); + if (res[0]!=0) res[0][3]=a2; + a5=1.; + if (res[1]!=0) res[1][0]=a5; + if (res[1]!=0) res[1][1]=a5; + a5=-1.; + if (res[1]!=0) res[1][2]=a5; + a5=(1./a3); + a5=(-a5); + if (res[1]!=0) res[1][3]=a5; + a5=2.0000000000000001e-01; + a0=(a0+a0); + a5=(a5*a0); + a5=(a5+a4); + a1=(a1*a5); + a1=(a1/a3); + a2=(a2/a3); + a1=(a1+a2); + a1=(-a1); + if (res[1]!=0) res[1][4]=a1; + return 0; +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_jac_uxt_zt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_jac_uxt_zt_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_jac_uxt_zt_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void long_constr_h_e_fun_jac_uxt_zt_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_jac_uxt_zt_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void long_constr_h_e_fun_jac_uxt_zt_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void long_constr_h_e_fun_jac_uxt_zt_incref(void) { +} + +CASADI_SYMBOL_EXPORT void long_constr_h_e_fun_jac_uxt_zt_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int long_constr_h_e_fun_jac_uxt_zt_n_in(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_int long_constr_h_e_fun_jac_uxt_zt_n_out(void) { return 3;} + +CASADI_SYMBOL_EXPORT casadi_real long_constr_h_e_fun_jac_uxt_zt_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* long_constr_h_e_fun_jac_uxt_zt_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* long_constr_h_e_fun_jac_uxt_zt_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_e_fun_jac_uxt_zt_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s1; + case 3: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_e_fun_jac_uxt_zt_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s3; + case 1: return casadi_s4; + case 2: return casadi_s5; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_jac_uxt_zt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4; + if (sz_res) *sz_res = 3; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int long_constr_h_e_fun_jac_uxt_zt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*); + if (sz_res) *sz_res = 3*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constraints.h b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constraints.h index d0abd78bd1..da0429a243 100644 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constraints.h +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constraints.h @@ -58,6 +58,22 @@ int long_constr_h_fun_n_out(void); +int long_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int long_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *long_constr_h_e_fun_jac_uxt_zt_sparsity_in(int); +const int *long_constr_h_e_fun_jac_uxt_zt_sparsity_out(int); +int long_constr_h_e_fun_jac_uxt_zt_n_in(void); +int long_constr_h_e_fun_jac_uxt_zt_n_out(void); + +int long_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int long_constr_h_e_fun_work(int *, int *, int *, int *); +const int *long_constr_h_e_fun_sparsity_in(int); +const int *long_constr_h_e_fun_sparsity_out(int); +int long_constr_h_e_fun_n_in(void); +int long_constr_h_e_fun_n_out(void); + + + #ifdef __cplusplus } /* extern "C" */ diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index c001e79956..3bc500c88d 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -172,6 +172,7 @@ def gen_long_ocp(): (a_max - a_ego), ((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.)) ocp.model.con_h_expr = constraints + ocp.model.con_h_expr_e = constraints x0 = np.zeros(X_DIM) ocp.constraints.x0 = x0 @@ -184,10 +185,17 @@ def gen_long_ocp(): ocp.cost.Zl = cost_weights ocp.cost.Zu = cost_weights ocp.cost.zu = cost_weights + ocp.cost.zl_e = cost_weights + ocp.cost.Zl_e = cost_weights + ocp.cost.Zu_e = cost_weights + ocp.cost.zu_e = cost_weights ocp.constraints.lh = np.zeros(CONSTR_DIM) ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM) ocp.constraints.idxsh = np.arange(CONSTR_DIM) + ocp.constraints.lh_e = np.zeros(CONSTR_DIM) + ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM) + ocp.constraints.idxsh_e = np.arange(CONSTR_DIM) # The HPIPM solver can give decent solutions even when it is stopped early # Which is critical for our purpose where compute time is strictly bounded @@ -264,7 +272,7 @@ class LongitudinalMpc: # Set L2 slack cost on lower bound constraints Zl = np.array(constraint_cost_weights) - for i in range(N): + for i in range(N+1): self.solver.cost_set(i, 'Zl', Zl) self._cached_cost_weights = key diff --git a/openpilot/starpilot/longitudinal/tests/test_mpc_cost_cache.py b/openpilot/starpilot/longitudinal/tests/test_mpc_cost_cache.py index 35517c540a..f0719d61a8 100644 --- a/openpilot/starpilot/longitudinal/tests/test_mpc_cost_cache.py +++ b/openpilot/starpilot/longitudinal/tests/test_mpc_cost_cache.py @@ -15,7 +15,7 @@ from openpilot.starpilot.longitudinal.profile_runtime import ProfileTuning from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import messages -WRITES_PER_UPDATE = 2 * long_mpc.N + 1 +WRITES_PER_UPDATE = 2 * (long_mpc.N + 1) class SolverRecorder: diff --git a/openpilot/starpilot/longitudinal/tests/test_mpc_terminal_constraints.py b/openpilot/starpilot/longitudinal/tests/test_mpc_terminal_constraints.py new file mode 100644 index 0000000000..89480641ce --- /dev/null +++ b/openpilot/starpilot/longitudinal/tests/test_mpc_terminal_constraints.py @@ -0,0 +1,80 @@ +from unittest.mock import patch + +import numpy as np +import pytest + +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR +from openpilot.cereal import log +from openpilot.selfdrive.controls.lib import longitudinal_planner +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib import long_mpc +from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import messages +from openpilot.starpilot.longitudinal.upstream import mpc as stock_mpc, planner as stock_planner + + +PERSONALITIES = (log.LongitudinalPersonality.relaxed, log.LongitudinalPersonality.standard, + log.LongitudinalPersonality.aggressive) +REVERSE_PLAN = np.array([ + [0., 20.65, 1.05], [1.436, 20.72, 1.05], [5.776, 20.94, 1.05], [13.108, 21.29, .95], + [23.554, 21.66, .59], [37.173, 21.86, .034], [53.812, 21.62, -.67], [72.856, 20.35, -2.13], + [92.651, 17.42, -3.5], [110.775, 13.29, -3.5], [125.258, 8.67, -3.5], [134.177, 3.56, -3.5], + [127.994, -15.95, -20.93], +]) + + +def seed_reverse_plan(mpc): + mpc.set_cur_state(20.65, 1.05) + for stage, state in enumerate(REVERSE_PLAN): + mpc.solver.set(stage, 'x', state) + mpc.a_prev = np.interp(long_mpc.T_IDXS + mpc.dt, long_mpc.T_IDXS, REVERSE_PLAN[:, 2]) + + +def assert_bounded_terminal(mpc): + assert mpc.solution_status == 0 + assert np.isfinite(mpc.x_sol).all() + assert np.min(mpc.v_solution) >= -.01 + assert mpc.params[-1, 0] - .01 <= mpc.a_solution[-1] <= mpc.params[-1, 1] + .01 + + +@pytest.mark.parametrize('personality', PERSONALITIES) +@pytest.mark.parametrize('mode', ('stock', 'starpilot', 'stop_line')) +def test_native_reverse_terminal_plan_recovers_for_both_planners(mode, personality): + module = stock_mpc if mode == 'stock' else long_mpc + mpc = module.LongitudinalMpc() + seed_reverse_plan(mpc) + radar = log.RadarState.new_message() + if mode != 'stop_line': + for lead in (radar.leadOne, radar.leadTwo): + lead.present, lead.dRel, lead.vLead, lead.aLeadK = True, 76., 0., 0. + lead.aLeadTau, lead.modelProb = 1.5, 1. + with patch.object(mpc, 'reset', side_effect=AssertionError('native solver unexpectedly reset')): + for _ in range(20): + mpc.set_weights(personality=personality) + mpc.set_cur_state(20.65, 1.05) + mpc.update(radar, personality=personality, **({'stop_line_m': 70., 'acceleration_max': 1.05} + if mode == 'stop_line' else {})) + assert mpc.solution_status == 0 + assert_bounded_terminal(mpc) + assert np.interp(.2, long_mpc.T_IDXS, mpc.a_solution) < 0. + + +@pytest.mark.parametrize('planner_class', (stock_planner.LongitudinalPlanner, longitudinal_planner.LongitudinalPlanner)) +@pytest.mark.parametrize('personality', PERSONALITIES) +@pytest.mark.parametrize('speed', (0., 10., 20.65, 30.)) +def test_native_terminal_bounds_through_real_planner_transitions(planner_class, personality, speed): + cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = planner_class(cp, init_v=speed) + with patch.object(planner.mpc, 'reset', side_effect=AssertionError('native solver unexpectedly reset')): + for tick in range(80): + sm, _ = messages(lead=20 <= tick < 50, e2e=50 <= tick < 60, off=tick < 10) + sm['carState'].vEgo = speed + sm['carState'].standstill = speed == 0. + sm['carControl'].longActive = tick >= 10 + sm['selfdriveState'].personality = personality + sm['carState'].vCruise = 55. + for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo): + lead.dRel, lead.vLead, lead.vRel = 150., max(0., speed - 3.), -min(3., speed) + planner.update(sm) + assert_bounded_terminal(planner.mpc) + assert np.isfinite(planner.output_a_target) + diff --git a/openpilot/starpilot/longitudinal/tests/test_upstream_planner.py b/openpilot/starpilot/longitudinal/tests/test_upstream_planner.py index 41e1cbb981..48bd07ab03 100644 --- a/openpilot/starpilot/longitudinal/tests/test_upstream_planner.py +++ b/openpilot/starpilot/longitudinal/tests/test_upstream_planner.py @@ -28,6 +28,7 @@ class TestUpstreamPlanner(unittest.TestCase): np.testing.assert_equal(np.diag(stock.solver.costs[0, 'W']), [3., 0., 0., 0., 200., 5.]) stock.set_weights(personality=log.LongitudinalPersonality.aggressive) np.testing.assert_equal(np.diag(stock.solver.costs[0, 'W']), [3., 0., 0., 0., 100., 2.5]) + np.testing.assert_equal(stock.solver.costs[mpc.N, 'Zl'], stock.solver.costs[0, 'Zl']) stock.set_weights(prev_accel_constraint=False) self.assertEqual(stock.solver.costs[0, 'W'][4, 4], 0) self.assertEqual(mpc.get_T_FOLLOW(log.LongitudinalPersonality.relaxed), 1.75) diff --git a/openpilot/starpilot/longitudinal/upstream/mpc.py b/openpilot/starpilot/longitudinal/upstream/mpc.py index 0a0722fbc6..ec64aa9806 100755 --- a/openpilot/starpilot/longitudinal/upstream/mpc.py +++ b/openpilot/starpilot/longitudinal/upstream/mpc.py @@ -258,7 +258,7 @@ class LongitudinalMpc: # Set L2 slack cost on lower bound constraints Zl = np.array(constraint_cost_weights) - for i in range(N): + for i in range(N+1): self.solver.cost_set(i, 'Zl', Zl) def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): diff --git a/openpilot/starpilot/longitudinal/upstream/provenance.json b/openpilot/starpilot/longitudinal/upstream/provenance.json index 81ec277e35..3d4372e40c 100644 --- a/openpilot/starpilot/longitudinal/upstream/provenance.json +++ b/openpilot/starpilot/longitudinal/upstream/provenance.json @@ -2,7 +2,7 @@ "upstream_commit": "521db4c825d37eb5f29acf955daa88da003e4433", "upstream_project": "commaai/openpilot", "base_installed": "645a636a4576b9fba91f7ad4f4d2d9f4b59d34a2", - "solver_contract": "All upstream module assignments and generator functions AST-identical to installed native MPC; reused generated solver Cython import unchanged", + "solver_contract": "Shared native solver is generated by the active longitudinal MPC with terminal speed, acceleration and distance constraints; stock runtime applies its unchanged constraint penalties at all N+1 stages", "files": { "plannerd.py": { "upstream_path": "openpilot/selfdrive/controls/plannerd.py", @@ -19,8 +19,8 @@ "mpc.py": { "upstream_path": "openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py", "upstream_sha256": "63b03a511d8e0a955f6abc3ef010e79e8304d5ee9fe13637741f9c13e0ca92d5", - "adapted_sha256": "63b03a511d8e0a955f6abc3ef010e79e8304d5ee9fe13637741f9c13e0ca92d5", - "adaptation": "Only planner/MPC import paths redirected; no arithmetic/state/publisher changes" + "adapted_sha256": "401186999585a892d7ddfdb4cc91d297621685d11f5ccc5125e016b6a6c6bb45", + "adaptation": "Stock tuning and planner arithmetic unchanged; apply stock slack penalties at terminal stage for the shared constrained native solver" } } }