From 44e6435709ed762ae550d8ed42571fe8710f2ed1 Mon Sep 17 00:00:00 2001 From: David Walsh Date: Sat, 16 Mar 2019 17:06:47 -0600 Subject: [PATCH] Add time gap and distance cost parameters to Longitudinal MPC (#23) * Update longitudinal mpc with follow distance param Update generator.cpp to parameterize the follow time. Then, using the process outlined in the ACADO docs (https://acado.github.io/cgt_overview.html), rebuild the generator binary and run it to regenerate the source files in lib_mpc_export, then manually copy them into the openpilot repo. Update longitudinal_mpc.c, libmpc_py.py, and planner.py to take in the additional follow_time parameter. Incorporate arne182's PR to update the Makefile (https://github.com/commaai/openpilot/pull/248/). Note that I did not use the generate recipie inside the repo; I created the files outside of the repo as stated above. * Add distance cost parameter to longitudinal mpc The distance cost was hardcoded, allow with the time gap. But as we lower the time gap we will need to increase the distance cost for reliable control * Scale the time gap up as we slow down --- .../controls/lib/longitudinal_mpc/Makefile | 10 +-- .../lib/longitudinal_mpc/generator.cpp | 18 +++-- .../lib_mpc_export/acado_common.h | 18 ++--- .../lib_mpc_export/acado_integrator.c | 1 + .../lib_mpc_export/acado_solver.c | 81 ++++++++++--------- .../lib/longitudinal_mpc/libmpc_py.py | 5 +- .../lib/longitudinal_mpc/longitudinal_mpc.c | 28 +++++-- selfdrive/controls/lib/planner.py | 24 +++++- 8 files changed, 118 insertions(+), 67 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc/Makefile b/selfdrive/controls/lib/longitudinal_mpc/Makefile index 17f78eada990df..4e5bfd3807d39c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/Makefile +++ b/selfdrive/controls/lib/longitudinal_mpc/Makefile @@ -13,12 +13,10 @@ QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONEL ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado -ifeq ($(UNAME_S),Darwin) - ACADO_LIBS := -lacado_toolkit_s -else ifeq ($(UNAME_M),aarch64) - ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a +ifeq ($(UNAME_M),aarch64) + ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit_s.so -l:libacado_casadi.a -l:libacado_csparse.a else - ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a + ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit_s.so -l:libacado_casadi.a -l:libacado_csparse.a endif OBJS = \ @@ -80,7 +78,7 @@ generator: generator.cpp .PHONY: generate generate: generator - ./generator + LD_LIBRARY_PATH=../../../../phonelibs/acado/x64/lib ./generator .PHONY: clean clean: diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp index f5c95394d8d3f1..33206891556ede 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp +++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp @@ -5,10 +5,9 @@ const int controlHorizon = 50; using namespace std; #define G 9.81 -#define TR 1.8 -#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) -#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1)) +#define RW(v_ego, v_l, follow_time) (v_ego * follow_time - (v_l - v_ego) * follow_time + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) +#define NORM_RW_ERROR(v_ego, v_l, p, follow_time) ((RW(v_ego, v_l, follow_time) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1)) int main( ) { @@ -24,7 +23,12 @@ int main( ) Control j_ego; - auto desired = 4.0 + RW(v_ego, v_l); + // follow distance expressed as a stopping distance in seconds + // see https://github.com/rhinodavid/CommaButtons + // see https://github.com/acado/acado/issues/54 for a discussion of `OnlineData` + OnlineData follow_time; + + auto desired = 4.0 + RW(v_ego, v_l, follow_time); auto d_l = x_l - x_ego; // Directly calculate a_l to prevent instabilites due to discretization @@ -41,7 +45,7 @@ int main( ) // Running cost Function h; - h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired)); + h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l, follow_time)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired, follow_time)); h << (d_l - desired) / (0.05 * v_ego + 0.5); h << a_ego * (0.1 * v_ego + 1.0); h << j_ego * (0.1 * v_ego + 1.0); @@ -51,7 +55,7 @@ int main( ) // Terminal cost Function hN; - hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired)); + hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l, follow_time)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired, follow_time)); hN << (d_l - desired) / (0.05 * v_ego + 0.5); hN << a_ego * (0.1 * v_ego + 1.0); @@ -79,7 +83,7 @@ int main( ) ocp.minimizeLSQEndTerm(QN, hN); ocp.subjectTo( 0.0 <= v_ego); - ocp.setNOD(2); + ocp.setNOD(3); OCPexport mpc(ocp); mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h index cecba6e7408cba..db1be5e0c71be8 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h @@ -64,7 +64,7 @@ extern "C" /** Number of control/estimation intervals. */ #define ACADO_N 20 /** Number of online data values. */ -#define ACADO_NOD 2 +#define ACADO_NOD 3 /** Number of path constraints. */ #define ACADO_NPAC 0 /** Number of control variables. */ @@ -114,11 +114,11 @@ real_t x[ 126 ]; */ real_t u[ 20 ]; -/** Matrix of size: 21 x 2 (row major format) +/** Matrix of size: 21 x 3 (row major format) * * Matrix containing 21 online data vectors. */ -real_t od[ 42 ]; +real_t od[ 63 ]; /** Column vector of size: 80 * @@ -160,14 +160,14 @@ real_t rhs_aux[ 10 ]; real_t rk_ttt; -/** Row vector of size: 51 */ -real_t rk_xxx[ 51 ]; +/** Row vector of size: 52 */ +real_t rk_xxx[ 52 ]; /** Matrix of size: 4 x 48 (row major format) */ real_t rk_kkk[ 192 ]; -/** Row vector of size: 51 */ -real_t state[ 51 ]; +/** Row vector of size: 52 */ +real_t state[ 52 ]; /** Column vector of size: 120 */ real_t d[ 120 ]; @@ -187,8 +187,8 @@ real_t evGu[ 120 ]; /** Column vector of size: 30 */ real_t objAuxVar[ 30 ]; -/** Row vector of size: 9 */ -real_t objValueIn[ 9 ]; +/** Row vector of size: 10 */ +real_t objValueIn[ 10 ]; /** Row vector of size: 32 */ real_t objValueOut[ 32 ]; diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c index 2e77d54147dbee..d348a5ecbfb161 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c @@ -145,6 +145,7 @@ rk_eta[47] = 0.0000000000000000e+00; acadoWorkspace.rk_xxx[48] = rk_eta[48]; acadoWorkspace.rk_xxx[49] = rk_eta[49]; acadoWorkspace.rk_xxx[50] = rk_eta[50]; +acadoWorkspace.rk_xxx[51] = rk_eta[51]; for (run1 = 0; run1 < 1; ++run1) { diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c index 676787bbfcbc7b..3b88710e5fd164 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c @@ -45,8 +45,9 @@ acadoWorkspace.state[4] = acadoVariables.x[lRun1 * 6 + 4]; acadoWorkspace.state[5] = acadoVariables.x[lRun1 * 6 + 5]; acadoWorkspace.state[48] = acadoVariables.u[lRun1]; -acadoWorkspace.state[49] = acadoVariables.od[lRun1 * 2]; -acadoWorkspace.state[50] = acadoVariables.od[lRun1 * 2 + 1]; +acadoWorkspace.state[49] = acadoVariables.od[lRun1 * 3]; +acadoWorkspace.state[50] = acadoVariables.od[lRun1 * 3 + 1]; +acadoWorkspace.state[51] = acadoVariables.od[lRun1 * 3 + 2]; ret = acado_integrate(acadoWorkspace.state, 1, lRun1); @@ -108,36 +109,37 @@ void acado_evaluateLSQ(const real_t* in, real_t* out) { const real_t* xd = in; const real_t* u = in + 6; +const real_t* od = in + 7; /* Vector of auxiliary variables; number of elements: 30. */ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]); a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[9] = (a[8]*(real_t)(5.0000000000000000e-01)); a[10] = (a[4]*a[4]); -a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); +a[11] = (((real_t)(2.9999999999999999e-01)*((((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01))); a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[16] = (a[15]*(real_t)(5.0000000000000000e-01)); a[17] = (a[14]*a[14]); -a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); -a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); +a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[19] = (((real_t)(2.9999999999999999e-01)*(((((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[12]))-((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]); a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); +a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); +a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[28] = (a[26]*a[26]); @@ -145,7 +147,7 @@ a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); /* Compute outputs: */ out[0] = (a[1]-a[3]); -out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[4] = a[6]; @@ -155,10 +157,10 @@ out[7] = a[20]; out[8] = (a[22]-a[25]); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]); -out[11] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); +out[11] = ((((real_t)(0.0000000000000000e+00)-((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); out[12] = (real_t)(0.0000000000000000e+00); out[13] = a[26]; -out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]); +out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[29])))*a[26]); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (real_t)(0.0000000000000000e+00); out[17] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -181,36 +183,37 @@ out[31] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) { const real_t* xd = in; +const real_t* od = in + 6; /* Vector of auxiliary variables; number of elements: 30. */ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]); a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[9] = (a[8]*(real_t)(5.0000000000000000e-01)); a[10] = (a[4]*a[4]); -a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); +a[11] = (((real_t)(2.9999999999999999e-01)*((((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01))); a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[16] = (a[15]*(real_t)(5.0000000000000000e-01)); a[17] = (a[14]*a[14]); -a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); -a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); +a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[19] = (((real_t)(2.9999999999999999e-01)*(((((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[12]))-((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]); a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); +a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); +a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[28] = (a[26]*a[26]); @@ -218,7 +221,7 @@ a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); /* Compute outputs: */ out[0] = (a[1]-a[3]); -out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = a[6]; out[4] = (a[11]-a[19]); @@ -227,10 +230,10 @@ out[6] = a[20]; out[7] = (a[22]-a[25]); out[8] = (real_t)(0.0000000000000000e+00); out[9] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]); -out[10] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); +out[10] = ((((real_t)(0.0000000000000000e+00)-((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); out[11] = (real_t)(0.0000000000000000e+00); out[12] = a[26]; -out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]); +out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[29])))*a[26]); out[14] = (real_t)(0.0000000000000000e+00); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -383,8 +386,9 @@ acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 6 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.x[runObj * 6 + 4]; acadoWorkspace.objValueIn[5] = acadoVariables.x[runObj * 6 + 5]; acadoWorkspace.objValueIn[6] = acadoVariables.u[runObj]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 2 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 3]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 3 + 1]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 3 + 2]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0]; @@ -403,8 +407,9 @@ acadoWorkspace.objValueIn[2] = acadoVariables.x[122]; acadoWorkspace.objValueIn[3] = acadoVariables.x[123]; acadoWorkspace.objValueIn[4] = acadoVariables.x[124]; acadoWorkspace.objValueIn[5] = acadoVariables.x[125]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[40]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[41]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[60]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[61]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[62]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; @@ -5205,8 +5210,9 @@ acadoWorkspace.state[3] = acadoVariables.x[index * 6 + 3]; acadoWorkspace.state[4] = acadoVariables.x[index * 6 + 4]; acadoWorkspace.state[5] = acadoVariables.x[index * 6 + 5]; acadoWorkspace.state[48] = acadoVariables.u[index]; -acadoWorkspace.state[49] = acadoVariables.od[index * 2]; -acadoWorkspace.state[50] = acadoVariables.od[index * 2 + 1]; +acadoWorkspace.state[49] = acadoVariables.od[index * 3]; +acadoWorkspace.state[50] = acadoVariables.od[index * 3 + 1]; +acadoWorkspace.state[51] = acadoVariables.od[index * 3 + 2]; acado_integrate(acadoWorkspace.state, index == 0, index); @@ -5257,8 +5263,9 @@ else { acadoWorkspace.state[48] = acadoVariables.u[19]; } -acadoWorkspace.state[49] = acadoVariables.od[40]; -acadoWorkspace.state[50] = acadoVariables.od[41]; +acadoWorkspace.state[49] = acadoVariables.od[60]; +acadoWorkspace.state[50] = acadoVariables.od[61]; +acadoWorkspace.state[51] = acadoVariables.od[62]; acado_integrate(acadoWorkspace.state, 1, 19); @@ -5333,8 +5340,9 @@ acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 6 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.x[lRun1 * 6 + 4]; acadoWorkspace.objValueIn[5] = acadoVariables.x[lRun1 * 6 + 5]; acadoWorkspace.objValueIn[6] = acadoVariables.u[lRun1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 2 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 3]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 3 + 1]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 3 + 2]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4]; @@ -5348,8 +5356,9 @@ acadoWorkspace.objValueIn[2] = acadoVariables.x[122]; acadoWorkspace.objValueIn[3] = acadoVariables.x[123]; acadoWorkspace.objValueIn[4] = acadoVariables.x[124]; acadoWorkspace.objValueIn[5] = acadoVariables.x[125]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[40]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[41]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[60]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[61]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[62]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index 36b6f8b7e0b8d3..15b83d072345e7 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -36,10 +36,10 @@ def _get_libmpc(mpc_id): double cost; } log_t; - void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); + void init(double ttcCost, double defaultDistanceCost, double accelerationCost, double jerkCost); void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); int run_mpc(state_t * x0, log_t * solution, - double l, double a_l_0); + double l, double a_l_0, double time_gap, double distance_cost); """) return (ffi, ffi.dlopen(libmpc_fn)) @@ -48,3 +48,4 @@ def _get_libmpc(mpc_id): def get_libmpc(mpc_id): return mpcs[mpc_id - 1] + \ No newline at end of file diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c index c0b43e53c93771..369e18d8480f92 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c @@ -14,6 +14,8 @@ #define N ACADO_N /* Number of intervals in the horizon. */ +const int STEP_MULTIPLIER = 3; + ACADOvariables acadoVariables; ACADOworkspace acadoWorkspace; @@ -21,7 +23,6 @@ typedef struct { double x_ego, v_ego, a_ego, x_l, v_l, a_l; } state_t; - typedef struct { double x_ego[N+1]; double v_ego[N+1]; @@ -33,10 +34,13 @@ typedef struct { double cost; } log_t; -void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ +void init(double ttcCost, double defaultDistanceCost, double accelerationCost, double jerkCost){ + // Comma Buttons https://github.com/rhinodavid/CommaButtons + // set the defaultFollowDistance cost here, but we'll add it as a parameter of the update + // function in order to change it as we change the time gap + acado_initializeSolver(); int i; - const int STEP_MULTIPLIER = 3; /* Initialize the states and controls. */ for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; @@ -56,12 +60,12 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j f = STEP_MULTIPLIER; } acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc) - acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance + acadoVariables.W[16 * i + 5] = defaultDistanceCost * f; // desired distance acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk } acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone - acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance + acadoVariables.WN[4] = defaultDistanceCost * STEP_MULTIPLIER; // desired distance acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration } @@ -118,12 +122,24 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0 for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; } -int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ +int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double time_gap, double distance_cost){ int i; + // For CommaButtons https://github.com/rhinodavid/CommaButtons + // Update the distance_cost as we change the time gap + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4) { + f = STEP_MULTIPLIER; + } + acadoVariables.W[16 * i + 5] = distance_cost * f; // desired distance + } + acadoVariables.WN[4] = distance_cost * STEP_MULTIPLIER; // desired distance + for (i = 0; i <= NOD * N; i+= NOD){ acadoVariables.od[i] = l; acadoVariables.od[i+1] = a_l_0; + acadoVariables.od[i+2] = time_gap; } acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego; diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index e0a53e99736d95..d60c6da3102955 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -64,6 +64,22 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): a_target[1] = min(a_target[1], a_x_allowed) return a_target +def scale_time_gap(v_ego, commanded_time_gap): + """ + Raise the time gap as we slow down. Use the full time gap at 30mph and increase + it to 1.8s at 25mph + Keyword arguments: + v_ego -- the vehicle speed in m/s + commanded_time_gap -- the time gap the set by the user in s + See Openpilot Buttons -- https://github.com/rhinodavid/OpenpilotButtons + """ + ms30 = 30. * CV.MPH_TO_MS + ms20 = 20. * CV.MPH_TO_MS + if v_ego > ms30: + return commanded_time_gap + if v_ego < ms20: + return 1.8 + return round(float(np.interp(v_ego, [ms20, ms30], [1.8, commanded_time_gap])), 2) class FCWChecker(object): def __init__(self): @@ -211,7 +227,13 @@ def update(self, CS, lead, v_cruise_setpoint): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + + # hardcode follow time for now + follow_time = 1.8 + distance_cost = 0.1 + follow_time_value = scale_time_gap(v_ego, follow_time) + + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, follow_time_value, distance_cost) duration = int((sec_since_boot() - t) * 1e9) self.send_mpc_solution(n_its, duration)