Skip to content

Commit

Permalink
Revert "Add MPC from 0.5.13 (hybrid from @Gernby) (commaai#184)"
Browse files Browse the repository at this point in the history 
This reverts commit 9c5abe3.
  • Loading branch information
Comma Device committed Jul 8, 2019
1 parent b323587 commit 5b95bbc
Show file tree
Hide file tree
Showing 5 changed files with 33 additions and 28 deletions.
8 changes: 5 additions & 3 deletions selfdrive/controls/lib/lateral_mpc/Makefile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,13 @@ QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONEL
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado

ifeq ($(UNAME_S),Darwin)
ACADO_LIBS := -lacado_toolkit_s
ACADO_LIBS := -lacado_toolkit_s
else ifeq ($(UNAME_M),aarch64)
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
else
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
endif

OBJS = \
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/lib/lateral_mpc/generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ int main( )
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_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;

auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
Expand Down
25 changes: 14 additions & 11 deletions selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ typedef struct {
double y[N+1];
double psi[N+1];
double delta[N+1];
double rate[N+1];
double rate[N];
double cost;
} log_t;

Expand All @@ -51,16 +51,17 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate
if (i > 4){
f = STEP_MULTIPLIER;
}
acadoVariables.W[25 * i + 0] = pathCost * f;
acadoVariables.W[25 * i + 6] = laneCost * f;
acadoVariables.W[25 * i + 12] = laneCost * f;
acadoVariables.W[25 * i + 18] = headingCost * f;
acadoVariables.W[25 * i + 24] = steerRateCost * f;
// 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.WN[0] = pathCost * STEP_MULTIPLIER;
acadoVariables.WN[5] = laneCost * STEP_MULTIPLIER;
acadoVariables.WN[10] = laneCost * STEP_MULTIPLIER;
acadoVariables.WN[15] = headingCost * STEP_MULTIPLIER;
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;
}

int run_mpc(state_t * x0, log_t * solution,
Expand Down Expand Up @@ -113,7 +114,9 @@ int run_mpc(state_t * x0, log_t * solution,
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
solution->rate[i] = acadoVariables.u[i];
if (i < N){
solution->rate[i] = acadoVariables.u[i];
}
}
solution->cost = acado_getObjective();

Expand Down
24 changes: 12 additions & 12 deletions selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,19 +103,19 @@ const real_t* od = in + 5;
real_t* a = acadoWorkspace.objAuxVar;

/* Compute intermediate quantities: */
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[1] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[7]);
a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[11] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[12] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[11]);
a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
Expand Down Expand Up @@ -166,19 +166,19 @@ const real_t* od = in + 4;
real_t* a = acadoWorkspace.objAuxVar;

/* Compute intermediate quantities: */
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[1] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[7]);
a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[11] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[12] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[11]);
a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/lib/lateral_mpc/libmpc_py.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
double y[21];
double psi[21];
double delta[21];
double rate[21];
double rate[20];
double cost;
} log_t;
Expand Down

0 comments on commit 5b95bbc

Please sign in to comment.