pendulum_dae_nmpc_test.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 #include <iostream>
00027 #include <fstream>
00028 #include <vector>
00029 #include <string>
00030 #include <iomanip>
00031 #include <cstring>
00032 #include <cstdlib>
00033 #include <cmath>
00034 
00035 using namespace std;
00036 
00037 #include "acado_common.h"
00038 #include "acado_auxiliary_functions.h"
00039 
00040 #define NX          ACADO_NX    /* number of differential states */
00041 #define NXA         ACADO_NXA   /* number of alg. states */
00042 #define NU          ACADO_NU    /* number of control inputs */
00043 #define N               ACADO_N         /* number of control intervals */
00044 #define NY                      ACADO_NY        /* number of references, nodes 0..N - 1 */
00045 #define NYN                     ACADO_NYN
00046 #define NUM_STEPS   100                 /* number of simulation steps */
00047 #define VERBOSE     1                   /* show iterations: 1, silent: 0 */
00048 
00049 ACADOvariables acadoVariables;
00050 ACADOworkspace acadoWorkspace;
00051 
00052 int main()
00053 {
00054         unsigned  i, j, iter;
00055         timer t;
00056         real_t t1, t2;
00057         real_t fdbSum = 0.0;
00058         real_t prepSum = 0.0;
00059         int status;
00060 
00061         t1 = t2 = 0;
00062 
00063         // Reset all solver memory
00064         memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
00065         memset(&acadoVariables, 0, sizeof( acadoVariables ));
00066 
00067         //
00068         // Initialize the solver
00069         //
00070         initializeSolver();
00071 
00072         //
00073         // Prepare a consistent initial guess
00074         //
00075 
00076         for (i = 0; i < N + 1; ++i)
00077         {
00078                 acadoVariables.x[i * NX + 0] = 1;
00079                 acadoVariables.x[i * NX + 1] = sqrt(1.0 - 0.1 * 0.1);
00080                 acadoVariables.x[i * NX + 2] = 0.9;
00081                 acadoVariables.x[i * NX + 3] = 0;
00082                 acadoVariables.x[i * NX + 4] = 0;
00083                 acadoVariables.x[i * NX + 5] = 0;
00084         }
00085 
00086         //
00087         // Prepare references
00088         //
00089 
00090         for (i = 0; i < N; ++i)
00091         {
00092                 acadoVariables.y[i * NY + 0] = 0; // x
00093                 acadoVariables.y[i * NY + 1] = 1.0; // y
00094                 acadoVariables.y[i * NY + 2] = 0; // w
00095                 acadoVariables.y[i * NY + 3] = 0;
00096                 acadoVariables.y[i * NY + 4] = 0;
00097                 acadoVariables.y[i * NY + 5] = 0;
00098                 acadoVariables.y[i * NY + 6] = 0; // u
00099         }
00100 
00101         acadoVariables.yN[ 0 ] = 0; // x
00102         acadoVariables.yN[ 1 ] = 1.0; // y
00103         acadoVariables.yN[ 2 ] = 0; // w
00104         acadoVariables.yN[ 3 ] = 0;
00105         acadoVariables.yN[ 4 ] = 0;
00106         acadoVariables.yN[ 5 ] = 0;
00107 
00108         //
00109         // Current state feedback
00110         //
00111         for (i = 0; i < NX; ++i)
00112                 acadoVariables.x0[ i ] = acadoVariables.x[ i ];
00113 
00114         //
00115         // Logger initialization
00116         //
00117         vector< vector< double > > log;
00118 
00119         log.resize(NUM_STEPS);
00120         for (i = 0; i < log.size(); ++i)
00121                 log[ i ].resize(NX + NXA + NU + 5, 0.0);
00122 
00123 
00124         //
00125         // Warm-up the solver
00126         //
00127         preparationStep();
00128 
00129         //
00130         // Real-time iterations loop
00131         //
00132         for( iter = 0; iter < NUM_STEPS; iter++ )
00133         {
00134                 tic( &t );
00135                 status = feedbackStep( );
00136                 t2 = toc( &t );
00137 
00138 #if VERBOSE
00139 //              printDifferentialVariables();
00140 //              printControlVariables();
00141 #endif // VERBOSE
00142 
00143                 if ( status )
00144                 {
00145                         cout << "Iteration:" << iter << ", QP problem! QP status: " << status << endl;
00146 
00147                         break;
00148                 }
00149 
00150                 //
00151                 // Logging
00152                 //
00153 
00154                 for(i = 0; i < NX; i++)
00155                         log[ iter ][ i ] = acadoVariables.x[ i ];
00156                 for(j = 0; i < NX + NXA; i++, j++)
00157                         log[ iter ][ i ] = acadoVariables.z[ j ];
00158                 for(j = 0;  i < NX + NXA + NU; i++, j++)
00159                         log[ iter ][ i ] = acadoVariables.u[ j ];
00160 
00161                 log[ iter ][ i++ ] = t1;
00162                 log[ iter ][ i++ ] = t2;
00163                 log[ iter ][ i++ ] = getObjective();
00164                 log[ iter ][ i++ ] = getKKT();
00165                 log[ iter ][ i++ ] = getNWSR();
00166 
00167 #if VERBOSE
00168                 cout    << "Iteration #" << setw( 4 ) << iter
00169                                 << ", KKT value: " << scientific << getKKT()
00170                                 << ", objective value: " << scientific << getObjective()
00171                                 << endl;
00172 #endif // VERBOSE
00173 
00174                 //
00175                 // Prepare for the next iteration
00176                 //
00177 
00178                 // In this simple example, we feed the NMPC with an ideal feedback signal
00179                 // i.e. what NMPC really expects in the next sampling interval
00180                 for (i = 0; i < NX; ++i)
00181                         acadoVariables.x0[ i ] = acadoVariables.x[NX + i];
00182 
00183                 // Shift states and control and prepare for the next iteration
00184                 shiftStates(2, 0, 0);
00185                 shiftControls( 0 );
00186 
00187                 tic( &t );
00188                 preparationStep();
00189                 t1 = toc( &t );
00190 
00191                 //
00192                 // More logging...
00193                 //
00194                 prepSum += t1;
00195                 fdbSum += t2;
00196         }
00197 
00198 #if VERBOSE
00199         cout << "Average feedback time:    " << scientific << fdbSum / NUM_STEPS * 1e6 << " microseconds" << endl;
00200         cout << "Average preparation time: " << scientific << prepSum / NUM_STEPS * 1e6 << " microseconds" << endl;
00201 #endif // VERBOSE
00202 
00203         //
00204         // Save log to a file
00205         //
00206         ofstream dataLog( "./pendulum_dae_nmpc_test_sim_log.txt" );
00207         if ( dataLog.is_open() )
00208         {
00209                 for (i = 0; i < log.size(); i++)
00210                 {
00211                         for (j = 0; j < log[ i ].size(); j++)
00212                                 dataLog << log[ i ][ j ] << " ";
00213                         dataLog << endl;
00214                 }
00215 
00216                 dataLog.close();
00217         }
00218         else
00219         {
00220                 cout << "Log file could not be opened" << endl;
00221 
00222                 return 1;
00223         }
00224 
00225         // For debugging
00226         if ( status )
00227         {
00228                 cout << "Solver failed!" << endl;
00229                 return EXIT_FAILURE;
00230         }
00231 
00232     return EXIT_SUCCESS;
00233 }


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:34