irk_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 
35 
36 using namespace std;
37 
39 
40 //
41 // PUBLIC MEMBER FUNCTIONS:
42 //
43 
45  const std::string& _commonHeaderName
46  ) : RungeKuttaExport( _userInteraction,_commonHeaderName )
47 {
48  NX1 = 0;
49  NX2 = 0;
50  NDX2 = 0;
51  NVARS2 = 0;
52  NX3 = 0;
53  NDX3 = 0;
54  NXA3 = 0;
55  NVARS3 = 0;
56 
57  diffsDim = 0;
58  inputDim = 0;
59  numIts = 3; // DEFAULT value
60  numItsInit = 0; // DEFAULT value
61  REUSE = true;
62  CONTINUOUS_OUTPUT = false;
63 
64  solver = 0;
65 }
66 
68 {
69  NX1 = arg.NX1;
70  NX2 = arg.NX2;
71  NDX2 = arg.NDX2;
72  NVARS2 = arg.NVARS2;
73  NX3 = arg.NX3;
74  NDX3 = arg.NDX3;
75  NXA3 = arg.NXA3;
76  NVARS3 = arg.NVARS3;
77 
78  diffsDim = 0;
79  inputDim = 0;
80  numIts = arg.numIts ;
81  numItsInit = arg.numItsInit ;
83  outputs = arg.outputs;
85  solver = arg.solver;
86  REUSE = arg.REUSE;;
88 }
89 
90 
92 {
93  if ( solver )
94  delete solver;
95  solver = 0;
96 
97  clear( );
98 }
99 
100 
102 
103  if( this != &arg ){
104 
106  copy( arg );
107  }
108  return *this;
109 }
110 
111 
113 {
114  if( rhs_.getDim() > 0 ) {
115  OnlineData dummy0;
116  Control dummy1;
117  DifferentialState dummy2;
118  AlgebraicState dummy3;
120  dummy0.clearStaticCounters();
121  dummy1.clearStaticCounters();
122  dummy2.clearStaticCounters();
123  dummy3.clearStaticCounters();
124  dummy4.clearStaticCounters();
125 
126  NX2 = rhs_.getDim() - NXA;
127  x = DifferentialState("", NX1+NX2, 1);
128  z = AlgebraicState("", NXA, 1);
129  u = Control("", NU, 1);
130  od = OnlineData("", NOD, 1);
131 
133  f << rhs_;
134 
135  NDX2 = f.getNDX();
136  if( NDX2 > 0 && (NDX2 < NX2 || NDX2 > (NX1+NX2)) ) {
137  return ACADOERROR( RET_INVALID_OPTION );
138  }
139  else if( NDX2 > 0 ) NDX2 = NX1+NX2;
141 
143  for( uint i = 0; i < rhs_.getDim(); i++ ) {
144  g << forwardDerivative( rhs_(i), x );
145  g << forwardDerivative( rhs_(i), z );
146  g << forwardDerivative( rhs_(i), u );
147  g << forwardDerivative( rhs_(i), dx );
148  }
149 
150  if( f.getNT() > 0 ) timeDependant = true;
151 
152  return (rhs.init( f,"rhs", NX,NXA,NU,NP,NDX,NOD ) &
153  diffs_rhs.init( g,"diffs", NX,NXA,NU,NP,NDX,NOD ) );
154  }
155  return SUCCESSFUL_RETURN;
156 }
157 
158 
159 returnValue ImplicitRungeKuttaExport::setModel( const std::string& _rhs, const std::string& _diffs_rhs ) {
160 
161  IntegratorExport::setModel( _rhs, _diffs_rhs );
162 
163  NDX2 = NDX;
164 
165  setup();
166 
167  return SUCCESSFUL_RETURN;
168 }
169 
170 
172 {
173  ExportVariable max;
174  if( NX1 > 0 ) {
176  }
177  if( NX2 > 0 || NXA > 0 ) {
178  if( rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
180  }
181  if( diffs_rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
183  }
184  }
185  if( NX3 > 0 ) {
186  if( rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
188  }
189  }
190  uint i;
191  for( i = 0; i < outputs.size(); i++ ) {
192  if( outputs[i].getGlobalExportVariable().getDim() >= max.getDim() ) {
193  max = outputs[i].getGlobalExportVariable();
194  }
195  }
196  return max;
197 }
198 
199 
201  ExportStruct dataStruct
202  ) const
203 {
204  if( solver->getDim() ) solver->getDataDeclarations( declarations,dataStruct );
205 
206  if( NX1 > 0 || exportRhs ) {
208  declarations.addDeclaration( max,dataStruct );
209  }
210 
211  int debugMode;
212  get( INTEGRATOR_DEBUG_MODE, debugMode );
213  if ( (bool)debugMode == true ) {
214  declarations.addDeclaration( debug_mat,dataStruct );
215  }
216  declarations.addDeclaration( rk_ttt,dataStruct );
217  declarations.addDeclaration( rk_xxx,dataStruct );
218  declarations.addDeclaration( rk_kkk,dataStruct );
219 
220  declarations.addDeclaration( rk_A,dataStruct );
221  declarations.addDeclaration( rk_b,dataStruct );
222  declarations.addDeclaration( rk_auxSolver,dataStruct );
223  declarations.addDeclaration( rk_rhsTemp,dataStruct );
224  declarations.addDeclaration( rk_diffsTemp2,dataStruct );
225 
226  if( CONTINUOUS_OUTPUT ) {
227  declarations.addDeclaration( rk_rhsOutputTemp,dataStruct );
228  declarations.addDeclaration( rk_outH,dataStruct );
229  declarations.addDeclaration( rk_out,dataStruct );
230 
231  int measGrid;
232  get( MEASUREMENT_GRID, measGrid );
233  if( (MeasurementGrid)measGrid == ONLINE_GRID ) {
234  for( uint i = 0; i < gridVariables.size(); i++ ) {
235  declarations.addDeclaration( gridVariables[i],dataStruct );
236  }
237  }
238  }
239 
240  return SUCCESSFUL_RETURN;
241 }
242 
243 
245  ) const
246 {
247  declarations.addDeclaration( integrate );
248 
249  if( NX2 != NX ) declarations.addDeclaration( fullRhs );
250  declarations.addDeclaration( rhs );
251  declarations.addDeclaration( diffs_rhs );
252 
253  return SUCCESSFUL_RETURN;
254 }
255 
256 
258 {
259  int sensGen;
260  get( DYNAMIC_SENSITIVITY, sensGen );
262 
263  int useOMP;
264  get(CG_USE_OPENMP, useOMP);
265  if ( useOMP ) {
267  max.setName( "auxVar" );
268  max.setDataStruct( ACADO_LOCAL );
269  if( NX2 > 0 || NXA > 0 ) {
272  }
273  if( NX3 > 0 ) {
275  }
276  for( uint i = 0; i < outputs.size(); i++ ) {
277  outputs[i].setGlobalExportVariable( max );
278  }
279 
281 
282  stringstream s;
283  s << "#pragma omp threadprivate( "
284  << max.getFullName() << ", "
285  << rk_ttt.getFullName() << ", "
286  << rk_xxx.getFullName() << ", "
287  << rk_kkk.getFullName() << ", "
288  << rk_rhsTemp.getFullName() << ", "
290  if( NX2 > 0 || NXA > 0 ) {
291  s << ", " << rk_A.getFullName();
292  s << ", " << rk_b.getFullName();
293  s << ", " << rk_diffsTemp2.getFullName();
295  }
296  s << " )" << endl << endl;
297  code.addStatement( s.str().c_str() );
298  }
299 
300  if( NX1 > 0 ) {
301  code.addFunction( lin_input );
302  code.addStatement( "\n\n" );
303  }
304  if( exportRhs ) {
305  if( NX2 > 0 || NXA > 0 ) {
306  code.addFunction( rhs );
307  code.addStatement( "\n\n" );
308  code.addFunction( diffs_rhs );
309  code.addStatement( "\n\n" );
310  }
311 
312  if( NX3 > 0 ) {
313  code.addFunction( rhs3 );
314  code.addStatement( "\n\n" );
315  }
316 
317  if( CONTINUOUS_OUTPUT ) {
318  uint i;
319  for( i = 0; i < outputs.size(); i++ ) {
320  code.addFunction( outputs[i] );
321  code.addStatement( "\n\n" );
322  }
323  }
324  }
325  returnValue ret;
326  if( NX2 > 0 || NXA > 0 ) ret = solver->getCode( code );
327  if( ret != SUCCESSFUL_RETURN ) return ret;
328  code.addLinebreak(2);
329 
330  int measGrid;
331  get( MEASUREMENT_GRID, measGrid );
332 
333  // export RK scheme
334  uint run5;
335  std::string tempString;
336 
339 
340  string moduleName;
341  get(CG_MODULE_NAME, moduleName);
342 
343  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
344  DMatrix tmp = AA;
345  ExportVariable Ah( moduleName+"_Ah_mat", tmp*=h, STATIC_CONST_REAL );
346  code.addDeclaration( Ah );
347  code.addLinebreak( 2 );
348  // TODO: Ask Milan why this does NOT work properly !!
349  Ah = ExportVariable( moduleName+"_Ah_mat", numStages, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
350 
351  DVector BB( bb );
352  ExportVariable Bh( moduleName+"_Bh_mat", DMatrix( BB*=h ) );
353 
354  DVector CC( cc );
355  ExportVariable C;
356  if( timeDependant ) {
357  C = ExportVariable( moduleName+"_C_mat", DMatrix( CC*=(1.0/grid.getNumIntervals()) ), STATIC_CONST_REAL );
358  code.addDeclaration( C );
359  code.addLinebreak( 2 );
360  C = ExportVariable( moduleName+"_C_mat", 1, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
361  }
362 
363  code.addComment(std::string("Fixed step size:") + toString(h));
364 
365  ExportVariable determinant( "det", 1, 1, REAL, ACADO_LOCAL, true );
366  integrate.addDeclaration( determinant );
367 
368  ExportIndex i( "i" );
369  ExportIndex j( "j" );
370  ExportIndex k( "k" );
371  ExportIndex run( "run" );
372  ExportIndex run1( "run1" );
373  ExportIndex tmp_index1("tmp_index1");
374  ExportIndex tmp_index2("tmp_index2");
375  ExportIndex tmp_index3("tmp_index3");
376  ExportIndex tmp_index4("tmp_index4");
377  ExportVariable tmp_meas("tmp_meas", 1, outputGrids.size(), INT, ACADO_LOCAL);
378 
379  ExportVariable numInt( moduleName+"_numInts", 1, 1, INT );
380  if( !equidistantControlGrid() ) {
381  ExportVariable numStepsV( moduleName+"_numSteps", numSteps, STATIC_CONST_INT );
382  code.addDeclaration( numStepsV );
383  code.addLinebreak( 2 );
384  integrate.addStatement( std::string( "int " ) + numInt.getName() + " = " + numStepsV.getName() + "[" + rk_index.getName() + "];\n" );
385  }
386 
387  prepareOutputEvaluation( code );
388 
389  integrate.addIndex( i );
390  integrate.addIndex( j );
391  integrate.addIndex( k );
392  integrate.addIndex( run );
393  integrate.addIndex( run1 );
394  integrate.addIndex( tmp_index1 );
395  integrate.addIndex( tmp_index2 );
396  if( rk_outputs.size() > 0 ) integrate.addIndex( tmp_index3 );
397  if( rk_outputs.size() > 0 && (grid.getNumIntervals() > 1 || !equidistantControlGrid()) ) {
398  integrate.addIndex( tmp_index4 );
399  }
400  ExportVariable time_tmp( "time_tmp", 1, 1, REAL, ACADO_LOCAL, true );
401  if( CONTINUOUS_OUTPUT ) {
402  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
403  ExportIndex numMeasTmp( (std::string)"numMeasTmp" + toString(run5) );
404  numMeas.push_back( numMeasTmp );
405  integrate.addIndex( numMeas[run5] );
406  }
407 
408  if( (MeasurementGrid)measGrid == ONLINE_GRID ) {
409  integrate.addDeclaration( tmp_meas );
411  integrate.addDeclaration( time_tmp );
412  }
413 
414  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
415  integrate.addStatement( numMeas[run5] == 0 );
416  }
417  }
419  if( inputDim > NX+NXA ) {
421  }
423 
424  if( NXA > 0 ) {
425  integrate.addStatement( std::string( "if( " ) + reset_int.getFullName() + " ) {\n" );
426  for( run5 = 0; run5 < NXA; run5++ ) {
427  for( uint iStage = 0; iStage < numStages; iStage++ ) {
428  integrate.addStatement( rk_kkk.getElement(NX+run5,iStage) == rk_eta.getCol(NX+run5) );
429  }
430  }
431  integrate.addStatement( std::string( "}\n" ) );
432  }
433 
434  // integrator loop:
435  ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
436  ExportStatementBlock *loop;
437  if( equidistantControlGrid() ) {
438  loop = &tmpLoop;
439  }
440  else {
441  loop = &integrate;
442  loop->addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
443  }
444 
445  if( CONTINUOUS_OUTPUT && (MeasurementGrid)measGrid == ONLINE_GRID ) {
446  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
447  loop->addStatement( tmp_index1 == numMeas[run5] );
448  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" );
449  loop->addStatement( tmp_index1 == tmp_index1+1 );
450  loop->addStatement( std::string("}\n") );
451  loop->addStatement( std::string(tmp_meas.get( 0,run5 )) + " = " + tmp_index1.getName() + " - " + numMeas[run5].getName() + ";\n" );
452  }
453  }
454 
455  // PART 1: The linear input system
456  prepareInputSystem( code );
457  solveInputSystem( loop, i, run1, j, tmp_index1, Ah );
458 
459  // PART 2: The fully implicit system
460  solveImplicitSystem( loop, i, run1, j, tmp_index1, ExportIndex(0), Ah, C, determinant );
461 
462  // PART 3: The linear output system
463  prepareOutputSystem( code );
464  solveOutputSystem( loop, i, run1, j, tmp_index1, Ah );
465 
466  // generate continuous OUTPUT:
467  generateOutput( loop, run, i, tmp_index2, tmp_index3, tmp_meas, time_tmp, 0 );
468 
469  // update rk_eta:
470  for( run5 = 0; run5 < NX; run5++ ) {
471  loop->addStatement( rk_eta.getCol( run5 ) += rk_kkk.getRow( run5 )*Bh );
472  }
473  if( NXA > 0) {
474  DMatrix tempCoefs( evaluateDerivedPolynomial( 0.0 ) );
475  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
476  loop->addStatement( std::string("if( run == 0 ) {\n") );
477  }
478  for( run5 = 0; run5 < NXA; run5++ ) {
479  loop->addStatement( rk_eta.getCol( NX+run5 ) == rk_kkk.getRow( NX+run5 )*tempCoefs );
480  }
481  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
482  loop->addStatement( std::string("}\n") );
483  }
484  }
485 
486  loop->addStatement( std::string( reset_int.get(0,0) ) + " = 0;\n" );
487 
488  for( run5 = 0; run5 < rk_outputs.size(); run5++ ) {
489  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
490  loop->addStatement( numMeas[run5].getName() + " += " + numMeasVariables[run5].get(0,run) + ";\n" );
491  }
492  else { // ONLINE_GRID
493  loop->addStatement( numMeas[run5].getName() + " += " + tmp_meas.get(0,run5) + ";\n" );
494  }
495  }
496  loop->addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
497 
498  // end of the integrator loop.
499  if( !equidistantControlGrid() ) {
500  loop->addStatement( "}\n" );
501  }
502  else {
503  integrate.addStatement( *loop );
504  }
505 
506  integrate.addStatement( std::string( "if( " ) + determinant.getFullName() + " < 1e-12 ) {\n" );
508  integrate.addStatement( std::string( "} else if( " ) + determinant.getFullName() + " < 1e-6 ) {\n" );
510  integrate.addStatement( std::string( "} else {\n" ) );
512  integrate.addStatement( std::string( "}\n" ) );
513 
514  code.addFunction( integrate );
515  code.addLinebreak( 2 );
516 
517  return SUCCESSFUL_RETURN;
518 }
519 
520 
522 {
523  if( NX1 > 0 ) {
524  DMatrix mat1 = formMatrix( M11, A11 );
525  rk_mat1 = ExportVariable( "rk_mat1", mat1, STATIC_CONST_REAL );
526  code.addDeclaration( rk_mat1 );
527  // TODO: Ask Milan why this does NOT work properly !!
529  }
530 
531  return SUCCESSFUL_RETURN;
532 }
533 
534 
536 {
537  if( NX3 > 0 ) {
538  DMatrix mat3 = formMatrix( M33, A33 );
539  rk_mat3 = ExportVariable( "rk_mat3", mat3, STATIC_CONST_REAL );
540  code.addDeclaration( rk_mat3 );
541  // TODO: Ask Milan why this does NOT work properly !!
543  }
544 
545  return SUCCESSFUL_RETURN;
546 }
547 
548 
550  if( jacobian.getNumRows() != jacobian.getNumCols() ) {
552  }
553  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
554  uint vars = jacobian.getNumRows();
555  uint i1, j1, i2, j2;
556  DMatrix result = zeros<double>(numStages*vars, numStages*vars);
557  for( i1 = 0; i1 < numStages; i1++ ){
558  for( j1 = 0; j1 < numStages; j1++ ) {
559  for( i2 = 0; i2 < vars; i2++ ){
560  for( j2 = 0; j2 < vars; j2++ ) {
561  if( i1 == j1 ) {
562  result(i1*vars+i2, j1*vars+j2) = mass(i2,j2) - AA(i1,j1)*h*jacobian(i2,j2);
563  }
564  else {
565  result(i1*vars+i2, j1*vars+j2) = -AA(i1,j1)*h*jacobian(i2,j2);
566  }
567  }
568  }
569  }
570  }
571 
572  return result.inverse();
573 }
574 
575 
577 {
578  if( NX1 > 0 ) {
579  block->addStatement( rk_xxx.getCols(0,NX1) == rk_eta.getCols(0,NX1) );
581  ExportForLoop loop( index1,0,numStages );
582  ExportForLoop loop1( index2,0,NX1 );
583  loop1.addStatement( tmp_index == index1*NX1+index2 );
584  loop1.addStatement( rk_b.getRow(tmp_index) == rk_rhsTemp.getRow(index2) );
585  loop.addStatement(loop1);
586  block->addStatement(loop);
587 
588  ExportForLoop loop4( index1,0,numStages );
589  ExportForLoop loop5( index2,0,NX1 );
590  loop5.addStatement( tmp_index == index1*NX1+index2 );
591  loop5.addStatement( rk_kkk.getElement(index2,index1) == rk_mat1.getElement(tmp_index,0)*rk_b.getRow(0) );
592  ExportForLoop loop6( index3,1,numStages*NX1 );
593  loop6.addStatement( rk_kkk.getElement(index2,index1) += rk_mat1.getElement(tmp_index,index3)*rk_b.getRow(index3) );
594  loop5.addStatement(loop6);
595  loop4.addStatement(loop5);
596  block->addStatement(loop4);
597  }
598 
599  return SUCCESSFUL_RETURN;
600 }
601 
602 
603 returnValue ImplicitRungeKuttaExport::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 )
604 {
605  if( NX2 > 0 || NXA > 0 ) {
606 
607  if( DERIVATIVES && REUSE ) block->addStatement( std::string( "if( " ) + reset_int.getFullName() + " ) {\n" );
608  // Initialization iterations:
609  ExportForLoop loop1( index1,0,numItsInit+1 ); // NOTE: +1 because 0 will lead to NaNs, so the minimum number of iterations is 1 at the initialization
610  ExportForLoop loop11( index2,0,numStages );
611  evaluateMatrix( &loop11, index2, index3, tmp_index, k_index, rk_A, Ah, C, true, DERIVATIVES );
612  loop1.addStatement( loop11 );
613  loop1.addStatement( det.getFullName() + " = " + ExportStatement::fcnPrefix + "_" + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_b.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
614  ExportForLoop loopTemp( index3,0,numStages );
615  loopTemp.addStatement( rk_kkk.getSubMatrix( k_index+NX1,k_index+NX1+NX2,index3,index3+1 ) += rk_b.getRows( index3*NX2,index3*NX2+NX2 ) ); // differential states
616  if(NXA > 0) loopTemp.addStatement( rk_kkk.getSubMatrix( k_index+NX,k_index+NX+NXA,index3,index3+1 ) += rk_b.getRows( index3*NXA+numStages*NX2,index3*NXA+numStages*NX2+NXA ) ); // algebraic states
617  loop1.addStatement( loopTemp );
618  block->addStatement( loop1 );
619  if( DERIVATIVES && REUSE ) block->addStatement( std::string( "}\n" ) );
620 
621  // the rest (numIts) of the Newton iterations with reuse of the Jacobian (no evaluation or factorization needed)
622  ExportForLoop loop2( index1,0,numIts );
623  ExportForLoop loop21( index2,0,numStages );
624  evaluateStatesImplicitSystem( &loop21, k_index, Ah, C, index2, index3, tmp_index );
625  evaluateRhsImplicitSystem( &loop21, k_index, index2 );
626  loop2.addStatement( loop21 );
628  loopTemp = ExportForLoop( index3,0,numStages );
629  loopTemp.addStatement( rk_kkk.getSubMatrix( k_index+NX1,k_index+NX1+NX2,index3,index3+1 ) += rk_b.getRows( index3*NX2,index3*NX2+NX2 ) ); // differential states
630  if(NXA > 0) loopTemp.addStatement( rk_kkk.getSubMatrix( k_index+NX,k_index+NX+NXA,index3,index3+1 ) += rk_b.getRows( index3*NXA+numStages*NX2,index3*NXA+numStages*NX2+NXA ) ); // algebraic states
631  loop2.addStatement( loopTemp );
632  block->addStatement( loop2 );
633 
634  if( DERIVATIVES ) {
635  // solution calculated --> evaluate and save the necessary derivatives in rk_diffsTemp and update the matrix rk_A:
636  ExportForLoop loop3( index2,0,numStages );
637  evaluateMatrix( &loop3, index2, index3, tmp_index, k_index, rk_A, Ah, C, false, DERIVATIVES );
638  block->addStatement( loop3 );
639  }
640 
641  // IF DEBUG MODE:
642  int debugMode;
643  get( INTEGRATOR_DEBUG_MODE, debugMode );
644  if ( (bool)debugMode == true ) {
645  block->addStatement( debug_mat == rk_A );
646  }
647  }
648 
649  return SUCCESSFUL_RETURN;
650 }
651 
652 
653 returnValue ImplicitRungeKuttaExport::solveOutputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index, const ExportVariable& Ah, bool DERIVATIVES )
654 {
655  if( NX3 > 0 ) {
656  ExportForLoop loop( index1,0,numStages );
657  evaluateStatesOutputSystem( &loop, Ah, index1 );
659  if( DERIVATIVES ) loop.addFunctionCall( getNameOutputDiffs(), rk_xxx, rk_diffsTemp3.getAddress(index1,0) );
660  block->addStatement( loop );
661 
662  ExportForLoop loop4( index1,0,numStages );
663  ExportForLoop loop5( index2,0,NX3 );
664  loop5.addStatement( tmp_index == index1*NX3+index2 );
665  loop5.addStatement( rk_kkk.getElement(NX1+NX2+index2,index1) == rk_mat3.getElement(tmp_index,0)*rk_b.getRow(0) );
666  ExportForLoop loop6( index3,1,numStages*NX3 );
667  loop6.addStatement( rk_kkk.getElement(NX1+NX2+index2,index1) += rk_mat3.getElement(tmp_index,index3)*rk_b.getRow(index3) );
668  loop5.addStatement(loop6);
669  loop4.addStatement(loop5);
670  block->addStatement(loop4);
671  }
672 
673  return SUCCESSFUL_RETURN;
674 }
675 
676 
678 {
679  ExportForLoop loop1( i, 0, NX1+NX2 );
680  loop1.addStatement( rk_xxx.getCol( i ) == rk_eta.getCol( i ) );
681  loop1.addStatement( tmp_index == k_index + i );
682  for( uint j = 0; j < numStages; j++ ) {
683  loop1.addStatement( rk_xxx.getCol( i ) += Ah.getElement(stage,j)*rk_kkk.getElement( tmp_index,j ) );
684  }
685  block->addStatement( loop1 );
686 
687  ExportForLoop loop3( i, 0, NXA );
688  loop3.addStatement( tmp_index == k_index + i + NX );
689  loop3.addStatement( rk_xxx.getCol( NX+i ) == rk_kkk.getElement( tmp_index,stage ) );
690  block->addStatement( loop3 );
691 
692  ExportForLoop loop4( i, 0, NDX2 );
693  loop4.addStatement( tmp_index == k_index + i );
694  loop4.addStatement( rk_xxx.getCol( inputDim-diffsDim+i ) == rk_kkk.getElement( tmp_index,stage ) );
695  block->addStatement( loop4 );
696 
697  if( C.getDim() > 0 ) { // There is a time dependence, so it must be set
698  block->addStatement( rk_xxx.getCol( inputDim-diffsDim+NDX ) == rk_ttt + C.getCol(stage) );
699  }
700 
701  return SUCCESSFUL_RETURN;
702 }
703 
704 
706 {
707  uint i,j;
708  for( i = 0; i < NX1+NX2; i++ ) {
709  block->addStatement( rk_xxx.getCol( i ) == rk_eta.getCol( i ) );
710  for( j = 0; j < numStages; j++ ) {
711  block->addStatement( rk_xxx.getCol( i ) += Ah.getElement(stage,j)*rk_kkk.getElement( i,j ) );
712  }
713  }
714  for( i = 0; i < NX3; i++ ) {
715  block->addStatement( rk_xxx.getCol( NX1+NX2+i ) == rk_eta.getCol( NX1+NX2+i ) );
716  }
717  for( i = 0; i < NXA3; i++ ) {
718  block->addStatement( rk_xxx.getCol( NX+i ) == rk_kkk.getElement( NX+i,stage ) );
719  }
720  for( i = 0; i < NDX3; i++ ) {
721  block->addStatement( rk_xxx.getCol( inputDim-diffsDim+i ) == rk_kkk.getElement( i,stage ) );
722  }
723 
724  return SUCCESSFUL_RETURN;
725 }
726 
727 
729 {
730  DMatrix zeroM = zeros<double>( NX2+NXA,1 );
732  // matrix rk_b:
733  if( NDX2 == 0 ) {
734  block->addStatement( rk_b.getRows( stage*(NX2+NXA),stage*(NX2+NXA)+NX2 ) == rk_kkk.getSubMatrix( k_index+NX1,k_index+NX1+NX2,stage,stage+1 ) - rk_rhsTemp.getRows( 0,NX2 ) );
735  }
736  else {
737  block->addStatement( rk_b.getRows( stage*(NX2+NXA),stage*(NX2+NXA)+NX2 ) == zeroM.getRows( 0,NX2-1 ) - rk_rhsTemp.getRows( 0,NX2 ) );
738  }
739  if( NXA > 0 ) {
740  block->addStatement( rk_b.getRows( stage*(NX2+NXA)+NX2,(stage+1)*(NX2+NXA) ) == zeroM.getRows( 0,NXA-1 ) - rk_rhsTemp.getRows( NX2,NX2+NXA ) );
741  }
742 
743  return SUCCESSFUL_RETURN;
744 }
745 
746 
747 returnValue ImplicitRungeKuttaExport::evaluateMatrix( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index, const ExportIndex& k_index, const ExportVariable& _rk_A, const ExportVariable& Ah, const ExportVariable& C, bool evaluateB, bool DERIVATIVES )
748 {
749  uint i;
750 
751  evaluateStatesImplicitSystem( block, k_index, Ah, C, index1, index2, tmp_index );
752 
753  ExportIndex indexDiffs(index1);
754  if( !DERIVATIVES ) indexDiffs = ExportIndex(0);
755 
756  block->addFunctionCall( getNameDiffsRHS(), rk_xxx, rk_diffsTemp2.getAddress(indexDiffs,0) );
757  ExportForLoop loop2( index2,0,NX2+NXA );
758  loop2.addStatement( tmp_index == index1*(NX2+NXA)+index2 );
759  for( i = 0; i < numStages; i++ ) { // differential states
760  if( NDX2 == 0 ) {
761  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) == Ah.getElement( index1,i )*rk_diffsTemp2.getSubMatrix( indexDiffs,indexDiffs+1,index2*(NVARS2)+NX1,index2*(NVARS2)+NX1+NX2 ) );
762  loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) " );
763  loop2.addStatement( _rk_A.getElement( tmp_index,index2+i*NX2 ) -= 1 );
764  }
765  else {
766  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) == Ah.getElement( index1,i )*rk_diffsTemp2.getSubMatrix( indexDiffs,indexDiffs+1,index2*(NVARS2)+NX1,index2*(NVARS2)+NX1+NX2 ) );
767  loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) {\n" );
768  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) += rk_diffsTemp2.getSubMatrix( indexDiffs,indexDiffs+1,index2*(NVARS2)+NVARS2-NX2,index2*(NVARS2)+NVARS2 ) );
769  loop2.addStatement( std::string( "}\n" ) );
770  }
771  }
772  if( NXA > 0 ) {
773  DMatrix zeroM = zeros<double>( 1,NXA );
774  for( i = 0; i < numStages; i++ ) { // algebraic states
775  loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) {\n" );
776  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,numStages*NX2+i*NXA,numStages*NX2+i*NXA+NXA ) == rk_diffsTemp2.getSubMatrix( indexDiffs,indexDiffs+1,index2*(NVARS2)+NX1+NX2,index2*(NVARS2)+NX1+NX2+NXA ) );
777  loop2.addStatement( std::string( "}\n else {\n" ) );
778  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,numStages*NX2+i*NXA,numStages*NX2+i*NXA+NXA ) == zeroM );
779  loop2.addStatement( std::string( "}\n" ) );
780  }
781  }
782  block->addStatement( loop2 );
783  if( evaluateB ) {
784  evaluateRhsImplicitSystem( block, k_index, index1 );
785  }
786 
787  return SUCCESSFUL_RETURN;
788 }
789 
790 
792  const ExportIndex& index1, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2,
793  const ExportVariable& tmp_meas, const ExportVariable& time_tmp, const uint directions )
794 {
795  uint i, j;
796  int measGrid;
797  get( MEASUREMENT_GRID, measGrid );
798 
799  for( i = 0; i < rk_outputs.size(); i++ ) {
800  ExportStatementBlock *loop1;
801  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
802  loop1 = block;
803  loop1->addStatement( std::string("for(") + index1.getName() + " = 0; " + index1.getName() + " < (int)" + numMeasVariables[i].get(0,index0) + "; " + index1.getName() + "++) {\n" );
804  loop1->addStatement( tmp_index1 == numMeas[i]+index1 );
805  for( j = 0; j < numStages; j++ ) {
806  loop1->addStatement( rk_outH.getRow(j) == polynVariables[i].getElement( tmp_index1,j ) );
807  }
808  if( numXA_output(i) > 0 || numDX_output(i) > 0 ) {
809  for( j = 0; j < numStages; j++ ) {
810  loop1->addStatement( rk_out.getRow(j) == polynDerVariables[i].getElement( tmp_index1,j ) );
811  }
812  }
813  }
814  else { // ONLINE_GRID
815  loop1 = block;
816  loop1->addStatement( std::string(tmp_index2.getName()) + " = " + tmp_meas.get( 0,i ) + ";\n" );
817  loop1->addStatement( std::string("for(") + index1.getName() + " = 0; " + index1.getName() + " < (int)" + tmp_index2.getName() + "; " + index1.getName() + "++) {\n" );
818  loop1->addStatement( tmp_index1 == numMeas[i]+index1 );
819 
820  uint scale = grid.getNumIntervals();
821  double scale2 = 1.0/grid.getNumIntervals();
822  loop1->addStatement( time_tmp.getName() + " = " + toString(scale) + "*(" + gridVariables[i].get(0,tmp_index1) + "-" + toString(scale2) + "*" + index0.getName() + ");\n" );
823 
824  std::string h = toString((grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals());
825  evaluatePolynomial( *loop1, rk_outH, time_tmp, h );
826  if( numXA_output(i) > 0 || numDX_output(i) > 0 ) evaluateDerivedPolynomial( *loop1, rk_out, time_tmp );
827  }
828 
829  DVector dependencyX, dependencyZ, dependencyDX;
830  if( exportRhs || crsFormat ) {
831  dependencyX = outputDependencies[i].getCols( 0,NX-1 ).sumRow();
832  if( numXA_output(i) > 0 ) dependencyZ = outputDependencies[i].getCols( NX,NX+NXA-1 ).sumRow();
833  if( numDX_output(i) > 0 ) dependencyDX = outputDependencies[i].getCols( NX+NXA+NU,NX+NXA+NU+NDX-1 ).sumRow();
834  }
835  for( j = 0; j < NX; j++ ) {
836  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyX(j)) != 0 ) {
837  loop1->addStatement( rk_xxx.getCol( j ) == rk_eta.getCol( j ) + rk_kkk.getRow( j )*rk_outH );
838  }
839  }
840  for( j = 0; j < numXA_output(i); j++ ) {
841  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyZ(j)) != 0 ) {
842  loop1->addStatement( rk_xxx.getCol( NX+j ) == rk_kkk.getRow( NX+j )*rk_out );
843  }
844  }
845  for( j = 0; j < numDX_output(i); j++ ) {
846  if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyDX(j)) != 0 ) {
848  }
849  }
851  uint numOutputs = getDimOUTPUT( i );
852  uint outputDim = numOutputs*(1+directions);
853  loop1->addStatement( tmp_index1 == numMeas[i]*outputDim+index1*(numOutputs*(1+directions)) );
854  for( j = 0; j < numOutputs; j++ ) {
855  loop1->addStatement( rk_outputs[i].getCol( tmp_index1+j ) == rk_rhsOutputTemp.getCol( j ) );
856  }
857  loop1->addStatement( "}\n" );
858  }
859 
860  return SUCCESSFUL_RETURN;
861 }
862 
863 
865 {
866  uint i, j, k;
868 
869  for( i = 0; i < numStages; i++ ) {
870  for( j = 0; j < numStages; j++ ) {
871  DD( i, j ) = 1;
872  for( k = 0; k < numStages; k++ ) {
873  if( k != j ) {
874  DD( i, j ) *= ((1+cc(i))-cc(k))/(cc(j)-cc(k));
875  }
876  }
877  }
878  }
879  return SUCCESSFUL_RETURN;
880 }
881 
882 
884 {
885  uint i, j, k, index;
886  double sum;
887  DVector cVec( numStages-1 );
888  DVector products;
890 
891  for( i = 0; i < numStages; i++ ) {
892  for( j = 0; j < numStages; j++ ) {
893  coeffs( i, j ) = 1/((double) numStages-i);
894  index = 0;
895  for( k = 0; k < numStages; k++ ) {
896  if( k != j ) {
897  coeffs( i, j ) *= 1/((double) cc(j)-cc(k));
898  cVec(index) = cc(k);
899  index++;
900  }
901  }
902 
903  if( i > 0 ) {
904  products = computeCombinations( cVec, 0, i );
905  sum = 0.0;
906  for( k = 0; k < products.getDim(); k++ ) {
907  sum += products(k);
908  }
909  if( i%2 == 0 ) {
910  coeffs( i, j ) *= sum;
911  }
912  else {
913  coeffs( i, j ) *= (-1.0*sum);
914  }
915  }
916  }
917  }
918 
919  return SUCCESSFUL_RETURN;
920 }
921 
922 
924  uint k, l;
925  DVector products;
926 
927  if( numEls == 0 ) {
928  products = DVector(1);
929  products(0) = 1;
930  return products;
931  }
932  products = DVector();
933  for( k = index; k < cVec.getDim()-numEls+1; k++ ) {
934  DVector temp = computeCombinations( cVec, k+1, numEls-1 );
935  for( l = 0; l < temp.getDim(); l++ ) {
936  temp(l) *= cVec(k);
937  }
938  products.append(temp);
939  }
940 
941  return products;
942 }
943 
944 
946 {
947  int measGrid;
948  get( MEASUREMENT_GRID, measGrid );
949  DMatrix polynV(totalMeas[index],numStages);
950 
951  double scale2 = 1.0/(grid.getLastTime() - grid.getFirstTime());
952  for( uint i = 0; i < totalMeas[index]; i++ ) {
953  double time = outputGrids[index].getTime(i);
954  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
955  uint interv = getIntegrationInterval( time );
956  time = (time-scale2*grid.getTime(interv))/(scale2*(grid.getTime(interv+1)-grid.getTime(interv)));
957  }
958  polynV.setRow( i, evaluatePolynomial( time ) );
959  }
960 
961  return polynV;
962 }
963 
964 
966 {
967  uint i, j;
968  DVector coeffsPolyn( numStages );
969 
970  for( j = 0; j < numStages; j++ ) {
971  coeffsPolyn( j ) = 0.0;
972  for( i = 0; i < numStages; i++ ) {
973  coeffsPolyn( j ) += pow( time, static_cast<int>(numStages-i) )*coeffs( i,j );
974  }
975  }
976 
977  return coeffsPolyn;
978 }
979 
980 
982 {
983  uint i, j;
984 
985  block.addStatement( polynEvalVar == gridVariable );
986  for( i = 0; i < numStages; i++ ) {
987  for( j = 0; j < numStages; j++ ) {
988  if( i == 0 ) {
989  block.addStatement( variable.getRow( j ) == polynEvalVar*coeffs( numStages-1-i,j ) );
990  }
991  else {
992  block.addStatement( variable.getRow( j ) += polynEvalVar*coeffs( numStages-1-i,j ) );
993  }
994  }
995  if( i < (numStages-1) ) block.addStatement( polynEvalVar == polynEvalVar*gridVariable );
996  }
997  for( j = 0; j < numStages; j++ ) {
998  block.addStatement( (std::string)variable.getFullName() + "[" + toString(j) + "] *= " + toString(h) + ";\n" );
999  }
1000 
1001  return SUCCESSFUL_RETURN;
1002 }
1003 
1004 
1006 {
1007  int measGrid;
1008  get( MEASUREMENT_GRID, measGrid );
1009  DMatrix polynDerV(totalMeas[index],numStages);
1010 
1011  double scale2 = 1.0/(grid.getLastTime() - grid.getFirstTime());
1012  for( uint i = 0; i < totalMeas[index]; i++ ) {
1013  double time = outputGrids[index].getTime(i);
1014  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
1015  uint interv = getIntegrationInterval( time );
1016  time = (time-scale2*grid.getTime(interv))/(scale2*(grid.getTime(interv+1)-grid.getTime(interv)));
1017  }
1018  polynDerV.setRow( i, evaluateDerivedPolynomial( time ) );
1019  }
1020 
1021  return polynDerV;
1022 }
1023 
1024 
1026 {
1027  uint i, j;
1028  DVector coeffsPolyn( numStages );
1029 
1030  // construct the Lagrange interpolating polynomials:
1031  for( i = 0; i < numStages; i++ ) {
1032  coeffsPolyn( i ) = 1.0;
1033  for( j = 0; j < numStages; j++ ) {
1034  if( i != j ) {
1035  coeffsPolyn( i ) *= (time-cc(j))/(cc(i)-cc(j));
1036  }
1037  }
1038  }
1039 
1040  return coeffsPolyn;
1041 }
1042 
1043 
1045 {
1046  uint i, j;
1047 
1048  // construct the Lagrange interpolating polynomials:
1049  for( i = 0; i < numStages; i++ ) {
1050  for( j = 0; j < numStages; j++ ) {
1051  if( (i == 0 && j == 1) || (i != j && j == 0) ) {
1052  block.addStatement( (std::string)variable.getFullName() + "[" + toString(i) + "] = (" + gridVariable.getName() + " - " + toString(cc(j)) + ")*" + toString(1/(cc(i)-cc(j))) + ";\n" );
1053  }
1054  else if( i != j ) {
1055  block.addStatement( (std::string)variable.getFullName() + "[" + toString(i) + "] *= (" + gridVariable.getName() + " - " + toString(cc(j)) + ")*" + toString(1/(cc(i)-cc(j))) + ";\n" );
1056  }
1057  }
1058  }
1059 
1060  return SUCCESSFUL_RETURN;
1061 }
1062 
1063 
1065 {
1066  DVector meas( grid.getNumIntervals() );
1067  meas.setZero();
1068 
1069  for( uint i = 0; i < outputGrids[index].getNumIntervals(); i++ ) {
1070  uint interv = getIntegrationInterval( outputGrids[index].getTime(i) );
1071  meas(interv) = meas(interv)+1;
1072  }
1073 
1074  return meas;
1075 }
1076 
1077 
1079 {
1081 
1082  int measGrid;
1083  get( MEASUREMENT_GRID, measGrid );
1084 
1085  int intMode;
1087  switch( (ImplicitIntegratorMode) intMode ) {
1088  case IFTR:
1089  REUSE = true;
1090  break;
1091  case IFT:
1092  REUSE = false;
1093  break;
1094  case LIFTED:
1095  REUSE = true;
1096  break;
1097  default:
1098  return ACADOERROR( RET_INVALID_OPTION );
1099  }
1100 
1101  int newNumIts;
1103  if (newNumIts >= 0) {
1104  numIts = newNumIts;
1105  }
1106 
1107  int newNumItsInit;
1109  if (newNumItsInit >= 0) {
1110  numItsInit = newNumItsInit;
1111  }
1112 
1113  int debugMode;
1114  get( INTEGRATOR_DEBUG_MODE, debugMode );
1115 
1117  dummy4.clearStaticCounters();
1119 
1120  AlgebraicState dummy3;
1121  dummy3.clearStaticCounters();
1122  z = AlgebraicState("", NXA, 1);
1123 
1124  uint Xmax = NX1;
1125  if( NX2 > Xmax ) Xmax = NX2;
1126  if( NX3 > Xmax ) Xmax = NX3;
1127  NVARS2 = NX1+NX2+NXA+NU+NDX2;
1128  NVARS3 = 0;
1129  diffsDim = 0;
1130  inputDim = NX+NXA + NU + NOD;
1131 
1132  int useOMP;
1133  get(CG_USE_OPENMP, useOMP);
1134  ExportStruct structWspace;
1135  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
1136 
1137  uint timeDep = 0;
1138  if( timeDependant ) timeDep = 1;
1139 
1140  rk_ttt = ExportVariable( "rk_ttt", 1, 1, REAL, structWspace, true );
1141  rk_xxx = ExportVariable( "rk_xxx", 1, inputDim+NDX+timeDep, REAL, structWspace );
1142  rk_kkk = ExportVariable( "rk_kkk", NX+NXA, numStages, REAL, structWspace );
1143  rk_A = ExportVariable( "rk_A", numStages*(NX2+NXA), numStages*(NX2+NXA), REAL, structWspace );
1144  if ( (bool)debugMode == true && useOMP ) {
1145  return ACADOERROR( RET_INVALID_OPTION );
1146  }
1147  else {
1149  }
1150  rk_b = ExportVariable( "rk_b", numStages*(Xmax+NXA), 1, REAL, structWspace );
1151  rk_rhsTemp = ExportVariable( "rk_rhsTemp", NX+NXA+NDX, 1, REAL, structWspace );
1152  rk_diffsTemp2 = ExportVariable( "rk_diffsTemp2", 1, (NX2+NXA)*(NVARS2), REAL, structWspace );
1153  rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
1154  rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
1155  integrate = ExportFunction( "integrate", rk_eta );
1156  uint i;
1157  for( i = 0; i < rk_outputs.size(); i++ ) {
1159  }
1163 
1164  rk_eta.setDoc( "Working array of size " + toString( rk_eta.getDim() ) + " to pass the input values and return the results." );
1165  for( i = 0; i < rk_outputs.size(); i++ ) {
1166  rk_outputs[i].setDoc( "Working array of size " + toString( rk_outputs[ i ].getDim() ) + " to return the extra output results." );
1167  }
1168  reset_int.setDoc( "The internal memory of the integrator can be reset." );
1169  rk_index.setDoc( "Number of the shooting interval." );
1170  error_code.setDoc( "Status code of the integrator." );
1171  integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
1172  integrate.addLinebreak( ); // TO MAKE SURE IT GETS EXPORTED
1173 
1175  rhs_out = ExportVariable( "f", NX+NXA, 1, REAL, ACADO_LOCAL );
1176  fullRhs = ExportFunction( "full_rhs", rhs_in, rhs_out );
1177  rhs_in.setDoc( "The state and parameter values." );
1178  rhs_out.setDoc( "Right-hand side evaluation." );
1179  fullRhs.doc( "Evaluates the right-hand side of the full model." );
1180  fullRhs.addLinebreak( ); // FIX: TO MAKE SURE IT GETS EXPORTED
1181 
1182  int gradientUp;
1183  get( LIFTED_GRADIENT_UPDATE, gradientUp );
1184  bool gradientUpdate = (bool) gradientUp;
1185 
1186  int sensGen;
1187  get( DYNAMIC_SENSITIVITY, sensGen );
1188  if( NX2 > 0 || NXA > 0 ) {
1189  // setup linear solver:
1190  int solverType;
1191  userInteraction->get( LINEAR_ALGEBRA_SOLVER,solverType );
1192 
1193  if ( solver )
1194  delete solver;
1195  solver = 0;
1196 
1197  switch( (LinearAlgebraSolver) solverType ) {
1198  case GAUSS_LU:
1200  if( (ImplicitIntegratorMode) intMode == LIFTED ) {
1201  solver->init( (NX2+NXA)*numStages, NX+NU+1 );
1202  }
1203  else {
1204  solver->init( (NX2+NXA)*numStages );
1205  }
1206  if( (ExportSensitivityType)sensGen == SYMMETRIC || (ExportSensitivityType)sensGen == FORWARD_OVER_BACKWARD || (ExportSensitivityType)sensGen == BACKWARD || gradientUpdate ) solver->setTranspose( true ); // BACKWARD propagation
1207  solver->setReuse( true ); // IFTR method
1208  solver->setup();
1210  break;
1211  case SIMPLIFIED_IRK_NEWTON:
1212  if( numStages == 3 || numStages == 4 ) {
1215  solver->init( NX2+NXA, NX+NU+1 );
1216  if( (ExportSensitivityType)sensGen == SYMMETRIC || (ExportSensitivityType)sensGen == FORWARD_OVER_BACKWARD || (ExportSensitivityType)sensGen == BACKWARD || gradientUpdate ) solver->setTranspose( true ); // BACKWARD propagation
1217  solver->setReuse( true ); // IFTR method
1218  solver->setup();
1220 
1221  if( numStages == 3 ){
1223  IRKsolver->setEigenvalues(eig);
1224 // IRKsolver->setTransformations(simplified_transf1, simplified_transf2);
1226 
1227  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
1228  IRKsolver->setStepSize(h);
1229  if( NDX2 > 0 || NXA > 0 ) {
1230  IRKsolver->setImplicit( true );
1231  solver->setup();
1232  }
1233  }
1234  else if( numStages == 4 ) {
1236  IRKsolver->setEigenvalues(eig);
1237 // IRKsolver->setTransformations(simplified_transf1, simplified_transf2);
1239 
1240  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
1241  IRKsolver->setStepSize(h);
1242  if( NDX2 > 0 || NXA > 0 ) {
1243  IRKsolver->setImplicit( true );
1244  solver->setup();
1245  }
1246  }
1247  }
1248  else {
1250  }
1251  break;
1252  case SINGLE_IRK_NEWTON:
1253  if( numStages == 3 || numStages == 4 ) {
1256  solver->init( NX2+NXA, NX+NU+1 );
1257  if( (ExportSensitivityType)sensGen == SYMMETRIC || (ExportSensitivityType)sensGen == FORWARD_OVER_BACKWARD || (ExportSensitivityType)sensGen == BACKWARD || gradientUpdate ) solver->setTranspose( true ); // BACKWARD propagation
1258  solver->setReuse( true ); // IFTR method
1259  solver->setup();
1261 
1262  if( numStages == 3 ) {
1263  ExportIRK3StageSingleNewton* IRKsolver = dynamic_cast<ExportIRK3StageSingleNewton *>(solver);
1265 
1266  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
1267  IRKsolver->setStepSize(h);
1268  if( NDX2 > 0 || NXA > 0 ) {
1269  IRKsolver->setImplicit( true );
1270  solver->setup();
1271  }
1272  }
1273  else if( numStages == 4 ) {
1274  ExportIRK4StageSingleNewton* IRKsolver = dynamic_cast<ExportIRK4StageSingleNewton *>(solver);
1276 
1277  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
1278  IRKsolver->setStepSize(h);
1279  if( NDX2 > 0 || NXA > 0 ) {
1280  IRKsolver->setImplicit( true );
1281  solver->setup();
1282  }
1283  }
1284  }
1285  else {
1287  }
1288  break;
1289  case HOUSEHOLDER_QR:
1291  solver->init( (NX2+NXA)*numStages );
1292  solver->setReuse( true ); // IFTR method
1293  solver->setup();
1295  if( (ImplicitIntegratorMode) intMode == LIFTED ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
1296  break;
1297  default:
1298  return ACADOERROR( RET_INVALID_OPTION );
1299  }
1300  }
1301 
1302  return SUCCESSFUL_RETURN;
1303 }
1304 
1305 
1307  eig = _eig;
1308 
1309  return SUCCESSFUL_RETURN;
1310 }
1311 
1312 
1313 returnValue ImplicitRungeKuttaExport::setSimplifiedTransformations( const DMatrix& _transf1, const DMatrix& _transf2, const DMatrix& _transf1_T, const DMatrix& _transf2_T ) {
1314  simplified_transf1 = _transf1;
1315  simplified_transf2 = _transf2;
1316  simplified_transf1_T = _transf1_T;
1317  simplified_transf2_T = _transf2_T;
1318 
1319  return SUCCESSFUL_RETURN;
1320 }
1321 
1322 
1324  simplified_transf1 = _transf1;
1325  simplified_transf2 = _transf2;
1326 
1327  return SUCCESSFUL_RETURN;
1328 }
1329 
1330 
1331 returnValue ImplicitRungeKuttaExport::setSingleTransformations( const double _tau, const DVector& _low_tria, const DMatrix& _transf1, const DMatrix& _transf2 ) {
1332  tau = _tau;
1333  low_tria = _low_tria;
1334  single_transf1 = _transf1;
1335  single_transf2 = _transf2;
1336 
1337  return SUCCESSFUL_RETURN;
1338 }
1339 
1340 
1341 returnValue ImplicitRungeKuttaExport::setSingleTransformations( const double _tau, const DVector& _low_tria, const DMatrix& _transf1, const DMatrix& _transf2, const DMatrix& _transf1_T, const DMatrix& _transf2_T ) {
1342  tau = _tau;
1343  low_tria = _low_tria;
1344  single_transf1 = _transf1;
1345  single_transf2 = _transf2;
1346  single_transf1_T = _transf1_T;
1347  single_transf2_T = _transf2_T;
1348 
1349  return SUCCESSFUL_RETURN;
1350 }
1351 
1352 
1353 returnValue ImplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_, const std::vector<Expression> outputExpressions_ ) {
1354 
1355  int sensGen;
1356  get( DYNAMIC_SENSITIVITY, sensGen );
1357  sensGen = ( (ExportSensitivityType)sensGen != NO_SENSITIVITY );
1358 
1360  CONTINUOUS_OUTPUT = true;
1361  if( outputGrids_.size() != outputExpressions_.size() ) return ACADOERROR( RET_INVALID_ARGUMENTS );
1362  outputGrids = outputGrids_;
1363  outputExpressions = outputExpressions_;
1364 
1365  int measGrid;
1366  get( MEASUREMENT_GRID, measGrid );
1367 
1368  numDX_output = DVector(outputGrids.size());
1369  numXA_output = DVector(outputGrids.size());
1371 
1372  uint i;
1373  uint maxOutputs = 0;
1374  uint maxVARS = 0;
1375  rk_outputs.clear();
1376  outputs.clear();
1377  diffs_outputs.clear();
1378  for( i = 0; i < outputGrids.size(); i++ ) {
1379  uint numOutputs = outputExpressions_[i].getDim();
1380  uint outputDim = outputGrids[i].getNumIntervals( )*numOutputs*(NX+NU+1);
1381  if( !sensGen ) outputDim = outputGrids[i].getNumIntervals( )*numOutputs;
1382 
1383  if( numOutputs > maxOutputs ) maxOutputs = numOutputs;
1384 
1385  OutputFcn f_Output;
1386  f_Output << outputExpressions_[i];
1387 
1388  if( f_Output.getNDX() > 0 ) numDX_output(i) = NDX;
1389  else numDX_output(i) = 0;
1390 
1391  if( f_Output.getNXA() > 0 ) numXA_output(i) = NXA;
1392  else numXA_output(i) = 0;
1393 
1394  OutputFcn g_Output;
1395  for( uint j = 0; j < outputExpressions_[i].getDim(); j++ ) {
1396  g_Output << forwardDerivative( outputExpressions_[i](j), x );
1397  g_Output << forwardDerivative( outputExpressions_[i](j), z );
1398  g_Output << forwardDerivative( outputExpressions_[i](j), u );
1399  if( numDX_output(i) > 0 ) g_Output << forwardDerivative( outputExpressions_[i](j), dx );
1400  }
1401  if( numDX_output(i) > 0 ) {
1402  numVARS_output(i) = NX+NXA+NU+NDX;
1403  }
1404  else {
1405  numVARS_output(i) = NX+NXA+NU;
1406  }
1407  if( numVARS_output(i) > maxVARS ) maxVARS = numVARS_output(i);
1408 
1409  ExportAcadoFunction OUTPUT, diffs_OUTPUT;
1410  val = val & OUTPUT.init( f_Output,std::string("acado_output")+toString(i)+"_rhs",NX,NXA,NU,NP,NDX,NOD ) & diffs_OUTPUT.init( g_Output,std::string("acado_output")+toString(i)+"_diffs",NX,NXA,NU,NP,NDX,NOD );
1411 
1412  ExportVariable rk_output( std::string("rk_output")+toString(i), 1, outputDim, REAL );
1413  rk_outputs.push_back( rk_output );
1414 
1415  outputs.push_back( OUTPUT );
1416  if( sensGen ) diffs_outputs.push_back( diffs_OUTPUT );
1417 
1418  DMatrix dependencyMat = outputExpressions[i].getDependencyPattern( x );
1419  if(z.getDim()>0) dependencyMat.appendCols( outputExpressions[i].getDependencyPattern( z ) );
1420  if(u.getDim()>0) dependencyMat.appendCols( outputExpressions[i].getDependencyPattern( u ) );
1421  if(dx.getDim()>0) dependencyMat.appendCols( outputExpressions[i].getDependencyPattern( dx ) );
1422 
1423  outputDependencies.push_back( dependencyMat );
1424  totalMeas.push_back( outputGrids[i].getNumIntervals() );
1425 
1426  // Export output grids
1427  ExportVariable gridVariable( (std::string)"gridOutput" + toString( i ), 1, totalMeas[i], REAL, ACADO_VARIABLES );
1428  gridVariables.push_back( gridVariable );
1429  }
1430 
1431  int useOMP;
1432  get(CG_USE_OPENMP, useOMP);
1433  ExportStruct structWspace;
1434  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
1435 
1436  setup();
1437  rk_rhsOutputTemp = ExportVariable( "rk_rhsOutputTemp", 1, maxOutputs, REAL, structWspace );
1438  if( sensGen ) rk_diffsOutputTemp = ExportVariable( "rk_diffsOutputTemp", 1, maxOutputs*(maxVARS), REAL, structWspace );
1439  rk_outH = ExportVariable( "rk_outH", numStages, 1, REAL, structWspace );
1440  rk_out = ExportVariable( "rk_out2", numStages, 1, REAL, structWspace );
1441  polynEvalVar = ExportVariable( "tmp_polyn", 1, 1, REAL, ACADO_LOCAL, true );
1442 
1443  return ( val );
1444 }
1445 
1446 
1447 returnValue ImplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_,
1448  const std::vector<std::string> _outputNames,
1449  const std::vector<std::string> _diffs_outputNames,
1450  const std::vector<uint> _dims_output ) {
1451 
1452  if(rhs.isExternal() == true && (rk_outputs.size() + outputs.size() + diffs_outputs.size()) == 0) {
1453  int sensGen;
1454  get( DYNAMIC_SENSITIVITY, sensGen );
1455  sensGen = ( (ExportSensitivityType)sensGen != NO_SENSITIVITY );
1456 
1457  CONTINUOUS_OUTPUT = true;
1458  if( outputGrids_.size() != _outputNames.size() || outputGrids_.size() != _diffs_outputNames.size() || outputGrids_.size() != _dims_output.size() ) {
1460  }
1461  outputGrids = outputGrids_;
1462  num_outputs = _dims_output;
1463 
1464  int measGrid;
1465  get( MEASUREMENT_GRID, measGrid );
1466 
1467  numDX_output = DVector(outputGrids.size());
1468  numXA_output = DVector(outputGrids.size());
1470 
1471  uint i;
1472  uint maxOutputs = 0;
1473  rk_outputs.clear();
1474  outputs.clear();
1475  diffs_outputs.clear();
1476  for( i = 0; i < outputGrids.size(); i++ ) {
1477  uint numOutputs = num_outputs[i];
1478  uint outputDim = outputGrids[i].getNumIntervals( )*numOutputs*(NX+NU+1);
1479  if( !sensGen ) outputDim = outputGrids[i].getNumIntervals( )*numOutputs;
1480 
1481  if( numOutputs > maxOutputs ) maxOutputs = numOutputs;
1482 
1483  numDX_output(i) = NDX; // worst-case scenario
1484  numXA_output(i) = NXA; // worst-case scenario
1485  numVARS_output(i) = NX+NXA+NU+NDX;
1486 
1487  ExportAcadoFunction OUTPUT(_outputNames[i]);
1488  ExportAcadoFunction diffs_OUTPUT(_diffs_outputNames[i]);
1489 
1490  outputs.push_back( OUTPUT );
1491  if( sensGen ) diffs_outputs.push_back( diffs_OUTPUT );
1492 
1493  ExportVariable rk_output( std::string("rk_output")+toString(i), 1, outputDim, REAL );
1494  rk_outputs.push_back( rk_output );
1495 
1496  totalMeas.push_back( outputGrids[i].getNumIntervals() );
1497 
1498  // Export output grids
1499  ExportVariable gridVariable( (std::string)"gridOutput" + toString(i), 1, totalMeas[i], REAL, ACADO_VARIABLES );
1500  gridVariables.push_back( gridVariable );
1501  }
1502  uint maxVARS = NX+NXA+NU+NDX;
1503 
1504  int useOMP;
1505  get(CG_USE_OPENMP, useOMP);
1506  ExportStruct structWspace;
1507  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
1508 
1509  setup();
1510  rk_rhsOutputTemp = ExportVariable( "rk_rhsOutputTemp", 1, maxOutputs, REAL, structWspace );
1511  if( sensGen ) rk_diffsOutputTemp = ExportVariable( "rk_diffsOutputTemp", 1, maxOutputs*(maxVARS), REAL, structWspace );
1512  rk_outH = ExportVariable( "rk_outH", numStages, 1, REAL, structWspace );
1513  rk_out = ExportVariable( "rk_out2", numStages, 1, REAL, structWspace );
1514  polynEvalVar = ExportVariable( "tmp_polyn", 1, 1, REAL, ACADO_LOCAL, true );
1515 
1516  exportRhs = false;
1517  }
1518  else {
1519  return ACADOERROR( RET_INVALID_OPTION );
1520  }
1521 
1522 
1523  return SUCCESSFUL_RETURN;
1524 }
1525 
1526 
1527 returnValue ImplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_,
1528  const std::vector<std::string> _outputNames,
1529  const std::vector<std::string> _diffs_outputNames,
1530  const std::vector<uint> _dims_output,
1531  const std::vector<DMatrix> _outputDependencies ) {
1532 
1533  outputDependencies = _outputDependencies;
1534  crsFormat = true;
1535 
1536  return setupOutput( outputGrids_, _outputNames, _diffs_outputNames, _dims_output );
1537 }
1538 
1539 
1540 
1541 // PROTECTED:
1542 
1543 
1545 {
1546  uint i;
1547  int measGrid;
1548  get( MEASUREMENT_GRID, measGrid );
1549 
1550  for( i = 0; i < outputGrids.size(); i++ ) {
1551  if( (MeasurementGrid)measGrid != ONLINE_GRID ) {
1552  DMatrix polynV = evaluatePolynomial( i );
1553  DMatrix polynDerV = evaluateDerivedPolynomial( i );
1554  DVector measurements = divideMeasurements( i );
1555 
1556  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
1557  ExportVariable polynVariable = ExportVariable( (std::string)"polynOutput" + toString( i ), polynV*=h, STATIC_CONST_REAL );
1558  code.addDeclaration( polynVariable );
1559  polynVariable = ExportVariable( (std::string)"polynOutput" + toString( i ), totalMeas[i], numStages, STATIC_CONST_REAL );
1560  polynVariables.push_back( polynVariable );
1561 
1562  if( NXA > 0 || NDX > 0 ) {
1563  ExportVariable polynDerVariable( (std::string)"polynDerOutput" + toString( i ), polynDerV, STATIC_CONST_REAL );
1564  code.addDeclaration( polynDerVariable );
1565  polynDerVariable = ExportVariable( (std::string)"polynDerOutput" + toString( i ), totalMeas[i], numStages, STATIC_CONST_REAL );
1566  polynDerVariables.push_back( polynDerVariable );
1567  }
1568 
1569  ExportVariable numMeasVariable( (std::string)"numMeas" + toString( i ), measurements, STATIC_CONST_INT );
1570  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
1571  code.addDeclaration( numMeasVariable );
1572  }
1573  numMeasVariables.push_back( numMeasVariable );
1574  }
1575  }
1576 
1577  return SUCCESSFUL_RETURN;
1578 }
1579 
1580 
1582  )
1583 {
1584  numStages = arg.numStages;
1585  numIts = arg.numIts;
1586  numItsInit = arg.numItsInit;
1587  diffsDim = arg.diffsDim;
1588  inputDim = arg.inputDim;
1589 
1590  rhs = arg.rhs;
1591  outputs = arg.outputs;
1592  diffs_rhs = arg.diffs_rhs;
1594  num_outputs = arg.num_outputs;
1595  grid = arg.grid;
1596  outputGrids = arg.outputGrids;
1597  solver = arg.solver;
1598 
1599  // ExportVariables
1600  rk_ttt = arg.rk_ttt;
1601  rk_xxx = arg.rk_xxx;
1602  rk_kkk = arg.rk_kkk;
1603  rk_A = arg.rk_A;
1604  debug_mat = arg.debug_mat;
1605  rk_b = arg.rk_b;
1606  rk_diffK = arg.rk_diffK;
1607  rk_rhsTemp = arg.rk_rhsTemp;
1610  rk_diffsNew1 = arg.rk_diffsNew1;
1612  rk_diffsNew2 = arg.rk_diffsNew2;
1614  rk_diffsNew3 = arg.rk_diffsNew3;
1616  rk_eta = arg.rk_eta;
1619  rk_outH = arg.rk_outH;
1620  rk_out = arg.rk_out;
1621  polynEvalVar = arg.polynEvalVar;
1622  rk_outputs = arg.rk_outputs;
1623 
1625  totalMeas = arg.totalMeas;
1626 
1627  // ExportFunctions
1628  integrate = arg.integrate;
1629  fullRhs = arg.fullRhs;
1630 
1631  REUSE = arg.REUSE;
1633 
1634  DD = arg.DD;
1635  coeffs = arg.coeffs;
1636 
1637  eig = arg.eig;
1642 
1643  tau = arg.tau;
1644  low_tria = arg.low_tria;
1649 
1650  return SUCCESSFUL_RETURN;
1651 }
1652 
1653 
1655 {
1656  return numIts;
1657 }
1658 
1659 
1661 {
1662  return numItsInit;
1663 }
1664 
1665 
1667 
1668 // end of file.
DVector computeCombinations(const DVector &cVec, uint index, uint numEls)
Definition: irk_export.cpp:923
ExportVariable rk_diffsPrev1
ExportVariable debug_mat
Definition: irk_export.hpp:572
returnValue setTransformations(const DMatrix &_transf1, const DMatrix &_transf2, const DMatrix &_transf1_T, const DMatrix &_transf2_T)
returnValue setStepSize(double _stepsize)
uint getIntegrationInterval(double time)
ExportVariable getRow(const ExportIndex &idx) const
ExportVariable rk_diffK
Definition: irk_export.hpp:571
returnValue addArgument(const ExportArgument &_argument1, 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)
DVector evaluatePolynomial(double time)
Definition: irk_export.cpp:965
Allows to setup and evaluate output functions based on SymbolicExpressions.
Definition: output_fcn.hpp:55
returnValue initializeDDMatrix()
Definition: irk_export.cpp:864
double getTime(uint pointIdx) const
ExportVariable getGlobalExportVariable() const
virtual returnValue setupOutput(const std::vector< Grid > outputGrids_, const std::vector< Expression > rhs)
returnValue setTransformations(const DMatrix &_transf1, const DMatrix &_transf2, const DMatrix &_transf1_T, const DMatrix &_transf2_T)
std::vector< ExportVariable > gridVariables
Definition: irk_export.hpp:549
returnValue setImplicit(BooleanType _implicit)
returnValue setSimplifiedTransformations(const DMatrix &_transf1, const DMatrix &_transf2)
double getFirstTime() const
Allows to export a tailored IRK solver based on Gaussian elimination of specific dimensions.
LinearAlgebraSolver
GenericMatrix & appendCols(const GenericMatrix &_arg)
Definition: matrix.cpp:83
Allows to export a tailored Runge-Kutta integrator for fast model predictive control.
Definition: rk_export.hpp:54
std::vector< Grid > outputGrids
std::vector< ExportVariable > polynVariables
Definition: irk_export.hpp:553
UserInteraction * userInteraction
Allows to export code of an ACADO function.
ExportAcadoFunction diffs_rhs
int getNDX() const
Definition: function.cpp:217
returnValue get(OptionsName name, int &value) const
Definition: options.cpp:69
ExportVariable rk_index
returnValue setTranspose(const bool &transpose)
std::vector< ExportAcadoFunction > outputs
virtual ExportVariable getGlobalExportVariable(const uint factor) const
virtual returnValue evaluateRhsImplicitSystem(ExportStatementBlock *block, const ExportIndex &k_index, const ExportIndex &stage)
Definition: irk_export.cpp:728
ExportVariable rk_diffsPrev3
Allows to pass back messages to the calling function.
ExportVariable rk_kkk
Definition: rk_export.hpp:189
virtual returnValue setModel(const std::string &_name_ODE, const std::string &_name_diffs_ODE)
DifferentialState x
returnValue initializeCoefficients()
Definition: irk_export.cpp:883
Expression forwardDerivative(const Expression &arg1, const Expression &arg2)
ExportLinearSolver * solver
Definition: irk_export.hpp:530
virtual ~ImplicitRungeKuttaExport()
Definition: irk_export.cpp:91
returnValue addComment(const std::string &_comment)
IntermediateState pow(const Expression &arg1, const Expression &arg2)
Block< Derived > block(Index startRow, Index startCol, Index blockRows, Index blockCols)
Definition: BlockMethods.h:56
std::vector< DMatrix > outputDependencies
const std::string getNameOUTPUT(uint index) const
ImplicitRungeKuttaExport(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
Definition: irk_export.cpp:44
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
std::vector< ExportAcadoFunction > diffs_outputs
ExportVariable rk_diffsPrev2
virtual returnValue prepareInputSystem(ExportStatementBlock &code)
Definition: irk_export.cpp:521
ExportVariable rhs_out
Allows to export code of a for-loop.
virtual returnValue evaluateStatesImplicitSystem(ExportStatementBlock *block, const ExportIndex &k_index, const ExportVariable &Ah, const ExportVariable &C, const ExportIndex &stage, const ExportIndex &i, const ExportIndex &tmp_index)
Definition: irk_export.cpp:677
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 getNameOutputDiffs() const
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
ImplicitIntegratorMode
ExportVariable getElement(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
ExportVariable rk_diffsTemp2
#define CLOSE_NAMESPACE_ACADO
ExportVariable rk_diffsNew3
returnValue setImplicit(BooleanType _implicit)
ExportVariable rk_rhsOutputTemp
Definition: irk_export.hpp:541
int getNXA() const
Definition: function.cpp:212
returnValue setSingleTransformations(const double _tau, const DVector &_low_tria, const DMatrix &_transf1, const DMatrix &_transf2)
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
GenericMatrix< double > DMatrix
Definition: matrix.hpp:457
const std::string getNameDiffsRHS() const
Defines a scalar-valued index variable to be used for exporting code.
ExportAcadoFunction rhs3
virtual returnValue copy(const ImplicitRungeKuttaExport &arg)
returnValue setImplicit(BooleanType _implicit)
ExportVariable rhs_in
returnValue setTransformations(const double _tau, const DVector &_low_tria, const DMatrix &_transf1, const DMatrix &_transf2, const DMatrix &_transf1_T, const DMatrix &_transf2_T)
std::vector< uint > totalMeas
Definition: irk_export.hpp:550
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
Allows to export a tailored implicit Runge-Kutta integrator for fast model predictive control...
Definition: irk_export.hpp:55
virtual returnValue setDoc(const std::string &_doc)
std::string commonHeaderName
ExportVariable rk_eta
Expression jacobian(const Expression &arg1, const Expression &arg2)
virtual returnValue evaluateMatrix(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index, const ExportIndex &k_index, const ExportVariable &_rk_A, const ExportVariable &Ah, const ExportVariable &C, bool evaluateB, bool DERIVATIVES)
Definition: irk_export.cpp:747
ExportStruct
std::vector< ExportVariable > numMeasVariables
Definition: irk_export.hpp:555
virtual ExportFunction & doc(const std::string &_doc)
static std::string fcnPrefix
DVector evaluateDerivedPolynomial(double time)
std::vector< Expression > outputExpressions
const std::string getNameOutputRHS() const
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
returnValue setModel(const std::string &_rhs, const std::string &_diffs_rhs)
Definition: irk_export.cpp:159
uint getNumItsInit() const
ExportVariable rk_auxSolver
Definition: irk_export.hpp:563
Allows to export Gaussian elimination for solving linear systems of specific dimensions.
int acadoRoundAway(double x)
unsigned getDim() const
Definition: vector.hpp:172
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
GenericMatrix & setRow(unsigned _idx, const GenericVector< T > &_values)
Definition: matrix.hpp:213
const std::string getNameSolveReuseFunction()
const std::string getNameRHS() const
const std::string get(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
ExportVariable reset_int
returnValue setImplicit(BooleanType _implicit)
Allows to export a tailored IRK solver based on Gaussian elimination of specific dimensions.
returnValue setTransformations(const double _tau, const DVector &_low_tria, const DMatrix &_transf1, const DMatrix &_transf2, const DMatrix &_transf1_T, const DMatrix &_transf2_T)
Derived & setZero(Index size)
Encapsulates all user interaction for setting options, logging data and plotting results.
ExportVariable rk_diffsOutputTemp
Definition: irk_export.hpp:542
Allows to export code of an arbitrary function.
virtual uint getDim() const
returnValue setDataStruct(ExportStruct _dataStruct)
Definition: export_data.cpp:80
virtual DMatrix formMatrix(const DMatrix &mass, const DMatrix &jacobian)
Definition: irk_export.cpp:549
returnValue addStatement(const ExportStatement &_statement)
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
Definition: irk_export.cpp:244
int getNT() const
Definition: function.cpp:251
ExportFunction integrate
returnValue setReuse(const bool &reuse)
std::string getFullName() const
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: irk_export.cpp:200
returnValue evaluateStatesOutputSystem(ExportStatementBlock *block, const ExportVariable &Ah, const ExportIndex &stage)
Definition: irk_export.cpp:705
ExportFunction fullRhs
unsigned getNumRows() const
Definition: matrix.hpp:185
returnValue addLinebreak(uint num=1)
std::vector< ExportVariable > polynDerVariables
Definition: irk_export.hpp:554
GenericVector & append(const GenericVector &_arg)
Definition: vector.cpp:42
std::vector< uint > num_outputs
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
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
ExportVariable rk_diffsNew2
unsigned getNumCols() const
Definition: matrix.hpp:189
uint getNumIntervals() const
GenericVector< double > DVector
Definition: vector.hpp:329
virtual returnValue setup()=0
returnValue setGlobalExportVariable(const ExportVariable &var)
ImplicitRungeKuttaExport & operator=(const ImplicitRungeKuttaExport &arg)
Definition: irk_export.cpp:101
returnValue setStepSize(double _stepsize)
returnValue prepareOutputEvaluation(ExportStatementBlock &code)
ExportVariable rk_xxx
DifferentialStateDerivative dx
std::string getName() const
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
ExportAcadoFunction lin_input
returnValue setEigenvalues(const DMatrix &_eig)
double getLastTime() const
ExportVariable rk_ttt
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
returnValue clearStaticCounters()
Definition: expression.hpp:398
RungeKuttaExport & operator=(const RungeKuttaExport &arg)
Definition: rk_export.cpp:65
Allows to export a tailored IRK solver based on Gaussian elimination of specific dimensions.
returnValue addFunction(const ExportFunction &_function)
MeasurementGrid
Allows to export Householder QR Factorization for solving linear systems of specific dimensions...
virtual returnValue prepareOutputSystem(ExportStatementBlock &code)
Definition: irk_export.cpp:535
virtual ExportVariable getAuxVariable() const
Definition: irk_export.cpp:171
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const =0
Allows to export a tailored IRK solver based on Gaussian elimination of specific dimensions.
ExportVariable rk_diffsNew1
ExportVariable rk_diffsTemp3
Definition: irk_export.hpp:569
std::vector< ExportIndex > numMeas
Definition: irk_export.hpp:556
virtual returnValue setDifferentialEquation(const Expression &rhs)
Definition: irk_export.cpp:112
DVector divideMeasurements(uint index)
ExportVariable polynEvalVar
Definition: irk_export.hpp:545
ExportVariable rk_outH
Definition: irk_export.hpp:543
virtual returnValue clear()
virtual returnValue getCode(ExportStatementBlock &code)=0
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
GenericMatrix getRows(unsigned _start, unsigned _end) const
Definition: matrix.hpp:235
returnValue init(const uint newDim, const bool &reuse=true, const bool &unrolling=false)
virtual returnValue appendVariableNames(std::stringstream &string)=0
ExportVariable getCol(const ExportIndex &idx) const
returnValue init(const Function &_f, const std::string &_name="acadoFcn", const uint _numX=0, const uint _numXA=0, const uint _numU=0, const uint _numP=0, const uint _numDX=0, const uint _numOD=0)
ExportFunction & addIndex(const ExportIndex &_index)
virtual returnValue getCode(ExportStatementBlock &code)
Definition: irk_export.cpp:257
#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()
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
std::string getName() const
Definition: export_data.cpp:86
uint getDim() const


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