Code generation tool and minimizeLSQ

[Espen Skjong]

Hi,

I'm trying to use the code generation tool to optimize my MPC. The code below is a simplified example of my MPC. When running the code generation and compiling the optimized result it seems to be no response in the control r. I tried to run the code, as it is, without code generation to see if there were response in the control r. The code run smoothly and worked perfectly. I guess there is something wrong with my LSQ implementation.

#include <acado_code_generation.hpp>

int main( )
{
USING_NAMESPACE_ACADO

// DEFINE THE VARIABLES:
// ----------------------------------------------------------
DifferentialState       x;
DifferentialState       y;
DifferentialState       nu_x;
DifferentialState       nu_y;
DifferentialState       psi;
Control   r;

// DEFINE THE MODEL EQUATIONS:
// ----------------------------------------------------------

DifferentialEquation  f;
f << dot(x) == nu_x*cos(psi) - nu_y*sin(psi);
f << dot(y) == nu_x*sin(psi) + nu_y*cos(psi);
f << dot(nu_x) == 0;
f << dot(nu_y) == 0;
f << dot(psi) == r;

// DEFINE THE WEIGHTING MATRICES:
// ----------------------------------------------------------

Matrix Q (1,1);
Q(0,0) = 10;

Function h;

h << sqrt((x-300)*(x-300) + (y-300)*(y-300))-300;


// SET UP THE MPC - OPTIMAL CONTROL PROBLEM:
// ----------------------------------------------------------
OCP ocp( 0.0,20.0, 19 );

ocp.minimizeLSQ       ( Q,h );

ocp.subjectTo( f );
ocp.subjectTo( -1.0 <= r <= 1.0 );


// DEFINE AN MPC EXPORT MODULE AND GENERATE THE CODE:
// ----------------------------------------------------------
MPCexport mpc( ocp );

mpc.set( HESSIAN_APPROXIMATION,       GAUSS_NEWTON    );
mpc.set( DISCRETIZATION_TYPE,         SINGLE_SHOOTING );
mpc.set( INTEGRATOR_TYPE,             INT_RK4         );
mpc.set( NUM_INTEGRATOR_STEPS,        30              );
mpc.set( QP_SOLVER,                   QP_QPOASES      );
mpc.set( MAX_NUM_QP_ITERATIONS,       20              );
// mpc.set( HOTSTART_QP,                 YES             );
// mpc.set( LEVENBERG_MARQUARDT,         1.0e-4          );
mpc.set( GENERATE_TEST_FILE,          YES             );
mpc.set( GENERATE_MAKE_FILE,          YES             );
mpc.set( GENERATE_SIMULINK_INTERFACE, NO              );
// mpc.set( OPERATING_SYSTEM,            OS_WINDOWS      );
// mpc.set( USE_SINGLE_PRECISION,        YES             );

mpc.exportCode( "mpc_export" );

mpc.printDimensionsQP( );

return 0;

}

The generated main file (with initial values) is given by

#include "acado.h"
#include "auxiliary_functions.c"


// SOME CONVENIENT DEFINTIONS: 
// --------------------------------------------------------------- 
   #define NX          5      // number of differential states  
   #define NU          1      // number of control inputs       
   #define N           19     // number of control intervals    
   #define NUM_STEPS   5      // number of real time iterations 
   #define VERBOSE     1      // show iterations: 1, silent: 0  
// ---------------------------------------------------------------


// GLOBAL VARIABLES FOR THE ACADO REAL-TIME ALGORITHM: 
// --------------------------------------------------- 
   ACADOvariables acadoVariables;
   ACADOworkspace acadoWorkspace;

// GLOBAL VARIABLES FOR THE QP SOLVER: 
// ----------------------------------- 
   Vars         vars;
   Params       params;


// A TEMPLATE FOR TESTING THE REAL-TIME IMPLEMENTATION: 
// ---------------------------------------------------- 
int main(){

   // INTRODUCE AUXILIARY VAIRABLES: 
   // ------------------------------ 
      int    i, iter        ;
      real_t measurement[NX];

   // INITIALIZE THE STATES AND CONTROLS: 
   // ---------------------------------------- 
      for( i = 0; i < NX*N; ++i )  acadoVariables.x[i] = 0.0;
      for( i = 0; i < NU*N; ++i )  acadoVariables.u[i] = 0.0;

   // INITIALIZE THE STATES AND CONTROL REFERENCE: 
   // -------------------------------------------- 
      for( i = 0; i < NX*N; ++i )  acadoVariables.xRef[i]  =  0.0;
      for( i = 0; i < NU*N; ++i )  acadoVariables.uRef[i]  =  0.0;


   // SETUP THE FIRST STATE MEASUREMENT: 
   // ------------------------------------------------ 
      for( i = 0; i < NX; ++i )  measurement[i] = 0.0;

      acadoVariables.x[0] = 10; // x
      measurement     [0] = 10;

      acadoVariables.x[1] = 10; // y
      measurement     [1] = 10;

      acadoVariables.x[2] = 0; // nu_x
      measurement     [2] = 0;

      acadoVariables.x[3] = 10; // nu_y
      measurement     [3] = 10;

      acadoVariables.x[4] = 0; // psi
      measurement     [4] = 0;

      if( VERBOSE ) printHeader();


   // PREPARE FIRST STEP: 
   // ------------------- 
      preparationStep();


   // GET THE TIME BEFORE START THE LOOP: 
   // ---------------------------------------------- 
      real_t t1 = getTime();


   // THE REAL-TIME ITERATION LOOP: 
   // ---------------------------------------------- 
      for( iter = 0; iter < NUM_STEPS; ++iter ){

        // TAKE A MEASUREMENT: 
        // ----------------------------- 
        // meausrement = ...

        // PERFORM THE FEEDBACK STEP: 
        // ----------------------------- 
           feedbackStep( measurement );

        // APPLY THE NEW CONTROL IMMEDIATELY TO THE PROCESS: 
        // ------------------------------------------------- 
        // send first piece of acadoVariables.u to process;
           if( VERBOSE )
       printf("=================================================================\n\n" );
           if( VERBOSE ) printf("      Real-Time Iteration %d:  KKT Tolerance = %.3e\n", iter, getKKT() );
           if( VERBOSE )
       printf("\n=================================================================\n" );

        // OPTIONAL: SHIFT THE INITIALIZATION: 
        // ----------------------------------- 
        // shiftControls( acadoVariables.uRef );
        // shiftStates  ( acadoVariables.xRef );

        // PREPARE NEXT STEP: 
        // ------------------ 
           preparationStep();
      }
  if( VERBOSE ) printf("\n\n              END OF THE REAL-TIME LOOP. \n\n\n");


   // GET THE TIME AT THE END OF THE LOOP: 
   // ---------------------------------------------- 
  real_t t2 = getTime();


   // PRINT DURATION AND RESULTS: 
   // ----------------------------------------------
      if( !VERBOSE )
      printf("\n\n AVERAGE DURATION OF ONE REAL-TIME ITERATION:   %.3g μs\n\n", 1e6*(t2-t1)/NUM_STEPS );

      printStates();
      printControls();

    return 0;
}

Does anyone see something wrong with my implementation and have a solution to my problem?

I tried to run the getting_started example (code generation) where the LSQ was implemented as

Matrix Q  = eye(4);
Matrix R  = eye(1);

Matrix P  = eye(4);
P *= 5.0;

ocp.minimizeLSQ       ( Q,R );
ocp.minimizeLSQEndTerm( P   );

I don't get it...

[Milan Vukov]

Hi, which version of ACADO are you running? Cheers, Milan

[Espen Skjong]

I think it is Version 3, 29 June 2007.

[Espen Skjong]

Hi Milan,

I updated ACADO to latest stable release and now it works. However I do not want to use the minimizeLSQEndTerm since it violates my constraints. I want to use

            Function h;

            h << sqrt((x-300)*(x-300) + (y-300)*(y-300))-300;

            Matrix Q = eye(h.getDim());
            Q *=10;

            ocp.minimizeLSQ       ( Q,h );

However, running this results in an error: "Only standard LSQ objective supported for code generation"

When using

            Function h;
            Function hN;

            h << sqrt((x-300)*(x-300) + (y-300)*(y-300))-300;
            h << r;
            hN << sqrt((x-300)*(x-300) + (y-300)*(y-300)-300);

            Matrix Q = eye(h.getDim());
            Q *=10;
            Matrix QN = eye(hN.getDim());
            QN *=10;

            ocp.minimizeLSQ       ( Q,h );
            ocp.minimizeLSQEndTerm(QN, hN);

The control r violates it's boundaries given by

            ocp.subjectTo( -0.1 <= r <= 0.1 );

The output is then

Differential variables:
[
-1.000000e+03   -1.000000e+01   0.000000e+00    1.000000e+01        0.000000e+00
-9.931104e+02   -9.706280e+00   0.000000e+00    1.000000e+01    -3.056379e+00
-9.999500e+02   -1.058531e+01   0.000000e+00    1.000000e+01    -6.112759e+00
-9.932100e+02   -9.127352e+00   0.000000e+00    1.000000e+01    -9.169138e+00
-9.998015e+02   -1.115366e+01   0.000000e+00    1.000000e+01    -1.222552e+01
-9.934064e+02   -8.573708e+00   0.000000e+00    1.000000e+01    -1.528190e+01
-9.995587e+02   -1.168858e+01   0.000000e+00    1.000000e+01    -1.833828e+01
-1.008667e+03   -8.691753e+00   0.000000e+00    1.000000e+01    -1.685498e+01
-1.006100e+03   -1.509077e+01   0.000000e+00    1.000000e+01    -1.379801e+01
-1.008117e+03   -8.497704e+00   0.000000e+00    1.000000e+01    -1.074104e+01
-1.006665e+03   -1.523763e+01   0.000000e+00    1.000000e+01    -7.684069e+00
-1.007540e+03   -8.397593e+00   0.000000e+00    1.000000e+01    -4.627690e+00
-1.007248e+03   -1.528601e+01   0.000000e+00    1.000000e+01    -1.570720e+00
-1.006957e+03   -8.397549e+00   0.000000e+00    1.000000e+01    1.486249e+00
-1.007829e+03   -1.523675e+01   0.000000e+00    1.000000e+01    4.543219e+00
-1.008700e+03   -2.207744e+01   0.000000e+00    1.000000e+01    1.486840e+00
-1.008413e+03   -1.518881e+01   0.000000e+00    1.000000e+01    -1.570130e+00
-9.979347e+02   -1.606065e+01   0.000000e+00    1.000000e+01    -1.737496e+00
-9.891017e+02   -1.169456e+01   0.000000e+00    1.000000e+01    -4.859560e-01
-9.841853e+02   -2.386890e+00   0.000000e+00    1.000000e+01    -4.859560e-01
]


Control variables:
[
-2.903560e+00
-2.903560e+00
-2.903560e+00
-2.903560e+00
-2.903560e+00
-2.903560e+00
1.409133e+00
2.904121e+00
2.904121e+00
2.904121e+00
2.903560e+00
2.904121e+00
2.904121e+00
2.904121e+00
-2.903560e+00
-2.904121e+00
-1.589982e-01
1.188963e+00
0.000000e+00
]

In the MPC problem it is not certain that the control objective is reached in one horizon. Thus specifying an end term would not be the perfect choice...

Any suggestions?

[Mohamed Mehrez]

Hi Espen,
I have few comments on your problem that hopefully may help you:
1- in your system model, I guess you can avoid using

f << dot(nu_x) == 0;
f << dot(nu_y) == 0;

because this means that these two variables are basically constants. That's why you are getting the 3rd and the 4th columns of (Differential variables:) as constants.

2- I believe that you are using a differential drive robot model in your problem formulation, and you use nu_x and nu_y as the velocities in x-direction and y-direction, respectively. So, why are you are putting them as just constants where in fact they are also control inputs. So, I would imagine your problem formulation to be something like:

// DEFINE THE VARIABLES:
// ----------------------------------------------------------
DifferentialState       x;
DifferentialState       y;
DifferentialState       nu_x;
DifferentialState       nu_y;
DifferentialState       psi;
Control   r;
Control   a_x; // acceleration in x-direction
Control   a_y; // acceleration in y-direction

// DEFINE THE MODEL EQUATIONS:
// ----------------------------------------------------------

DifferentialEquation  f;
f << dot(x) == nu_x*cos(psi) - nu_y*sin(psi);
f << dot(y) == nu_x*sin(psi) + nu_y*cos(psi);
f << dot(nu_x) == a_x;
f << dot(nu_y) == a_y;
f << dot(psi) == r;

3- you are using a prediction horizon of length

OCP ocp( 0.0,20.0, 19 );

and based on that, your time step is basically (20/19 = 1.05 seconds). I guess this is a quite long update step for your system.

4- in case you want to eliminate the use of a terminal cost function, you can try

hN <<0;

You are simply putting zero in this function.

If you think that I am missing anything in my comments just ignore them.

Best Regards,
Mohamed

[Espen Skjong]

Hi Mohamed,

Thank you for your answer. I actually do not control the velocities nu_x and nu_y. Basically what I am doing is feeding waypoints (x,y) calculated by the MPC to a lower level autopilot in order to control an UAV. The UAV is flying by the (measured) velocities nu_x and nu_y (which for now are assumed to be constant). The only control is psi, which determines the UAV's heading. I have also included a to-axis gimbal system in the UAV where I control the gimbal's pan and tilt rates, which means that I have DifferentialState for pan and tilt angles together with Controls for pan and tilt rate. The only thing left in the implementation process is to make an optimized solution with the code generation tool. The issue, which I realized when running the optimized code is that the control r's constraint is violated (-0.1<=r<=0.1), which is not good at all. The UAV has a roll angle given by phi = 5*sin(-r). Since the maximal turn rate is given by the maximum roll angle of about 30 degrees, the solution (print of control r above) is not good at all.

The gimbal's camera is used to find objects with a camera vision tool, and the objects' coordinates are fed to the MPC. The MPC uses the objects' coordinates to calculate gimbal angles and waypoints.

I'll try your solution and hope for the best. Thanks! :-)

[Espen Skjong]

It seems to be something wrong with the LSQ implementation. I'll give the full code example below:

#include <acado_toolkit.hpp>


int main( )
{
    USING_NAMESPACE_ACADO

    // DEFINE THE VARIABLES:
    // ----------------------------------------------------------
    DifferentialState       x;
    DifferentialState       y;
    DifferentialState       nu_x;
    DifferentialState       nu_y;
    DifferentialState       psi;
    Control   r;

    // DEFINE THE MODEL EQUATIONS:
    // ----------------------------------------------------------

    DifferentialEquation  f;
    f << dot(x) == nu_x*cos(psi) - nu_y*sin(psi);
    f << dot(y) == nu_x*sin(psi) + nu_y*cos(psi);
    f << dot(nu_x) == 0;
    f << dot(nu_y) == 0;
    f << dot(psi) == r;

    // DEFINE THE WEIGHTING MATRICES:
    // ----------------------------------------------------------

    Function h;
    Function hN;

    h << sqrt((x-0)*(x-0) + (y-300)*(y-300))-300;
    //h << psi;
    hN << 0;

    Matrix Q = eye(h.getDim());
    Q *=10;
    Matrix QN = eye(hN.getDim());
    QN *=10;

    // SET UP THE MPC - OPTIMAL CONTROL PROBLEM:
    // ----------------------------------------------------------
    OCP ocp( 0.0,20.0, 20 );

    ocp.minimizeLSQ       ( Q,h );
    ocp.minimizeLSQEndTerm(QN, hN);

    ocp.subjectTo( f );
    ocp.subjectTo( -0.1 <= r <= 0.1 );


    // DEFINE AN MPC EXPORT MODULE AND GENERATE THE CODE:
    // ----------------------------------------------------------
    OCPexport mpc( ocp );

    mpc.set( HESSIAN_APPROXIMATION,       GAUSS_NEWTON    );
        mpc.set( DISCRETIZATION_TYPE,         SINGLE_SHOOTING );
    mpc.set( INTEGRATOR_TYPE,             INT_RK4         );
    mpc.set( NUM_INTEGRATOR_STEPS,        30              );
    mpc.set( QP_SOLVER,                   QP_QPOASES      );
    //mpc.set( MAX_NUM_QP_ITERATIONS,       20              );
    //  mpc.set( HOTSTART_QP,                 YES             );
    //  mpc.set( LEVENBERG_MARQUARDT,         1.0e-4          );
    mpc.set( GENERATE_TEST_FILE,          YES             );
    mpc.set( GENERATE_MAKE_FILE,          YES             );
    mpc.set( GENERATE_SIMULINK_INTERFACE, NO              );
    //  mpc.set( OPERATING_SYSTEM,            OS_WINDOWS      );
    //  mpc.set( USE_SINGLE_PRECISION,        YES             );

    mpc.exportCode( "mpc_export" );

    mpc.printDimensionsQP( );
    return 0;
}

The auto-generated .c file with initialization is given as:

    #include "acado_common.h"
    #include "acado_auxiliary_functions.h"

    #include <stdio.h>
    #include <string.h>

    /* Some convenient definitions. */
    #define NX          ACADO_NX  /* Number of differential state variables.  */
    #define NXA         ACADO_NXA /* Number of algebraic variables. */
    #define NU          ACADO_NU  /* Number of control inputs. */
    #define NP          ACADO_NP  /* Number of parameters. */

    #define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
    #define NYN         ACADO_NYN /* Number of measurements/references on node N. */

    #define N           ACADO_N   /* Number of intervals in the horizon. */

    #define NUM_STEPS   10        /* Number of real-time iterations. */
    #define VERBOSE     1         /* Show iterations: 1, silent: 0.  */

    /* Global variables used by the solver. */
    ACADOvariables acadoVariables;
    ACADOworkspace acadoWorkspace;

    /* A template for testing of the solver. */
    int main()
    {
        /* Some temporary variables. */
        int    i, iter;
        timer t;

        /* Clear solver memory. */
        memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
        memset(&acadoVariables, 0, sizeof( acadoVariables ));

        /* Initialize the solver. */
        initializeSolver();

        /* Initialize the states and controls. */
        for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
        for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;

        /* Initialize the measurements/reference. */
        for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
        for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;

        /* MPC: initialize the current state feedback. */
    #if ACADO_INITIAL_STATE_FIXED
            for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0;

            acadoVariables.x0[0] = -0; // x
            acadoVariables.x0[1] = -10; // y
            acadoVariables.x0[2] = 0; // nu_x
            acadoVariables.x0[3] = 10; // nu_y
            acadoVariables.x0[4] = 0; // psi
    #endif

        if( VERBOSE ) printHeader();

        /* Prepare first step */
        preparationStep();

        /* Get the time before start of the loop. */
        tic( &t );

        /* The "real-time iterations" loop. */
        for(iter = 0; iter < NUM_STEPS; ++iter)
        {
            /* Perform the feedback step. */
            feedbackStep( );

            /* Apply the new control immediately to the process, first NU components. */

            if( VERBOSE ) printf("\tReal-Time Iteration %d:  KKT Tolerance = %.3e\n\n", iter, getKKT() );

            /* Optional: shift the initialization (look at acado_common.h). */
            /* shiftStates(2, 0, 0); */
            /* shiftControls( 0 ); */

            /* Prepare for the next step. */
            preparationStep();
        }
        /* Read the elapsed time. */
        real_t te = toc( &t );

        if( VERBOSE ) printf("\n\nEnd of the RTI loop. \n\n\n");

        /* Eye-candy. */

            if( !VERBOSE )
            printf("\n\n Average time of one real-time iteration:   %.3g microseconds\n\n", 1e6 * te / NUM_STEPS);

            printDifferentialVariables();
            printControlVariables();

            return 0;
        }

What I want is to make the UAV circle around a stationary point given by (300,300) with a circle radius of 300. The results from running the code is

Differential variables:
[
    -1.000000e+02   -1.000000e+02   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -9.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -8.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -7.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -6.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -5.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -4.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -3.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -2.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -1.000000e+01   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   -1.776357e-15   0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   1.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   2.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   3.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   4.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   5.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   6.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   7.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   8.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   9.000000e+01    0.000000e+00    1.000000e+01    0.000000e+00
    -1.000000e+02   1.000000e+02    0.000000e+00    1.000000e+01    0.000000e+00
]


Control variables:
[
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
    0.000000e+00
]

Is there a magic trick that has to be performed to make the optimized code run as I want? Without optimization (not using the code generator) the MPC works like a charm, however faster computation cycles would be preferred.

Thanks in advance.

[Mohamed Mehrez]

Hi Espen,
I guess that the way the cost function (h) is defined in your problem formulation is misleading, i.e. if the definition of h is for the sake of a circular trajectory tracking, this is in fact not the right way to define it.

defining the reference is done basically by setting the values of

        /* Initialize the measurements/reference. */
        for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
        for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;

in the generated code.

You better look at this tutorial and it will show you how to do that.

http://acado.sourceforge.net/doc/html/db/daf/cgt_getting_started.html

Regards,
Mohamed

[Milan Vukov]

Hi,

Mohamed is right.

1) Check the manual
2) Examine the example in the tutorial section.

In short, you do not specify ||h - y||_Q^2 in the function h, but only function h. The actual reference is provided in the generated code, as Mohamed pointed out.

Check your ODE def once again. Are you sure you want to keep waypoints (references) as a part of the model?

Regards,
Milan