crane_kul_mhe_test.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 #include <iostream>
27 #include <sstream>
28 #include <fstream>
29 #include <vector>
30 #include <string>
31 #include <iomanip>
32 #include <cstring>
33 #include <cmath>
34 #include <cstdlib>
35 
36 using namespace std;
37 
38 #include "acado_common.h"
39 #include "acado_auxiliary_functions.h"
40 
41 #define NX ACADO_NX /* number of differential states */
42 #define NXA ACADO_NXA /* number of algebraic states */
43 #define NU ACADO_NU /* number of control inputs */
44 #define N ACADO_N /* number of control intervals */
45 #define NY ACADO_NY /* number of measurements, nodes 0..N-1 */
46 #define NYN ACADO_NYN /* number of measurements, node N */
47 #define NUM_STEPS 100 /* number of simulation steps */
48 #define VERBOSE 1 /* show iterations: 1, silent: 0 */
49 
50 ACADOvariables acadoVariables;
52 
53 bool readDataFromFile( const char* fileName, vector< vector< double > >& data )
54 {
55  ifstream file( fileName );
56  string line;
57 
58  if ( file.is_open() )
59  {
60  while( getline(file, line) )
61  {
62  istringstream linestream( line );
63  vector< double > linedata;
64  double number;
65 
66  while( linestream >> number )
67  {
68  linedata.push_back( number );
69  }
70 
71  data.push_back( linedata );
72  }
73 
74  file.close();
75  }
76  else
77  return false;
78 
79  return true;
80 }
81 
82 int main()
83 {
84  unsigned i, j, iter;
85 
86  real_t t1, t2;
87  real_t fdbSum = 0.0;
88  real_t prepSum = 0.0;
89  int status;
90 
91  t1 = t2 = 0;
92 
93  acado_timer t;
94 
95  // Reset all solver memory
96  memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
97  memset(&acadoVariables, 0, sizeof( acadoVariables ));
98 
99  vector< vector< double > > measurements;
100  if (readDataFromFile("./crane_kul_mhe_data.txt", measurements) == false)
101  {
102  cout << "Cannot read measurements" << endl;
103  return EXIT_FAILURE;
104  }
105 
106  //
107  // Initialize the solver
108  //
109  acado_initializeSolver();
110 
111  // Init states with measurements
112  for (i = 0; i < N + 1; ++i)
113  {
114  acadoVariables.x[i * NX + 0] = measurements[ i ][ 0 ];
115  acadoVariables.x[i * NX + 2] = measurements[ i ][ 1 ];
116  acadoVariables.x[i * NX + 4] = measurements[ i ][ 2 ];
117 
118  acadoVariables.x[i * NX + 5] = measurements[ i ][ 3 ];
119  acadoVariables.x[i * NX + 6] = measurements[ i ][ 4 ];
120  }
121 
122  for (i = 0; i < N; ++i)
123  {
124  acadoVariables.u[i * NU + 0] = measurements[ i ][ 5 ];
125  acadoVariables.u[i * NU + 1] = measurements[ i ][ 6 ];
126  }
127 
128  //
129  // Logger initalization
130  //
131  vector< vector< double > > log;
132 
133  log.resize(NUM_STEPS);
134  for (i = 0; i < log.size(); ++i)
135  log[ i ].resize(NX + NXA + NU + 5, 0.0);
136 
137  //
138  // Warm-up the solver
139  //
140  acado_preparationStep();
141 
142  //
143  // Main simulation loop
144  //
145  for( iter = 0; iter < NUM_STEPS; iter++ )
146  {
147  //
148  // Read new measurements
149  //
150  for (i = 0; i < N; ++i)
151  for (j = 0; j < NY; ++j)
152  acadoVariables.y[i * NY + j] = measurements[iter + i][ j ];
153 
154  for (j = 0; j < NYN; ++j)
155  acadoVariables.yN[ j ] = measurements[iter + i][ j ];
156 
157  //
158  // Run the feedback step
159  //
160  acado_tic( &t );
161  status = acado_feedbackStep( );
162  t2 = acado_toc( &t );
163 
164 #if VERBOSE
165 // printDifferentialVariables();
166 // printControlVariables();
167 #endif // VERBOSE
168 
169  if ( status )
170  {
171  cout << "Iteration:" << iter << ", QP problem! QP status: " << status << endl;
172 
173  break;
174  }
175 
176  //
177  // Logging
178  //
179  for(i = 0; i < NX; i++)
180  log[ iter ][ i ] = acadoVariables.x[ i ];
181  for(j = 0; i < NX + NU; i++, j++)
182  log[ iter ][ i ] = acadoVariables.u[ j ];
183 
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();
189 
190 #if VERBOSE
191  cout << "Iteration #" << setw( 4 ) << iter
192  << ", KKT value: " << scientific << acado_getKKT()
193  << ", objective value: " << scientific << acado_getObjective()
194  << endl;
195 #endif // VERBOSE
196 
197  //
198  // Prepare for the next simulation step
199  //
200 
201  // Shift states and controls
202  acado_shiftStates(2, 0, 0);
203  acado_shiftControls( 0 );
204 
205  // Execute the preparation step of the RTI scheme
206  acado_tic( &t );
207  acado_preparationStep();
208  t1 = acado_toc( &t );
209 
210  //
211  // More logging
212  //
213  prepSum += t1;
214  fdbSum += t2;
215  }
216 
217 #if VERBOSE
218  cout << "Average feedback time: " << scientific << fdbSum / NUM_STEPS * 1e6 << " microseconds" << endl;
219  cout << "Average preparation time: " << scientific << prepSum / NUM_STEPS * 1e6 << " microseconds" << endl;
220 #endif // VERBOSE
221 
222  //
223  // Save log to a file
224  //
225  ofstream dataLog( "./pendulum_dae_nmpc_test_sim_log.txt" );
226  if ( dataLog.is_open() )
227  {
228  for (i = 0; i < log.size(); i++)
229  {
230  for (j = 0; j < log[ i ].size(); j++)
231  dataLog << log[ i ][ j ] << " ";
232  dataLog << endl;
233  }
234 
235  dataLog.close();
236  }
237  else
238  {
239  cout << "Log file could not be opened" << endl;
240 
241  return 1;
242  }
243 
244  // For debugging
245  if ( status )
246  {
247  cout << "Solver failed!" << endl;
248  return EXIT_FAILURE;
249  }
250 
251  return EXIT_SUCCESS;
252 }
253 
254 
#define N
ACADOworkspace acadoWorkspace
#define NY
ACADOvariables acadoVariables
#define NXA
bool readDataFromFile(const char *fileName, vector< vector< double > > &data)
#define NX
#define NU
#define NUM_STEPS
double real_t
Definition: AD_test.c:10
#define NYN
int main()
IntermediateState log(const Expression &arg)


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:31