mawei/openpilot-meb
1
0
Fork 0
Code Issues Pull Requests Actions 5 Packages Projects Releases Wiki Activity

longitudinal e2e mpc

Browse Source 
old-commit-hash: 34f2c0da75d9a50e2c85771e769586af6c6cf1c3
This commit is contained in:
Harald Schafer
2020-03-26 17:43:11 -07:00
parent 5f17520b8e
commit 18a05e304c
17 changed files with 488 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
SConstruct
View File

@@ -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'])

9
release/files_common
View File

@@ -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

77
selfdrive/controls/lib/long_mpc_model.py Normal file
View File

@@ -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

2
selfdrive/controls/lib/longitudinal_mpc_model/.gitignore vendored Normal file
View File

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

31
selfdrive/controls/lib/longitudinal_mpc_model/SConscript Normal file
View File

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

0
selfdrive/controls/lib/longitudinal_mpc_model/__init__.py Normal file
View File

99
selfdrive/controls/lib/longitudinal_mpc_model/generator.cpp Normal file
View File

@@ -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;
}

3
selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.c Normal file
View File

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

3
selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_auxiliary_functions.h Normal file
View File

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

3
selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_common.h Normal file
View File

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

3
selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_integrator.c Normal file
View File

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

3
selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.cpp Normal file
View File

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

3
selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_qpoases_interface.hpp Normal file
View File

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

3
selfdrive/controls/lib/longitudinal_mpc_model/lib_mpc_export/acado_solver.c Normal file
View File

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

32
selfdrive/controls/lib/longitudinal_mpc_model/libmpc_py.py Normal file
View File

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

141
selfdrive/controls/lib/longitudinal_mpc_model/longitudinal_mpc.c Normal file
View File

@@ -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();
}

75
selfdrive/debug/mpc/longitudinal_mpc_model.py Executable file
View File

@@ -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()