mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-11-02 20:13:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

MPC terminal constraints

Browse Source
This commit is contained in:
firestar5683
2026-10-07 23:55:41 -05:00
parent 3c406e4374
commit b7d6827450
16 changed files with 640 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+2
View File
@@ -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 + \
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json
+58 -13
View File
@@ -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,
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile
+2
View File
@@ -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
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.so
BIN
View File
Binary file not shown.
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.c
+66
View File
@@ -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;
}
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h
+5 -3
View File
@@ -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;
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so
BIN
View File
Binary file not shown.
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_e_fun.c
+185
View File
@@ -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 <math.h>
#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
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c
+211
View File
@@ -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 <math.h>
#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
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constraints.h
+16
View File
@@ -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" */
openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+9 -1
View File
@@ -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
openpilot/starpilot/longitudinal/tests/test_mpc_cost_cache.py
+1 -1
View File
@@ -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:
openpilot/starpilot/longitudinal/tests/test_mpc_terminal_constraints.py
+80
View File
@@ -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)
openpilot/starpilot/longitudinal/tests/test_upstream_planner.py
+1
View File
@@ -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)
openpilot/starpilot/longitudinal/upstream/mpc.py
+1 -1
View File
@@ -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):
openpilot/starpilot/longitudinal/upstream/provenance.json
+3 -3
View File
@@ -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"
}
}
}