Add time gap as a parameter of longitudinal mpc

Update longitudinal mpc with follow distance param
rhinodavid · Mar 9, 2019 · 7901ada · 7901ada
2 parents 737f2ac + f040018
commit 7901ada
Show file tree

Hide file tree

Showing 8 changed files with 77 additions and 61 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
@@ -39,7 +39,7 @@ def _get_libmpc(mpc_id):
     void init(double ttcCost, double distanceCost, 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 follow_time);
     """)
 
     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]
+
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
@@ -118,12 +118,13 @@ 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 follow_time){
   int i;
 
   for (i = 0; i <= NOD * N; i+= NOD){
     acadoVariables.od[i] = l;
     acadoVariables.od[i+1] = a_l_0;
+    acadoVariables.od[i+2] = follow_time;
   }
 
   acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;

diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
@@ -211,7 +211,9 @@ 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
+    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, follow_time)
     duration = int((sec_since_boot() - t) * 1e9)
     self.send_mpc_solution(n_its, duration)