Add time gap and distance cost parameters to Longitudinal MPC

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)