irk_lifted_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  ) : ForwardIRKExport( _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  }
92  }
95  }
96  }
97  if( NX3 > 0 ) {
98  if( rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
100  }
101  if( diffs_rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
103  }
104  }
105  uint i;
106  for( i = 0; i < outputs.size(); i++ ) {
107  if( outputs[i].getGlobalExportVariable().getDim() >= max.getDim() ) {
108  max = outputs[i].getGlobalExportVariable();
109  }
110  if( diffs_outputs[i].getGlobalExportVariable().getDim() >= max.getDim() ) {
111  max = diffs_outputs[i].getGlobalExportVariable();
112  }
113  }
114  return max;
115 }
116 
117 
119  ExportStruct dataStruct
120  ) const
121 {
122  ForwardIRKExport::getDataDeclarations( declarations, dataStruct );
123 
124  declarations.addDeclaration( rk_seed,dataStruct );
125  declarations.addDeclaration( rk_I,dataStruct );
126  declarations.addDeclaration( rk_diffSweep,dataStruct );
127  declarations.addDeclaration( rk_stageValues,dataStruct );
128 
129  declarations.addDeclaration( rk_Xprev,dataStruct );
130  declarations.addDeclaration( rk_Uprev,dataStruct );
131  declarations.addDeclaration( rk_delta,dataStruct );
132 
133  declarations.addDeclaration( rk_xxx_lin,dataStruct );
134  declarations.addDeclaration( rk_Khat_traj,dataStruct );
135  declarations.addDeclaration( rk_Xhat_traj,dataStruct );
136 
137  declarations.addDeclaration( rk_diffK_local,dataStruct );
138 
139  declarations.addDeclaration( rk_b_trans,dataStruct );
140 
141  declarations.addDeclaration( rk_adj_traj,dataStruct );
142 
143  declarations.addDeclaration( rk_adj_diffs_tmp,dataStruct );
144  declarations.addDeclaration( rk_seed2,dataStruct );
145  declarations.addDeclaration( rk_xxx_traj,dataStruct );
146 
147  return SUCCESSFUL_RETURN;
148 }
149 
150 
152  ) const
153 {
155 
156 
157 
158  return SUCCESSFUL_RETURN;
159 }
160 
161 
163 {
164  int sensGen;
165  get( DYNAMIC_SENSITIVITY,sensGen );
166  if( rhs_.getDim() > 0 ) {
167  OnlineData dummy0;
168  Control dummy1;
169  DifferentialState dummy2;
170  AlgebraicState dummy3;
172  dummy0.clearStaticCounters();
173  dummy1.clearStaticCounters();
174  dummy2.clearStaticCounters();
175  dummy3.clearStaticCounters();
176  dummy4.clearStaticCounters();
177 
178  NX2 = rhs_.getDim() - NXA;
179  x = DifferentialState("", NX1+NX2, 1);
180  z = AlgebraicState("", NXA, 1);
181  u = Control("", NU, 1);
182  od = OnlineData("", NOD, 1);
183 
185  f << rhs_;
186 
187  NDX2 = f.getNDX();
188  if( NDX2 > 0 && (NDX2 < NX2 || NDX2 > (NX1+NX2)) ) {
189  return ACADOERROR( RET_INVALID_OPTION );
190  }
191  else if( NDX2 > 0 ) NDX2 = NX1+NX2;
193 
195  for( uint i = 0; i < rhs_.getDim(); i++ ) {
196  g << forwardDerivative( rhs_(i), x );
197  g << forwardDerivative( rhs_(i), z );
198  g << forwardDerivative( rhs_(i), u );
199  g << forwardDerivative( rhs_(i), dx );
200  }
201 
203  if( (ExportSensitivityType)sensGen == INEXACT ) { // ONLY FOR INEXACT LIFTING
204  DifferentialState sX("", NX,NX+NU);
205  DifferentialState dKX("", NDX2,NX+NU);
206  DifferentialState dKZ("", NXA,NX+NU);
207 
208  Expression tmp = zeros<double>(NX+NXA,NX);
209  tmp.appendCols(forwardDerivative( rhs_, u ));
210 
211  if(NXA > 0 && NDX2 == 0) return ACADOERROR(RET_NOT_YET_IMPLEMENTED);
212 
213  // forward sweep
214  if(NXA == 0 && NDX2 == 0) {
215  h << multipleForwardDerivative( rhs_, x, sX ) + tmp;
216  }
217  else {
218  Expression tmp2 = tmp + multipleForwardDerivative( rhs_, dx, dKX );
219  Expression tmp3 = tmp2 + multipleForwardDerivative( rhs_, z, dKZ );
220  h << multipleForwardDerivative( rhs_, x, sX ) + tmp3;
221  }
222 
223  }
224 
225  int gradientUp;
226  get( LIFTED_GRADIENT_UPDATE, gradientUp );
227  bool gradientUpdate = (bool) gradientUp;
228 
229  DifferentialEquation backward;
230  if( gradientUpdate ) {
231  if(NXA > 0 || NDX2 > 0) return ACADOERROR(RET_NOT_YET_IMPLEMENTED);
232 
233  Expression arg;
234  arg << x;
235  arg << u;
236 
237  DifferentialState lambda("", NX,1);
238  backward << backwardDerivative( rhs_, arg, lambda );
239  }
240 
241  if( f.getNT() > 0 ) timeDependant = true;
242 
243  return (rhs.init( f,"acado_rhs",NX,NXA,NU,NP,NDX,NOD ) &
244  diffs_rhs.init( g,"acado_diffs",NX,NXA,NU,NP,NDX,NOD ) &
245  forward_sweep.init( h,"acado_forward",NX+(NX+NDX2+NXA)*(NX+NU),NXA,NU,NP,NDX,NOD ) &
246  adjoint_sweep.init( backward,"acado_backward",NX*(2+NX+NU),NXA,NU,NP,NDX,NOD ) );
247  }
248  return SUCCESSFUL_RETURN;
249 }
250 
251 
253 {
254  int mode;
255  get( IMPLICIT_INTEGRATOR_MODE, mode );
256 
257  int sensGen;
258  get( DYNAMIC_SENSITIVITY, sensGen );
259  int linSolver;
260  get( LINEAR_ALGEBRA_SOLVER, linSolver );
261  int liftMode = 1;
262  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) liftMode = 4;
263 
266 // if( liftMode != 1 && liftMode != 4 ) ACADOERROR( RET_NOT_IMPLEMENTED_YET );
267 // if( (ExportSensitivityType)sensGen == INEXACT && liftMode != 4 ) ACADOERROR( RET_INVALID_OPTION );
268 
270 
271  uint grad = 0;
272  int gradientUp;
273  get( LIFTED_GRADIENT_UPDATE, gradientUp );
274  bool gradientUpdate = (bool) gradientUp;
275  if( gradientUpdate ) grad = NX;
276 
277  if( gradientUpdate && (ExportSensitivityType)sensGen != INEXACT ) ACADOERROR( RET_INVALID_OPTION );
278 
279  int useOMP;
280  get(CG_USE_OPENMP, useOMP);
281  if ( useOMP ) {
283  max.setName( "auxVar" );
284  max.setDataStruct( ACADO_LOCAL );
285  if( NX2 > 0 || NXA > 0 ) {
289  }
290  if( NX3 > 0 ) {
293  }
294  for( uint i = 0; i < outputs.size(); i++ ) {
295  outputs[i].setGlobalExportVariable( max );
296  diffs_outputs[i].setGlobalExportVariable( max );
297  }
298 
300 
301  stringstream s;
302  s << "#pragma omp threadprivate( "
303  << max.getFullName() << ", "
304  << rk_ttt.getFullName() << ", "
305  << rk_xxx.getFullName() << ", "
306  << rk_kkk.getFullName() << ", "
307  << rk_diffK.getFullName() << ", "
308  << rk_rhsTemp.getFullName() << ", "
310  if( NX1 > 0 ) {
311  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev1.getFullName();
312  s << ", " << rk_diffsNew1.getFullName();
313  }
314  if( NX2 > 0 || NXA > 0 ) {
315  s << ", " << rk_A.getFullName();
316  if( (ExportSensitivityType)sensGen == INEXACT ) s << ", " << rk_seed.getFullName();
317  s << ", " << rk_b.getFullName();
318  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev2.getFullName();
319  s << ", " << rk_diffsNew2.getFullName();
320  s << ", " << rk_diffsTemp2.getFullName();
322  }
323  if( NX3 > 0 ) {
324  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev3.getFullName();
325  s << ", " << rk_diffsNew3.getFullName();
326  s << ", " << rk_diffsTemp3.getFullName();
327  }
328  s << " )" << endl << endl;
329  code.addStatement( s.str().c_str() );
330  }
331 
332  if( NX1 > 0 ) {
333  code.addFunction( lin_input );
334  code.addStatement( "\n\n" );
335  }
336  if( exportRhs ) {
337  if( NX2 > 0 || NXA > 0 ) {
338  code.addFunction( rhs );
339  code.addStatement( "\n\n" );
340  code.addFunction( diffs_rhs );
341  code.addStatement( "\n\n" );
342  code.addFunction( forward_sweep );
343  code.addStatement( "\n\n" );
344  code.addFunction( adjoint_sweep );
345  code.addStatement( "\n\n" );
346  }
347 
348  if( NX3 > 0 ) {
349  code.addFunction( rhs3 );
350  code.addStatement( "\n\n" );
351  code.addFunction( diffs_rhs3 );
352  code.addStatement( "\n\n" );
353  }
354 
355  if( CONTINUOUS_OUTPUT ) {
356  uint i;
357  for( i = 0; i < outputs.size(); i++ ) {
358  code.addFunction( outputs[i] );
359  code.addStatement( "\n\n" );
360  code.addFunction( diffs_outputs[i] );
361  code.addStatement( "\n\n" );
362  }
363  }
364  }
365  if( NX2 > 0 || NXA > 0 ) solver->getCode( code );
366  code.addLinebreak(2);
367 
368  int measGrid;
369  get( MEASUREMENT_GRID, measGrid );
370 
371  // export RK scheme
372  uint run5, run6;
373  std::string tempString;
374 
377 
378  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
379  DMatrix tmp = AA;
380  ExportVariable Ah( "Ah_mat", tmp*=h, STATIC_CONST_REAL );
381  code.addDeclaration( Ah );
382  code.addLinebreak( 2 );
383  // TODO: Ask Milan why this does NOT work properly !!
385 
386  DVector BB( bb );
387  ExportVariable Bh( "Bh_mat", DMatrix( BB*=h ) );
388 
389  DVector CC( cc );
390  ExportVariable C;
391  if( timeDependant ) {
392  C = ExportVariable( "C_mat", DMatrix( CC*=(1.0/grid.getNumIntervals()) ), STATIC_CONST_REAL );
393  code.addDeclaration( C );
394  code.addLinebreak( 2 );
396  }
397 
398  code.addComment(std::string("Fixed step size:") + toString(h));
399 
400  ExportVariable determinant( "det", 1, 1, REAL, ACADO_LOCAL, true );
401  integrate.addDeclaration( determinant );
402 
403  ExportIndex i( "i" );
404  ExportIndex j( "j" );
405  ExportIndex k( "k" );
406  ExportIndex run( "run" );
407  ExportIndex run1( "run1" );
408  ExportIndex tmp_index1("tmp_index1");
409  ExportIndex tmp_index2("tmp_index2");
410  ExportIndex tmp_index3("tmp_index3");
411  ExportIndex tmp_index4("tmp_index4");
412  ExportIndex k_index("k_index");
413  ExportIndex shooting_index("shoot_index");
414  ExportVariable tmp_meas("tmp_meas", 1, outputGrids.size(), INT, ACADO_LOCAL);
415 
416  ExportVariable numInt( "numInts", 1, 1, INT );
417  if( !equidistantControlGrid() ) {
418  ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
419  code.addDeclaration( numStepsV );
420  code.addLinebreak( 2 );
421  integrate.addStatement( std::string( "int " ) + numInt.getName() + " = " + numStepsV.getName() + "[" + rk_index.getName() + "];\n" );
422  }
423 
424  prepareOutputEvaluation( code );
425 
426  integrate.addIndex( i );
427  integrate.addIndex( j );
428  integrate.addIndex( k );
429  integrate.addIndex( run );
430  integrate.addIndex( run1 );
431  integrate.addIndex( tmp_index1 );
432  integrate.addIndex( tmp_index2 );
433  integrate.addIndex( tmp_index3 );
434  integrate.addIndex( shooting_index );
435  integrate.addIndex( k_index );
436  if( rk_outputs.size() > 0 && (grid.getNumIntervals() > 1 || !equidistantControlGrid()) ) {
437  integrate.addIndex( tmp_index4 );
438  }
439  ExportVariable time_tmp( "time_tmp", 1, 1, REAL, ACADO_LOCAL, true );
440  if( CONTINUOUS_OUTPUT ) {
441  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
442  ExportIndex numMeasTmp( (std::string)"numMeasTmp" + toString(run5) );
443  numMeas.push_back( numMeasTmp );
444  integrate.addIndex( numMeas[run5] );
445  }
446 
447  if( (MeasurementGrid)measGrid == ONLINE_GRID ) {
448  integrate.addDeclaration( tmp_meas );
450  integrate.addDeclaration( time_tmp );
451  }
452 
453  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
454  integrate.addStatement( numMeas[run5] == 0 );
455  }
456  }
457  integrate << shooting_index.getFullName() << " = " << rk_index.getFullName() << ";\n";
459  if( (inputDim-diffsDim) > NX+NXA ) {
461  if( (ExportSensitivityType)sensGen == INEXACT ) {
463  if( gradientUpdate ) {
465  }
466  }
467  }
469  if( liftMode == 1 || liftMode == 4 ) {
470  integrate.addStatement( rk_delta.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) - rk_Xprev.getRow(shooting_index) );
471  integrate.addStatement( rk_Xprev.getRow(shooting_index) == rk_eta.getCols( 0,NX ) );
472 
475  }
477 
478  if( gradientUpdate ) {
479  DMatrix zeroL = zeros<double>(1,NX+NU);
481  }
482 
483  // integrator loop:
484  ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
485  ExportStatementBlock *loop;
486  if( equidistantControlGrid() ) {
487  loop = &tmpLoop;
488  }
489  else {
490  loop = &integrate;
491  loop->addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
492  }
493 
494  if( CONTINUOUS_OUTPUT && (MeasurementGrid)measGrid == ONLINE_GRID ) {
495  for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
496  loop->addStatement( tmp_index1 == numMeas[run5] );
497  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" );
498  loop->addStatement( tmp_index1 == tmp_index1+1 );
499  loop->addStatement( std::string("}\n") );
500  loop->addStatement( std::string(tmp_meas.get( 0,run5 )) + " = " + tmp_index1.getName() + " - " + numMeas[run5].getName() + ";\n" );
501  }
502  }
503 
504  // if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
505  // Set rk_diffsPrev:
506  loop->addStatement( std::string("if( run > 0 ) {\n") );
507 // if( NX1 > 0 ) {
508 // ExportForLoop loopTemp1( i,0,NX1 );
509 // loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+2*NX+NXA,i*NX+2*NX+NXA+NX1 ) );
510 // if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,NX1,NX1+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+NX,i*NU+(NX+NXA)*(NX+1)+NX+NU ) );
511 // loop->addStatement( loopTemp1 );
512 // }
513  if( NX2 > 0 ) {
514  ExportForLoop loopTemp2( i,0,NX2 );
515  loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,0,NX1+NX2 ) == rk_eta.getCols( i*NX+NX+grad+NXA+NX1*NX,i*NX+NX+grad+NXA+NX1*NX+NX1+NX2 ) );
516  if( NU > 0 ) loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,NX1+NX2,NX1+NX2+NU ) == rk_eta.getCols( i*NU+grad+(NX+NXA)*(NX+1)+NX1*NU,i*NU+grad+(NX+NXA)*(NX+1)+NX1*NU+NU ) );
517  loop->addStatement( loopTemp2 );
518  }
519 // if( NX3 > 0 ) {
520 // ExportForLoop loopTemp3( i,0,NX3 );
521 // loopTemp3.addStatement( rk_diffsPrev3.getSubMatrix( i,i+1,0,NX ) == rk_eta.getCols( i*NX+2*NX+NXA+(NX1+NX2)*NX,i*NX+2*NX+NXA+(NX1+NX2)*NX+NX ) );
522 // 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+NX,i*NU+(NX+NXA)*(NX+1)+(NX1+NX2)*NU+NU+NX ) );
523 // loop->addStatement( loopTemp3 );
524 // }
525  loop->addStatement( std::string("}\nelse{\n") );
526  DMatrix eyeM = eye<double>(NX);
527  eyeM.appendCols(zeros<double>(NX,NU));
528  loop->addStatement( rk_diffsPrev2 == eyeM );
529  loop->addStatement( std::string("}\n") );
530  // }
531 
532  loop->addStatement( k_index == (shooting_index*grid.getNumIntervals()+run)*(NX+NXA) );
533 
534  // FIRST update using term from optimization variables:
535  if( liftMode == 1 || (liftMode == 4 && (ExportSensitivityType)sensGen == INEXACT) ) {
536  ExportForLoop loopTemp1( i,0,NX+NXA );
537  loopTemp1.addStatement( j == k_index+i );
538  loopTemp1.addStatement( tmp_index1 == j*(NX+NU) );
539  ExportForLoop loopTemp2( run1,0,numStages );
540  loopTemp2.addStatement( rk_kkk.getElement( j,run1 ) += rk_delta*rk_diffK.getSubMatrix( tmp_index1,tmp_index1+NX+NU,run1,run1+1 ) );
541  loopTemp1.addStatement( loopTemp2 );
542  loop->addStatement( loopTemp1 );
543  }
544 
545  // Evaluate all stage values for reuse:
546  evaluateAllStatesImplicitSystem( loop, k_index, Ah, C, run1, j, tmp_index1 );
547 
548  // SAVE rk_stageValues in the rk_xxx_traj variable:
549  loop->addStatement( rk_xxx_traj.getCols(run*numStages*(NX2+NXA),(run+1)*numStages*(NX2+NXA)) == rk_stageValues );
550 
551  solveImplicitSystem( loop, i, run1, j, tmp_index1, k_index, Ah, C, determinant, true );
552 
553  // NEW: UPDATE RK_B WITH THE CONSTANT COMING FROM THE PREVIOUS INTEGRATION STEP
554  if( liftMode == 1 ) { // EXACT LIFTING
555  loop->addStatement( std::string("if( run > 0 ) {\n") );
556  loop->addStatement( rk_Xhat_traj.getRows(run*NX,(run+1)*NX) == rk_Xhat_traj.getRows((run-1)*NX,run*NX) );
557  for( run5 = 0; run5 < numStages; run5++ ) {
558  loop->addStatement( rk_Xhat_traj.getRows(run*NX,(run+1)*NX) += rk_Khat_traj.getSubMatrix( run-1,run,run5*(NX2+NXA),run5*(NX2+NXA)+NX2 ).getTranspose()*Bh.getElement(run5,0) );
559  }
560  ExportForLoop deltaLoop( i,0,numStages );
561  ExportForLoop deltaLoop2( j,0,NX );
562  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) {
563  deltaLoop2.addStatement( tmp_index1 == i*(NX2+NXA)+j );
564  deltaLoop2.addStatement( rk_b.getElement(tmp_index1,0) -= rk_diffsTemp2.getSubMatrix(i,i+1,j*NVARS2,j*NVARS2+NX)*rk_Xhat_traj.getRows(run*NX,(run+1)*NX) );
565  }
566  else {
567  deltaLoop2.addStatement( tmp_index1 == i*NX2+j );
568  deltaLoop2.addStatement( rk_b.getElement(tmp_index1,0) -= rk_diffsTemp2.getSubMatrix(i,i+1,j*NVARS2,j*NVARS2+NX)*rk_Xhat_traj.getRows(run*NX,(run+1)*NX) );
569  }
570  deltaLoop.addStatement( deltaLoop2 );
571  loop->addStatement( deltaLoop );
572  loop->addStatement( std::string("}\nelse{\n") );
573  loop->addStatement( rk_Xhat_traj.getRows(0,NX) == zeros<double>(NX,1) );
574  loop->addStatement( std::string("}\n") );
575  }
576 
577 
578  // Evaluate sensitivities:
579  if( (ExportSensitivityType)sensGen != INEXACT ) {
580  evaluateRhsSensitivities( loop, run1, i, j, tmp_index1, tmp_index2 );
581  if( liftMode == 1 ) {
582  allSensitivitiesImplicitSystem( loop, run1, i, j, tmp_index1, tmp_index2, tmp_index3, k_index, Bh, false );
583  }
584  else {
585  allSensitivitiesImplicitSystem( loop, run1, i, j, tmp_index1, tmp_index2, tmp_index3, ExportIndex(0), Bh, false );
586  }
587  }
588  else {
589  evaluateRhsInexactSensitivities( loop, run1, i, j, tmp_index1, tmp_index2, tmp_index3, k_index, Ah );
590 
591  // GRADIENT UPDATE INIS SCHEME:
592  if( gradientUpdate ) {
593  loop->addStatement( tmp_index1 == shooting_index*grid.getNumIntervals()+run );
594  loop->addStatement( rk_eta.getCols(NX*(2+NX+NU),NX+diffsDim) += rk_adj_traj.getRow(tmp_index1)*rk_b.getCols(1,1+NX+NU) );
595  }
596 
597  allSensitivitiesImplicitSystem( loop, run1, i, j, tmp_index1, tmp_index2, tmp_index3, k_index, Bh, true );
598  }
599  if( liftMode == 1 ) { // EXACT LIFTING
600  // !!! update rk_xxx_lin (YOU NEED TO UPDATE THE LINEARIZATION POINT BEFORE YOU UPDATE RK_KKK): !!!
601  // (YOU SHOULD DO THIS AFTER YOU COMPUTE THE NEXT LINEARIZATION POINT IF YOU WANT TO BE FULLY EQUIVALENT WITH DIRECT COLLOCATION, see SYMMETRIC AND FOB VERSIONS):
602  for( run5 = 0; run5 < NX; run5++ ) {
603  loop->addStatement( rk_xxx_lin.getCol( run5 ) += rk_kkk.getRow( k_index+run5 )*Bh );
604  }
605  }
606 
607  ExportForLoop loopTemp( j,0,numStages );
608  for( run5 = 0; run5 < NX2; run5++ ) {
609  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) {
610  loopTemp.addStatement( rk_kkk.getElement( k_index+NX1+run5,j ) += rk_b.getElement( j*(NX2+NXA)+run5,0 ) ); // differential states
611  }
612  else {
613  loopTemp.addStatement( rk_kkk.getElement( k_index+NX1+run5,j ) += rk_b.getElement( j*NX2+run5,0 ) ); // differential states
614  }
615  }
616  for( run5 = 0; run5 < NXA; run5++ ) {
617  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) {
618  loopTemp.addStatement( rk_kkk.getElement( k_index+NX+run5,j ) += rk_b.getElement( j*(NX2+NXA)+NX2+run5,0 ) ); // algebraic states
619  }
620  else {
621  loopTemp.addStatement( rk_kkk.getElement( k_index+NX+run5,j ) += rk_b.getElement( numStages*NX2+j*NXA+run5,0 ) ); // algebraic states
622  }
623  }
624  loop->addStatement( loopTemp );
625  if( liftMode == 1 ) { // EXACT LIFTING
626  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) {
627  loop->addStatement( rk_Khat_traj.getRow(run) == rk_b.getCol(0).getTranspose() );
628  }
629  else {
630  for( run5 = 0; run5 < numStages; run5++ ) {
631  loop->addStatement( rk_Khat_traj.getSubMatrix(run,run+1,run5*(NX2+NXA),run5*(NX2+NXA)+NX2) == rk_b.getSubMatrix(run5*NX2,run5*NX2+NX2,0,1).getTranspose() );
632  }
633  for( run5 = 0; run5 < numStages; run5++ ) {
634  loop->addStatement( rk_Khat_traj.getSubMatrix(run,run+1,run5*(NX2+NXA)+NX2,(run5+1)*(NX2+NXA)) == rk_b.getSubMatrix(numStages*NX2+run5*NXA,numStages*NX2+run5*NXA+NXA,0,1).getTranspose() );
635  }
636  }
637  }
638 
639  // update rk_eta:
640  for( run5 = 0; run5 < NX; run5++ ) {
641  loop->addStatement( rk_eta.getCol( run5 ) += rk_kkk.getRow( k_index+run5 )*Bh );
642  }
643  if( NXA > 0) {
644  DMatrix tempCoefs( evaluateDerivedPolynomial( 0.0 ) );
645  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
646  loop->addStatement( std::string("if( run == 0 ) {\n") );
647  }
648  for( run5 = 0; run5 < NXA; run5++ ) {
649  loop->addStatement( rk_eta.getCol( NX+run5 ) == rk_kkk.getRow( k_index+NX+run5 )*tempCoefs );
650  }
651  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
652  loop->addStatement( std::string("}\n") );
653  }
654  }
655  if( liftMode != 1 ) { // INEXACT LIFTING
656  // !!! update rk_xxx_lin (YOU NEED TO UPDATE THE LINEARIZATION POINT AFTER YOU UPDATE RK_KKK): !!!
657  loop->addStatement( rk_xxx_lin == rk_eta.getCols(0,NX) );
658  }
659 
660 
661 // // Computation of the sensitivities using the CHAIN RULE:
662 // if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
663 // loop->addStatement( std::string( "if( run == 0 ) {\n" ) );
664 // }
665 // // PART 1
666 // updateInputSystem(loop, i, j, tmp_index2);
667 
668  // PART 2
669  updateImplicitSystem(loop, i, j, tmp_index2);
670 
671 // // PART 3
672 // updateOutputSystem(loop, i, j, tmp_index2);
673 
674 // if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
675 // loop->addStatement( std::string( "}\n" ) );
676 // loop->addStatement( std::string( "else {\n" ) );
679 //
680 // // PART 2
681 // propagateImplicitSystem(loop, i, j, k, tmp_index2);
682 //
685 // }
686 //
687 // if( rk_outputs.size() > 0 && (grid.getNumIntervals() > 1 || !equidistantControlGrid()) ) {
688 // propagateOutputs( loop, run, run1, i, j, k, tmp_index1, tmp_index2, tmp_index3, tmp_index4, tmp_meas );
689 // }
690 
691 // if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
692 // loop->addStatement( std::string( "}\n" ) );
693 // }
694 
695 // loop->addStatement( std::string( reset_int.get(0,0) ) + " = 0;\n" );
696 
697  for( run5 = 0; run5 < rk_outputs.size(); run5++ ) {
698  if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
699  loop->addStatement( numMeas[run5].getName() + " += " + numMeasVariables[run5].get(0,run) + ";\n" );
700  }
701  else { // ONLINE_GRID
702  loop->addStatement( numMeas[run5].getName() + " += " + tmp_meas.get(0,run5) + ";\n" );
703  }
704  }
705  loop->addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
706 
707  // end of the integrator loop.
708  if( !equidistantControlGrid() ) {
709  loop->addStatement( "}\n" );
710  }
711  else {
712  integrate.addStatement( *loop );
713  }
714 
715  if( gradientUpdate ) {
716  // COMPUTE THE INEXACT ADJOINT BASED ON LAMBDA AND THE INEXACT SENSITIVITIES:
717 // DMatrix zeroL = zeros<double>(1,NX+NU);
718 // integrate.addStatement( rk_eta.getCols(NX*(2+NX+NU),NX+diffsDim) == zeroL );
719 // for( run5 = 0; run5 < NX; run5 ++ ) {
720 // integrate.addStatement( rk_eta.getCols(NX*(2+NX+NU),NX+diffsDim) += rk_eta.getCol(NX+run5)*rk_diffsNew2.getRow(run5) );
721 // }
722 // integrate.addStatement( rk_eta.getCols(NX*(2+NX+NU),NX+diffsDim-NU) -= rk_eta.getCols(NX,2*NX) );
723 
724  // integrator BACKWARD loop:
725  integrate.addComment("------------ BACKWARD loop ------------:");
726  ExportForLoop tmpLoop2( run, grid.getNumIntervals()-1, -1, -1 );
727  ExportStatementBlock *loop2;
728  if( equidistantControlGrid() ) {
729  loop2 = &tmpLoop2;
730  }
731  else {
733  }
734 
735  loop2->addStatement( k_index == run*(NX+NXA)*(NX+NU) );
736 
737 
738  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) { // INEXACT
739  // Compute \hat{lambda}:
740  // vec(j*NX+1:j*NX+NX) = -Bh_vec(j+1)*dir_tmp;
741  for( run5 = 0; run5 < numStages; run5++ ) {
742  DMatrix zeroV = zeros<double>(1,NX);
743  loop2->addStatement( rk_b_trans.getCols(run5*NX,(run5+1)*NX) == zeroV );
744  loop2->addStatement( rk_b_trans.getCols(run5*NX,(run5+1)*NX) -= Bh.getRow(run5)*rk_eta.getCols(NX,2*NX) );
745  }
746  loop2->addStatement( tmp_index1 == shooting_index*grid.getNumIntervals()+run );
747  for( run5 = 0; run5 < numStages; run5++ ) {
748  loop2->addStatement( rk_seed2.getCols(0,NX) == rk_xxx_traj.getCols((run*numStages+run5)*(NX2+NXA),(run*numStages+run5+1)*(NX2+NXA)) );
749  loop2->addStatement( rk_seed2.getCols(NX,2*NX) == rk_adj_traj.getSubMatrix(tmp_index1,tmp_index1+1,run5*(NX+NXA),(run5+1)*(NX+NXA)) );
750  loop2->addFunctionCall( adjoint_sweep.getName(), rk_seed2, rk_adj_diffs_tmp );
751  for( run6 = 0; run6 < numStages; run6++ ) {
752  loop2->addStatement( rk_b_trans.getCols(run6*NX,(run6+1)*NX) -= Ah.getElement(run5,run6)*rk_adj_diffs_tmp.getCols(0,NX) );
753  }
754  loop2->addStatement( rk_b_trans.getCols(run5*NX,(run5+1)*NX) += rk_adj_traj.getSubMatrix(tmp_index1,tmp_index1+1,run5*(NX+NXA),(run5+1)*(NX+NXA)) ); // BECAUSE EXPLICIT ODE
755  }
757  loop2->addStatement( rk_adj_traj.getRow(tmp_index1) += rk_b_trans );
758  // loop2->addStatement( rk_b_trans == rk_adj_traj.getRow(tmp_index1) );
759  }
760  else return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
761 
762  for( run5 = 0; run5 < numStages; run5++ ) {
763  loop2->addStatement( rk_seed2.getCols(0,NX) == rk_xxx_traj.getCols((run*numStages+run5)*(NX2+NXA),(run*numStages+run5+1)*(NX2+NXA)) );
764  loop2->addStatement( rk_seed2.getCols(NX,2*NX) == rk_adj_traj.getSubMatrix(tmp_index1,tmp_index1+1,run5*NX,(run5+1)*NX) );
765  loop2->addFunctionCall( adjoint_sweep.getName(), rk_seed2, rk_adj_diffs_tmp.getAddress(0,0) );
766  loop2->addStatement( rk_eta.getCols(NX,2*NX) += rk_adj_diffs_tmp.getCols(0,NX) );
767 // loop2->addStatement( rk_eta.getCols(NX*(2+NX+NU),NX+diffsDim) -= rk_adj_diffs_tmp.getCols(0,NX+NU) );
768  }
769 
770  loop2->addStatement( rk_ttt -= DMatrix(1.0/grid.getNumIntervals()) );
771  // end of the backward integrator loop.
772  if( !equidistantControlGrid() ) {
774  }
775  else {
776  integrate.addStatement( *loop2 );
777  }
778  }
779 
780  // PART 1
781  if( NX1 > 0 ) {
782  DMatrix zeroR = zeros<double>(1, NX2+NX3);
783  ExportForLoop loop1( i,0,NX1 );
784  loop1.addStatement( rk_eta.getCols( i*NX+NX+grad+NXA+NX1,i*NX+NX+grad+NXA+NX ) == zeroR );
785  integrate.addStatement( loop1 );
786  }
787  // PART 2
788  DMatrix zeroR = zeros<double>(1, NX3);
789  if( NX2 > 0 && NX3 > 0 ) {
790  ExportForLoop loop2( i,NX1,NX1+NX2 );
791  loop2.addStatement( rk_eta.getCols( i*NX+NX+grad+NXA+NX1+NX2,i*NX+NX+grad+NXA+NX ) == zeroR );
792  integrate.addStatement( loop2 );
793  }
794  if( NXA > 0 && NX3 > 0 ) {
795  ExportForLoop loop3( i,NX,NX+NXA );
796  loop3.addStatement( rk_eta.getCols( i*NX+NX+grad+NXA+NX1+NX2,i*NX+NX+grad+NXA+NX ) == zeroR );
797  integrate.addStatement( loop3 );
798  }
799 
800 // integrate.addStatement( std::string( "if( " ) + determinant.getFullName() + " < 1e-12 ) {\n" );
801 // integrate.addStatement( error_code == 2 );
802 // integrate.addStatement( std::string( "} else if( " ) + determinant.getFullName() + " < 1e-6 ) {\n" );
803 // integrate.addStatement( error_code == 1 );
804 // integrate.addStatement( std::string( "} else {\n" ) );
806 // integrate.addStatement( std::string( "}\n" ) );
807 
808  code.addFunction( integrate );
809  code.addLinebreak( 2 );
810 
811  return SUCCESSFUL_RETURN;
812 }
813 
814 
815 returnValue ForwardLiftedIRKExport::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 )
816 {
817  int sensGen;
818  get( DYNAMIC_SENSITIVITY, sensGen);
819  int linSolver;
820  get( LINEAR_ALGEBRA_SOLVER, linSolver );
821 
822  int liftMode = 1;
823  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) liftMode = 4;
824 
825  if( NX2 > 0 || NXA > 0 ) {
826 
827  // Perform iteration by system solve:
828  if( liftMode == 4 ) {
829  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
830  block->addStatement( "if( run == 0 ) {\n" );
831  }
832 
833  // evaluate rk_J (only explicit ODE for now!)
834  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) {
835  evaluateStatesImplicitSystem( block, k_index, Ah, C, ExportIndex(0), index3, tmp_index );
837 
838  DMatrix zeroZ = zeros<double>(1,NXA);
839  ExportForLoop loop0( index2,0,NX+NXA );
840  loop0.addStatement( rk_A.getSubMatrix( index2,index2+1,0,NX ) == rk_diffsTemp2.getSubMatrix( index2,index2+1,NX1,NX1+NX2 ) );
841  loop0.addStatement( rk_A.getSubMatrix( index2,index2+1,NX,NX+NXA ) == zeroZ );
842  block->addStatement( loop0 );
843 
844  if( NDX2 > 0 ) {
845  ExportForLoop loop01( index2,0,NX+NXA );
846  loop01.addStatement( rk_I.getSubMatrix( index2,index2+1,0,NX ) == rk_diffsTemp2.getSubMatrix( index2,index2+1,NX+NXA+NU,NVARS2 ) );
847  loop01.addStatement( rk_I.getSubMatrix( index2,index2+1,NX,NX+NXA ) == rk_diffsTemp2.getSubMatrix( index2,index2+1,NX,NX+NXA ) );
848  block->addStatement( loop01 );
849  }
850 
851  ExportForLoop loop01( index2,0,numStages );
852  evaluateInexactMatrix( &loop01, index2, index3, tmp_index, k_index, rk_A, Ah, C, true, DERIVATIVES );
853  block->addStatement( loop01 );
854 
855  if( NDX2 > 0 ) {
856  block->addStatement( det.getFullName() + " = " + ExportStatement::fcnPrefix + "_" + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_I.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
857  }
858  else {
859  block->addStatement( det.getFullName() + " = " + ExportStatement::fcnPrefix + "_" + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
860  }
861  }
862  else {
863  ExportForLoop loop01( index2,0,numStages );
864  evaluateMatrix( &loop01, index2, index3, tmp_index, k_index, rk_A, Ah, C, true, false );
865  block->addStatement( loop01 );
866 
867  block->addStatement( det.getFullName() + " = " + ExportStatement::fcnPrefix + "_" + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
868  }
869 
870  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
871  block->addStatement( "}\n else {\n" );
872  ExportForLoop loop02( index2,0,numStages );
873  evaluateStatesImplicitSystem( &loop02, k_index, Ah, C, index2, index3, tmp_index );
874  evaluateRhsImplicitSystem( &loop02, k_index, index2 );
875  block->addStatement( loop02 );
876  block->addStatement( "}\n" );
877  }
878  }
879  else { // EXACT LIFTING
880  ExportForLoop loop1( index2,0,numStages );
881  evaluateMatrix( &loop1, index2, index3, tmp_index, k_index, rk_A, Ah, C, true, DERIVATIVES );
882 // if( liftMode == 1 ) { // Right-hand side term from update optimization variables:
883 // for( uint i = 0; i < NX2+NXA; i++ ) {
884 // loop1.addStatement( rk_b.getRow( index2*(NX2+NXA)+i ) -= rk_diffsTemp2.getSubMatrix( index2,index2+1,i*(NVARS2)+NX1,i*(NVARS2)+NX1+NX2 )*(rk_delta.getCols( NX1,NX1+NX2 ).getTranspose()) );
885 // loop1.addStatement( rk_b.getRow( index2*(NX2+NXA)+i ) -= rk_diffsTemp2.getSubMatrix( index2,index2+1,i*(NVARS2)+NX1+NX2+NXA,i*(NVARS2)+NX1+NX2+NXA+NU )*(rk_delta.getCols( NX,NX+NU ).getTranspose()) );
886 // }
887 // }
888  block->addStatement( loop1 );
889  block->addStatement( det.getFullName() + " = " + ExportStatement::fcnPrefix + "_" + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
890  }
891 
892 // // IF DEBUG MODE:
893 // int debugMode;
894 // get( INTEGRATOR_DEBUG_MODE, debugMode );
895 // if ( (bool)debugMode == true ) {
896 // block->addStatement( debug_mat == rk_A );
897 // }
898  }
899 
900  return SUCCESSFUL_RETURN;
901 }
902 
903 
904 returnValue ForwardLiftedIRKExport::evaluateInexactMatrix( 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 )
905 {
906  uint i;
907  int linSolver;
908  get( LINEAR_ALGEBRA_SOLVER, linSolver );
909 
910  if( (LinearAlgebraSolver) linSolver != SIMPLIFIED_IRK_NEWTON && (LinearAlgebraSolver) linSolver != SINGLE_IRK_NEWTON ) {
911  ExportForLoop loop2( index2,0,NX2+NXA );
912  loop2.addStatement( tmp_index == index1*(NX2+NXA)+index2 );
913  for( i = 0; i < numStages; i++ ) { // differential states
914  if( NDX2 == 0 ) {
915  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) == Ah.getElement( index1,i )*rk_diffsTemp2.getSubMatrix( index2,index2+1,NX1,NX1+NX2 ) );
916  loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) " );
917  loop2.addStatement( _rk_A.getElement( tmp_index,index2+i*NX2 ) -= 1 );
918  }
919  else {
920  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) == Ah.getElement( index1,i )*rk_diffsTemp2.getSubMatrix( index2,index2+1,NX1,NX1+NX2 ) );
921  loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) {\n" );
922  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) += rk_diffsTemp2.getSubMatrix( index2,index2+1,NVARS2-NX2,NVARS2 ) );
923  loop2.addStatement( std::string( "}\n" ) );
924  }
925  }
926  if( NXA > 0 ) {
927  DMatrix zeroM = zeros<double>( 1,NXA );
928  for( i = 0; i < numStages; i++ ) { // algebraic states
929  loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) {\n" );
930  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,numStages*NX2+i*NXA,numStages*NX2+i*NXA+NXA ) == rk_diffsTemp2.getSubMatrix( index2,index2+1,NX1+NX2,NX1+NX2+NXA ) );
931  loop2.addStatement( std::string( "}\n else {\n" ) );
932  loop2.addStatement( _rk_A.getSubMatrix( tmp_index,tmp_index+1,numStages*NX2+i*NXA,numStages*NX2+i*NXA+NXA ) == zeroM );
933  loop2.addStatement( std::string( "}\n" ) );
934  }
935  }
936  block->addStatement( loop2 );
937  }
938  if( evaluateB ) {
939  evaluateStatesImplicitSystem( block, k_index, Ah, C, index1, index2, tmp_index );
940  evaluateRhsImplicitSystem( block, k_index, index1 );
941  }
942 
943  return SUCCESSFUL_RETURN;
944 }
945 
946 
948  const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2,
949  const ExportIndex& tmp_index3, const ExportIndex& tmp_index4, const ExportVariable& tmp_meas )
950 {
951 
953 }
954 
955 
957 {
958 
960 }
961 
962 
964 {
965 
967 }
968 
969 
971 {
972 
974 }
975 
976 
978 {
979  ExportForLoop loop0( stage,0,numStages );
980  ExportForLoop loop1( i, 0, NX1+NX2 );
981  loop1.addStatement( rk_stageValues.getCol( stage*(NX+NXA)+i ) == rk_xxx_lin.getCol( i ) );
982  loop1.addStatement( tmp_index == k_index + i );
983  for( uint j = 0; j < numStages; j++ ) {
984  loop1.addStatement( rk_stageValues.getCol( stage*(NX+NXA)+i ) += Ah.getElement(stage,j)*rk_kkk.getElement( tmp_index,j ) );
985  }
986  loop0.addStatement( loop1 );
987 
988  ExportForLoop loop3( i, 0, NXA );
989  loop3.addStatement( tmp_index == k_index + i + NX );
990  loop3.addStatement( rk_stageValues.getCol( stage*(NX+NXA)+NX+i ) == rk_kkk.getElement( tmp_index,stage ) );
991  loop0.addStatement( loop3 );
992  block->addStatement( loop0 );
993 
994  return SUCCESSFUL_RETURN;
995 }
996 
997 
999 {
1000  ExportForLoop loop( i, 0, NX+NXA );
1001  loop.addStatement( rk_xxx.getCol( i ) == rk_stageValues.getCol( stage*(NX+NXA)+i ) );
1002  block->addStatement( loop );
1003 
1004  ExportForLoop loop4( i, 0, NDX2 );
1005  loop4.addStatement( tmp_index == k_index + i );
1006  loop4.addStatement( rk_xxx.getCol( inputDim-diffsDim+i ) == rk_kkk.getElement( tmp_index,stage ) );
1007  block->addStatement( loop4 );
1008 
1009  if( C.getDim() > 0 ) { // There is a time dependence, so it must be set
1010  block->addStatement( rk_xxx.getCol( inputDim-diffsDim+NDX ) == rk_ttt + C.getCol(stage) );
1011  }
1012 
1013  return SUCCESSFUL_RETURN;
1014 }
1015 
1016 
1018 {
1019  uint i;
1020  DMatrix zeroM = zeros<double>( NX2+NXA,1 );
1022  // matrix rk_b:
1023  if( NDX2 == 0 ) {
1024  for( i = 0; i < NX2; i++ ) {
1025  block->addStatement( rk_b.getElement( stage*(NX2+NXA)+i,0 ) == rk_kkk.getElement( k_index+NX1+i,stage ) - rk_rhsTemp.getRow( i ) );
1026  }
1027  }
1028  else {
1029  for( i = 0; i < NX2; i++ ) {
1030  block->addStatement( rk_b.getElement( stage*(NX2+NXA)+i,0 ) == zeroM.getRow( i ) - rk_rhsTemp.getRow( i ) );
1031  }
1032  }
1033  for( i = 0; i < NXA; i++ ) {
1034  block->addStatement( rk_b.getElement( stage*(NX2+NXA)+NX2+i,0 ) == zeroM.getRow( i ) - rk_rhsTemp.getRow( NX2+i ) );
1035  }
1036 
1037  return SUCCESSFUL_RETURN;
1038 }
1039 
1040 
1041 returnValue ForwardLiftedIRKExport::evaluateRhsInexactSensitivities( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2, const ExportIndex& tmp_index3, const ExportIndex& k_index, const ExportVariable& Ah )
1042 {
1043  if( NX2 > 0 ) {
1044  uint j;
1045 
1046  ExportForLoop loop1( index2,0,numStages );
1047  loop1.addStatement( rk_seed.getCols(0,NX) == rk_stageValues.getCols(index2*(NX+NXA),index2*(NX+NXA)+NX) );
1049 
1050  ExportForLoop loop2( index3,0,NX2 );
1051  loop2.addStatement( tmp_index1 == k_index + index3 );
1052  ExportForLoop loop3( index1,0,NX2 );
1053  loop3.addStatement( tmp_index2 == tmp_index1*(NX+NU) + index1 );
1054  for( j = 0; j < numStages; j++ ) {
1055  loop3.addStatement( rk_seed.getCol( NX+index3*(NX+NU)+index1 ) += Ah.getElement(index2,j)*rk_diffK.getElement(tmp_index2,j) );
1056  }
1057  loop2.addStatement( loop3 );
1058 
1059  ExportForLoop loop4( index1,0,NU );
1060  loop4.addStatement( tmp_index2 == tmp_index1*(NX+NU) + NX + index1 );
1061  for( j = 0; j < numStages; j++ ) {
1062  loop4.addStatement( rk_seed.getCol( NX+index3*(NX+NU)+NX+index1 ) += Ah.getElement(index2,j)*rk_diffK.getElement(tmp_index2,j) );
1063  }
1064  loop2.addStatement( loop4 );
1065  loop1.addStatement( loop2 );
1066 
1067  if( NDX2 > 0 ) {
1068  ExportForLoop loop5( index3,0,NDX2 );
1069  loop5.addStatement( tmp_index1 == k_index + index3 );
1070  ExportForLoop loop51( index1,0,NX2 );
1071  loop51.addStatement( tmp_index2 == tmp_index1*(NX+NU) + index1 );
1072  loop51.addStatement( rk_seed.getCol( NX+(NX+index3)*(NX+NU)+index1 ) == rk_diffK.getElement(tmp_index2,index2) );
1073  loop5.addStatement( loop51 );
1074 
1075  ExportForLoop loop52( index1,0,NU );
1076  loop52.addStatement( tmp_index2 == tmp_index1*(NX+NU) + NX + index1 );
1077  loop52.addStatement( rk_seed.getCol( NX+(NX+index3)*(NX+NU)+NX+index1 ) == rk_diffK.getElement(tmp_index2,index2) );
1078  loop5.addStatement( loop52 );
1079  loop1.addStatement( loop5 );
1080 
1081  loop1.addStatement( rk_seed.getCols(NX+(NX+NDX2+NXA)*(NX+NU)+NXA+NU+NOD,rk_seed.getNumCols()) == rk_kkk.getSubMatrix( k_index,k_index+NX,index2,index2+1 ).getTranspose() );
1082  }
1083 
1084  if( NXA > 0 ) {
1085  ExportForLoop loop6( index3,0,NXA );
1086  loop6.addStatement( tmp_index1 == k_index + NX + index3 );
1087  ExportForLoop loop61( index1,0,NX2 );
1088  loop61.addStatement( tmp_index2 == tmp_index1*(NX+NU) + index1 );
1089  loop61.addStatement( rk_seed.getCol( NX+(NX+NDX2+index3)*(NX+NU)+index1 ) == rk_diffK.getElement(tmp_index2,index2) );
1090  loop6.addStatement( loop61 );
1091 
1092  ExportForLoop loop62( index1,0,NU );
1093  loop62.addStatement( tmp_index2 == tmp_index1*(NX+NU) + NX + index1 );
1094  loop62.addStatement( rk_seed.getCol( NX+(NX+NDX2+index3)*(NX+NU)+NX+index1 ) == rk_diffK.getElement(tmp_index2,index2) );
1095  loop6.addStatement( loop62 );
1096  loop1.addStatement( loop6 );
1097 
1098  loop1.addStatement( rk_seed.getCols(NX+(NX+NDX2+NXA)*(NX+NU),NX+(NX+NDX2+NXA)*(NX+NU)+NXA) == rk_stageValues.getCols(index2*(NX+NXA)+NX,index2*(NX+NXA)+NX+NXA) );
1099  }
1100 
1102 
1103  ExportForLoop loop02( index3,0,NX2+NXA );
1104  loop02.addStatement( tmp_index1 == k_index + index3 );
1105  loop02.addStatement( tmp_index3 == index2*(NX2+NXA)+index3 );
1106  ExportForLoop loop03( index1,0,NX2 );
1107  loop03.addStatement( tmp_index2 == tmp_index1*(NX+NU) + index1 );
1108  loop03.addStatement( rk_b.getElement( tmp_index3,1+index1 ) == 0.0 - rk_diffSweep.getElement( index3,index1 ) );
1109  if( NDX2 == 0 ) loop03.addStatement( rk_b.getElement( tmp_index3,1+index1 ) += rk_diffK.getElement(tmp_index2,index2) );
1110 // else return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
1111  loop02.addStatement( loop03 );
1112 
1113  ExportForLoop loop04( index1,0,NU );
1114  loop04.addStatement( tmp_index2 == tmp_index1*(NX+NU) + NX + index1 );
1115  loop04.addStatement( rk_b.getElement( tmp_index3,1+NX+index1 ) == 0.0 - rk_diffSweep.getElement( index3,NX+index1 ) );
1116  if( NDX2 == 0 ) loop04.addStatement( rk_b.getElement( tmp_index3,1+NX+index1 ) += rk_diffK.getElement(tmp_index2,index2) );
1117 // else return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
1118  loop02.addStatement( loop04 );
1119  loop1.addStatement( loop02 );
1120  block->addStatement( loop1 );
1121  }
1122 
1123  return SUCCESSFUL_RETURN;
1124 }
1125 
1126 
1127 returnValue ForwardLiftedIRKExport::allSensitivitiesImplicitSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2, const ExportIndex& tmp_index3, const ExportIndex& k_index, const ExportVariable& Bh, bool update )
1128 {
1129  if( NX2 > 0 ) {
1130  int linSolver;
1131  get( LINEAR_ALGEBRA_SOLVER, linSolver );
1132  DMatrix tempCoefs( evaluateDerivedPolynomial( 0.0 ) ); // We compute the algebraic variables at the beginning of the shooting interval !
1133 
1134  // call the linear solver:
1135  if( NDX2 > 0 ) {
1137  }
1138  else {
1140  }
1141 
1142  // update rk_diffK with the new sensitivities:
1143  ExportForLoop loop20( index2,0,numStages );
1144  ExportForLoop loop21( index3,0,NX2 );
1145  if( update ) {
1146  loop21.addStatement( tmp_index1 == (k_index + NX1 + index3)*(NX+NU) );
1147  }
1148  else {
1149  loop21.addStatement( tmp_index1 == (NX1 + index3)*(NX+NU) );
1150  }
1151  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) {
1152  loop21.addStatement( tmp_index3 == index2*(NX2+NXA)+index3 );
1153  }
1154  else {
1155  loop21.addStatement( tmp_index3 == index2*NX2+index3 );
1156  }
1157  ExportForLoop loop22( index1,0,NX2 );
1158  loop22.addStatement( tmp_index2 == tmp_index1+index1 );
1159  if( update ) {
1160  loop22.addStatement( rk_diffK.getElement(tmp_index2,index2) += rk_b.getElement(tmp_index3,1+index1) );
1161  }
1162  else {
1163  loop22.addStatement( rk_diffK_local.getElement(tmp_index2,index2) == rk_b.getElement(tmp_index3,1+index1) );
1164  }
1165  loop21.addStatement( loop22 );
1166 
1167  ExportForLoop loop23( index1,0,NU );
1168  loop23.addStatement( tmp_index2 == tmp_index1+NX+index1 );
1169  if( update ) {
1170  loop23.addStatement( rk_diffK.getElement(tmp_index2,index2) += rk_b.getElement(tmp_index3,1+NX+index1) );
1171  }
1172  else {
1173  loop23.addStatement( rk_diffK_local.getElement(tmp_index2,index2) == rk_b.getElement(tmp_index3,1+NX+index1) );
1174  }
1175  loop21.addStatement( loop23 );
1176  loop20.addStatement( loop21 );
1177  if( NXA > 0 ) {
1178  ExportForLoop loop24( index3,0,NXA );
1179  if( update ) {
1180  loop24.addStatement( tmp_index1 == (k_index + NX + index3)*(NX+NU) );
1181  }
1182  else {
1183  loop24.addStatement( tmp_index1 == (NX + index3)*(NX+NU) );
1184  }
1185  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) {
1186  loop24.addStatement( tmp_index3 == index2*(NX2+NXA)+NX2+index3 );
1187  }
1188  else {
1189  loop24.addStatement( tmp_index3 == numStages*NX2+index2*NXA+index3 );
1190  }
1191  ExportForLoop loop25( index1,0,NX2 );
1192  loop25.addStatement( tmp_index2 == tmp_index1+index1 );
1193  if( update ) {
1194  loop25.addStatement( rk_diffK.getElement(tmp_index2,index2) += rk_b.getElement(tmp_index3,1+index1) );
1195  }
1196  else {
1197  loop25.addStatement( rk_diffK_local.getElement(tmp_index2,index2) == rk_b.getElement(tmp_index3,1+index1) );
1198  }
1199  loop24.addStatement( loop25 );
1200 
1201  ExportForLoop loop26( index1,0,NU );
1202  loop26.addStatement( tmp_index2 == tmp_index1+NX+index1 );
1203  if( update ) {
1204  loop26.addStatement( rk_diffK.getElement(tmp_index2,index2) += rk_b.getElement(tmp_index3,1+NX+index1) );
1205  }
1206  else {
1207  loop26.addStatement( rk_diffK_local.getElement(tmp_index2,index2) == rk_b.getElement(tmp_index3,1+NX+index1) );
1208  }
1209  loop24.addStatement( loop26 );
1210  loop20.addStatement( loop24 );
1211  }
1212  block->addStatement( loop20 );
1213 
1214  // update rk_diffK USING RK_DIFFK_LOCAL !! (PROPAGATION OF SENSITIVITIES)
1215  if( !update ) {
1216  ExportForLoop loop40( index2,0,numStages );
1217  ExportForLoop loop41( index3,0,NX2+NXA );
1218  loop41.addStatement( tmp_index1 == (k_index + NX1 + index3)*(NX+NU) );
1219  loop41.addStatement( tmp_index3 == (NX1 + index3)*(NX+NU) );
1220  ExportForLoop loop42( index1,0,NX );
1221  loop42.addStatement( tmp_index2 == tmp_index1+index1 );
1222  loop42.addStatement( rk_diffK.getElement(tmp_index2,index2) == 0.0 );
1223  for( uint loop_i = 0; loop_i < NX; loop_i++ ) {
1224  loop42.addStatement( rk_diffK.getElement(tmp_index2,index2) += rk_diffK_local.getElement(tmp_index3+loop_i,index2)*rk_diffsPrev2.getElement(loop_i,index1) );
1225  }
1226  loop41.addStatement( loop42 );
1227  ExportForLoop loop43( index1,0,NU );
1228  loop43.addStatement( tmp_index2 == tmp_index1+NX+index1 );
1229  loop43.addStatement( tmp_index3 == (NX1 + index3)*(NX+NU)+NX+index1 );
1230  loop43.addStatement( rk_diffK.getElement(tmp_index2,index2) == rk_diffK_local.getElement(tmp_index3,index2) );
1231  loop43.addStatement( tmp_index3 == (NX1 + index3)*(NX+NU) );
1232  for( uint loop_i = 0; loop_i < NX; loop_i++ ) {
1233  loop43.addStatement( rk_diffK.getElement(tmp_index2,index2) += rk_diffK_local.getElement(tmp_index3+loop_i,index2)*rk_diffsPrev2.getElement(loop_i,NX+index1) );
1234  }
1235  loop41.addStatement( loop43 );
1236  loop40.addStatement( loop41 );
1237  block->addStatement( loop40 );
1238  }
1239 
1240  // update rk_diffsNew with the new sensitivities:
1241  ExportForLoop loop3( index2,0,NX2 );
1242  loop3.addStatement( tmp_index1 == (k_index + NX1 + index2)*(NX+NU) );
1243  ExportForLoop loop31( index1,0,NX2+NU );
1244  loop31.addStatement( tmp_index2 == tmp_index1+index1 );
1245  loop31.addStatement( rk_diffsNew2.getElement( index2,index1 ) == rk_diffsPrev2.getElement( index2,index1 ) );
1246  loop31.addStatement( rk_diffsNew2.getElement( index2,index1 ) += rk_diffK.getRow( tmp_index2 )*Bh );
1247  loop3.addStatement( loop31 );
1248  block->addStatement( loop3 );
1249 
1250  if( NXA > 0 ) {
1251  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
1252  block->addStatement( std::string("if( run == 0 ) {\n") );
1253  }
1254  ExportForLoop loop5( index2,0,NXA );
1255  loop5.addStatement( tmp_index1 == (k_index + NX + index2)*(NX+NU) );
1256  ExportForLoop loop51( index1,0,NX2+NU );
1257  loop51.addStatement( tmp_index2 == tmp_index1+index1 );
1258  loop51.addStatement( rk_diffsNew2.getElement( index2+NX2,index1 ) == rk_diffK.getRow( tmp_index2 )*tempCoefs );
1259  loop5.addStatement( loop51 );
1260  block->addStatement( loop5 );
1261  if( !equidistantControlGrid() || grid.getNumIntervals() > 1 ) {
1262  block->addStatement( std::string("}\n") );
1263  }
1264  }
1265  // TODO: The computation of these derivatives for the algebraic variables should be based on the diffK_local variable !!!
1266  }
1267 
1268  return SUCCESSFUL_RETURN;
1269 }
1270 
1271 
1272 returnValue ForwardLiftedIRKExport::evaluateRhsSensitivities( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2 )
1273 {
1274  if( NX2 > 0 ) {
1275  ExportForLoop loop1( index2,0,numStages );
1276  ExportForLoop loop2( index3,0,NX2+NXA );
1277  loop2.addStatement( tmp_index1 == index2*(NX2+NXA)+index3 );
1278  ExportForLoop loop3( index1,0,NX2 );
1279  loop3.addStatement( tmp_index2 == index1+index3*(NVARS2) );
1280  loop3.addStatement( rk_b.getElement( tmp_index1,1+index1 ) == 0.0 - rk_diffsTemp2.getElement( index2,tmp_index2 ) );
1281  loop2.addStatement( loop3 );
1282 
1283  ExportForLoop loop4( index1,0,NU );
1284  loop4.addStatement( tmp_index2 == index1+index3*(NVARS2)+NX1+NX2+NXA );
1285  loop4.addStatement( rk_b.getElement( tmp_index1,1+NX+index1 ) == 0.0 - rk_diffsTemp2.getElement( index2,tmp_index2 ) );
1286  loop2.addStatement( loop4 );
1287  loop1.addStatement( loop2 );
1288  block->addStatement( loop1 );
1289  }
1290 
1291  return SUCCESSFUL_RETURN;
1292 }
1293 
1294 
1296 {
1297  uint grad = 0;
1298  int gradientUp;
1299  get( LIFTED_GRADIENT_UPDATE, gradientUp );
1300  bool gradientUpdate = (bool) gradientUp;
1301  if( gradientUpdate ) grad = NX;
1302 
1303  if( NX2 > 0 ) {
1304  ExportForLoop loop01( index1,NX1,NX1+NX2 );
1305  ExportForLoop loop02( index2,0,NX1+NX2 );
1306  loop02.addStatement( tmp_index == index2+index1*NX );
1307  loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA+grad ) == rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,index2,index2+1 ) );
1308  loop01.addStatement( loop02 );
1309 
1310  if( NU > 0 ) {
1311  ExportForLoop loop03( index2,0,NU );
1312  loop03.addStatement( tmp_index == index2+index1*NU );
1313  loop03.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX)+grad ) == rk_diffsNew2.getSubMatrix( index1-NX1,index1-NX1+1,NX1+NX2+index2,NX1+NX2+index2+1 ) );
1314  loop01.addStatement( loop03 );
1315  }
1316  block->addStatement( loop01 );
1317  }
1318  // ALGEBRAIC STATES
1319  if( NXA > 0 ) {
1320  ExportForLoop loop01( index1,NX,NX+NXA );
1321  ExportForLoop loop02( index2,0,NX1+NX2 );
1322  loop02.addStatement( tmp_index == index2+index1*NX );
1323  loop02.addStatement( rk_eta.getCol( tmp_index+NX+NXA+grad ) == rk_diffsNew2.getSubMatrix( index1-NX1-NX3,index1-NX1-NX3+1,index2,index2+1 ) );
1324  loop01.addStatement( loop02 );
1325 
1326  if( NU > 0 ) {
1327  ExportForLoop loop03( index2,0,NU );
1328  loop03.addStatement( tmp_index == index2+index1*NU );
1329  loop03.addStatement( rk_eta.getCol( tmp_index+(NX+NXA)*(1+NX)+grad ) == rk_diffsNew2.getSubMatrix( index1-NX1-NX3,index1-NX1-NX3+1,NX1+NX2+index2,NX1+NX2+index2+1 ) );
1330  loop01.addStatement( loop03 );
1331  }
1332  block->addStatement( loop01 );
1333  }
1334 
1335  return SUCCESSFUL_RETURN;
1336 }
1337 
1338 
1339 returnValue ForwardLiftedIRKExport::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 )
1340 {
1341 
1343 }
1344 
1345 
1347  const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2,
1348  const ExportIndex& tmp_index3, const ExportVariable& tmp_meas, const ExportVariable& time_tmp, bool STATES, uint base )
1349 {
1350 
1352 }
1353 
1354 
1356 {
1358 
1359 
1360  integrate = ExportFunction( "integrate", rk_eta );
1361  uint i;
1362  for( i = 0; i < rk_outputs.size(); i++ ) {
1364  }
1367  integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
1368  integrate.addLinebreak( ); // TO MAKE SURE IT GETS EXPORTED
1369 
1370  int useOMP;
1371  get(CG_USE_OPENMP, useOMP);
1372  ExportStruct structWspace;
1373  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
1374 
1375  uint timeDep = 0;
1376  if( timeDependant ) timeDep = 1;
1377 
1378  rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", NX, NX+NU, REAL, structWspace );
1379 
1380  int sensGen;
1381  get( DYNAMIC_SENSITIVITY, sensGen);
1382  int linSolver;
1383  get( LINEAR_ALGEBRA_SOLVER, linSolver );
1384  int liftMode = 1;
1385  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) liftMode = 4;
1386 
1387  if( (ExportSensitivityType)sensGen == INEXACT || liftMode == 4 ) {
1388  rk_seed = ExportVariable( "rk_seed", 1, NX+(NX+NDX2+NXA)*(NX+NU)+NXA+NU+NOD+NDX+timeDep, REAL, structWspace );
1389  rk_diffsTemp2 = ExportVariable( "rk_diffsTemp2", NX2+NXA, NVARS2, REAL, structWspace );
1390  rk_diffSweep = ExportVariable( "rk_diffSweep", NX2+NXA, NX+NU, REAL, structWspace );
1391  }
1392 
1393  rk_stageValues = ExportVariable( "rk_stageValues", 1, numStages*(NX+NXA), REAL, structWspace );
1395  if( liftMode == 1 || liftMode == 4 ) {
1397  }
1398  else {
1399  rk_diffK = ExportVariable( "rk_diffK", (NX+NXA)*(NX+NU), numStages, REAL, structWspace );
1400  }
1401 
1402  if( (ExportSensitivityType)sensGen != BACKWARD ) {
1403  rk_Xprev = ExportVariable( "rk_Xprev", N, NX, REAL, ACADO_VARIABLES );
1404  rk_Uprev = ExportVariable( "rk_Uprev", N, NU, REAL, ACADO_VARIABLES );
1405  rk_delta = ExportVariable( "rk_delta", 1, NX+NU, REAL, ACADO_WORKSPACE );
1406  }
1407 
1408  if( (LinearAlgebraSolver) linSolver == SIMPLIFIED_IRK_NEWTON || (LinearAlgebraSolver) linSolver == SINGLE_IRK_NEWTON ) {
1409  rk_A = ExportVariable( "rk_J", NX2+NXA, NX2+NXA, REAL, structWspace );
1410  if(NDX2 > 0) rk_I = ExportVariable( "rk_I", NX2+NXA, NX2+NXA, REAL, structWspace );
1411  }
1412  rk_b = ExportVariable( "rk_b", numStages*(NX+NXA), 1+NX+NU, REAL, structWspace );
1413 
1414  rk_xxx_lin = ExportVariable( "rk_xxx_lin", 1, NX, REAL, structWspace );
1415 
1416  int gradientUp;
1417  get( LIFTED_GRADIENT_UPDATE, gradientUp );
1418  bool gradientUpdate = (bool) gradientUp;
1419 
1420  if( gradientUpdate ) {
1421  diffsDim = NX + (NX+NXA)*(NX+NU) + NX+NU;
1422  inputDim = NX + diffsDim + NU + NOD;
1423  rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
1424 
1425  rk_b_trans = ExportVariable( "rk_b_trans", 1, numStages*(NX+NXA), REAL, structWspace );
1427 
1428  rk_adj_diffs_tmp = ExportVariable( "rk_adjoint", 1, NX+NU, REAL, structWspace );
1429 
1430  rk_seed2 = ExportVariable( "rk_seed_b", 1, NX*(2+NX+NU)+NU+NOD+timeDep, REAL, structWspace );
1431 
1432  rk_xxx_traj = ExportVariable( "rk_stageV_traj", 1, grid.getNumIntervals()*numStages*(NX+NXA), REAL, structWspace );
1433  }
1434 
1435  if( liftMode == 1 ) {
1436  rk_Khat_traj = ExportVariable( "rk_Khat", grid.getNumIntervals(), numStages*(NX+NXA), REAL, structWspace );
1437  rk_Xhat_traj = ExportVariable( "rk_Xhat", grid.getNumIntervals()*NX, 1, REAL, structWspace );
1438  }
1439  if( liftMode == 1 ) rk_diffK_local = ExportVariable( "rk_diffKtraj_aux", (NX+NXA)*(NX+NU), numStages, REAL, structWspace );
1440 
1441  return SUCCESSFUL_RETURN;
1442 }
1443 
1444 
1445 
1446 // PROTECTED:
1447 
1448 
1450 
1451 // end of file.
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
ExportVariable rk_diffsPrev1
virtual returnValue setup()
Allows to export a tailored lifted implicit Runge-Kutta integrator with forward sensitivity generatio...
ForwardLiftedIRKExport(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
virtual returnValue allSensitivitiesImplicitSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index1, const ExportIndex &tmp_index2, const ExportIndex &tmp_index3, const ExportIndex &k_index, const ExportVariable &Bh, bool update)
ExportVariable getRow(const ExportIndex &idx) const
ExportVariable rk_diffK
Definition: irk_export.hpp:571
Expression backwardDerivative(const Expression &arg1, const Expression &arg2)
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)
const std::string getNameSolveTransposeReuseFunction()
returnValue initializeDDMatrix()
Definition: irk_export.cpp:864
ExportVariable getGlobalExportVariable() const
std::vector< ExportVariable > gridVariables
Definition: irk_export.hpp:549
double getFirstTime() const
LinearAlgebraSolver
ExportVariable getTranspose() const
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
GenericMatrix & appendCols(const GenericMatrix &_arg)
Definition: matrix.cpp:83
std::vector< Grid > outputGrids
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)
ExportAcadoFunction diffs_rhs
int getNDX() const
Definition: function.cpp:217
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)
ExportVariable rk_index
std::vector< ExportAcadoFunction > outputs
ExportVariable rk_diffsPrev3
Allows to pass back messages to the calling function.
ExportVariable rk_kkk
Definition: rk_export.hpp:189
DifferentialState x
returnValue initializeCoefficients()
Definition: irk_export.cpp:883
Expression forwardDerivative(const Expression &arg1, const Expression &arg2)
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
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
ImplicitIntegratorMode
ExportVariable getElement(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
ExportVariable rk_diffsTemp2
ForwardIRKExport & operator=(const ForwardIRKExport &arg)
#define CLOSE_NAMESPACE_ACADO
ExportVariable rk_diffsNew3
virtual returnValue evaluateRhsSensitivities(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index1, const ExportIndex &tmp_index2)
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
GenericMatrix< double > DMatrix
Definition: matrix.hpp:457
virtual returnValue prepareInputSystem(ExportStatementBlock &code)
const std::string getNameDiffsRHS() const
Defines a scalar-valued index variable to be used for exporting code.
virtual returnValue prepareOutputSystem(ExportStatementBlock &code)
ExportAcadoFunction rhs3
virtual returnValue copy(const ImplicitRungeKuttaExport &arg)
virtual returnValue evaluateRhsImplicitSystem(ExportStatementBlock *block, const ExportIndex &k_index, const ExportIndex &stage)
std::vector< uint > totalMeas
Definition: irk_export.hpp:550
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
ExportVariable rk_eta
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
ExportAcadoFunction diffs_rhs3
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)
virtual returnValue sensitivitiesInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportVariable &Bh, bool STATES)
ExportVariable getAuxVariable() const
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
Expression multipleForwardDerivative(const Expression &arg1, const Expression &arg2, const Expression &seed)
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
ExportVariable rk_auxSolver
Definition: irk_export.hpp:563
ExportVariable makeRowVector() const
const std::string getNameSolveReuseFunction()
const std::string getNameRHS() const
Encapsulates all user interaction for setting options, logging data and plotting results.
Allows to export code of an arbitrary function.
virtual uint getDim() const
returnValue setDataStruct(ExportStruct _dataStruct)
Definition: export_data.cpp:80
returnValue addStatement(const ExportStatement &_statement)
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)
int getNT() const
Definition: function.cpp:251
ExportFunction integrate
std::string getFullName() const
returnValue addLinebreak(uint num=1)
virtual returnValue evaluateAllStatesImplicitSystem(ExportStatementBlock *block, const ExportIndex &k_index, const ExportVariable &Ah, const ExportVariable &C, const ExportIndex &stage, const ExportIndex &i, const ExportIndex &tmp_index)
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
ExportVariable rk_diffsNew2
uint getNumIntervals() const
returnValue setGlobalExportVariable(const ExportVariable &var)
returnValue prepareOutputEvaluation(ExportStatementBlock &code)
ExportVariable rk_xxx
DifferentialStateDerivative dx
virtual uint getNumCols() const
std::string getName() const
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
virtual returnValue evaluateRhsInexactSensitivities(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index1, const ExportIndex &tmp_index2, const ExportIndex &tmp_index3, const ExportIndex &k_index, const ExportVariable &Ah)
ExportAcadoFunction lin_input
virtual returnValue updateImplicitSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index)
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
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)
Allows to export a tailored implicit Runge-Kutta integrator with forward sensitivity generation for f...
virtual returnValue setDifferentialEquation(const Expression &rhs)
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
Expression & appendCols(const Expression &arg)
Definition: expression.cpp:162
virtual returnValue clear()
virtual returnValue getCode(ExportStatementBlock &code)=0
virtual returnValue evaluateInexactMatrix(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)
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Allows to export code for a block of statements.
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
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)
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)
GenericVector< T > getRow(unsigned _idx) const
Definition: matrix.hpp:197
ExportFunction & addIndex(const ExportIndex &_index)
#define ACADOERROR(retval)
virtual bool equidistantControlGrid() const
ForwardLiftedIRKExport & operator=(const ForwardLiftedIRKExport &arg)
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)
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
std::string getName() const
Definition: export_data.cpp:86
virtual returnValue getCode(ExportStatementBlock &code)
uint getDim() const


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