Merge pull request commaai#170 from ShaneSmiskol/0510

Replaced 0.5.10 longitudinal MPC with 0.5.9 MPC to fix dynamic-follow

Author: sshane

selfdrive/controls/lib/longitudinal_mpc/generator.cpp

@@ -18,21 +18,30 @@ int main( )
   DifferentialEquation f;
 
   DifferentialState x_ego, v_ego, a_ego;
-  OnlineData x_l, v_l;
+  DifferentialState x_l, v_l, t;
+
+  OnlineData lambda, a_l_0;
 
   Control j_ego;
 
   auto desired = 4.0 + RW(v_ego, v_l);
   auto d_l = x_l - x_ego;
 
+  // Directly calculate a_l to prevent instabilites due to discretization
+  auto a_l = a_l_0 * exp(-lambda * t * t / 2);
+
   // Equations of motion
   f << dot(x_ego) == v_ego;
   f << dot(v_ego) == a_ego;
   f << dot(a_ego) == j_ego;
 
+  f << dot(x_l) == v_l;
+  f << dot(v_l) == a_l;
+  f << dot(t) == 1;
+
   // Running cost
   Function h;
-  h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
+  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 << (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);
@@ -42,7 +51,7 @@ int main( )
 
   // Terminal cost
   Function hN;
-  hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
+  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 << (d_l - desired) / (0.05 * v_ego + 0.5);
   hN << a_ego * (0.1 * v_ego + 1.0);
 

selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o

@@ -70,7 +70,7 @@ extern "C"
 /** Number of control variables. */
 #define ACADO_NU 1
 /** Number of differential variables. */
-#define ACADO_NX 3
+#define ACADO_NX 6
 /** Number of algebraic variables. */
 #define ACADO_NXA 0
 /** Number of differential derivative variables. */
@@ -80,7 +80,7 @@ extern "C"
 /** Number of references/measurements on the last (N + 1)st node. */
 #define ACADO_NYN 3
 /** Total number of QP optimization variables. */
-#define ACADO_QP_NV 23
+#define ACADO_QP_NV 26
 /** Number of Runge-Kutta stages per integration step. */
 #define ACADO_RK_NSTAGES 4
 /** Providing interface for arrival cost. */
@@ -102,11 +102,11 @@ extern "C"
 typedef struct ACADOvariables_
 {
 int dummy;
-/** Matrix of size: 21 x 3 (row major format)
+/** Matrix of size: 21 x 6 (row major format)
  * 
  *  Matrix containing 21 differential variable vectors.
  */
-real_t x[ 63 ];
+real_t x[ 126 ];
 
 /** Column vector of size: 20
  * 
@@ -138,11 +138,11 @@ real_t W[ 320 ];
 /** Matrix of size: 3 x 3 (row major format) */
 real_t WN[ 9 ];
 
-/** Column vector of size: 3
+/** Column vector of size: 6
  * 
  *  Current state feedback vector.
  */
-real_t x0[ 3 ];
+real_t x0[ 6 ];
 
 
 } ACADOvariables;
@@ -155,112 +155,115 @@ real_t x0[ 3 ];
  */
 typedef struct ACADOworkspace_
 {
+/** Column vector of size: 10 */
+real_t rhs_aux[ 10 ];
+
 real_t rk_ttt;
 
-/** Row vector of size: 18 */
-real_t rk_xxx[ 18 ];
+/** Row vector of size: 51 */
+real_t rk_xxx[ 51 ];
 
-/** Matrix of size: 4 x 15 (row major format) */
-real_t rk_kkk[ 60 ];
+/** Matrix of size: 4 x 48 (row major format) */
+real_t rk_kkk[ 192 ];
 
-/** Row vector of size: 18 */
-real_t state[ 18 ];
+/** Row vector of size: 51 */
+real_t state[ 51 ];
 
-/** Column vector of size: 60 */
-real_t d[ 60 ];
+/** Column vector of size: 120 */
+real_t d[ 120 ];
 
 /** Column vector of size: 80 */
 real_t Dy[ 80 ];
 
 /** Column vector of size: 3 */
 real_t DyN[ 3 ];
 
-/** Matrix of size: 60 x 3 (row major format) */
-real_t evGx[ 180 ];
+/** Matrix of size: 120 x 6 (row major format) */
+real_t evGx[ 720 ];
 
-/** Column vector of size: 60 */
-real_t evGu[ 60 ];
+/** Column vector of size: 120 */
+real_t evGu[ 120 ];
 
-/** Column vector of size: 15 */
-real_t objAuxVar[ 15 ];
+/** Column vector of size: 30 */
+real_t objAuxVar[ 30 ];
 
-/** Row vector of size: 6 */
-real_t objValueIn[ 6 ];
+/** Row vector of size: 9 */
+real_t objValueIn[ 9 ];
 
-/** Row vector of size: 20 */
-real_t objValueOut[ 20 ];
+/** Row vector of size: 32 */
+real_t objValueOut[ 32 ];
 
-/** Matrix of size: 60 x 3 (row major format) */
-real_t Q1[ 180 ];
+/** Matrix of size: 120 x 6 (row major format) */
+real_t Q1[ 720 ];
 
-/** Matrix of size: 60 x 4 (row major format) */
-real_t Q2[ 240 ];
+/** Matrix of size: 120 x 4 (row major format) */
+real_t Q2[ 480 ];
 
 /** Column vector of size: 20 */
 real_t R1[ 20 ];
 
 /** Matrix of size: 20 x 4 (row major format) */
 real_t R2[ 80 ];
 
-/** Column vector of size: 60 */
-real_t S1[ 60 ];
+/** Column vector of size: 120 */
+real_t S1[ 120 ];
 
-/** Matrix of size: 3 x 3 (row major format) */
-real_t QN1[ 9 ];
+/** Matrix of size: 6 x 6 (row major format) */
+real_t QN1[ 36 ];
 
-/** Matrix of size: 3 x 3 (row major format) */
-real_t QN2[ 9 ];
+/** Matrix of size: 6 x 3 (row major format) */
+real_t QN2[ 18 ];
 
-/** Column vector of size: 3 */
-real_t Dx0[ 3 ];
+/** Column vector of size: 6 */
+real_t Dx0[ 6 ];
 
-/** Matrix of size: 3 x 3 (row major format) */
-real_t T[ 9 ];
+/** Matrix of size: 6 x 6 (row major format) */
+real_t T[ 36 ];
 
-/** Column vector of size: 630 */
-real_t E[ 630 ];
+/** Column vector of size: 1260 */
+real_t E[ 1260 ];
 
-/** Column vector of size: 630 */
-real_t QE[ 630 ];
+/** Column vector of size: 1260 */
+real_t QE[ 1260 ];
 
-/** Matrix of size: 60 x 3 (row major format) */
-real_t QGx[ 180 ];
+/** Matrix of size: 120 x 6 (row major format) */
+real_t QGx[ 720 ];
 
-/** Column vector of size: 60 */
-real_t Qd[ 60 ];
+/** Column vector of size: 120 */
+real_t Qd[ 120 ];
 
-/** Column vector of size: 63 */
-real_t QDy[ 63 ];
+/** Column vector of size: 126 */
+real_t QDy[ 126 ];
 
-/** Matrix of size: 20 x 3 (row major format) */
-real_t H10[ 60 ];
+/** Matrix of size: 20 x 6 (row major format) */
+real_t H10[ 120 ];
 
-/** Matrix of size: 23 x 23 (row major format) */
-real_t H[ 529 ];
+/** Matrix of size: 26 x 26 (row major format) */
+real_t H[ 676 ];
 
-/** Matrix of size: 20 x 23 (row major format) */
-real_t A[ 460 ];
+/** Matrix of size: 20 x 26 (row major format) */
+real_t A[ 520 ];
 
-/** Column vector of size: 23 */
-real_t g[ 23 ];
+/** Column vector of size: 26 */
+real_t g[ 26 ];
 
-/** Column vector of size: 23 */
-real_t lb[ 23 ];
+/** Column vector of size: 26 */
+real_t lb[ 26 ];
 
-/** Column vector of size: 23 */
-real_t ub[ 23 ];
+/** Column vector of size: 26 */
+real_t ub[ 26 ];
 
 /** Column vector of size: 20 */
 real_t lbA[ 20 ];
 
 /** Column vector of size: 20 */
 real_t ubA[ 20 ];
 
-/** Column vector of size: 23 */
-real_t x[ 23 ];
+/** Column vector of size: 26 */
+real_t x[ 26 ];
 
-/** Column vector of size: 43 */
-real_t y[ 43 ];
+/** Column vector of size: 46 */
+real_t y[ 46 ];
 
 
 } ACADOworkspace;