longitudinal e2e mpc

old-commit-hash: 34f2c0da75

SConstruct

@@ -215,6 +215,7 @@ if arch != "Darwin":
 SConscript(['selfdrive/controls/lib/cluster/SConscript'])
 SConscript(['selfdrive/controls/lib/lateral_mpc/SConscript'])
 SConscript(['selfdrive/controls/lib/longitudinal_mpc/SConscript'])
+SConscript(['selfdrive/controls/lib/longitudinal_mpc_model/SConscript'])
 
 SConscript(['selfdrive/boardd/SConscript'])
 SConscript(['selfdrive/proclogd/SConscript'])

release/files_common

@@ -214,6 +214,7 @@ selfdrive/controls/lib/vehicle_model.py
 selfdrive/controls/lib/speed_smoother.py
 selfdrive/controls/lib/fcw.py
 selfdrive/controls/lib/long_mpc.py
+selfdrive/controls/lib/long_mpc_model.py
 selfdrive/controls/lib/gps_helpers.py
 
 selfdrive/controls/lib/cluster/*
@@ -234,6 +235,14 @@ selfdrive/controls/lib/longitudinal_mpc/generator.cpp
 selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
 selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
 
+selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/*
+selfdrive/controls/lib/longitudinal_mpc_model/.gitignore
+selfdrive/controls/lib/longitudinal_mpc_model/SConscript
+selfdrive/controls/lib/longitudinal_mpc_model/__init__.py
+selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp
+selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py
+selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c
+
 selfdrive/locationd/__init__.py
 selfdrive/locationd/.gitignore
 selfdrive/locationd/SConscript

selfdrive/controls/lib/long_mpc_model.py

@@ -0,0 +1,77 @@
+import numpy as np
+import math
+
+from selfdrive.swaglog import cloudlog
+from common.realtime import sec_since_boot
+from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py
+
+
+class LongitudinalMpcModel():
+  def __init__(self):
+
+    self.setup_mpc()
+    self.v_mpc = 0.0
+    self.v_mpc_future = 0.0
+    self.a_mpc = 0.0
+    self.last_cloudlog_t = 0.0
+    self.ts = list(range(10))
+
+    self.valid = False
+
+  def setup_mpc(self, v_ego=0.0):
+    self.libmpc = libmpc_py.libmpc
+    self.libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
+    self.libmpc.init_with_simulation(v_ego)
+
+    self.mpc_solution = libmpc_py.ffi.new("log_t *")
+    self.cur_state = libmpc_py.ffi.new("state_t *")
+
+    self.cur_state[0].x_ego = 0
+    self.cur_state[0].v_ego = 0
+    self.cur_state[0].a_ego = 0
+
+  def set_cur_state(self, v, a):
+    self.cur_state[0].x_ego = 0.0
+    self.cur_state[0].v_ego = v
+    self.cur_state[0].a_ego = a
+
+  def update(self, CS, model):
+    v_ego = CS.vEgo
+
+    longitudinal = model.longitudinal
+
+    if len(longitudinal.distances) == 0:
+      self.valid = False
+      return
+
+    x_poly = list(map(float, np.polyfit(self.ts, longitudinal.distances, 3)))
+    v_poly = list(map(float, np.polyfit(self.ts, longitudinal.speeds, 3)))
+    a_poly = list(map(float, np.polyfit(self.ts, longitudinal.accelerations, 3)))
+
+    # Calculate mpc
+    self.libmpc.run_mpc(self.cur_state, self.mpc_solution, x_poly, v_poly, a_poly)
+
+    # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
+    self.v_mpc = self.mpc_solution[0].v_ego[1]
+    self.a_mpc = self.mpc_solution[0].a_ego[1]
+    self.v_mpc_future = self.mpc_solution[0].v_ego[10]
+    self.valid = True
+
+    # Reset if NaN or goes through lead car
+    nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
+
+    t = sec_since_boot()
+    if nans:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning("Longitudinal model mpc reset - backwards")
+
+      self.libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
+      self.libmpc.init_with_simulation(v_ego)
+
+      self.cur_state[0].v_ego = v_ego
+      self.cur_state[0].a_ego = 0.0
+
+      self.v_mpc = v_ego
+      self.a_mpc = CS.aEgo
+      self.valid = False

selfdrive/controls/lib/longitudinal_mpc_model/.gitignore

@@ -0,0 +1,2 @@
+generator
+lib_qp/

selfdrive/controls/lib/longitudinal_mpc_model/SConscript

@@ -0,0 +1,31 @@
+Import('env', 'arch')
+
+
+cpp_path = [
+    "#phonelibs/acado/include",
+    "#phonelibs/acado/include/acado",
+    "#phonelibs/qpoases/INCLUDE",
+    "#phonelibs/qpoases/INCLUDE/EXTRAS",
+    "#phonelibs/qpoases/SRC/",
+    "#phonelibs/qpoases",
+    "lib_mpc_export"
+
+]
+
+mpc_files = [
+    "longitudinal_mpc.c",
+    Glob("lib_mpc_export/*.c"),
+    Glob("lib_mpc_export/*.cpp"),
+]
+
+interface_dir = Dir('lib_mpc_export')
+
+SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
+
+env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+
+# if arch != "aarch64":
+#     acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"),
+#                   File("#phonelibs/acado/x64/lib/libacado_casadi.a"),
+#                   File("#phonelibs/acado/x64/lib/libacado_csparse.a")]
+#     env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path)

selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp

@@ -0,0 +1,99 @@
+#include <acado_code_generation.hpp>
+
+const int controlHorizon = 50;
+
+using namespace std;
+
+
+int main( )
+{
+  USING_NAMESPACE_ACADO
+
+
+  DifferentialEquation f;
+
+  DifferentialState x_ego, v_ego, a_ego, t;
+
+  OnlineData x_poly_r0, x_poly_r1, x_poly_r2, x_poly_r3;
+  OnlineData v_poly_r0, v_poly_r1, v_poly_r2, v_poly_r3;
+  OnlineData a_poly_r0, a_poly_r1, a_poly_r2, a_poly_r3;
+
+  Control j_ego;
+
+  // Equations of motion
+  f << dot(x_ego) == v_ego;
+  f << dot(v_ego) == a_ego;
+  f << dot(a_ego) == j_ego;
+  f << dot(t) == 1;
+
+  auto poly_x = x_poly_r0*(t*t*t) + x_poly_r1*(t*t) + x_poly_r2*t + x_poly_r3;
+  auto poly_v = v_poly_r0*(t*t*t) + v_poly_r1*(t*t) + v_poly_r2*t + v_poly_r3;
+  auto poly_a = a_poly_r0*(t*t*t) + a_poly_r1*(t*t) + a_poly_r2*t + a_poly_r3;
+
+  // Running cost
+  Function h;
+  h << x_ego - poly_x;
+  h << v_ego - poly_v;
+  h << a_ego - poly_a;
+  h << a_ego * (0.1 * v_ego + 1.0);
+  h << j_ego * (0.1 * v_ego + 1.0);
+
+  // Weights are defined in mpc.
+  BMatrix Q(5,5); Q.setAll(true);
+
+  // Terminal cost
+  Function hN;
+  hN << x_ego - poly_x;
+  hN << v_ego - poly_v;
+  hN << a_ego - poly_a;
+  hN << a_ego * (0.1 * v_ego + 1.0);
+
+  // Weights are defined in mpc.
+  BMatrix QN(4,4); QN.setAll(true);
+
+  // Non uniform time grid
+  // First 5 timesteps are 0.2, after that it's 0.6
+  DMatrix numSteps(20, 1);
+  for (int i = 0; i < 5; i++){
+    numSteps(i) = 1;
+  }
+  for (int i = 5; i < 20; i++){
+    numSteps(i) = 3;
+  }
+
+  // Setup Optimal Control Problem
+  const double tStart = 0.0;
+  const double tEnd   = 10.0;
+
+  OCP ocp( tStart, tEnd, numSteps);
+  ocp.subjectTo(f);
+
+  ocp.minimizeLSQ(Q, h);
+  ocp.minimizeLSQEndTerm(QN, hN);
+
+  //ocp.subjectTo( 0.0 <= v_ego);
+  ocp.setNOD(12);
+
+  OCPexport mpc(ocp);
+  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
+  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
+  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
+  mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon);
+  mpc.set( MAX_NUM_QP_ITERATIONS, 500);
+  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
+
+  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
+  mpc.set( QP_SOLVER, QP_QPOASES );
+  mpc.set( HOTSTART_QP, YES );
+  mpc.set( GENERATE_TEST_FILE, NO);
+  mpc.set( GENERATE_MAKE_FILE, NO );
+  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
+  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
+
+  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
+    exit( EXIT_FAILURE );
+
+  mpc.printDimensionsQP( );
+
+  return EXIT_SUCCESS;
+}

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.c

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:36c26a2590e54135f7f03b8c784b434d2bd5ef0d42e7e2a9022c2bb56d0e2357
+size 4906

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.h

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:606244b9b31362cc30c324793191d9bd34a098d5ebf526612749f437a75a4270
+size 3428

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_common.h

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:75e4db5112c976591459850ff64e6765078c541e19bbbb7d57d7ecab478cf002
+size 8537

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_integrator.c

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:efee44ba07ee0b17daa1c672ad1ab36adc64cb4b3ce4994a99620593f8841f31
+size 17893

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4648c886b327c86b9e59da337272087a5a31c716e8bf9023b71ae55036bfbc82
+size 1890

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.hpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0d8783b453fbc75097fcb0f287c89a503ce7f858c0c84e3a19a0c581dd559055
+size 1820

selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_solver.c

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4985f68cc7d1fc1e587477faa2fd0ca4ebc9ece598f8c2d20e94555d7a51805a
+size 375557

selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py

@@ -0,0 +1,32 @@
+import os
+
+from cffi import FFI
+from common.ffi_wrapper import suffix
+
+mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
+libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix())
+
+ffi = FFI()
+ffi.cdef("""
+
+typedef struct {
+double x_ego, v_ego, a_ego;
+} state_t;
+
+
+typedef struct {
+double x_ego[21];
+double v_ego[21];
+double a_ego[21];
+double t[21];
+double j_ego[20];
+double cost;
+} log_t;
+
+
+void init(double xCost, double vCost, double aCost, double accelCost, double jerkCost);
+void init_with_simulation(double v_ego);
+int run_mpc(state_t * x0, log_t * solution, double x_poly[4], double v_poly[4], double a_poly[4]);
+""")
+
+libmpc = ffi.dlopen(libmpc_fn)

selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c

@@ -0,0 +1,141 @@
+#include "acado_common.h"
+#include "acado_auxiliary_functions.h"
+
+#include <stdio.h>
+#include <math.h>
+
+#define NX          ACADO_NX  /* Number of differential state variables.  */
+#define NXA         ACADO_NXA /* Number of algebraic variables. */
+#define NU          ACADO_NU  /* Number of control inputs. */
+#define NOD         ACADO_NOD  /* Number of online data values. */
+
+#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
+#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
+
+#define N           ACADO_N   /* Number of intervals in the horizon. */
+
+ACADOvariables acadoVariables;
+ACADOworkspace acadoWorkspace;
+
+typedef struct {
+  double x_ego, v_ego, a_ego;
+} state_t;
+
+
+typedef struct {
+  double x_ego[N+1];
+  double v_ego[N+1];
+  double a_ego[N+1];
+  double t[N+1];
+  double j_ego[N];
+  double cost;
+} log_t;
+
+void init(double xCost, double vCost, double aCost, double accelCost, double jerkCost){
+  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;
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
+
+  /* Initialize the measurements/reference. */
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+
+  /* MPC: initialize the current state feedback. */
+  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
+  // Set weights
+
+  for (i = 0; i < N; i++) {
+    int f = 1;
+    if (i > 4){
+      f = STEP_MULTIPLIER;
+    }
+    // Setup diagonal entries
+    acadoVariables.W[NY*NY*i + (NY+1)*0] = xCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*1] = vCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*2] = aCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*3] = accelCost * f;
+    acadoVariables.W[NY*NY*i + (NY+1)*4] = jerkCost * f;
+  }
+  acadoVariables.WN[(NYN+1)*0] = xCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*1] = vCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*2] = aCost * STEP_MULTIPLIER;
+  acadoVariables.WN[(NYN+1)*3] = accelCost * STEP_MULTIPLIER;
+
+}
+
+void init_with_simulation(double v_ego){
+  int i;
+
+  double x_ego = 0.0;
+
+  double dt = 0.2;
+  double t = 0.0;
+
+  for (i = 0; i < N + 1; ++i){
+    if (i > 4){
+      dt = 0.6;
+    }
+
+    acadoVariables.x[i*NX] = x_ego;
+    acadoVariables.x[i*NX+1] = v_ego;
+    acadoVariables.x[i*NX+2] = 0;
+    acadoVariables.x[i*NX+3] = t;
+
+    x_ego += v_ego * dt;
+    t += dt;
+  }
+
+  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
+  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
+  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
+}
+
+int run_mpc(state_t * x0, log_t * solution,
+            double x_poly[4], double v_poly[4], double a_poly[4]){
+  int i;
+
+  for (i = 0; i < N + 1; ++i){
+    acadoVariables.od[i*NOD+0] = x_poly[0];
+    acadoVariables.od[i*NOD+1] = x_poly[1];
+    acadoVariables.od[i*NOD+2] = x_poly[2];
+    acadoVariables.od[i*NOD+3] = x_poly[3];
+
+    acadoVariables.od[i*NOD+4] = v_poly[0];
+    acadoVariables.od[i*NOD+5] = v_poly[1];
+    acadoVariables.od[i*NOD+6] = v_poly[2];
+    acadoVariables.od[i*NOD+7] = v_poly[3];
+
+    acadoVariables.od[i*NOD+8] = a_poly[0];
+    acadoVariables.od[i*NOD+9] = a_poly[1];
+    acadoVariables.od[i*NOD+10] = a_poly[2];
+    acadoVariables.od[i*NOD+11] = a_poly[3];
+  }
+
+  acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
+  acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
+  acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
+  acadoVariables.x[3] = acadoVariables.x0[3] = 0;
+
+  acado_preparationStep();
+  acado_feedbackStep();
+
+  for (i = 0; i <= N; i++){
+    solution->x_ego[i] = acadoVariables.x[i*NX];
+    solution->v_ego[i] = acadoVariables.x[i*NX+1];
+    solution->a_ego[i] = acadoVariables.x[i*NX+2];
+    solution->t[i] = acadoVariables.x[i*NX+3];
+
+    if (i < N){
+      solution->j_ego[i] = acadoVariables.u[i];
+    }
+  }
+  solution->cost = acado_getObjective();
+
+  // Dont shift states here. Current solution is closer to next timestep than if
+  // we shift by 0.1 seconds.
+  return acado_getNWSR();
+}

selfdrive/debug/mpc/longitudinal_mpc_model.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+import numpy as np
+
+import matplotlib.pyplot as plt
+
+from selfdrive.controls.lib.longitudinal_mpc_model import libmpc_py
+
+libmpc = libmpc_py.libmpc
+
+dt = 1
+speeds = [6.109375, 5.9765625, 6.6367188, 7.6875, 8.7578125, 9.4375, 10.21875, 11.070312, 11.679688, 12.21875]
+accelerations = [0.15405273, 0.39575195, 0.36669922, 0.29248047, 0.27856445, 0.27832031, 0.29736328, 0.22705078, 0.16003418, 0.15185547]
+ts = [t * dt for t in range(len(speeds))]
+
+# TODO: Get from actual model packet
+x = 0.0
+positions = []
+for v in speeds:
+  positions.append(x)
+  x += v * dt
+
+
+# Polyfit trajectories
+x_poly = list(map(float, np.polyfit(ts, positions, 3)))
+v_poly = list(map(float, np.polyfit(ts, speeds, 3)))
+a_poly = list(map(float, np.polyfit(ts, accelerations, 3)))
+
+x_poly = libmpc_py.ffi.new("double[4]", x_poly)
+v_poly = libmpc_py.ffi.new("double[4]", v_poly)
+a_poly = libmpc_py.ffi.new("double[4]", a_poly)
+
+cur_state = libmpc_py.ffi.new("state_t *")
+cur_state[0].x_ego = 0
+cur_state[0].v_ego = 10
+cur_state[0].a_ego = 0
+
+libmpc.init(1.0, 1.0, 1.0, 1.0, 1.0)
+
+mpc_solution = libmpc_py.ffi.new("log_t *")
+libmpc.init_with_simulation(cur_state[0].v_ego)
+
+libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly)
+
+# Converge to solution
+for _ in range(10):
+  libmpc.run_mpc(cur_state, mpc_solution, x_poly, v_poly, a_poly)
+
+
+ts_sol = list(mpc_solution[0].t)
+x_sol = list(mpc_solution[0].x_ego)
+v_sol = list(mpc_solution[0].v_ego)
+a_sol = list(mpc_solution[0].a_ego)
+
+
+plt.figure()
+plt.subplot(3, 1, 1)
+plt.plot(ts, positions, 'k--')
+plt.plot(ts_sol, x_sol)
+plt.ylabel('Position [m]')
+plt.xlabel('Time [s]')
+
+plt.subplot(3, 1, 2)
+plt.plot(ts, speeds, 'k--')
+plt.plot(ts_sol, v_sol)
+plt.xlabel('Time [s]')
+plt.ylabel('Speed [m/s]')
+
+plt.subplot(3, 1, 3)
+plt.plot(ts, accelerations, 'k--')
+plt.plot(ts_sol, a_sol)
+
+plt.xlabel('Time [s]')
+plt.ylabel('Acceleration [m/s^2]')
+
+plt.show()