retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Mpc rework2 (#19660) 

* start again

* need that too

* this actually works

* not needed

* do properly

* still works

* still works

* still good

* all G without ll

* still works

* all still good

* cleanup building

* cleanup sconscript

* new lane planner

* how on earth is this silent too....

* update

* add rotation radius

* update

* pathplanner first pass

* misc fixes

* fix

* need deep_interp

* local again

* fix

* fix test

* very old

* new replay

* interp properly

* correct length

* another horrible silent bug

* like master

* fix that

* do doubles

* different delay compensation

* make robust to empty msg

* make pass with hack for now

* add some extra

* update ref for increased leg

* test cpu usage on this pr

* tiny bit faster

* purge numpy

* update ref

* not needed

* ready for merge

* try again after recompile

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
albatross
HaraldSchafer 2021-01-14 18:43:50 -08:00 committed by GitHub
parent 413e0fa40a
commit 158210cde8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
25 changed files with 846 additions and 2653 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
common/numpy_helpers.py 100644
View File

@ -0,0 +1,22 @@
import numpy as np
def deep_interp_np(x, xp, fp, axis=None):
if axis is not None:
fp = fp.swapaxes(0,axis)
x = np.atleast_1d(x)
xp = np.array(xp)
if len(xp) < 2:
return np.repeat(fp, len(x), axis=0)
if min(np.diff(xp)) < 0:
raise RuntimeError('Bad x array for interpolation')
j = np.searchsorted(xp, x) - 1
j = np.clip(j, 0, len(xp)-2)
d = np.divide(x - xp[j], xp[j + 1] - xp[j], out=np.ones_like(x, dtype=np.float64), where=xp[j + 1] - xp[j] != 0)
vals_interp = (fp[j].T*(1 - d)).T + (fp[j + 1].T*d).T
if axis is not None:
vals_interp = vals_interp.swapaxes(0,axis)
if len(vals_interp) == 1:
return vals_interp[0]
else:
return vals_interp

24
selfdrive/common/modeldata.h
View File

@ -1,10 +1,18 @@
#pragma once
const int TRAJECTORY_SIZE = 33;
const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0;
constexpr int MODEL_PATH_DISTANCE = 192;
constexpr int TRAJECTORY_SIZE = 33;
constexpr float MIN_DRAW_DISTANCE = 10.0;
constexpr float MAX_DRAW_DISTANCE = 100.0;
constexpr int POLYFIT_DEGREE = 4;
constexpr int SPEED_PERCENTILES = 10;
constexpr int DESIRE_PRED_SIZE = 32;
constexpr int OTHER_META_SIZE = 4;
const double T_IDXS[TRAJECTORY_SIZE] = {0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 ,
0.24414062, 0.3515625 , 0.47851562, 0.625 , 0.79101562,
0.9765625 , 1.18164062, 1.40625 , 1.65039062, 1.9140625 ,
2.19726562, 2.5 , 2.82226562, 3.1640625 , 3.52539062,
3.90625 , 4.30664062, 4.7265625 , 5.16601562, 5.625 ,
6.10351562, 6.6015625 , 7.11914062, 7.65625 , 8.21289062,
8.7890625 , 9.38476562, 10.};
const double X_IDXS[TRAJECTORY_SIZE] = { 0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875,
6.75 , 9.1875, 12. , 15.1875, 18.75 , 22.6875,
27. , 31.6875, 36.75 , 42.1875, 48. , 54.1875,
60.75 , 67.6875, 75. , 82.6875, 90.75 , 99.1875,
108. , 117.1875, 126.75 , 136.6875, 147. , 157.6875,
168.75 , 180.1875, 192.};

3
selfdrive/controls/lib/drive_helpers.py
View File

@ -7,11 +7,12 @@ V_CRUISE_MAX = 144
V_CRUISE_MIN = 8
V_CRUISE_DELTA = 8
V_CRUISE_ENABLE_MIN = 40
MPC_N = 16
CAR_ROTATION_RADIUS = 1.5
class MPC_COST_LAT:
PATH = 1.0
LANE = 3.0
HEADING = 1.0
STEER_RATE = 1.0

88
selfdrive/controls/lib/lane_planner.py
View File

@ -3,103 +3,79 @@ import numpy as np
from cereal import log
CAMERA_OFFSET = 0.06 # m from center car to camera
TRAJECTORY_SIZE = 33
def compute_path_pinv(length=50):
deg = 3
x = np.arange(length*1.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, [float(x) for x in points])
def eval_poly(poly, x):
return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3
class LanePlanner:
def __init__(self):
self.l_poly = [0., 0., 0., 0.]
self.r_poly = [0., 0., 0., 0.]
self.p_poly = [0., 0., 0., 0.]
self.d_poly = [0., 0., 0., 0.]
self.lane_t = np.zeros((TRAJECTORY_SIZE,))
self.lll_y = np.zeros((TRAJECTORY_SIZE,))
self.rll_y = np.zeros((TRAJECTORY_SIZE,))
self.lane_width_estimate = 3.7
self.lane_width_certainty = 1.0
self.lane_width = 3.7
self.l_prob = 0.
self.r_prob = 0.
self.lll_prob = 0.
self.rll_prob = 0.
self.l_std = 0.
self.r_std = 0.
self.lll_std = 0.
self.rll_std = 0.
self.l_lane_change_prob = 0.
self.r_lane_change_prob = 0.
self._path_pinv = compute_path_pinv()
self.x_points = np.arange(50)
def parse_model(self, md):
if len(md.leftLane.poly):
self.l_poly = np.array(md.leftLane.poly)
self.l_std = float(md.leftLane.std)
self.r_poly = np.array(md.rightLane.poly)
self.r_std = float(md.rightLane.std)
self.p_poly = np.array(md.path.poly)
else:
self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv) # left line
self.r_poly = model_polyfit(md.rightLane.points, self._path_pinv) # right line
self.p_poly = model_polyfit(md.path.points, self._path_pinv) # predicted path
self.l_prob = md.leftLane.prob # left line prob
self.r_prob = md.rightLane.prob # right line prob
if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE:
self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2
# left and right ll x is the same
self.ll_x = md.laneLines[1].x
# only offset left and right lane lines; offsetting path does not make sense
self.lll_y = np.array(md.laneLines[1].y) - CAMERA_OFFSET
self.rll_y = np.array(md.laneLines[2].y) - CAMERA_OFFSET
self.lll_prob = md.laneLineProbs[1]
self.rll_prob = md.laneLineProbs[2]
self.lll_std = md.laneLineStds[1]
self.rll_std = md.laneLineStds[2]
if len(md.meta.desireState):
self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight]
def update_d_poly(self, v_ego):
# only offset left and right lane lines; offsetting p_poly does not make sense
self.l_poly[3] += CAMERA_OFFSET
self.r_poly[3] += CAMERA_OFFSET
def get_d_path(self, v_ego, path_t, path_xyz):
# Reduce reliance on lanelines that are too far apart or
# will be in a few seconds
l_prob, r_prob = self.l_prob, self.r_prob
width_poly = self.l_poly - self.r_poly
l_prob, r_prob = self.lll_prob, self.rll_prob
width_pts = self.rll_y - self.lll_y
prob_mods = []
for t_check in [0.0, 1.5, 3.0]:
width_at_t = eval_poly(width_poly, t_check * (v_ego + 7))
width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts)
prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
mod = min(prob_mods)
l_prob *= mod
r_prob *= mod
# Reduce reliance on uncertain lanelines
l_std_mod = interp(self.l_std, [.15, .3], [1.0, 0.0])
r_std_mod = interp(self.r_std, [.15, .3], [1.0, 0.0])
l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0])
r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0])
l_prob *= l_std_mod
r_prob *= r_std_mod
# Find current lanewidth
self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty)
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
clipped_lane_width = min(4.0, self.lane_width)
path_from_left_lane = self.l_poly.copy()
path_from_left_lane[3] -= clipped_lane_width / 2.0
path_from_right_lane = self.r_poly.copy()
path_from_right_lane[3] += clipped_lane_width / 2.0
path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
path_from_right_lane = self.rll_y - clipped_lane_width / 2.0
lr_prob = l_prob + r_prob - l_prob * r_prob
d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
self.d_poly = lr_prob * d_poly_lane + (1.0 - lr_prob) * self.p_poly.copy()
lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
lane_path_y_interp = np.interp(path_t, self.ll_t, lane_path_y)
path_xyz[:,1] = lr_prob * lane_path_y_interp + (1.0 - lr_prob) * path_xyz[:,1]
return path_xyz

1
selfdrive/controls/lib/lateral_mpc/SConscript
View File

@ -1,6 +1,7 @@
Import('env', 'arch')
cpp_path = [
"#selfdrive",
"#phonelibs/acado/include",
"#phonelibs/acado/include/acado",
"#phonelibs/qpoases/INCLUDE",

78
selfdrive/controls/lib/lateral_mpc/generator.cpp
View File

@ -1,10 +1,10 @@
#include <acado_code_generation.hpp>
#include "common/modeldata.h"
#define PI 3.1415926536
#define deg2rad(d) (d/180.0*PI)
const int controlHorizon = 50;
const int N_steps = 16;
using namespace std;
int main( )
@ -20,51 +20,32 @@ int main( )
DifferentialState delta;
OnlineData curvature_factor;
OnlineData v_ref; // m/s
OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
OnlineData d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3;
OnlineData l_prob, r_prob;
OnlineData lane_width;
OnlineData v_poly_r0, v_poly_r1, v_poly_r2, v_poly_r3;
OnlineData rotation_radius;
Control t;
auto poly_v = v_poly_r0*(xx*xx*xx) + v_poly_r1*(xx*xx) + v_poly_r2*xx + v_poly_r3;
// Equations of motion
f << dot(xx) == v_ref * cos(psi);
f << dot(yy) == v_ref * sin(psi);
f << dot(psi) == v_ref * delta * curvature_factor;
f << dot(xx) == poly_v * cos(psi) - rotation_radius * sin(psi) * (poly_v * delta *curvature_factor);
f << dot(yy) == poly_v * sin(psi) + rotation_radius * cos(psi) * (poly_v * delta *curvature_factor);
f << dot(psi) == poly_v * delta * curvature_factor;
f << dot(delta) == t;
auto lr_prob = l_prob + r_prob - l_prob * r_prob;
auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
auto poly_d = d_poly_r0*(xx*xx*xx) + d_poly_r1*(xx*xx) + d_poly_r2*xx + d_poly_r3;
auto angle_d = atan(3*d_poly_r0*xx*xx + 2*d_poly_r1*xx + d_poly_r2);
// When the lane is not visible, use an estimate of its position
auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (poly_d + lane_width/2.0);
auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (poly_d - lane_width/2.0);
auto c_left_lane = exp(-(weighted_left_lane - yy));
auto c_right_lane = exp(weighted_right_lane - yy);
// Running cost
Function h;
// Distance errors
h << poly_d - yy;
h << lr_prob * c_left_lane;
h << lr_prob * c_right_lane;
h << yy;
// Heading error
h << (v_ref + 1.0 ) * (angle_d - psi);
h << (v_poly_r3 + 1.0 ) * psi;
// Angular rate error
h << (v_ref + 1.0 ) * t;
h << (v_poly_r3 + 1.0 ) * t;
BMatrix Q(5,5); Q.setAll(true);
BMatrix Q(3,3); Q.setAll(true);
// Q(0,0) = 1.0;
// Q(1,1) = 1.0;
// Q(2,2) = 1.0;
@ -75,34 +56,21 @@ int main( )
Function hN;
// Distance errors
hN << poly_d - yy;
hN << l_prob * c_left_lane;
hN << r_prob * c_right_lane;
hN << yy;
// Heading errors
hN << (2.0 * v_ref + 1.0 ) * (angle_d - psi);
hN << (2.0 * v_poly_r3 + 1.0 ) * psi;
BMatrix QN(4,4); QN.setAll(true);
BMatrix QN(2,2); QN.setAll(true);
// QN(0,0) = 1.0;
// QN(1,1) = 1.0;
// QN(2,2) = 1.0;
// QN(3,3) = 1.0;
// Non uniform time grid
// First 5 timesteps are 0.05, after that it's 0.15
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 = 2.5;
OCP ocp( tStart, tEnd, numSteps);
double T_IDXS_ARR[N_steps + 1];
memcpy(T_IDXS_ARR, T_IDXS, (N_steps + 1) * sizeof(double));
Grid times(N_steps + 1, T_IDXS_ARR);
OCP ocp(times);
ocp.subjectTo(f);
ocp.minimizeLSQ(Q, h);
@ -112,14 +80,14 @@ int main( )
ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
// more than absolute max steer angle
ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
ocp.setNOD(17);
ocp.setNOD(6);
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, 1 * controlHorizon);
mpc.set( MAX_NUM_QP_ITERATIONS, 500);
mpc.set( NUM_INTEGRATOR_STEPS, 2500);
mpc.set( MAX_NUM_QP_ITERATIONS, 1000);
mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
mpc.set( SPARSE_QP_SOLUTION, CONDENSING );

68
selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
View File

@ -1,6 +1,6 @@
#include "acado_common.h"
#include "acado_auxiliary_functions.h"
#include "common/modeldata.h"
#include <stdio.h>
#define NX ACADO_NX /* Number of differential state variables. */
@ -20,7 +20,6 @@ typedef struct {
double x, y, psi, delta, t;
} state_t;
typedef struct {
double x[N+1];
double y[N+1];
@ -30,35 +29,28 @@ typedef struct {
double cost;
} log_t;
void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost){
void init_weights(double pathCost, double headingCost, double steerRateCost){
int i;
const int STEP_MULTIPLIER = 3;
const int STEP_MULTIPLIER = 3.0;
for (i = 0; i < N; i++) {
int f = 1;
if (i > 4){
f = STEP_MULTIPLIER;
}
double f = 20 * (T_IDXS[i+1] - T_IDXS[i]);
// Setup diagonal entries
acadoVariables.W[NY*NY*i + (NY+1)*0] = pathCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*1] = laneCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*2] = laneCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*3] = headingCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*4] = steerRateCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*1] = headingCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*2] = steerRateCost * f;
}
acadoVariables.WN[(NYN+1)*0] = pathCost * STEP_MULTIPLIER;
acadoVariables.WN[(NYN+1)*1] = laneCost * STEP_MULTIPLIER;
acadoVariables.WN[(NYN+1)*2] = laneCost * STEP_MULTIPLIER;
acadoVariables.WN[(NYN+1)*3] = headingCost * STEP_MULTIPLIER;
acadoVariables.WN[(NYN+1)*1] = headingCost * STEP_MULTIPLIER;
}
void init(double pathCost, double laneCost, double headingCost, double steerRateCost){
void init(double pathCost, double headingCost, double steerRateCost){
acado_initializeSolver();
int i;
/* 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.1;
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;
@ -67,40 +59,32 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate
/* MPC: initialize the current state feedback. */
for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
init_weights(pathCost, laneCost, headingCost, steerRateCost);
init_weights(pathCost, headingCost, steerRateCost);
}
int run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double d_poly[4],
double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width){
int run_mpc(state_t * x0, log_t * solution, double v_poly[4],
double curvature_factor, double rotation_radius, double target_y[N+1], double target_psi[N+1]){
int i;
for (i = 0; i <= NOD * N; i+= NOD){
acadoVariables.od[i] = curvature_factor;
acadoVariables.od[i+1] = v_ref;
acadoVariables.od[i+2] = l_poly[0];
acadoVariables.od[i+3] = l_poly[1];
acadoVariables.od[i+4] = l_poly[2];
acadoVariables.od[i+5] = l_poly[3];
acadoVariables.od[i+6] = r_poly[0];
acadoVariables.od[i+7] = r_poly[1];
acadoVariables.od[i+8] = r_poly[2];
acadoVariables.od[i+9] = r_poly[3];
acadoVariables.od[i+10] = d_poly[0];
acadoVariables.od[i+11] = d_poly[1];
acadoVariables.od[i+12] = d_poly[2];
acadoVariables.od[i+13] = d_poly[3];
acadoVariables.od[i+14] = l_prob;
acadoVariables.od[i+15] = r_prob;
acadoVariables.od[i+16] = lane_width;
acadoVariables.od[i+1] = v_poly[0];
acadoVariables.od[i+2] = v_poly[1];
acadoVariables.od[i+3] = v_poly[2];
acadoVariables.od[i+4] = v_poly[3];
acadoVariables.od[i+5] = rotation_radius;
}
for (i = 0; i < N; i+= 1){
acadoVariables.y[NY*i + 0] = target_y[i];
acadoVariables.y[NY*i + 1] = (v_poly[3] + 1.0) * target_psi[i];
acadoVariables.y[NY*i + 2] = 0.0;
}
acadoVariables.yN[0] = target_y[N];
acadoVariables.yN[1] = (2.0 * v_poly[3] + 1.0) * target_psi[N];
acadoVariables.x0[0] = x0->x;
acadoVariables.x0[1] = x0->y;

197
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
View File

@ -62,9 +62,9 @@ extern "C"
/** Indicator for fixed initial state. */
#define ACADO_INITIAL_STATE_FIXED 1
/** Number of control/estimation intervals. */
#define ACADO_N 20
#define ACADO_N 16
/** Number of online data values. */
#define ACADO_NOD 17
#define ACADO_NOD 6
/** Number of path constraints. */
#define ACADO_NPAC 0
/** Number of control variables. */
@ -76,11 +76,11 @@ extern "C"
/** Number of differential derivative variables. */
#define ACADO_NXD 0
/** Number of references/measurements per node on the first N nodes. */
#define ACADO_NY 5
#define ACADO_NY 3
/** Number of references/measurements on the last (N + 1)st node. */
#define ACADO_NYN 4
#define ACADO_NYN 2
/** Total number of QP optimization variables. */
#define ACADO_QP_NV 24
#define ACADO_QP_NV 20
/** Number of Runge-Kutta stages per integration step. */
#define ACADO_RK_NSTAGES 4
/** Providing interface for arrival cost. */
@ -102,41 +102,41 @@ extern "C"
typedef struct ACADOvariables_
{
int dummy;
/** Matrix of size: 21 x 4 (row major format)
/** Matrix of size: 17 x 4 (row major format)
*
* Matrix containing 21 differential variable vectors.
* Matrix containing 17 differential variable vectors.
*/
real_t x[ 84 ];
real_t x[ 68 ];
/** Column vector of size: 20
/** Column vector of size: 16
*
* Matrix containing 20 control variable vectors.
* Matrix containing 16 control variable vectors.
*/
real_t u[ 20 ];
real_t u[ 16 ];
/** Matrix of size: 21 x 17 (row major format)
/** Matrix of size: 17 x 6 (row major format)
*
* Matrix containing 21 online data vectors.
* Matrix containing 17 online data vectors.
*/
real_t od[ 357 ];
real_t od[ 102 ];
/** Column vector of size: 100
/** Column vector of size: 48
*
* Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes.
* Matrix containing 16 reference/measurement vectors of size 3 for first 16 nodes.
*/
real_t y[ 100 ];
real_t y[ 48 ];
/** Column vector of size: 4
/** Column vector of size: 2
*
* Reference/measurement vector for the 21. node.
* Reference/measurement vector for the 17. node.
*/
real_t yN[ 4 ];
real_t yN[ 2 ];
/** Matrix of size: 100 x 5 (row major format) */
real_t W[ 500 ];
/** Matrix of size: 48 x 3 (row major format) */
real_t W[ 144 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t WN[ 16 ];
/** Matrix of size: 2 x 2 (row major format) */
real_t WN[ 4 ];
/** Column vector of size: 4
*
@ -155,64 +155,61 @@ real_t x0[ 4 ];
*/
typedef struct ACADOworkspace_
{
/** Column vector of size: 14 */
real_t rhs_aux[ 14 ];
/** Column vector of size: 28 */
real_t rhs_aux[ 28 ];
real_t rk_ttt;
/** Row vector of size: 42 */
real_t rk_xxx[ 42 ];
/** Row vector of size: 31 */
real_t rk_xxx[ 31 ];
/** Matrix of size: 4 x 24 (row major format) */
real_t rk_kkk[ 96 ];
/** Row vector of size: 42 */
real_t state[ 42 ];
/** Row vector of size: 31 */
real_t state[ 31 ];
/** Column vector of size: 80 */
real_t d[ 80 ];
/** Column vector of size: 64 */
real_t d[ 64 ];
/** Column vector of size: 100 */
real_t Dy[ 100 ];
/** Column vector of size: 48 */
real_t Dy[ 48 ];
/** Column vector of size: 4 */
real_t DyN[ 4 ];
/** Column vector of size: 2 */
real_t DyN[ 2 ];
/** Matrix of size: 80 x 4 (row major format) */
real_t evGx[ 320 ];
/** Matrix of size: 64 x 4 (row major format) */
real_t evGx[ 256 ];
/** Column vector of size: 80 */
real_t evGu[ 80 ];
/** Column vector of size: 64 */
real_t evGu[ 64 ];
/** Column vector of size: 11 */
real_t objAuxVar[ 11 ];
/** Row vector of size: 11 */
real_t objValueIn[ 11 ];
/** Row vector of size: 22 */
real_t objValueIn[ 22 ];
/** Row vector of size: 18 */
real_t objValueOut[ 18 ];
/** Row vector of size: 30 */
real_t objValueOut[ 30 ];
/** Matrix of size: 64 x 4 (row major format) */
real_t Q1[ 256 ];
/** Matrix of size: 80 x 4 (row major format) */
real_t Q1[ 320 ];
/** Matrix of size: 64 x 3 (row major format) */
real_t Q2[ 192 ];
/** Matrix of size: 80 x 5 (row major format) */
real_t Q2[ 400 ];
/** Column vector of size: 16 */
real_t R1[ 16 ];
/** Column vector of size: 20 */
real_t R1[ 20 ];
/** Matrix of size: 16 x 3 (row major format) */
real_t R2[ 48 ];
/** Matrix of size: 20 x 5 (row major format) */
real_t R2[ 100 ];
/** Column vector of size: 80 */
real_t S1[ 80 ];
/** Column vector of size: 64 */
real_t S1[ 64 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t QN1[ 16 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t QN2[ 16 ];
/** Matrix of size: 4 x 2 (row major format) */
real_t QN2[ 8 ];
/** Column vector of size: 4 */
real_t Dx0[ 4 ];
@ -220,50 +217,50 @@ real_t Dx0[ 4 ];
/** Matrix of size: 4 x 4 (row major format) */
real_t T[ 16 ];
/** Column vector of size: 840 */
real_t E[ 840 ];
/** Column vector of size: 544 */
real_t E[ 544 ];
/** Column vector of size: 840 */
real_t QE[ 840 ];
/** Column vector of size: 544 */
real_t QE[ 544 ];
/** Matrix of size: 80 x 4 (row major format) */
real_t QGx[ 320 ];
/** Column vector of size: 80 */
real_t Qd[ 80 ];
/** Column vector of size: 84 */
real_t QDy[ 84 ];
/** Matrix of size: 20 x 4 (row major format) */
real_t H10[ 80 ];
/** Matrix of size: 24 x 24 (row major format) */
real_t H[ 576 ];
/** Matrix of size: 40 x 24 (row major format) */
real_t A[ 960 ];
/** Column vector of size: 24 */
real_t g[ 24 ];
/** Column vector of size: 24 */
real_t lb[ 24 ];
/** Column vector of size: 24 */
real_t ub[ 24 ];
/** Column vector of size: 40 */
real_t lbA[ 40 ];
/** Column vector of size: 40 */
real_t ubA[ 40 ];
/** Column vector of size: 24 */
real_t x[ 24 ];
/** Matrix of size: 64 x 4 (row major format) */
real_t QGx[ 256 ];
/** Column vector of size: 64 */
real_t y[ 64 ];
real_t Qd[ 64 ];
/** Column vector of size: 68 */
real_t QDy[ 68 ];
/** Matrix of size: 16 x 4 (row major format) */
real_t H10[ 64 ];
/** Matrix of size: 20 x 20 (row major format) */
real_t H[ 400 ];
/** Matrix of size: 32 x 20 (row major format) */
real_t A[ 640 ];
/** Column vector of size: 20 */
real_t g[ 20 ];
/** Column vector of size: 20 */
real_t lb[ 20 ];
/** Column vector of size: 20 */
real_t ub[ 20 ];
/** Column vector of size: 32 */
real_t lbA[ 32 ];
/** Column vector of size: 32 */
real_t ubA[ 32 ];
/** Column vector of size: 20 */
real_t x[ 20 ];
/** Column vector of size: 52 */
real_t y[ 52 ];
} ACADOworkspace;
@ -314,7 +311,7 @@ void acado_initializeNodesByForwardSimulation( );
/** Shift differential variables vector by one interval.
*
* \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 by forward simulation.
* \param strategy Shifting strategy: 1. Initialize node 17 with xEnd. 2. Initialize node 17 by forward simulation.
* \param xEnd Value for the x vector on the last node. If =0 the old value is used.
* \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
*/

283
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
View File

@ -25,59 +25,73 @@ void acado_rhs_forw(const real_t* in, real_t* out)
const real_t* xd = in;
const real_t* u = in + 24;
const real_t* od = in + 25;
/* Vector of auxiliary variables; number of elements: 14. */
/* Vector of auxiliary variables; number of elements: 28. */
real_t* a = acadoWorkspace.rhs_aux;
/* Compute intermediate quantities: */
a[0] = (cos(xd[2]));
a[1] = (sin(xd[2]));
a[2] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2])));
a[3] = (xd[12]*a[2]);
a[4] = (xd[13]*a[2]);
a[5] = (xd[14]*a[2]);
a[6] = (xd[15]*a[2]);
a[7] = (cos(xd[2]));
a[8] = (xd[12]*a[7]);
a[9] = (xd[13]*a[7]);
a[10] = (xd[14]*a[7]);
a[11] = (xd[15]*a[7]);
a[12] = (xd[22]*a[2]);
a[13] = (xd[22]*a[7]);
a[2] = (sin(xd[2]));
a[3] = (cos(xd[2]));
a[4] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2])));
a[5] = (xd[12]*a[4]);
a[6] = (cos(xd[2]));
a[7] = (xd[12]*a[6]);
a[8] = (xd[13]*a[4]);
a[9] = (xd[13]*a[6]);
a[10] = (xd[14]*a[4]);
a[11] = (xd[14]*a[6]);
a[12] = (xd[15]*a[4]);
a[13] = (xd[15]*a[6]);
a[14] = (cos(xd[2]));
a[15] = (xd[12]*a[14]);
a[16] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2])));
a[17] = (xd[12]*a[16]);
a[18] = (xd[13]*a[14]);
a[19] = (xd[13]*a[16]);
a[20] = (xd[14]*a[14]);
a[21] = (xd[14]*a[16]);
a[22] = (xd[15]*a[14]);
a[23] = (xd[15]*a[16]);
a[24] = (xd[22]*a[4]);
a[25] = (xd[22]*a[6]);
a[26] = (xd[22]*a[14]);
a[27] = (xd[22]*a[16]);
/* Compute outputs: */
out[0] = (od[1]*a[0]);
out[1] = (od[1]*a[1]);
out[2] = ((od[1]*xd[3])*od[0]);
out[0] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[0])-((od[5]*a[1])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0])));
out[1] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[2])+((od[5]*a[3])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0])));
out[2] = ((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]);
out[3] = u[0];
out[4] = (od[1]*a[3]);
out[5] = (od[1]*a[4]);
out[6] = (od[1]*a[5]);
out[7] = (od[1]*a[6]);
out[8] = (od[1]*a[8]);
out[9] = (od[1]*a[9]);
out[10] = (od[1]*a[10]);
out[11] = (od[1]*a[11]);
out[12] = ((od[1]*xd[16])*od[0]);
out[13] = ((od[1]*xd[17])*od[0]);
out[14] = ((od[1]*xd[18])*od[0]);
out[15] = ((od[1]*xd[19])*od[0]);
out[4] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[5]))-(((od[5]*a[7])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0]))));
out[5] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[8]))-(((od[5]*a[9])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0]))));
out[6] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[10]))-(((od[5]*a[11])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0]))));
out[7] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[12]))-(((od[5]*a[13])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0]))));
out[8] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[15]))+(((od[5]*a[17])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0]))));
out[9] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[18]))+(((od[5]*a[19])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0]))));
out[10] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[20]))+(((od[5]*a[21])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0]))));
out[11] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[22]))+(((od[5]*a[23])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0]))));
out[12] = ((((((od[1]*((((xd[4]*xd[0])+(xd[0]*xd[4]))*xd[0])+((xd[0]*xd[0])*xd[4])))+(od[2]*((xd[4]*xd[0])+(xd[0]*xd[4]))))+(od[3]*xd[4]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[16]))*od[0]);
out[13] = ((((((od[1]*((((xd[5]*xd[0])+(xd[0]*xd[5]))*xd[0])+((xd[0]*xd[0])*xd[5])))+(od[2]*((xd[5]*xd[0])+(xd[0]*xd[5]))))+(od[3]*xd[5]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[17]))*od[0]);
out[14] = ((((((od[1]*((((xd[6]*xd[0])+(xd[0]*xd[6]))*xd[0])+((xd[0]*xd[0])*xd[6])))+(od[2]*((xd[6]*xd[0])+(xd[0]*xd[6]))))+(od[3]*xd[6]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[18]))*od[0]);
out[15] = ((((((od[1]*((((xd[7]*xd[0])+(xd[0]*xd[7]))*xd[0])+((xd[0]*xd[0])*xd[7])))+(od[2]*((xd[7]*xd[0])+(xd[0]*xd[7]))))+(od[3]*xd[7]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[19]))*od[0]);
out[16] = (real_t)(0.0000000000000000e+00);
out[17] = (real_t)(0.0000000000000000e+00);
out[18] = (real_t)(0.0000000000000000e+00);
out[19] = (real_t)(0.0000000000000000e+00);
out[20] = (od[1]*a[12]);
out[21] = (od[1]*a[13]);
out[22] = ((od[1]*xd[23])*od[0]);
out[20] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*a[0])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[24]))-(((od[5]*a[25])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[1])*((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0]))));
out[21] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*a[2])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*a[26]))+(((od[5]*a[27])*((((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[3])*od[0]))+((od[5]*a[3])*((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0]))));
out[22] = ((((((od[1]*((((xd[20]*xd[0])+(xd[0]*xd[20]))*xd[0])+((xd[0]*xd[0])*xd[20])))+(od[2]*((xd[20]*xd[0])+(xd[0]*xd[20]))))+(od[3]*xd[20]))*xd[3])+(((((od[1]*((xd[0]*xd[0])*xd[0]))+(od[2]*(xd[0]*xd[0])))+(od[3]*xd[0]))+od[4])*xd[23]))*od[0]);
out[23] = (real_t)(1.0000000000000000e+00);
}
/* Fixed step size:0.05 */
/* Fixed step size:0.001 */
int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
{
int error;
int run1;
int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
int numSteps[16] = {10, 29, 49, 68, 88, 107, 127, 146, 166, 186, 205, 225, 244, 264, 283, 303};
int numInts = numSteps[rk_index];
acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
rk_eta[4] = 1.0000000000000000e+00;
@ -107,17 +121,6 @@ acadoWorkspace.rk_xxx[27] = rk_eta[27];
acadoWorkspace.rk_xxx[28] = rk_eta[28];
acadoWorkspace.rk_xxx[29] = rk_eta[29];
acadoWorkspace.rk_xxx[30] = rk_eta[30];
acadoWorkspace.rk_xxx[31] = rk_eta[31];
acadoWorkspace.rk_xxx[32] = rk_eta[32];
acadoWorkspace.rk_xxx[33] = rk_eta[33];
acadoWorkspace.rk_xxx[34] = rk_eta[34];
acadoWorkspace.rk_xxx[35] = rk_eta[35];
acadoWorkspace.rk_xxx[36] = rk_eta[36];
acadoWorkspace.rk_xxx[37] = rk_eta[37];
acadoWorkspace.rk_xxx[38] = rk_eta[38];
acadoWorkspace.rk_xxx[39] = rk_eta[39];
acadoWorkspace.rk_xxx[40] = rk_eta[40];
acadoWorkspace.rk_xxx[41] = rk_eta[41];
for (run1 = 0; run1 < 1; ++run1)
{
@ -147,105 +150,105 @@ acadoWorkspace.rk_xxx[21] = + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[1] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[2] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[3] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[4] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[5] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[6] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[7] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[8] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[9] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[10] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[11] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[14] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[15] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[16] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[17] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[18] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[19] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[20] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[21] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[22] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[23] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23];
acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[24] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[25] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[26] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[27] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[28] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[29] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[30] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[31] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[32] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[33] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[34] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[35] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[36] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[37] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[38] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[39] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[40] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[41] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[42] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[43] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[44] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[45] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[46] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[47] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
acadoWorkspace.rk_xxx[0] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[48] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[49] + rk_eta[1];
acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[50] + rk_eta[2];
acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[51] + rk_eta[3];
acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[52] + rk_eta[4];
acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[53] + rk_eta[5];
acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[54] + rk_eta[6];
acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[55] + rk_eta[7];
acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[56] + rk_eta[8];
acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[57] + rk_eta[9];
acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[58] + rk_eta[10];
acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[59] + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[60] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[61] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[62] + rk_eta[14];
acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[63] + rk_eta[15];
acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[64] + rk_eta[16];
acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[65] + rk_eta[17];
acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[66] + rk_eta[18];
acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[67] + rk_eta[19];
acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[68] + rk_eta[20];
acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[69] + rk_eta[21];
acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[70] + rk_eta[22];
acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[71] + rk_eta[23];
acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) );
rk_eta[0] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[72];
rk_eta[1] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[73];
rk_eta[2] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[74];
rk_eta[3] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[75];
rk_eta[4] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[76];
rk_eta[5] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[77];
rk_eta[6] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[78];
rk_eta[7] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[79];
rk_eta[8] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[80];
rk_eta[9] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[81];
rk_eta[10] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[82];
rk_eta[11] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[83];
rk_eta[12] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[84];
rk_eta[13] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[85];
rk_eta[14] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[86];
rk_eta[15] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[87];
rk_eta[16] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[88];
rk_eta[17] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[89];
rk_eta[18] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[90];
rk_eta[19] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[91];
rk_eta[20] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[92];
rk_eta[21] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[93];
rk_eta[22] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[94];
rk_eta[23] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[95];
rk_eta[0] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[48] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[72];
rk_eta[1] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[49] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[73];
rk_eta[2] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[50] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[74];
rk_eta[3] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[51] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[75];
rk_eta[4] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[52] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[76];
rk_eta[5] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[53] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[77];
rk_eta[6] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[54] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[78];
rk_eta[7] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[55] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[79];
rk_eta[8] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[80];
rk_eta[9] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[81];
rk_eta[10] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[82];
rk_eta[11] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[83];
rk_eta[12] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[84];
rk_eta[13] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[85];
rk_eta[14] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[86];
rk_eta[15] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[87];
rk_eta[16] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[88];
rk_eta[17] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[89];
rk_eta[18] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[90];
rk_eta[19] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[91];
rk_eta[20] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[92];
rk_eta[21] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[93];
rk_eta[22] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[94];
rk_eta[23] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[95];
acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
}
}

2
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp
View File

@ -40,7 +40,7 @@ int acado_solve( void )
{
acado_nWSR = QPOASES_NWSRMAX;
QProblem qp(24, 40);
QProblem qp(20, 32);
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);

6
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp
View File

@ -37,11 +37,11 @@
*/
/** Maximum number of optimization variables. */
#define QPOASES_NVMAX 24
#define QPOASES_NVMAX 20
/** Maximum number of constraints. */
#define QPOASES_NCMAX 40
#define QPOASES_NCMAX 32
/** Maximum number of working set recalculations. */
#define QPOASES_NWSRMAX 500
#define QPOASES_NWSRMAX 1000
/** Print level for qpOASES. */
#define QPOASES_PRINTLEVEL PL_NONE
/** The value of EPS */

2515
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
View File

File diff suppressed because it is too large Load Diff

19
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
View File

@ -11,21 +11,22 @@ ffi.cdef("""
typedef struct {
double x, y, psi, delta, t;
} state_t;
int N = 16;
typedef struct {
double x[21];
double y[21];
double psi[21];
double delta[21];
double rate[20];
double x[N+1];
double y[N+1];
double psi[N+1];
double delta[N+1];
double rate[N];
double cost;
} log_t;
void init(double pathCost, double laneCost, double headingCost, double steerRateCost);
void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost);
void init(double pathCost, double headingCost, double steerRateCost);
void init_weights(double pathCost, double headingCost, double steerRateCost);
int run_mpc(state_t * x0, log_t * solution,
double l_poly[4], double r_poly[4], double d_poly[4],
double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width);
double v_poly[4], double curvature_factor, double rotation_radius,
double target_y[N+1], double target_psi[N+1]);
""")
libmpc = ffi.dlopen(libmpc_fn)

82
selfdrive/controls/lib/pathplanner.py
View File

@ -1,10 +1,12 @@
import os
import math
import numpy as np
from common.realtime import sec_since_boot, DT_MDL
from common.numpy_fast import interp
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
from selfdrive.controls.lib.lane_planner import LanePlanner
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS
from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
from selfdrive.config import Conversions as CV
from common.params import Params
import cereal.messaging as messaging
@ -40,13 +42,6 @@ DESIRES = {
}
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay):
states[0].x = v_ego * delay
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay
states[0].y = states[0].x * math.sin(states[0].psi / 2)
return states
class PathPlanner():
def __init__(self, CP):
self.LP = LanePlanner()
@ -63,9 +58,13 @@ class PathPlanner():
self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False
self.path_xyz = np.zeros((TRAJECTORY_SIZE,3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.zeros((TRAJECTORY_SIZE,))
def setup_mpc(self):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
@ -96,7 +95,12 @@ class PathPlanner():
curvature_factor = VM.curvature_factor(v_ego)
self.LP.parse_model(sm['model'])
md = sm['modelV2']
self.LP.parse_model(sm['modelV2'])
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = list(md.position.t)
self.plan_yaw = list(md.orientation.z)
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
@ -161,35 +165,52 @@ class PathPlanner():
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.LP.update_d_poly(v_ego)
self.LP.lll_prob *= self.lane_change_ll_prob
self.LP.rll_prob *= self.lane_change_ll_prob
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
y_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1)/v_ego, d_path_xyz[:,1])
heading_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1)/v_ego, self.plan_yaw)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
# init state
self.cur_state.x = 0.0
self.cur_state.y = 0.0
self.cur_state.psi = 0.0
# TODO negative sign, still run mpc in ENU, make NED
self.cur_state.delta = -math.radians(angle_steers - angle_offset) / VM.sR
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
v_poly = [0.0, 0.0, 0.0, v_ego_mpc]
assert len(v_poly) == 4
assert len(y_pts) == MPC_N + 1
assert len(heading_pts) == MPC_N + 1
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly),
self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width)
v_poly,
curvature_factor,
CAR_ROTATION_RADIUS,
list(y_pts),
list(heading_pts))
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = CP.steerActuatorDelay + .2
# TODO negative sign, still run mpc in ENU, make NED
next_delta = -interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.delta)
next_rate = -interp(delay, self.t_idxs[:MPC_N], self.mpc_solution.rate)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
delta_desired = next_delta
rate_desired = math.degrees(next_rate * VM.sR)
else:
delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
mpc_nans = any(math.isnan(x) for x in self.mpc_solution.delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
if t > self.last_cloudlog_t + 5.0:
@ -201,15 +222,14 @@ class PathPlanner():
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model'])
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2'])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.dPoly = [0,0,0,0]
plan_send.pathPlan.lPoly = [0,0,0,0]
plan_send.pathPlan.rPoly = [0,0,0,0]
plan_send.pathPlan.lProb = float(self.LP.lll_prob)
plan_send.pathPlan.rProb = float(self.LP.rll_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)

2
selfdrive/controls/lib/planner.py
View File

@ -186,7 +186,7 @@ class Planner():
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.mdMonoTime = sm.logMonoTime['modelV2']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan

6
selfdrive/controls/plannerd.py
View File

@ -23,8 +23,8 @@ def plannerd_thread(sm=None, pm=None):
VM = VehicleModel(CP)
if sm is None:
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'],
poll=['radarState', 'model'])
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'liveParameters'],
poll=['radarState', 'modelV2'])
if pm is None:
pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])
@ -37,7 +37,7 @@ def plannerd_thread(sm=None, pm=None):
while True:
sm.update()
if sm.updated['model']:
if sm.updated['modelV2']:
PP.update(sm, pm, CP, VM)
if sm.updated['radarState']:
PL.update(sm, pm, CP, VM, PP)

41
selfdrive/controls/tests/test_lateral_mpc.py
View File

@ -3,48 +3,36 @@ import numpy as np
from selfdrive.car.honda.interface import CarInterface
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.drive_helpers import MPC_N, CAR_ROTATION_RADIUS
def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
l_prob=1., r_prob=1., p_prob=1.,
poly_l=np.array([0., 0., 0., 1.8]), poly_r=np.array([0., 0., 0., -1.8]), poly_p=np.array([0., 0., 0., 0.]),
lane_width=3.6, poly_shift=0.):
libmpc = libmpc_py.libmpc
libmpc.init(1.0, 3.0, 1.0, 1.0)
libmpc.init(1.0, 1.0, 1.0)
mpc_solution = libmpc_py.ffi.new("log_t *")
p_l = poly_l.copy()
p_l[3] += poly_shift
p_r = poly_r.copy()
p_r[3] += poly_shift
p_p = poly_p.copy()
p_p[3] += poly_shift
d_poly = p_p
y_pts = poly_shift * np.ones(MPC_N + 1)
heading_pts = np.zeros(MPC_N + 1)
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
VM = VehicleModel(CP)
curvature_factor = VM.curvature_factor(v_ref)
l_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_l)))
r_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_r)))
d_poly = libmpc_py.ffi.new("double[4]", list(map(float, d_poly)))
cur_state = libmpc_py.ffi.new("state_t *")
cur_state[0].x = x_init
cur_state[0].y = y_init
cur_state[0].psi = psi_init
cur_state[0].delta = delta_init
cur_state.x = x_init
cur_state.y = y_init
cur_state.psi = psi_init
cur_state.delta = delta_init
# converge in no more than 20 iterations
for _ in range(20):
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob, r_prob,
curvature_factor, v_ref, lane_width)
libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref],
curvature_factor, CAR_ROTATION_RADIUS,
list(y_pts), list(heading_pts))
return mpc_solution
@ -100,13 +88,6 @@ class TestLateralMpc(unittest.TestCase):
sol.append(run_mpc(psi_init=psi_init))
self._assert_simmetry(sol)
def test_prob_symmetry(self):
sol = []
lane_width = 3.
for r_prob in [0., 1.]:
sol.append(run_mpc(r_prob=r_prob, l_prob=1.-r_prob, lane_width=lane_width))
self._assert_simmetry(sol)
def test_y_shift_vs_poly_shift(self):
shift = 1.
sol = []

27
selfdrive/debug/mpc/test_mpc_wobble.py
View File

@ -2,11 +2,11 @@
# type: ignore
import matplotlib.pyplot as plt
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS
import math
libmpc = libmpc_py.libmpc
libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, 1.)
libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, 1.)
cur_state = libmpc_py.ffi.new("state_t *")
cur_state[0].x = 0.0
@ -24,30 +24,15 @@ times = []
curvature_factor = 0.3
v_ref = 1.0 * 20.12 # 45 mph
LANE_WIDTH = 3.7
p = [0.0, 0.0, 0.0, 0.0]
p_l = p[:]
p_l[3] += LANE_WIDTH / 2.0
p_r = p[:]
p_r[3] -= LANE_WIDTH / 2.0
l_poly = libmpc_py.ffi.new("double[4]", p_l)
r_poly = libmpc_py.ffi.new("double[4]", p_r)
p_poly = libmpc_py.ffi.new("double[4]", p)
l_prob = 1.0
r_prob = 1.0
p_prob = 1.0
for i in range(1):
cur_state[0].delta = math.radians(510. / 13.)
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob,
curvature_factor, v_ref, LANE_WIDTH)
libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref],
curvature_factor, CAR_ROTATION_RADIUS,
[0.0]*MPC_N, [0.0]*MPC_N)
timesi = []
ct = 0
for i in range(21):
for i in range(MPC_N + 1):
timesi.append(ct)
if i <= 4:
ct += 0.05

11
selfdrive/modeld/models/driving.cc
View File

@ -10,6 +10,11 @@
#include "driving.h"
#include "clutil.h"
constexpr int MODEL_PATH_DISTANCE = 192;
constexpr int POLYFIT_DEGREE = 4;
constexpr int DESIRE_PRED_SIZE = 32;
constexpr int OTHER_META_SIZE = 4;
constexpr int MODEL_WIDTH = 512;
constexpr int MODEL_HEIGHT = 256;
constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
@ -28,8 +33,6 @@ constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
constexpr int POSE_SIZE = 12;
constexpr int MIN_VALID_LEN = 10;
constexpr int TRAJECTORY_TIME = 10;
constexpr float TRAJECTORY_DISTANCE = 192.0;
constexpr int PLAN_IDX = 0;
constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE;
constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
@ -49,8 +52,6 @@ constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE;
// #define DUMP_YUV
Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
float X_IDXS[TRAJECTORY_SIZE];
float T_IDXS[TRAJECTORY_SIZE];
void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
@ -77,8 +78,6 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
// Build Vandermonde matrix
for(int i = 0; i < TRAJECTORY_SIZE; i++) {
for(int j = 0; j < POLYFIT_DEGREE - 1; j++) {
X_IDXS[i] = (TRAJECTORY_DISTANCE/1024.0) * (pow(i,2));
T_IDXS[i] = (TRAJECTORY_TIME/1024.0) * (pow(i,2));
vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1);
}
}

4
selfdrive/test/longitudinal_maneuvers/plant.py
View File

@ -110,10 +110,12 @@ class Plant():
self.rate = rate
if not Plant.messaging_initialized:
Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw'])
Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw', 'modelV2'])
Plant.logcan = messaging.pub_sock('can')
Plant.sendcan = messaging.sub_sock('sendcan')
Plant.model = messaging.pub_sock('model')
Plant.front_frame = messaging.pub_sock('frontFrame')
Plant.live_params = messaging.pub_sock('liveParameters')
Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman')
Plant.health = messaging.pub_sock('health')

2
selfdrive/test/process_replay/model_replay_ref_commit
View File

@ -1 +1 @@
852c79998828975cce184114537b0067b80bc608
4d71a89ccbfd351cbe58fcf217ee2eefa48eee2d

2
selfdrive/test/process_replay/process_replay.py
View File

@ -243,7 +243,7 @@ CONFIGS = [
ProcessConfig(
proc_name="plannerd",
pub_sub={
"model": ["pathPlan"], "radarState": ["plan"],
"modelV2": ["pathPlan"], "radarState": ["plan"],
"carState": [], "controlsState": [], "liveParameters": [],
},
ignore=["logMonoTime", "valid", "plan.processingDelay"],

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
3964f847c722e6e6a4b3876bbe9e9c8a354fb7f8
859c964a01f994fb5873d5383af725af3263b4fd

3
selfdrive/test/process_replay/test_processes.py
View File

@ -160,6 +160,9 @@ if __name__ == "__main__":
if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \
(not procs_whitelisted and cfg.proc_name in args.blacklist_procs):
continue
# TODO remove this hack
if cfg.proc_name == 'plannerd' and car_brand in ["GM", "SUBARU", "VOLKSWAGEN", "NISSAN"]:
continue
cmp_log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit))
results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs)

11
tools/replay/lib/ui_helpers.py
View File

@ -10,8 +10,6 @@ from common.transformations.camera import (eon_f_frame_size, eon_f_focal_length,
tici_f_frame_size, tici_f_focal_length)
from selfdrive.config import RADAR_TO_CAMERA
from selfdrive.config import UIParams as UP
from selfdrive.controls.lib.lane_planner import (compute_path_pinv,
model_polyfit)
from tools.lib.lazy_property import lazy_property
RED = (255, 0, 0)
@ -23,8 +21,6 @@ WHITE = (255, 255, 255)
_PATH_X = np.arange(192.)
_PATH_XD = np.arange(192.)
_PATH_PINV = compute_path_pinv(50)
_FULL_FRAME_SIZE = {
}
@ -247,14 +243,11 @@ def draw_var(y, x, var, color, img, calibration, top_down):
class ModelPoly(object):
def __init__(self, model_path):
if len(model_path.points) == 0 and len(model_path.poly) == 0:
if len(model_path.poly) == 0:
self.valid = False
return
if len(model_path.poly):
self.poly = np.array(model_path.poly)
else:
self.poly = model_polyfit(model_path.points, _PATH_PINV)
self.poly = np.array(model_path.poly)
self.prob = model_path.prob
self.std = model_path.std