MPC - Cost Function

[Prashin Sharma]

Hi all,
I developed a tracking MPC controller for my aerial vehicle from ACADO, and used code generation for porting it to my embeded platform. The overall process was very easy, however I am confused with the following :

1) While using just the toolkit you can define the cost function of the optimal control problem as the follows :

ocp.minimizeLSQ(Q,h,ref)
ocp.minimizeLSQEndTerm(Q,h,ref)

with Q - weighting matrix
h - states
ref - reference for the states

2) While code generation you need to have the cost function without ref i.e.

ocp.minimizeLSQ(Q,h)
ocp.minimizeLSQEndTerm(Q,h)

So I am not sure how to provide reference in the c code obtained from code generation. Any suggestions and recommendations would be appreciated.

Thanking you

[Rien Quirynen]

Hi,

You should have a look at the generated header file, which is typically called 'acado_common.h'.
This header file defines a struct called acadoVariables with all the variables that should be specified in order for the MPC controller to work, including the reference trajectory:

/ The structure containing the user data.
* Via this structure the user "communicates" with the solver code.
/
typedef struct ACADOvariables_
{
/ Matrix of size: 11 x 8 (row major format)
* Matrix containing 11 differential variable vectors.
/
real_t x[ 88 ];

/ Matrix of size: 10 x 2 (row major format)
* Matrix containing 10 control variable vectors.
/
real_t u[ 20 ];

/ Column vector of size: 100
* Matrix containing 10 reference/measurement vectors of size 10 for first 10 nodes.
/
real_t y[ 100 ];

/ Column vector of size: 8
* Reference/measurement vector for the 11. node.
/
real_t yN[ 8 ];

/ Matrix of size: 10 x 10 (row major format) /
real_t W[ 100 ];

/ Matrix of size: 8 x 8 (row major format) /
real_t WN[ 64 ];

/ Column vector of size: 8
* Current state feedback vector.
/
real_t x0[ 8 ];

} ACADOvariables;

So the "y" vector contains the reference values over the control horizon, while "yN" are the reference values on the terminal node. Similarly, "W" is the weighting matrix over the control horizon, while "WN" are the weighting values on the terminal node, etc.

[Prashin Sharma]

Hi Rein,

Thank you for your reply, with that understanding of reference, I generated c code using code generation and wrote a small hexacopter_test program similar to auto generated test.c . In the program , I am trying to simulate the hexacopter to got 1m height which in the program is at :
acadoVariables.y[10] = 1.0

and I run the program with initial values set to zero. But I get the control inputs as zero, not sure why. I have attached the code. Any suggestion would be appreciated.

Thanks again

[Rien Quirynen]

But you set the value to 1.0 only for one time point then? You should provide a reference trajectory for the full control horizon instead (based on the time discretization that you specified when using the ACADO code generation tool). For example, you could provide the setpoint of 1m for the full MPC control horizon, including the final reference value that is defined in "acadoVariables.yN"

[Prashin Sharma]

So, I do set for y for all the time horizon, code snippet below:

for(i = 0; i<N + 1 ; ++i)
{
acadoVariables.x[i * NX +0 ] = 0.0;
acadoVariables.x[i * NX +1 ] = 0.0;
acadoVariables.x[i * NX +2 ] = 0.0;
acadoVariables.x[i * NX +3 ] = 0.0;
acadoVariables.x[i * NX +4 ] = 0.0;
acadoVariables.x[i * NX +5 ] = 0.0;
acadoVariables.x[i * NX +6 ] = 0.0;
acadoVariables.x[i * NX +7 ] = 0.0;
acadoVariables.x[i * NX +8 ] = 0.0;
acadoVariables.x[i * NX +9 ] = 0.0;
acadoVariables.x[i * NX +10 ] = 0.0;
acadoVariables.x[i * NX +11 ] = 0.0;
}

//Prepare reference 
//
for(i = 0; i<N ; ++i)
{
    acadoVariables.y[i * NY +0 ] = 0.0;
    acadoVariables.y[i * NY +1 ] = 0.0;
    acadoVariables.y[i * NY +2 ] = 0.0;
    acadoVariables.y[i * NY +3 ] = 0.0;
    acadoVariables.y[i * NY +4 ] = 0.0;
    acadoVariables.y[i * NY +5 ] = 0.0;
    acadoVariables.y[i * NY +6 ] = 0.0;
    acadoVariables.y[i * NY +7 ] = 0.0;
    acadoVariables.y[i * NY +8 ] = 0.0;
    acadoVariables.y[i * NY +9 ] = 0.0;
    acadoVariables.y[i * NY +10 ] = 1.0;
    acadoVariables.y[i * NY +11 ] = 0.0;
}

acadoVariables.yN[0] = 0.0;
acadoVariables.yN[1] = 0.0;
acadoVariables.yN[2] = 0.0;
acadoVariables.yN[3] = 0.0;
acadoVariables.yN[4] = 0.0;
acadoVariables.yN[5] = 0.0;
acadoVariables.yN[6] = 0.0;
acadoVariables.yN[7] = 0.0;
acadoVariables.yN[8] = 0.0;
acadoVariables.yN[9] = 0.0;
acadoVariables.yN[10] = 1.0;
acadoVariables.yN[11] = 0.0;

// Current State Feedback
cout<<"Initial State Feedback"<<endl;
for(i<0; i<NX ; ++i)
{
    acadoVariables.x0[i] = acadoVariables.x[i] ; 
    cout<<acadoVariables.x0[i];
}

acado_initializeSolver();
//Warm-up Solver
acado_preparationStep();

for(iter = 0 ; iter <NUM_STEPS; iter++)
{
    acado_tic(&t);
    status = acado_feedbackStep();

    cout<<"Control : "<<acadoVariables.u[0]<<" "<<acadoVariables.u[1]<<" "<<acadoVariables.u[2]<<" "<<acadoVariables.u[3]<<" "<<acadoVariables.u[4]<<" "<<acadoVariables.u[5]<<endl;
    t2 = acado_toc(&t);

        // Ideal feedback signal
    cout<<"Feedback Signal "<<endl;
    for(i = 0 ; i<NX ; ++i)
    {
        acadoVariables.x0[i] = acadoVariables.x[NX +i ];
        cout<<acadoVariables.x0[i]<<endl;
    }

    acado_tic(&t);
    acado_preparationStep();
    t1 = acado_toc(&t);

    prepSum+= t1;
    fbdSum+= t2;

}

And below is the code used to generate it :
//Introduce the variables

DifferentialState p,q,r ;
DifferentialState phi,theta,psi ;
DifferentialState x,y,z ;
DifferentialState xdot, ydot, zdot;

Control T1,T2,T3,T4,T5,T6 ;

const double Jx = 0.0469;
const double Jy = 0.0358; 
const double Jz = 0.101;
const double L = 0.225 ; // m 
const double m = 1.035 ; //Kg
const double g = 9.81 ; // m/s^2 
const double Ct = 1.9693e-08; // N/RPM^2
const double Cq = 2.1374e-10; // Nm/RPM^2
const double gamma = Cq/Ct ;

// Differential Equation

DifferentialEquation f;

f<<dot(p) == ((Jy*q*r-Jz*q*r) - L*T1/2 + L*T2/2 + L*T3 + L*T4/2 - L*T5/2 - L*T6)/Jx ;
f<<dot(q) == (sqrt(3)*L*T4/2 - sqrt(3)*L*T1/2 -sqrt(3)*L*T2/2 + sqrt(3)*L*T5/2 - (Jx*p*r - Jz*p*r))/Jy;
f<<dot(r) == (T2*gamma - T1*gamma - T3*gamma + T4*gamma - T5*gamma + T6*gamma + (Jx*p*q-Jy*p*q))/Jz ;

f<<dot(phi) == cos(theta)*p + sin(theta)*r ; 
f<<dot(theta) == sin(theta)*tan(phi)*p + q - cos(theta)*tan(phi)*r;
f<<dot(psi) == -sin(theta)/cos(phi)*p -cos(theta)/cos(phi)*r;
f<<dot(x) == xdot; 
f<<dot(y) == ydot; 
f<<dot(z) == zdot;
f<<dot(xdot) == (cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi))*(T1+T2+T3+T4+T5+T6)/m;
f<<dot(ydot) == (sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi))*(T1+T2+T3+T4+T5+T6)/m;
f<<dot(zdot) == (cos(phi)*cos(theta))*(T1+T2+T3+T4+T5+T6)/m -g;

// State Variable and weighting matrix

Function h,hN ;
h << p << q << r << phi << theta << psi << x << xdot << y << ydot << z << zdot ;
hN << p << q << r << phi << theta << psi << x << xdot << y << ydot << z << zdot ;

DMatrix Q(12,12);
Q(0,0) = 100; Q(1,1) = 100; Q(2,2) = 100;
Q(3,3) = 10; Q(4,4) = 10; Q(5,5) = 10;
Q(6,6) = 50; Q(7,7) = 10; Q(8,8) = 50;
Q(9,9) = 10; Q(10,10)= 50; Q(11,11) = 10;


DMatrix R(12,12);
R(0,0) = 100; R(1,1) = 100; R(2,2) = 100;
R(3,3) = 10; R(4,4) = 10; R(5,5) = 10;
R(6,6) = 50; R(7,7) = 10; R(8,8) = 50;
R(9,9) = 10; R(10,10)= 50; R(11,11) = 10;

const double tStart = 0.0;
const double tEnd =1.0 ;
const double tHorizon =10; // 10

OCP ocp(tStart , tEnd , tHorizon);
ocp.minimizeLSQ(Q,h);
ocp.minimizeLSQEndTerm(R,h);

ocp.subjectTo( f );         // OCP subject to dynamics
ocp.subjectTo(0 <= T1 <= 6); // OCP subject to actuator constraints max Thrust = 6N
ocp.subjectTo(0 <= T2 <= 6);
ocp.subjectTo(0 <= T3 <= 6);
ocp.subjectTo(0 <= T3 <= 6);
ocp.subjectTo(0 <= T4 <= 6);
ocp.subjectTo(0 <= T5 <= 6);
ocp.subjectTo(0 <= T6 <= 6);

// Run Opimizer
OCPexport mpc(ocp);
mpc.set(QP_SOLVER, QP_QPOASES);
mpc.set(INTEGRATOR_TYPE, INT_RK4);
mpc.set(GENERATE_TEST_FILE, YES);
mpc.set(GENERATE_MAKE_FILE, YES);

if(mpc.exportCode("hexacopter_export_v3_final")!= SUCCESSFUL_RETURN)
    exit( EXIT_FAILURE);

// mpc.printDimensionsQP();
return EXIT_SUCCESS;

[Prashin Sharma]

So upon closer inspection

status = acado_feedbackStep()

gave a non zero value, to be specific I got status = 31, which basically says theres some error in the qpsolver. I dived further into from where the status is being returned from, I think it is returned from getHessianInverse residing in SolutionAnalysis.cpp .

And the returned value might be from RET_HOTSTART_FAILED .. not sure why

also any idea what status = 31 means,

Any thoughts

[Rien Quirynen]

That is exactly what I wanted to warn you about:
you should probably include the control inputs in the cost function. If you want the control inputs to be penalized only slightly, you can make the weight values (much) smaller than the current values in the Q matrix, but this error from the QP solver will likely go away once you add a nonzero weight value for the control inputs. Note that most QP solvers will have problems with this, given that your current problem formulation is likely to be ill-defined because of non-uniqueness of the optimal control solution caused by the semi-definite Hessian matrix by not having any penalization of control inputs in your control objective.

[Prashin Sharma]

Oh fantastic , got it to work thanks a ton...

just a final question :

acadoVariables..x and acadoVariables.y has the same sequence of variables as
Function h,hN ;
h << p << q << r .....;

However when you use acado_printDifferentialVariables(); it will print them in order they were defined
DifferentialState p,q,r ;
DifferentialState phi,theta,psi ;
DifferentialState x,y,z ;
......

Is that the right understanding ?

[Rien Quirynen]

Great!

Hmm, yes and no. acadoVariables.y will indeed have the order of the reference function defined in:

Function h,hN ;
h << p << q << r .....;

However, acadoVariables.x (and therefore also the printing function) follows the order in which you define the state variables and the corresponding differential equations:

DifferentialState p,q,r ;
DifferentialState phi,theta,psi ;
DifferentialState x,y,z ; 
......

[Prashin Sharma]

ok , Thanks for your help again..