crane_kul_mhe_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 <sstream>
00028 #include <fstream>
00029 #include <vector>
00030 #include <string>
00031 #include <iomanip>
00032 #include <cstring>
00033 #include <cmath>
00034 #include <cstdlib>
00035 
00036 using namespace std;
00037 
00038 #include "acado_common.h"
00039 #include "acado_auxiliary_functions.h"
00040 
00041 #define NX          ACADO_NX    /* number of differential states */
00042 #define NXA         ACADO_NXA   /* number of algebraic states */
00043 #define NU          ACADO_NU    /* number of control inputs */
00044 #define N               ACADO_N         /* number of control intervals */
00045 #define NY                      ACADO_NY        /* number of measurements, nodes 0..N-1 */
00046 #define NYN                     ACADO_NYN       /* number of measurements, node N */
00047 #define NUM_STEPS   100         /* number of simulation steps */
00048 #define VERBOSE     1                   /* show iterations: 1, silent: 0  */
00049 
00050 ACADOvariables acadoVariables;
00051 ACADOworkspace acadoWorkspace;
00052 
00053 bool readDataFromFile( const char* fileName, vector< vector< double > >& data )
00054 {
00055         ifstream file( fileName );
00056         string line;
00057 
00058         if ( file.is_open() )
00059         {
00060                 while( getline(file, line) )
00061                 {
00062                         istringstream linestream( line );
00063                         vector< double > linedata;
00064                         double number;
00065 
00066                         while( linestream >> number )
00067                         {
00068                                 linedata.push_back( number );
00069                         }
00070 
00071                         data.push_back( linedata );
00072                 }
00073 
00074                 file.close();
00075         }
00076         else
00077                 return false;
00078 
00079         return true;
00080 }
00081 
00082 int main()
00083 {
00084         unsigned i, j, iter;
00085 
00086         real_t t1, t2;
00087         real_t fdbSum = 0.0;
00088         real_t prepSum = 0.0;
00089         int status;
00090 
00091         t1 = t2 = 0;
00092 
00093         timer t;
00094 
00095         // Reset all solver memory
00096         memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
00097         memset(&acadoVariables, 0, sizeof( acadoVariables ));
00098 
00099         vector< vector< double > > measurements;
00100         if (readDataFromFile("./crane_kul_mhe_data.txt", measurements) == false)
00101         {
00102                 cout << "Cannot read measurements" << endl;
00103                 return EXIT_FAILURE;
00104         }
00105 
00106         //
00107         // Initialize the solver
00108         //
00109         initializeSolver();
00110 
00111         // Init states with measurements
00112         for (i = 0; i < N + 1; ++i)
00113         {
00114                 acadoVariables.x[i * NX + 0] = measurements[ i ][ 0 ];
00115                 acadoVariables.x[i * NX + 2] = measurements[ i ][ 1 ];
00116                 acadoVariables.x[i * NX + 4] = measurements[ i ][ 2 ];
00117 
00118                 acadoVariables.x[i * NX + 5] = measurements[ i ][ 3 ];
00119                 acadoVariables.x[i * NX + 6] = measurements[ i ][ 4 ];
00120         }
00121 
00122         for (i = 0; i < N; ++i)
00123         {
00124                 acadoVariables.u[i * NU + 0] = measurements[ i ][ 5 ];
00125                 acadoVariables.u[i * NU + 1] = measurements[ i ][ 6 ];
00126         }
00127 
00128         //
00129         // Logger initalization
00130         //
00131         vector< vector< double > > log;
00132 
00133         log.resize(NUM_STEPS);
00134         for (i = 0; i < log.size(); ++i)
00135                 log[ i ].resize(NX + NXA + NU + 5, 0.0);
00136 
00137         //
00138         // Warm-up the solver
00139         //
00140         preparationStep();
00141 
00142         //
00143         // Main simulation loop
00144         //
00145         for( iter = 0; iter < NUM_STEPS; iter++ )
00146         {
00147                 //
00148                 // Read new measurements
00149                 //
00150                 for (i = 0; i < N; ++i)
00151                         for (j = 0; j < NY; ++j)
00152                                 acadoVariables.y[i * NY + j] = measurements[iter + i][ j ];
00153 
00154                 for (j = 0; j < NYN; ++j)
00155                         acadoVariables.yN[ j ] = measurements[iter + i][ j ];
00156 
00157                 //
00158                 // Run the feedback step
00159                 //
00160                 tic( &t );
00161                 status = feedbackStep( );
00162                 t2 = toc( &t );
00163 
00164 #if VERBOSE
00165 //              printDifferentialVariables();
00166 //              printControlVariables();
00167 #endif // VERBOSE
00168 
00169                 if ( status )
00170                 {
00171                         cout << "Iteration:" << iter << ", QP problem! QP status: " << status << endl;
00172 
00173                         break;
00174                 }
00175 
00176                 //
00177                 // Logging
00178                 //
00179                 for(i = 0; i < NX; i++)
00180                         log[ iter ][ i ] = acadoVariables.x[ i ];
00181                 for(j = 0;  i < NX + NU; i++, j++)
00182                         log[ iter ][ i ] = acadoVariables.u[ j ];
00183 
00184                 log[ iter ][ i++ ] = t1;
00185                 log[ iter ][ i++ ] = t2;
00186                 log[ iter ][ i++ ] = getObjective();
00187                 log[ iter ][ i++ ] = getKKT();
00188                 log[ iter ][ i++ ] = getNWSR();
00189 
00190 #if VERBOSE
00191                 cout    << "Iteration #" << setw( 4 ) << iter
00192                                 << ", KKT value: " << scientific << getKKT()
00193                                 << ", objective value: " << scientific << getObjective()
00194                                 << endl;
00195 #endif // VERBOSE
00196 
00197                 //
00198                 // Prepare for the next simulation step
00199                 //
00200 
00201                 // Shift states and controls
00202                 shiftStates(2, 0, 0);
00203                 shiftControls( 0 );
00204 
00205                 // Execute the preparation step of the RTI scheme
00206                 tic( &t );
00207                 preparationStep();
00208                 t1 = toc( &t );
00209 
00210                 //
00211                 // More logging
00212                 //
00213                 prepSum += t1;
00214                 fdbSum += t2;
00215         }
00216 
00217 #if VERBOSE
00218         cout << "Average feedback time:    " << scientific << fdbSum / NUM_STEPS * 1e6 << " microseconds" << endl;
00219         cout << "Average preparation time: " << scientific << prepSum / NUM_STEPS * 1e6 << " microseconds" << endl;
00220 #endif // VERBOSE
00221 
00222         //
00223         // Save log to a file
00224         //
00225         ofstream dataLog( "./pendulum_dae_nmpc_test_sim_log.txt" );
00226         if ( dataLog.is_open() )
00227         {
00228                 for (i = 0; i < log.size(); i++)
00229                 {
00230                         for (j = 0; j < log[ i ].size(); j++)
00231                                 dataLog << log[ i ][ j ] << " ";
00232                         dataLog << endl;
00233                 }
00234 
00235                 dataLog.close();
00236         }
00237         else
00238         {
00239                 cout << "Log file could not be opened" << endl;
00240 
00241                 return 1;
00242         }
00243 
00244         // For debugging
00245         if ( status )
00246         {
00247                 cout << "Solver failed!" << endl;
00248                 return EXIT_FAILURE;
00249         }
00250 
00251     return EXIT_SUCCESS;
00252 }
00253 
00254 


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:58:04