irk_forward_export.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 
27 
36 
37 using namespace std;
38 
40 
41 //
42 // PUBLIC MEMBER FUNCTIONS:
43 //
44 
46  const std::string& _commonHeaderName
47  ) : ImplicitRungeKuttaExport( _userInteraction,_commonHeaderName )
48 {
49 }
50 
52 {
53 }
54 
55 
57 {
58  if ( solver )
59  delete solver;
60  solver = 0;
61 
62  clear( );
63 }
64 
65 
67 
68  if( this != &arg ){
69 
71  copy( arg );
72  }
73  return *this;
74 }
75 
76 
78 {
79  ExportVariable max;
80  if( NX1 > 0 ) {
82  }
83  if( NX2 > 0 || NXA > 0 ) {
84  if( rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
86  }
87  if( diffs_rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
89  }
90  }
91  if( NX3 > 0 ) {
92  if( rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
94  }
95  if( diffs_rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
97  }
98  }
99  uint i;
100  for( i = 0; i < outputs.size(); i++ ) {
101  if( outputs[i].getGlobalExportVariable().getDim() >= max.getDim() ) {
102  max = outputs[i].getGlobalExportVariable();
103  }
104  if( diffs_outputs[i].getGlobalExportVariable().getDim() >= max.getDim() ) {
105  max = diffs_outputs[i].getGlobalExportVariable();
106  }
107  }
108  return max;
109 }
110 
111 
113  ExportStruct dataStruct
114  ) const
115 {
116  ImplicitRungeKuttaExport::getDataDeclarations( declarations, dataStruct );
117 
118  declarations.addDeclaration( rk_diffK,dataStruct );
119 
120  declarations.addDeclaration( rk_diffsTemp3,dataStruct );
121 
122 // if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
123  declarations.addDeclaration( rk_diffsPrev1,dataStruct );
124  declarations.addDeclaration( rk_diffsPrev2,dataStruct );
125  declarations.addDeclaration( rk_diffsPrev3,dataStruct );
126 // }
127 
128  declarations.addDeclaration( rk_diffsNew1,dataStruct );
129  declarations.addDeclaration( rk_diffsNew2,dataStruct );
130  declarations.addDeclaration( rk_diffsNew3,dataStruct );
131 
132  if( CONTINUOUS_OUTPUT ) {
133  declarations.addDeclaration( rk_diffsOutputTemp,dataStruct );
134  }
135 
136  return SUCCESSFUL_RETURN;
137 }
138 
139 
141  ) const
142 {
144 
145  return SUCCESSFUL_RETURN;
146 }
147 
148 
150 {
151  int sensGen;
152  get( DYNAMIC_SENSITIVITY, sensGen );
154 
155  int useOMP;
156  get(CG_USE_OPENMP, useOMP);
157  if ( useOMP ) {
159  max.setName( "auxVar" );
160  max.setDataStruct( ACADO_LOCAL );
161  if( NX2 > 0 || NXA > 0 ) {
164  }
165  if( NX3 > 0 ) {
168  }
169  for( uint i = 0; i < outputs.size(); i++ ) {
170  outputs[i].setGlobalExportVariable( max );
171  diffs_outputs[i].setGlobalExportVariable( max );
172  }
173 
175 
176  stringstream s;
177  s << "#pragma omp threadprivate( "
178  << max.getFullName() << ", "
179  << rk_ttt.getFullName() << ", "
180  << rk_xxx.getFullName() << ", "
181  << rk_kkk.getFullName() << ", "
182  << rk_diffK.getFullName() << ", "
183  << rk_rhsTemp.getFullName() << ", "
185  if( NX1 > 0 ) {
186  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev1.getFullName();
187  s << ", " << rk_diffsNew1.getFullName();
188  }
189  if( NX2 > 0 || NXA > 0 ) {
190  s << ", " << rk_A.getFullName();
191  s << ", " << rk_b.getFullName();
192  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev2.getFullName();
193  s << ", " << rk_diffsNew2.getFullName();
194  s << ", " << rk_diffsTemp2.getFullName();
196  }
197  if( NX3 > 0 ) {
198  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev3.getFullName();
199  s << ", " << rk_diffsNew3.getFullName();
200  s << ", " << rk_diffsTemp3.getFullName();
201  }
202  s << " )" << endl << endl;
203  code.addStatement( s.str().c_str() );
204  }
205 
206  if( NX1 > 0 ) {
207  code.addFunction( lin_input );
208  code.addStatement( "\n\n" );
209  }
210  if( exportRhs ) {
211  if( NX2 > 0 || NXA > 0 ) {
212  code.addFunction( rhs );
213  code.addStatement( "\n\n" );
214  code.addFunction( diffs_rhs );
215  code.addStatement( "\n\n" );
216  }
217 
218  if( NX3 > 0 ) {
219  code.addFunction( rhs3 );
220  code.addStatement( "\n\n" );
221  code.addFunction( diffs_rhs3 );
222  code.addStatement( "\n\n" );
223  }
224 
225  if( CONTINUOUS_OUTPUT ) {
226  uint i;
227  for( i = 0; i < outputs.size(); i++ ) {
228  code.addFunction( outputs[i] );
229  code.addStatement( "\n\n" );
230  code.addFunction( diffs_outputs[i] );
231  code.addStatement( "\n\n" );
232  }
233  }
234  }
235  if( NX2 > 0 || NXA > 0 ) solver->getCode( code );
236  code.addLinebreak(2);
237 
238  int measGrid;
239  get( MEASUREMENT_GRID, measGrid );
240 
241  // export RK scheme
242  uint run5;
243  std::string tempString;
244 
247 
248  string moduleName;
249  get(CG_MODULE_NAME, moduleName);
250 
251  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
252  DMatrix tmp = AA;
253  ExportVariable Ah( moduleName+"_Ah_mat", tmp*=h, STATIC_CONST_REAL );
254  code.addDeclaration( Ah );
255  code.addLinebreak( 2 );
256  // TODO: Ask Milan why this does NOT work properly !!
257  Ah = ExportVariable( moduleName+"_Ah_mat", numStages, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
258 
259  DVector BB( bb );
260  ExportVariable Bh( moduleName+"_Bh_mat", DMatrix( BB*=h ) );
261 
262  DVector CC( cc );
263  ExportVariable C;
264  if( timeDependant ) {
265  C = ExportVariable( moduleName+"_C_mat", DMatrix( CC*=(1.0/grid.getNumIntervals()) ), STATIC_CONST_REAL );
266  code.addDeclaration( C );
267  code.addLinebreak( 2 );
268  C = ExportVariable( moduleName+"_C_mat", 1, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
269  }
270 
271  code.addComment(std::string("Fixed step size:") + toString(h));
272 
273  ExportVariable determinant( "det", 1, 1, REAL, ACADO_LOCAL, true );
274  integrate.addDeclaration( determinant );
275 
276  ExportIndex i( "i" );
277  ExportIndex j( "j" );
278  ExportIndex k( "k" );
279  ExportIndex run( "run" );
280  ExportIndex run1( "run1" );
281  ExportIndex tmp_index1("tmp_index1");
282  ExportIndex tmp_index2("tmp_index2");
283  ExportIndex tmp_index3("tmp_index3");
284  ExportIndex tmp_index4("tmp_index4");
285  ExportVariable tmp_meas("tmp_meas", 1, outputGrids.size(), INT, ACADO_LOCAL);
286 
287  ExportVariable numInt( moduleName+"_numInts", 1, 1, INT );
288  if( !equidistantControlGrid() ) {
289  ExportVariable numStepsV( moduleName+"_numSteps", numSteps, STATIC_CONST_INT );
290  code.addDeclaration( numStepsV );
291  code.addLinebreak( 2 );
292  integrate.addStatement( std::string( "int " ) + numInt.getName() + " = " + numStepsV.getName() + "[" + rk_index.getName() + "];\n" );
293  }
294 
295  prepareOutputEvaluation( code );
296 
297  integrate.addIndex( i );
298  integrate.addIndex( j );
299  integrate.addIndex( k );
300  integrate.addIndex( run );
301  integrate.addIndex( run1 );
302  integrate.addIndex( tmp_index1 );
303  integrate.addIndex( tmp_index2 );
304  if( rk_outputs.size() > 0 ) integrate.addIndex( tmp_index3 );
305  if( rk_outputs.size() > 0 && (grid.getNumIntervals() > 1 || !equidistantControlGrid()) ) {
306  integrate.addIndex( tmp_index4 );
307  }
308  ExportVariable time_tmp( "time_tmp", 1, 1, REAL, ACADO_LOCAL, true );
309  if( CONTINUOUS_OUTPUT ) {
310  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
311  ExportIndex numMeasTmp( (std::string)"numMeasTmp" + toString(run5) );
312  numMeas.push_back( numMeasTmp );
313  integrate.addIndex( numMeas[run5] );
314  }
315 
316  if( (MeasurementGrid)measGrid == ONLINE_GRID ) {
317  integrate.addDeclaration( tmp_meas );
319  integrate.addDeclaration( time_tmp );
320  }
321 
322  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
323  integrate.addStatement( numMeas[run5] == 0 );
324  }
325  }
327  if( (inputDim-diffsDim) > NX+NXA ) {
329  }
331 
332  if( NXA > 0 ) {
333  integrate.addStatement( std::string( "if( " ) + reset_int.getFullName() + " ) {\n" );
334  for( run5 = 0; run5 < NXA; run5++ ) {
335  for( uint iStage = 0; iStage < numStages; iStage++ ) {
336  integrate.addStatement( rk_kkk.getElement(NX+run5,iStage) == rk_eta.getCol(NX+run5) );
337  }
338  }
339  integrate.addStatement( std::string( "}\n" ) );
340  }
341 
342  // integrator loop:
343  ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
344  ExportStatementBlock *loop;
345  if( equidistantControlGrid() ) {
346  loop = &tmpLoop;
347  }
348  else {
349  loop = &integrate;
350  loop->addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
351  }
352 
353  if( CONTINUOUS_OUTPUT && (MeasurementGrid)measGrid == ONLINE_GRID ) {
354  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
355  loop->addStatement( tmp_index1 == numMeas[run5] );
356  loop->addStatement( std::string("while( ") + tmp_index1.getName() + " < " + toString(totalMeas[run5]) + " && " + gridVariables[run5].get(0,tmp_index1) + " <= (" + rk_ttt.getFullName() + "+" + toString(1.0/grid.getNumIntervals()) + ") ) {\n" );
357  loop->addStatement( tmp_index1 == tmp_index1+1 );
358  loop->addStatement( std::string("}\n") );
359  loop->addStatement( std::string(tmp_meas.get( 0,run5 )) + " = " + tmp_index1.getName() + " - " + numMeas[run5].getName() + ";\n" );
360  }
361  }
362 
363  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
364  // Set rk_diffsPrev:
365  loop->addStatement( std::string("if( run > 0 ) {\n") );
366  if( NX1 > 0 ) {
367  ExportForLoop loopTemp1( i,0,NX1 );
368  loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+NX+NXA,i*NX+NX+NXA+NX1 ) );
369  if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,NX1,NX1+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1),i*NU+(NX+NXA)*(NX+1)+NU ) );
370  loop->addStatement( loopTemp1 );
371  }
372  if( NX2 > 0 ) {
373  ExportForLoop loopTemp2( i,0,NX2 );
374  loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,0,NX1+NX2 ) == rk_eta.getCols( i*NX+NX+NXA+NX1*NX,i*NX+NX+NXA+NX1*NX+NX1+NX2 ) );
375  if( NU > 0 ) loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,NX1+NX2,NX1+NX2+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+NX1*NU,i*NU+(NX+NXA)*(NX+1)+NX1*NU+NU ) );
376  loop->addStatement( loopTemp2 );
377  }
378  if( NX3 > 0 ) {
379  ExportForLoop loopTemp3( i,0,NX3 );
380  loopTemp3.addStatement( rk_diffsPrev3.getSubMatrix( i,i+1,0,NX ) == rk_eta.getCols( i*NX+NX+NXA+(NX1+NX2)*NX,i*NX+NX+NXA+(NX1+NX2)*NX+NX ) );
381  if( NU > 0 ) loopTemp3.addStatement( rk_diffsPrev3.getSubMatrix( i,i+1,NX,NX+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+(NX1+NX2)*NU,i*NU+(NX+NXA)*(NX+1)+(NX1+NX2)*NU+NU ) );
382  loop->addStatement( loopTemp3 );
383  }
384  loop->addStatement( std::string("}\n") );
385  }
386 
387  // PART 1: The linear input system
388  prepareInputSystem( code );
389  solveInputSystem( loop, i, run1, j, tmp_index1, Ah );
390 
391  // PART 2: The fully implicit system
392  solveImplicitSystem( loop, i, run1, j, tmp_index1, ExportIndex(0), Ah, C, determinant, true );
393 
394  // PART 3: The linear output system
395  prepareOutputSystem( code );
396  solveOutputSystem( loop, i, run1, j, tmp_index1, Ah, true );
397 
398  // generate continuous OUTPUT:
399  generateOutput( loop, run, i, tmp_index2, tmp_index3, tmp_meas, time_tmp, NX+NU );
400 
401  // DERIVATIVES wrt the states (IFT):
402  if( NX1 > 0 ) {
403  ExportForLoop loop4( run1,0,NX1 );
404  // PART 1: The linear input system
405  sensitivitiesInputSystem( &loop4, run1, i, Bh, true );
406  // PART 2: The fully implicit system
407  sensitivitiesImplicitSystem( &loop4, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, true, 1 );
408  // PART 3: The linear output system
409  sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 1 );
410  // generate sensitivities wrt states for continuous output:
411  sensitivitiesOutputs( &loop4, run, run1, i, tmp_index1, tmp_index2, tmp_index3, tmp_meas, time_tmp, true, 0 );
412  loop->addStatement( loop4 );
413  }
414  if( NX2 > 0 ) {
415  ExportForLoop loop4( run1,NX1,NX1+NX2 );
416  // PART 2: The fully implicit system
417  sensitivitiesImplicitSystem( &loop4, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, true, 2 );
418  // PART 3: The linear output system
419  sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 2 );
420  // generate sensitivities wrt states for continuous output:
421  sensitivitiesOutputs( &loop4, run, run1, i, tmp_index1, tmp_index2, tmp_index3, tmp_meas, time_tmp, true, NX1 );
422  loop->addStatement( loop4 );
423  }
424  if( NX3 > 0 ) {
425  ExportForLoop loop4( run1,NX1+NX2,NX );
426  // PART 3: The linear output system
427  sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 3 );
428  // generate sensitivities wrt states for continuous output:
429  sensitivitiesOutputs( &loop4, run, run1, i, tmp_index1, tmp_index2, tmp_index3, tmp_meas, time_tmp, true, NX1+NX2 );
430  loop->addStatement( loop4 );
431  }
432 
433 
434  // DERIVATIVES wrt the control inputs (IFT):
435  if( NU > 0 ) {
436  ExportForLoop loop5( run1,0,NU );
437  // PART 1: The linear input system
438  sensitivitiesInputSystem( &loop5, run1, i, Bh, false );
439  // PART 2: The fully implicit system
440  sensitivitiesImplicitSystem( &loop5, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, false, 0 );
441  // PART 3: The linear output system
442  sensitivitiesOutputSystem( &loop5, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, false, 0 );
443  // generate sensitivities wrt controls for continuous output:
444  sensitivitiesOutputs( &loop5, run, run1, i, tmp_index1, tmp_index2, tmp_index3, tmp_meas, time_tmp, false, 0 );
445  loop->addStatement( loop5 );
446  }
447 
448  // update rk_eta:
449  for( run5 = 0; run5 < NX; run5++ ) {
450  loop->addStatement( rk_eta.getCol( run5 ) += rk_kkk.getRow( run5 )*Bh );
451  }
452  if( NXA > 0) {
453  DMatrix tempCoefs( evaluateDerivedPolynomial( 0.0 ) );
454  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
455  loop->addStatement( std::string("if( run == 0 ) {\n") );
456  }
457  for( run5 = 0; run5 < NXA; run5++ ) {
458  loop->addStatement( rk_eta.getCol( NX+run5 ) == rk_kkk.getRow( NX+run5 )*tempCoefs );
459  }
460  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
461  loop->addStatement( std::string("}\n") );
462  }
463  }
464 
465 
466  // Computation of the sensitivities using the CHAIN RULE:
467  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
468  loop->addStatement( std::string( "if( run == 0 ) {\n" ) );
469  }
470  // PART 1
471  updateInputSystem(loop, i, j, tmp_index2);
472  // PART 2
473  updateImplicitSystem(loop, i, j, tmp_index2);
474  // PART 3
475  updateOutputSystem(loop, i, j, tmp_index2);
476 
477  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
478  loop->addStatement( std::string( "}\n" ) );
479  loop->addStatement( std::string( "else {\n" ) );
480  // PART 1
481  propagateInputSystem(loop, i, j, k, tmp_index2);
482  // PART 2
483  propagateImplicitSystem(loop, i, j, k, tmp_index2);
484  // PART 3
485  propagateOutputSystem(loop, i, j, k, tmp_index2);
486  }
487 
488  if( rk_outputs.size() > 0 && (grid.getNumIntervals() > 1 || !equidistantControlGrid()) ) {
489  propagateOutputs( loop, run, run1, i, j, k, tmp_index1, tmp_index2, tmp_index3, tmp_index4, tmp_meas );
490  }
491 
492  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
493  loop->addStatement( std::string( "}\n" ) );
494  }
495 
496  loop->addStatement( std::string( reset_int.get(0,0) ) + " = 0;\n" );
497 
498  for( run5 = 0; run5 < rk_outputs.size(); run5++ ) {
499  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
500  loop->addStatement( numMeas[run5].getName() + " += " + numMeasVariables[run5].get(0,run) + ";\n" );
501  }
502  else { // ONLINE_GRID
503  loop->addStatement( numMeas[run5].getName() + " += " + tmp_meas.get(0,run5) + ";\n" );
504  }
505  }
506  loop->addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
507 
508  // end of the integrator loop.
509  if( !equidistantControlGrid() ) {
510  loop->addStatement( "}\n" );
511  }
512  else {
513  integrate.addStatement( *loop );
514  }
515  // PART 1
516  if( NX1 > 0 ) {
517  DMatrix zeroR = zeros<double>(1, NX2+NX3);
518  ExportForLoop loop1( i,0,NX1 );
519  loop1.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1,i*NX+NX+NXA+NX ) == zeroR );
520  integrate.addStatement( loop1 );
521  }
522  // PART 2
523  DMatrix zeroR = zeros<double>(1, NX3);
524  if( NX2 > 0 ) {
525  ExportForLoop loop2( i,NX1,NX1+NX2 );
526  loop2.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1+NX2,i*NX+NX+NXA+NX ) == zeroR );
527  integrate.addStatement( loop2 );
528  }
529  if( NXA > 0 ) {
530  ExportForLoop loop3( i,NX,NX+NXA );
531  loop3.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1+NX2,i*NX+NX+NXA+NX ) == zeroR );
532  integrate.addStatement( loop3 );
533  }
534 
535  integrate.addStatement( std::string( "if( " ) + determinant.getFullName() + " < 1e-12 ) {\n" );
537  integrate.addStatement( std::string( "} else if( " ) + determinant.getFullName() + " < 1e-6 ) {\n" );
539  integrate.addStatement( std::string( "} else {\n" ) );
541  integrate.addStatement( std::string( "}\n" ) );
542 
543  code.addFunction( integrate );
544  code.addLinebreak( 2 );
545 
546  return SUCCESSFUL_RETURN;
547 }
548 
549 
551  const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2,
552  const ExportIndex& tmp_index3, const ExportIndex& tmp_index4, const ExportVariable& tmp_meas )
553 {
554  uint i;
555  int measGrid;
556  get( MEASUREMENT_GRID, measGrid );
557 
558  // chain rule for the sensitivities of the continuous output:
559  for( i = 0; i < rk_outputs.size(); i++ ) {
560  ExportStatementBlock *loop01;
561  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
562  loop01 = block;
563  loop01->addStatement( std::string("for(") + index0.getName() + " = 0; " + index0.getName() + " < (int)" + numMeasVariables[i].get(0,index) + "; " + index0.getName() + "++) {\n" );
564  }
565  else { // ONLINE_GRID
566  loop01 = block;
567  loop01->addStatement( std::string("for(") + index0.getName() + " = 0; " + index0.getName() + " < (int)" + tmp_meas.get(0,i) + "; " + index0.getName() + "++) {\n" );
568  }
569 
570  uint numOutputs = getDimOUTPUT( i );
571  uint outputDim = numOutputs*(NX+NU+1);
572  loop01->addStatement( tmp_index1 == numMeas[i]*outputDim+index0*(numOutputs*(NX+NU+1)) );
573  ExportForLoop loop02( index3,0,numOutputs*(NX+NU) );
574  loop02.addStatement( tmp_index2 == tmp_index1+index3 );
575  loop02.addStatement( rk_diffsOutputTemp.getCol( index3 ) == rk_outputs[i].getCol( tmp_index2+numOutputs ) );
576  loop01->addStatement( loop02 );
577 
578  loop01->addStatement( tmp_index1 == numMeas[i]*outputDim+index0*(numOutputs*(NX+NU+1)) );
579  ExportForLoop loop03( index1,0,numOutputs );
580  loop03.addStatement( tmp_index2 == tmp_index1+index1*NX );
581 
582  ExportForLoop loop04( index2,0,NX1 );
583  loop04.addStatement( tmp_index3 == tmp_index2+index2 );
584  loop04.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) == 0.0 );
585  ExportForLoop loop05( index3,0,NX1 );
586  loop05.addStatement( tmp_index4 == index1*NX+index3 );
587  loop05.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev1.getElement( index3,index2 ) );
588  loop04.addStatement( loop05 );
589  ExportForLoop loop06( index3,NX1,NX1+NX2 );
590  loop06.addStatement( tmp_index4 == index1*NX+index3 );
591  loop06.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev2.getElement( index3-NX1,index2 ) );
592  loop04.addStatement( loop06 );
593  ExportForLoop loop062( index3,NX1+NX2,NX );
594  loop062.addStatement( tmp_index4 == index1*NX+index3 );
595  loop062.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev3.getElement( index3-NX1-NX2,index2 ) );
596  loop04.addStatement( loop062 );
597  loop03.addStatement( loop04 );
598 
599  ExportForLoop loop07( index2,NX1,NX1+NX2 );
600  loop07.addStatement( tmp_index3 == tmp_index2+index2 );
601  loop07.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) == 0.0 );
602  ExportForLoop loop08( index3,NX1,NX1+NX2 );
603  loop08.addStatement( tmp_index4 == index1*NX+index3 );
604  loop08.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev2.getElement( index3-NX1,index2 ) );
605  loop07.addStatement( loop08 );
606  ExportForLoop loop082( index3,NX1+NX2,NX );
607  loop082.addStatement( tmp_index4 == index1*NX+index3 );
608  loop082.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev3.getElement( index3-NX1-NX2,index2 ) );
609  loop07.addStatement( loop082 );
610  loop03.addStatement( loop07 );
611 
612  ExportForLoop loop09( index2,NX1+NX2,NX );
613  loop09.addStatement( tmp_index3 == tmp_index2+index2 );
614  loop09.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) == 0.0 );
615  ExportForLoop loop10( index3,NX1+NX2,NX );
616  loop10.addStatement( tmp_index4 == index1*NX+index3 );
617  loop10.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev3.getElement( index3-NX1-NX2,index2 ) );
618  loop09.addStatement( loop10 );
619  loop03.addStatement( loop09 );
620 
621  if( NU > 0 ) {
622  loop03.addStatement( tmp_index2 == tmp_index1+index1*NU );
623  ExportForLoop loop11( index2,0,NU );
624  loop11.addStatement( tmp_index3 == tmp_index2+index2 );
625  loop11.addStatement( tmp_index4 == index1*NU+index2 );
626  loop11.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs*(1+NX) ) == rk_diffsOutputTemp.getCol( tmp_index4+numOutputs*NX ) );
627  ExportForLoop loop12( index3,0,NX1 );
628  loop12.addStatement( tmp_index4 == index1*NX+index3 );
629  loop12.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs*(1+NX) ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev1.getElement( index3,NX1+index2 ) );
630  loop11.addStatement( loop12 );
631  ExportForLoop loop13( index3,NX1,NX1+NX2 );
632  loop13.addStatement( tmp_index4 == index1*NX+index3 );
633  loop13.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs*(1+NX) ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev2.getElement( index3-NX1,NX1+NX2+index2 ) );
634  loop11.addStatement( loop13 );
635  ExportForLoop loop132( index3,NX1+NX2,NX );
636  loop132.addStatement( tmp_index4 == index1*NX+index3 );
637  loop132.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs*(1+NX) ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev3.getElement( index3-NX1-NX2,NX+index2 ) );
638  loop11.addStatement( loop132 );
639  loop03.addStatement( loop11 );
640  }
641 
642  loop01->addStatement( loop03 );
643  loop01->addStatement( "}\n" );
644  }
645 
646  return SUCCESSFUL_RETURN;
647 }
648 
649 
651 {
652  if( NX1 > 0 ) {
653  DMatrix mat1 = formMatrix( M11, A11 );
654  rk_mat1 = ExportVariable( "rk_mat1", mat1, STATIC_CONST_REAL );
655  code.addDeclaration( rk_mat1 );
656  // TODO: Ask Milan why this does NOT work properly !!
658 
659  DMatrix sens = zeros<double>(NX1*(NX1+NU), numStages);
660  uint i, j, k;
661  for( i = 0; i < NX1; i++ ) {
662  DVector vec(NX1*numStages);
663  for( j = 0; j < numStages; j++ ) {
664  for( k = 0; k < NX1; k++ ) {
665  vec(j*NX1+k) = A11(k,i);
666  }
667  }
668  DVector sol = mat1*vec;
669  for( j = 0; j < numStages; j++ ) {
670  for( k = 0; k < NX1; k++ ) {
671  sens(i*NX1+k,j) = sol(j*NX1+k);
672  }
673  }
674  }
675  for( i = 0; i < NU; i++ ) {
676  DVector vec(NX1*numStages);
677  for( j = 0; j < numStages; j++ ) {
678  for( k = 0; k < NX1; k++ ) {
679  vec(j*NX1+k) = B11(k,i);
680  }
681  }
682  DVector sol = mat1*vec;
683  for( j = 0; j < numStages; j++ ) {
684  for( k = 0; k < NX1; k++ ) {
685  sens(NX1*NX1+i*NX1+k,j) = sol(j*NX1+k);
686  }
687  }
688  }
689  rk_dk1 = ExportVariable( "rk_dk1", sens, STATIC_CONST_REAL );
690  code.addDeclaration( rk_dk1 );
691  // TODO: Ask Milan why this does NOT work properly !!
692  rk_dk1 = ExportVariable( "rk_dk1", NX1*(NX1+NU), numStages, STATIC_CONST_REAL, ACADO_LOCAL );
693  }
694 
695  return SUCCESSFUL_RETURN;
696 }
697 
698 
700 {
701  if( NX3 > 0 ) {
702  DMatrix mat3 = formMatrix( M33, A33 );
703  rk_mat3 = ExportVariable( "rk_mat3", mat3, STATIC_CONST_REAL );
704  code.addDeclaration( rk_mat3 );
705  // TODO: Ask Milan why this does NOT work properly !!
707 
708  DMatrix sens = zeros<double>(NX3*NX3, numStages);
709  uint i, j, k;
710  for( i = 0; i < NX3; i++ ) {
711  DVector vec(NX3*numStages);
712  for( j = 0; j < numStages; j++ ) {
713  for( k = 0; k < NX3; k++ ) {
714  vec(j*NX3+k) = A33(k,i);
715  }
716  }
717  DVector sol = mat3*vec;
718  for( j = 0; j < numStages; j++ ) {
719  for( k = 0; k < NX3; k++ ) {
720  sens(i*NX3+k,j) = sol(j*NX3+k);
721  }
722  }
723  }
724  rk_dk3 = ExportVariable( "rk_dk3", sens, STATIC_CONST_REAL );
725  code.addDeclaration( rk_dk3 );
726  // TODO: Ask Milan why this does NOT work properly !!
727  rk_dk3 = ExportVariable( "rk_dk3", NX3*NX3, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
728  }
729 
730  return SUCCESSFUL_RETURN;
731 }
732 
733 
735 {
736  if( NX1 > 0 ) {
737  // update rk_diffK with the new sensitivities:
738  if( STATES ) block->addStatement( rk_diffK.getRows(0,NX1) == rk_dk1.getRows(index1*NX1,index1*NX1+NX1) );
739  else block->addStatement( rk_diffK.getRows(0,NX1) == rk_dk1.getRows(index1*NX1+NX1*NX1,index1*NX1+NX1+NX1*NX1) );
740  // update rk_diffsNew with the new sensitivities:
741  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) block->addStatement( std::string( "if( run == 0 ) {\n" ) );
742  ExportForLoop loop3( index2,0,NX1 );
743  if( STATES ) loop3.addStatement( std::string(rk_diffsNew1.get( index2,index1 )) + " = (" + index2.getName() + " == " + index1.getName() + ");\n" );
744 
745  if( STATES ) loop3.addStatement( rk_diffsNew1.getElement( index2,index1 ) += rk_diffK.getRow( index2 )*Bh );
746  else loop3.addStatement( rk_diffsNew1.getElement( index2,index1+NX1 ) == rk_diffK.getRow( index2 )*Bh );
747  block->addStatement( loop3 );
748  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) block->addStatement( std::string( "}\n" ) );
749  }
750 
751  return SUCCESSFUL_RETURN;
752 }
753 
754 
755 returnValue ForwardIRKExport::sensitivitiesImplicitSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2, const ExportVariable& Ah, const ExportVariable& Bh, const ExportVariable& det, bool STATES, uint number )
756 {
757  if( NX2 > 0 ) {
758  DMatrix zeroM = zeros<double>( NX2+NXA,1 );
759  DMatrix tempCoefs( evaluateDerivedPolynomial( 0.0 ) );
760  uint i;
761 
762  ExportForLoop loop1( index2,0,numStages );
763  if( STATES && number == 1 ) {
764  ExportForLoop loop2( index3,0,NX1 );
765  loop2.addStatement( std::string(rk_rhsTemp.get( index3,0 )) + " = -(" + index3.getName() + " == " + index1.getName() + ");\n" );
766  for( i = 0; i < numStages; i++ ) {
767  loop2.addStatement( rk_rhsTemp.getRow( index3 ) -= rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
768  }
769  loop1.addStatement( loop2 );
770  ExportForLoop loop3( index3,0,NX2+NXA );
771  loop3.addStatement( tmp_index1 == index2*(NX2+NXA)+index3 );
772  loop3.addStatement( rk_b.getRow( tmp_index1 ) == rk_diffsTemp2.getSubMatrix( index2,index2+1,index3*(NVARS2),index3*(NVARS2)+NX1 )*rk_rhsTemp.getRows(0,NX1) );
773  if( NDX2 > 0 ) {
774  loop3.addStatement( rk_b.getRow( tmp_index1 ) -= rk_diffsTemp2.getSubMatrix( index2,index2+1,index3*(NVARS2)+NVARS2-NX1-NX2,index3*(NVARS2)+NVARS2-NX2 )*rk_diffK.getSubMatrix( 0,NX1,index2,index2+1 ) );
775  }
776  loop1.addStatement( loop3 );
777  }
778  else if( STATES && number == 2 ) {
779  for( i = 0; i < NX2+NXA; i++ ) {
780  loop1.addStatement( rk_b.getRow( index2*(NX2+NXA)+i ) == zeroM.getRow( 0 ) - rk_diffsTemp2.getElement( index2,index1+i*(NVARS2) ) );
781  }
782  }
783  else { // ~= STATES
784  ExportForLoop loop2( index3,0,NX1 );
785  loop2.addStatement( rk_rhsTemp.getRow( index3 ) == rk_diffK.getElement( index3,0 )*Ah.getElement(index2,0) );
786  for( i = 1; i < numStages; i++ ) {
787  loop2.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
788  }
789  loop1.addStatement( loop2 );
790  ExportForLoop loop3( index3,0,NX2+NXA );
791  loop3.addStatement( tmp_index1 == index2*(NX2+NXA)+index3 );
792  loop3.addStatement( tmp_index2 == index1+index3*(NVARS2) );
793  loop3.addStatement( rk_b.getRow( tmp_index1 ) == zeroM.getRow( 0 ) - rk_diffsTemp2.getElement( index2,tmp_index2+NX1+NX2+NXA ) );
794  loop3.addStatement( rk_b.getRow( tmp_index1 ) -= rk_diffsTemp2.getSubMatrix( index2,index2+1,index3*(NVARS2),index3*(NVARS2)+NX1 )*rk_rhsTemp.getRows(0,NX1) );
795  if( NDX2 > 0 ) {
796  loop3.addStatement( rk_b.getRow( tmp_index1 ) -= rk_diffsTemp2.getSubMatrix( index2,index2+1,index3*(NVARS2)+NVARS2-NX1-NX2,index3*(NVARS2)+NVARS2-NX2 )*rk_diffK.getSubMatrix( 0,NX1,index2,index2+1 ) );
797  }
798  loop1.addStatement( loop3 );
799  }
800  block->addStatement( loop1 );
801  if( STATES && (number == 1 || NX1 == 0) ) {
802  block->addStatement( std::string( "if( 0 == " ) + index1.getName() + " ) {\n" ); // factorization of the new matrix rk_A not yet calculated!
803  block->addStatement( det.getFullName() + " = " + ExportStatement::fcnPrefix + "_" + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_b.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
804  block->addStatement( std::string( "}\n else {\n" ) );
805  }
807  if( STATES && (number == 1 || NX1 == 0) ) block->addStatement( std::string( "}\n" ) );
808  // update rk_diffK with the new sensitivities:
809  ExportForLoop loop2( index2,0,numStages );
810  loop2.addStatement( rk_diffK.getSubMatrix(NX1,NX1+NX2,index2,index2+1) == rk_b.getRows(index2*NX2,index2*NX2+NX2) );
811  loop2.addStatement( rk_diffK.getSubMatrix(NX,NX+NXA,index2,index2+1) == rk_b.getRows(numStages*NX2+index2*NXA,numStages*NX2+index2*NXA+NXA) );
812  block->addStatement( loop2 );
813  // update rk_diffsNew with the new sensitivities:
814  ExportForLoop loop3( index2,0,NX2 );
815  if( STATES && number == 2 ) loop3.addStatement( std::string(rk_diffsNew2.get( index2,index1 )) + " = (" + index2.getName() + " == " + index1.getName() + "-" + toString(NX1) + ");\n" );
816 
817  if( STATES && number == 2 ) loop3.addStatement( rk_diffsNew2.getElement( index2,index1 ) += rk_diffK.getRow( NX1+index2 )*Bh );
818  else if( STATES ) loop3.addStatement( rk_diffsNew2.getElement( index2,index1 ) == rk_diffK.getRow( NX1+index2 )*Bh );
819  else loop3.addStatement( rk_diffsNew2.getElement( index2,index1+NX1+NX2 ) == rk_diffK.getRow( NX1+index2 )*Bh );
820  block->addStatement( loop3 );
821  if( NXA > 0 ) {
822  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
823  block->addStatement( std::string("if( run == 0 ) {\n") );
824  }
825  ExportForLoop loop4( index2,0,NXA );
826  if( STATES ) loop4.addStatement( rk_diffsNew2.getElement( index2+NX2,index1 ) == rk_diffK.getRow( NX+index2 )*tempCoefs );
827  else loop4.addStatement( rk_diffsNew2.getElement( index2+NX2,index1+NX1+NX2 ) == rk_diffK.getRow( NX+index2 )*tempCoefs );
828  block->addStatement( loop4 );
829  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
830  block->addStatement( std::string("}\n") );
831  }
832  }
833  }
834 
835  return SUCCESSFUL_RETURN;
836 }
837 
838 
839 returnValue ForwardIRKExport::sensitivitiesOutputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& index4, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2, const ExportVariable& Ah, const ExportVariable& Bh, bool STATES, uint number )
840 {
841  if( NX3 > 0 ) {
842  uint i;
843  ExportForLoop loop1( index2,0,numStages );
844  if( STATES && number == 1 ) {
845  ExportForLoop loop2( index3,0,NX1 );
846  loop2.addStatement( std::string(rk_rhsTemp.get( index3,0 )) + " = (" + index3.getName() + " == " + index1.getName() + ");\n" );
847  for( i = 0; i < numStages; i++ ) {
848  loop2.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
849  }
850  loop1.addStatement( loop2 );
851  ExportForLoop loop3( index3,NX1,NX1+NX2 );
852  loop3.addStatement( rk_rhsTemp.getRow( index3 ) == 0.0 );
853  for( i = 0; i < numStages; i++ ) {
854  loop3.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
855  }
856  loop1.addStatement( loop3 );
857  ExportForLoop loop4( index3,0,NX3 );
858  loop4.addStatement( tmp_index1 == index2*NX3+index3 );
859  loop4.addStatement( rk_b.getRow( tmp_index1 ) == rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3),index3*(NVARS3)+NX1+NX2 )*rk_rhsTemp.getRows(0,NX1+NX2) );
860  if( NXA3 > 0 ) {
861  loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NX1+NX2,index3*(NVARS3)+NX1+NX2+NXA )*rk_diffK.getSubMatrix( NX,NX+NXA,index2,index2+1 ) );
862  }
863  if( NDX3 > 0 ) {
864  loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NVARS3-NX1-NX2,index3*(NVARS3)+NVARS3 )*rk_diffK.getSubMatrix( 0,NX1+NX2,index2,index2+1 ) );
865  }
866  loop1.addStatement( loop4 );
867  }
868  else if( STATES && number == 2 ) {
869  ExportForLoop loop3( index3,NX1,NX1+NX2 );
870  loop3.addStatement( std::string(rk_rhsTemp.get( index3,0 )) + " = (" + index3.getName() + " == " + index1.getName() + ");\n" );
871  for( i = 0; i < numStages; i++ ) {
872  loop3.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
873  }
874  loop1.addStatement( loop3 );
875  ExportForLoop loop4( index3,0,NX3 );
876  loop4.addStatement( tmp_index1 == index2*NX3+index3 );
877  loop4.addStatement( rk_b.getRow( tmp_index1 ) == rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NX1,index3*(NVARS3)+NX1+NX2 )*rk_rhsTemp.getRows(NX1,NX1+NX2) );
878  if( NXA3 > 0 ) {
879  loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NX1+NX2,index3*(NVARS3)+NX1+NX2+NXA )*rk_diffK.getSubMatrix( NX,NX+NXA,index2,index2+1 ) );
880  }
881  if( NDX3 > 0 ) {
882  loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NVARS3-NX2,index3*(NVARS3)+NVARS3 )*rk_diffK.getSubMatrix( NX1,NX1+NX2,index2,index2+1 ) );
883  }
884  loop1.addStatement( loop4 );
885  }
886  else if( !STATES ) {
887  ExportForLoop loop2( index3,0,NX1 );
888  loop2.addStatement( rk_rhsTemp.getRow( index3 ) == rk_diffK.getElement( index3,0 )*Ah.getElement(index2,0) );
889  for( i = 1; i < numStages; i++ ) {
890  loop2.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
891  }
892  loop1.addStatement( loop2 );
893  ExportForLoop loop3( index3,NX1,NX1+NX2 );
894  loop3.addStatement( rk_rhsTemp.getRow( index3 ) == rk_diffK.getElement( index3,0 )*Ah.getElement(index2,0) );
895  for( i = 1; i < numStages; i++ ) {
896  loop3.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
897  }
898  loop1.addStatement( loop3 );
899  ExportForLoop loop4( index3,0,NX3 );
900  loop4.addStatement( tmp_index1 == index2*NX3+index3 );
901  loop4.addStatement( tmp_index2 == index1+index3*(NVARS3) );
902  loop4.addStatement( rk_b.getRow( tmp_index1 ) == rk_diffsTemp3.getElement( index2,tmp_index2+NX1+NX2+NXA3 ) );
903  loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3),index3*(NVARS3)+NX1+NX2 )*rk_rhsTemp.getRows(0,NX1+NX2) );
904  if( NXA3 > 0 ) {
905  loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NX1+NX2,index3*(NVARS3)+NX1+NX2+NXA )*rk_diffK.getSubMatrix( NX,NX+NXA,index2,index2+1 ) );
906  }
907  if( NDX3 > 0 ) {
908  loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NVARS3-NX1-NX2,index3*(NVARS3)+NVARS3 )*rk_diffK.getSubMatrix( 0,NX1+NX2,index2,index2+1 ) );
909  }
910  loop1.addStatement( loop4 );
911  }
912  if( !STATES || number != 3 ) block->addStatement( loop1 );
913 
914  // update rk_diffK with the new sensitivities:
915  if( STATES && number == 3 ) {
916  block->addStatement( rk_diffK.getRows(NX1+NX2,NX) == rk_dk3.getRows(index1*NX3-(NX1+NX2)*NX3,index1*NX3+NX3-(NX1+NX2)*NX3) );
917  }
918  else {
919  ExportForLoop loop4( index2,0,numStages );
920  ExportForLoop loop5( index3,0,NX3 );
921  loop5.addStatement( tmp_index1 == index2*NX3+index3 );
922  loop5.addStatement( rk_diffK.getElement(NX1+NX2+index3,index2) == rk_mat3.getElement(tmp_index1,0)*rk_b.getRow(0) );
923  ExportForLoop loop6( index4,1,numStages*NX3 );
924  loop6.addStatement( rk_diffK.getElement(NX1+NX2+index3,index2) += rk_mat3.getElement(tmp_index1,index4)*rk_b.getRow(index4) );
925  loop5.addStatement(loop6);
926  loop4.addStatement(loop5);
927  block->addStatement(loop4);
928  }
929  // update rk_diffsNew with the new sensitivities:
930  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) block->addStatement( std::string( "if( run == 0 ) {\n" ) );
931  ExportForLoop loop8( index2,0,NX3 );
932  if( STATES && number == 3 ) loop8.addStatement( std::string(rk_diffsNew3.get( index2,index1 )) + " = (" + index2.getName() + " == " + index1.getName() + "-" + toString(NX1+NX2) + ");\n" );
933 
934  if( STATES && number == 3 ) loop8.addStatement( rk_diffsNew3.getElement( index2,index1 ) += rk_diffK.getRow( NX1+NX2+index2 )*Bh );
935  else if( STATES ) loop8.addStatement( rk_diffsNew3.getElement( index2,index1 ) == rk_diffK.getRow( NX1+NX2+index2 )*Bh );
936  else loop8.addStatement( rk_diffsNew3.getElement( index2,index1+NX ) == rk_diffK.getRow( NX1+NX2+index2 )*Bh );
937  block->addStatement( loop8 );
938  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) block->addStatement( std::string( "}\n" ) );
939  }
940 
941  return SUCCESSFUL_RETURN;
942 }
943 
944 
946  const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2,
947  const ExportIndex& tmp_index3, const ExportVariable& tmp_meas, const ExportVariable& time_tmp, bool STATES, uint base )
948 {
949  uint i, j, k;
950  int measGrid;
951  get( MEASUREMENT_GRID, measGrid );
952 
953  for( i = 0; i < rk_outputs.size(); i++ ) {
954  uint NVARS = numVARS_output(i);
955  ExportStatementBlock *loop;
956  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
957  loop = block;
958  loop->addStatement( std::string("for(") + index2.getName() + " = 0; " + index2.getName() + " < (int)" + numMeasVariables[i].get(0,index0) + "; " + index2.getName() + "++) {\n" );
959  loop->addStatement( tmp_index2 == numMeas[i]+index2 );
960  for( j = 0; j < numStages; j++ ) {
961  loop->addStatement( rk_outH.getRow(j) == polynVariables[i].getElement( tmp_index2,j ) );
962  }
963  if( numXA_output(i) > 0 || numDX_output(i) > 0 ) {
964  for( j = 0; j < numStages; j++ ) {
965  loop->addStatement( rk_out.getRow(j) == polynDerVariables[i].getElement( tmp_index2,j ) );
966  }
967  }
968  }
969  else { // ONLINE_GRID
970  loop = block;
971  loop->addStatement( std::string(tmp_index3.getName()) + " = " + tmp_meas.get( 0,i ) + ";\n" );
972  loop->addStatement( std::string("for(") + index2.getName() + " = 0; " + index2.getName() + " < (int)" + tmp_index3.getName() + "; " + index2.getName() + "++) {\n" );
973  loop->addStatement( tmp_index2 == numMeas[i]+index2 );
974 
975  uint scale = grid.getNumIntervals();
976  double scale2 = 1.0/grid.getNumIntervals();
977  loop->addStatement( time_tmp.getName() + " = " + toString(scale) + "*(" + gridVariables[i].get(0,tmp_index2) + "-" + toString(scale2) + "*" + index0.getName() + ");\n" );
978 
979  std::string h = toString((grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals());
980  evaluatePolynomial( *loop, rk_outH, time_tmp, h );
981  if( numXA_output(i) > 0 || numDX_output(i) > 0 ) evaluateDerivedPolynomial( *loop, rk_out, time_tmp );
982  }
983 
984  DVector dependencyX, dependencyZ, dependencyDX;
985  if( exportRhs || crsFormat ) {
986  dependencyX = outputDependencies[i].getCols( 0,NX-1 ).sumRow();
987  if( numXA_output(i) > 0 ) dependencyZ = outputDependencies[i].getCols( NX,NX+NXA-1 ).sumRow();
988  if( numDX_output(i) > 0 ) dependencyDX = outputDependencies[i].getCols( NX+NXA+NU,NX+NXA+NU+NDX-1 ).sumRow();
989  }
990  for( j = 0; j < NX; j++ ) {
991  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyX(j)) != 0 ) {
992  if( STATES && j >= base ) {
993  loop->addStatement( std::string(rk_rhsTemp.get( j,0 )) + " = (" + toString(j) + " == " + index1.getName() + ");\n" );
994  }
995  else if( j >= base ) {
996  loop->addStatement( rk_rhsTemp.getRow( j ) == 0.0 );
997  }
998  if( j >= base ) loop->addStatement( rk_rhsTemp.getRow( j ) += rk_diffK.getRows( j,j+1 )*rk_outH );
999  loop->addStatement( rk_xxx.getCol( j ) == rk_eta.getCol( j ) + rk_kkk.getRow( j )*rk_outH );
1000  }
1001  }
1002  for( j = 0; j < numXA_output(i); j++ ) {
1003  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyZ(j)) != 0 ) {
1004  if( base < NX1+NX2 ) loop->addStatement( rk_rhsTemp.getRow( NX+j ) == rk_diffK.getRows( NX+j,NX+j+1 )*rk_out );
1005  loop->addStatement( rk_xxx.getCol( NX+j ) == rk_kkk.getRow( NX+j )*rk_out );
1006  }
1007  }
1008  for( j = 0; j < numDX_output(i); j++ ) {
1009  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyDX(j)) != 0 ) {
1010  if( j >= base ) loop->addStatement( rk_rhsTemp.getRow( NX+NXA+j ) == rk_diffK.getRows( j,j+1 )*rk_out );
1012  }
1013  }
1014 
1016  uint numOutputs = getDimOUTPUT( i );
1017  uint outputDim = numOutputs*(NX+NU+1);
1018  loop->addStatement( tmp_index1 == numMeas[i]*outputDim+index2*(numOutputs*(NX+NU+1)) );
1019  loop->addStatement( tmp_index2 == tmp_index1+index1 );
1020  for( j = 0; j < numOutputs; j++ ) {
1021  if( exportRhs || crsFormat ) {
1022  dependencyX = (outputDependencies[i].getCols( 0,NX-1 )).getRow( j );
1023  if( NXA > 0 ) dependencyZ = (outputDependencies[i].getCols( NX,NX+NXA-1 )).getRow( j );
1024  if( NDX > 0 ) dependencyDX = (outputDependencies[i].getCols( NX+NXA+NU,NX+NXA+NU+NDX-1 )).getRow( j );
1025  }
1026  uint offset;
1027  if( STATES ) {
1028  offset = numOutputs+j*NX;
1029  loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) == 0.0 );
1030  }
1031  else {
1032  offset = numOutputs*(1+NX)+j*NU;
1033  loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) == rk_diffsOutputTemp.getCol( index1+j*NVARS+NX+NXA ) );
1034  }
1035 
1036  for( k = base; k < NX; k++ ) {
1037  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyX(k)) != 0 ) {
1038  loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) += rk_diffsOutputTemp.getCol( j*NVARS+k )*rk_rhsTemp.getRow( k ) );
1039  }
1040  }
1041  if( base < NX1+NX2 ) {
1042  for( k = 0; k < numXA_output(i); k++ ) {
1043  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyZ(k)) != 0 ) {
1044  loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) += rk_diffsOutputTemp.getCol( j*NVARS+NX+k )*rk_rhsTemp.getRow( NX+k ) );
1045  }
1046  }
1047  }
1048  for( k = base; k < numDX_output(i); k++ ) {
1049  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyDX(k)) != 0 ) {
1050  loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) += rk_diffsOutputTemp.getCol( j*NVARS+NX+NXA+NU+k )*rk_rhsTemp.getRow( NX+NXA+k ) );
1051  }
1052  }
1053  }
1054  loop->addStatement( "}\n" );
1055  }
1056 
1057  return SUCCESSFUL_RETURN;
1058 }
1059 
1060 
1062 {
1064 
1065  NVARS3 = NX1+NX2+NXA3+NU+NDX3;
1066  diffsDim = (NX+NXA)*(NX+NU);
1067  inputDim = (NX+NXA)*(NX+NU+1) + NU + NOD;
1068 
1069  int useOMP;
1070  get(CG_USE_OPENMP, useOMP);
1071  ExportStruct structWspace;
1072  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
1073 
1074  rk_diffK = ExportVariable( "rk_diffK", NX+NXA, numStages, REAL, structWspace );
1075  rk_diffsTemp2 = ExportVariable( "rk_diffsTemp2", numStages, (NX2+NXA)*(NVARS2), REAL, structWspace );
1076  rk_diffsTemp3 = ExportVariable( "rk_diffsTemp3", numStages, NX3*NVARS3, REAL, structWspace );
1077  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
1078  rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", NX1, NX1+NU, REAL, structWspace );
1079  rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", NX2, NX1+NX2+NU, REAL, structWspace );
1080  rk_diffsPrev3 = ExportVariable( "rk_diffsPrev3", NX3, NX+NU, REAL, structWspace );
1081  }
1082  rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX1, NX1+NU, REAL, structWspace );
1083  rk_diffsNew2 = ExportVariable( "rk_diffsNew2", NX2+NXA, NX1+NX2+NU, REAL, structWspace );
1084  rk_diffsNew3 = ExportVariable( "rk_diffsNew3", NX3, NX+NU, REAL, structWspace );
1085  rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
1086 
1087  return SUCCESSFUL_RETURN;
1088 }
1089 
1090 
1091 
1092 // PROTECTED:
1093 
1094 
1096 
1097 // end of file.
ExportVariable rk_diffsPrev1
virtual returnValue setup()
ExportVariable getRow(const ExportIndex &idx) const
ExportVariable rk_diffK
Definition: irk_export.hpp:571
DVector evaluatePolynomial(double time)
Definition: irk_export.cpp:965
returnValue initializeDDMatrix()
Definition: irk_export.cpp:864
ExportVariable getGlobalExportVariable() const
std::vector< ExportVariable > gridVariables
Definition: irk_export.hpp:549
double getFirstTime() const
returnValue sensitivitiesOutputs(ExportStatementBlock *block, const ExportIndex &index0, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index1, const ExportIndex &tmp_index2, const ExportIndex &tmp_index3, const ExportVariable &tmp_meas, const ExportVariable &time_tmp, bool STATES, uint base)
std::vector< Grid > outputGrids
std::vector< ExportVariable > polynVariables
Definition: irk_export.hpp:553
virtual returnValue updateImplicitSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index)
ExportAcadoFunction diffs_rhs
ExportVariable rk_index
std::vector< ExportAcadoFunction > outputs
ExportVariable rk_diffsPrev3
virtual returnValue prepareInputSystem(ExportStatementBlock &code)
Allows to pass back messages to the calling function.
ExportVariable rk_kkk
Definition: rk_export.hpp:189
virtual returnValue prepareOutputSystem(ExportStatementBlock &code)
returnValue initializeCoefficients()
Definition: irk_export.cpp:883
ExportLinearSolver * solver
Definition: irk_export.hpp:530
returnValue addComment(const std::string &_comment)
Block< Derived > block(Index startRow, Index startCol, Index blockRows, Index blockCols)
Definition: BlockMethods.h:56
std::vector< DMatrix > outputDependencies
virtual returnValue propagateImplicitSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &_index3, const ExportIndex &tmp_index)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
std::vector< ExportAcadoFunction > diffs_outputs
ExportVariable rk_diffsPrev2
Allows to export code of a for-loop.
string toString(T const &value)
returnValue setName(const std::string &_name)
Definition: export_data.cpp:61
ExportVariable rk_mat1
Definition: irk_export.hpp:558
const std::string getNameDiffsOUTPUT(uint index) const
virtual returnValue propagateOutputs(ExportStatementBlock *block, const ExportIndex &index, const ExportIndex &index0, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index1, const ExportIndex &tmp_index2, const ExportIndex &tmp_index3, const ExportIndex &tmp_index4, const ExportVariable &tmp_meas)
virtual returnValue solveImplicitSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index, const ExportIndex &k_index, const ExportVariable &Ah, const ExportVariable &C, const ExportVariable &det, bool DERIVATIVES=false)
Definition: irk_export.cpp:603
ForwardIRKExport(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
ExportVariable getElement(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
ExportVariable rk_diffsTemp2
ForwardIRKExport & operator=(const ForwardIRKExport &arg)
#define CLOSE_NAMESPACE_ACADO
ExportVariable rk_diffsNew3
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
GenericMatrix< double > DMatrix
Definition: matrix.hpp:457
virtual returnValue getCode(ExportStatementBlock &code)
Defines a scalar-valued index variable to be used for exporting code.
ExportAcadoFunction rhs3
virtual returnValue copy(const ImplicitRungeKuttaExport &arg)
std::vector< uint > totalMeas
Definition: irk_export.hpp:550
Allows to export a tailored implicit Runge-Kutta integrator for fast model predictive control...
Definition: irk_export.hpp:55
ExportVariable rk_eta
ExportAcadoFunction diffs_rhs3
ExportStruct
std::vector< ExportVariable > numMeasVariables
Definition: irk_export.hpp:555
static std::string fcnPrefix
DVector evaluateDerivedPolynomial(double time)
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
ExportVariable rk_auxSolver
Definition: irk_export.hpp:563
int acadoRoundAway(double x)
virtual returnValue solveOutputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index, const ExportVariable &Ah, bool DERIVATIVES=false)
Definition: irk_export.cpp:653
const std::string getNameSolveReuseFunction()
virtual ExportVariable getAuxVariable() const
const std::string get(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
virtual returnValue propagateOutputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index)
ExportVariable reset_int
Encapsulates all user interaction for setting options, logging data and plotting results.
ExportVariable rk_diffsOutputTemp
Definition: irk_export.hpp:542
virtual returnValue sensitivitiesOutputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &index4, const ExportIndex &tmp_index1, const ExportIndex &tmp_index2, const ExportVariable &Ah, const ExportVariable &Bh, bool STATES, uint number)
virtual uint getDim() const
returnValue setDataStruct(ExportStruct _dataStruct)
Definition: export_data.cpp:80
virtual returnValue propagateInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index)
virtual DMatrix formMatrix(const DMatrix &mass, const DMatrix &jacobian)
Definition: irk_export.cpp:549
returnValue addStatement(const ExportStatement &_statement)
virtual returnValue updateInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index)
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
Definition: irk_export.cpp:244
ExportFunction integrate
std::string getFullName() const
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: irk_export.cpp:200
returnValue addLinebreak(uint num=1)
std::vector< ExportVariable > polynDerVariables
Definition: irk_export.hpp:554
uint getDimOUTPUT(uint index) const
returnValue generateOutput(ExportStatementBlock *block, const ExportIndex &index0, const ExportIndex &index1, const ExportIndex &tmp_index1, const ExportIndex &tmp_index2, const ExportVariable &tmp_meas, const ExportVariable &time_tmp, const uint directions)
Definition: irk_export.cpp:791
ExportVariable rk_mat3
Definition: irk_export.hpp:567
ExportVariable rk_diffsNew2
uint getNumIntervals() const
returnValue setGlobalExportVariable(const ExportVariable &var)
ImplicitRungeKuttaExport & operator=(const ImplicitRungeKuttaExport &arg)
Definition: irk_export.cpp:101
returnValue prepareOutputEvaluation(ExportStatementBlock &code)
ExportVariable rk_xxx
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
ExportAcadoFunction lin_input
double getLastTime() const
ExportVariable rk_ttt
virtual returnValue sensitivitiesImplicitSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index1, const ExportIndex &tmp_index2, const ExportVariable &Ah, const ExportVariable &Bh, const ExportVariable &det, bool STATES, uint number)
std::vector< ExportVariable > rk_outputs
Definition: irk_export.hpp:552
const std::string getNameSolveFunction()
#define BEGIN_NAMESPACE_ACADO
ExportVariable error_code
ExportVariable rk_rhsTemp
Definition: irk_export.hpp:564
Allows to export a tailored implicit Runge-Kutta integrator with forward sensitivity generation for f...
returnValue addFunction(const ExportFunction &_function)
MeasurementGrid
ExportVariable rk_diffsNew1
ExportVariable rk_diffsTemp3
Definition: irk_export.hpp:569
std::vector< ExportIndex > numMeas
Definition: irk_export.hpp:556
ExportVariable polynEvalVar
Definition: irk_export.hpp:545
ExportVariable rk_outH
Definition: irk_export.hpp:543
virtual returnValue clear()
virtual returnValue sensitivitiesInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportVariable &Bh, bool STATES)
virtual returnValue getCode(ExportStatementBlock &code)=0
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Allows to export code for a block of statements.
virtual returnValue solveInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index, const ExportVariable &Ah)
Definition: irk_export.cpp:576
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
virtual returnValue updateOutputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index)
virtual returnValue appendVariableNames(std::stringstream &string)=0
ExportVariable getCol(const ExportIndex &idx) const
GenericVector< T > getRow(unsigned _idx) const
Definition: matrix.hpp:197
ExportFunction & addIndex(const ExportIndex &_index)
#define ACADOERROR(retval)
virtual bool equidistantControlGrid() const
Defines a matrix-valued variable to be used for exporting code.
ExportAcadoFunction rhs
returnValue addFunctionCall(const std::string &_fName, const ExportArgument &_argument1=emptyConstExportArgument, const ExportArgument &_argument2=emptyConstExportArgument, const ExportArgument &_argument3=emptyConstExportArgument, const ExportArgument &_argument4=emptyConstExportArgument, const ExportArgument &_argument5=emptyConstExportArgument, const ExportArgument &_argument6=emptyConstExportArgument, const ExportArgument &_argument7=emptyConstExportArgument, const ExportArgument &_argument8=emptyConstExportArgument, const ExportArgument &_argument9=emptyConstExportArgument)
virtual returnValue setup()
std::string getName() const
Definition: export_data.cpp:86


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