38 #include "acado_common.h" 39 #include "acado_auxiliary_functions.h" 55 ifstream file( fileName );
60 while( getline(file, line) )
62 istringstream linestream( line );
63 vector< double > linedata;
66 while( linestream >> number )
68 linedata.push_back( number );
71 data.push_back( linedata );
96 memset(&acadoWorkspace, 0,
sizeof( acadoWorkspace ));
99 vector< vector< double > > measurements;
102 cout <<
"Cannot read measurements" << endl;
109 acado_initializeSolver();
112 for (i = 0; i <
N + 1; ++i)
122 for (i = 0; i <
N; ++i)
131 vector< vector< double > >
log;
134 for (i = 0; i < log.size(); ++i)
135 log[ i ].resize(
NX +
NXA +
NU + 5, 0.0);
140 acado_preparationStep();
145 for( iter = 0; iter <
NUM_STEPS; iter++ )
150 for (i = 0; i <
N; ++i)
151 for (j = 0; j <
NY; ++j)
154 for (j = 0; j <
NYN; ++j)
161 status = acado_feedbackStep( );
162 t2 = acado_toc( &t );
171 cout <<
"Iteration:" << iter <<
", QP problem! QP status: " << status << endl;
179 for(i = 0; i <
NX; i++)
181 for(j = 0; i < NX +
NU; i++, j++)
184 log[ iter ][ i++ ] = t1;
185 log[ iter ][ i++ ] = t2;
186 log[ iter ][ i++ ] = acado_getObjective();
187 log[ iter ][ i++ ] = acado_getKKT();
188 log[ iter ][ i++ ] = acado_getNWSR();
191 cout <<
"Iteration #" << setw( 4 ) << iter
192 <<
", KKT value: " << scientific << acado_getKKT()
193 <<
", objective value: " << scientific << acado_getObjective()
202 acado_shiftStates(2, 0, 0);
203 acado_shiftControls( 0 );
207 acado_preparationStep();
208 t1 = acado_toc( &t );
218 cout <<
"Average feedback time: " << scientific << fdbSum / NUM_STEPS * 1e6 <<
" microseconds" << endl;
219 cout <<
"Average preparation time: " << scientific << prepSum / NUM_STEPS * 1e6 <<
" microseconds" << endl;
225 ofstream dataLog(
"./pendulum_dae_nmpc_test_sim_log.txt" );
226 if ( dataLog.is_open() )
228 for (i = 0; i < log.size(); i++)
230 for (j = 0; j < log[ i ].size(); j++)
231 dataLog << log[ i ][ j ] <<
" ";
239 cout <<
"Log file could not be opened" << endl;
247 cout <<
"Solver failed!" << endl;
ACADOworkspace acadoWorkspace
ACADOvariables acadoVariables
bool readDataFromFile(const char *fileName, vector< vector< double > > &data)
IntermediateState log(const Expression &arg)