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
(commaai#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

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:

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 );

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 ];

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)
 {

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]
+

selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c

@@ -14,14 +14,15 @@
 
 #define N           ACADO_N   /* Number of intervals in the horizon. */
 
+const int STEP_MULTIPLIER = 3;
+
 ACADOvariables acadoVariables;
 ACADOworkspace acadoWorkspace;
 
 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;

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)