irk_lifted_feedback_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 
34 #include <math.h>
37 
38 using namespace std;
39 
41 
42 //
43 // PUBLIC MEMBER FUNCTIONS:
44 //
45 
47  const std::string& _commonHeaderName
48  ) : ForwardIRKExport( _userInteraction,_commonHeaderName )
49 {
50  NF = 0;
51 }
52 
54 {
55  NF = 0;
56 }
57 
58 
60 {
61  if ( solver )
62  delete solver;
63  solver = 0;
64 
65  clear( );
66 }
67 
68 
70 
71  if( this != &arg ){
72 
74  copy( arg );
75  }
76  return *this;
77 }
78 
79 
81 {
82  ExportVariable max;
83  if( NX1 > 0 ) {
85  }
86  if( NF > 0 ) {
87  if( feedb.getGlobalExportVariable().getDim() >= max.getDim() ) {
89  }
90  if( sens_input.getGlobalExportVariable().getDim() >= max.getDim() ) {
92  }
93  if( sens_fdb.getGlobalExportVariable().getDim() >= max.getDim() ) {
95  }
96  }
97  return max;
98 }
99 
100 
102  ExportStruct dataStruct
103  ) const
104 {
105  ForwardIRKExport::getDataDeclarations( declarations, dataStruct );
106 
107  declarations.addDeclaration( rk_seed,dataStruct );
108  declarations.addDeclaration( rk_stageValues,dataStruct );
109 
110  declarations.addDeclaration( rk_Xprev,dataStruct );
111  declarations.addDeclaration( rk_Uprev,dataStruct );
112  declarations.addDeclaration( rk_delta,dataStruct );
113 
114  declarations.addDeclaration( rk_kTemp,dataStruct );
115 
116  declarations.addDeclaration( rk_dk1_tmp,dataStruct );
117  declarations.addDeclaration( rk_dk2_tmp,dataStruct );
118 
119  declarations.addDeclaration( rk_sensF,dataStruct );
120 
121  return SUCCESSFUL_RETURN;
122 }
123 
124 
126  ) const
127 {
129  declarations.addDeclaration( lin_input );
130  declarations.addDeclaration( feedb );
131  declarations.addDeclaration( sens_input );
132  declarations.addDeclaration( sens_fdb );
133 
134  return SUCCESSFUL_RETURN;
135 }
136 
137 
139 {
140  LOG( LVL_DEBUG ) << "Integrator: setNonlinearFeedback... " << endl;
141  if( C.isEmpty() || C.getNumCols() != feedb_.getDim() || C.getNumRows() != NX || NX != NX1 ) return ACADOERROR( RET_INVALID_OPTION );
142 
143  NF = C.getNumCols();
144  C11 = C;
145 
146  OutputFcn feedb_expr;
147  feedb_expr << feedb_;
148 
149  if( feedb_expr.getNP() > 0 || feedb_expr.getNXA() > 0 ) return RET_NOT_IMPLEMENTED_YET;
150 
151  setInputSystem( );
152 
153  NDX2 = feedb_expr.getNDX();
154  if( NDX2 > 0 ) NDX2 = NX;
155 
156  OnlineData dummy0;
157  Control dummy1;
158  DifferentialState dummy2;
159  dummy0.clearStaticCounters();
160  dummy1.clearStaticCounters();
161  dummy2.clearStaticCounters();
162  x = DifferentialState("", NX, 1);
163  u = Control("", NU, 1);
164  od = OnlineData("", NOD, 1);
165 
167  dummy4.clearStaticCounters();
168 
170 
171  DifferentialState f("", NF, 1);
172 
173  DifferentialEquation fun_input;
174  fun_input << A11*x+B11*u+C11*f;
175 // fun_input << A11*x+B11*u+C11*f - M11*dx;
176  lin_input.init(fun_input, "acado_linear_input", NX+NF, NXA, NU, 0, 0, NOD);
177 
178  DifferentialState sX("", NX,NX+NU);
179  DifferentialState A_row("", numStages, 1);
180  Expression Gx = sX.getCols(0,NX);
181  Expression Gu = sX.getCols(NX,NX+NU);
182 
183  Expression GKf;
184  Expression GKx;
185  Expression GKu;
186 
187  if( NDX2 > 0 ) {
188  DifferentialState sKX("", NDX2,NX+NU);
189  GKf = DifferentialState("", NDX2,numStages*NF);
190  GKx = sKX.getCols(0,NX);
191  GKu = sKX.getCols(NX,NX+NU);
192  }
193 
194 
196  for( uint j = 0; j < numStages; j++ ) {
197  Expression tmp = zeros<double>(NX,numStages*NF);
198  for( uint k1 = 0; k1 < NX; k1++ ) {
199  for( uint k2 = 0; k2 < numStages*NF; k2++ ) {
200  tmp(k1,k2) += sensMat(j*NX+k1,k2)*A_row.getRow(j);
201  }
202  }
203  Gf = Gf + tmp;
204  }
205 
206  OutputFcn fdb_;
207  for( uint i = 0; i < feedb_.getDim(); i++ ) {
208 
209  if( NDX2 > 0 ) {
210  fdb_ << feedb_(i);
211  fdb_ << multipleForwardDerivative( feedb_(i), x, Gx ) + multipleForwardDerivative( feedb_(i), dx, GKx );
212  fdb_ << multipleForwardDerivative( feedb_(i), x, Gu ) + forwardDerivative( feedb_(i), u ) + multipleForwardDerivative( feedb_(i), dx, GKu );
213  fdb_ << multipleForwardDerivative( feedb_(i), x, Gf ) + multipleForwardDerivative( feedb_(i), dx, GKf );
214  }
215  else {
216  fdb_ << feedb_(i);
217  fdb_ << multipleForwardDerivative( feedb_(i), x, Gx );
218  fdb_ << multipleForwardDerivative( feedb_(i), x, Gu ) + forwardDerivative( feedb_(i), u );
219  fdb_ << multipleForwardDerivative( feedb_(i), x, Gf );
220  }
221  }
222  LOG( LVL_DEBUG ) << "done!" << endl;
223 
224  if( NDX2 == 0 ) {
225  return feedb.init(fdb_, "acado_feedback", NX+NF+NX*(NX+NU)+numStages, NXA, NU, NP, NX, NOD);
226  } else {
227  return feedb.init(fdb_, "acado_feedback", NX+NF+NX*(NX+NU)+numStages+NDX2*(NX+NU+numStages*NF), NXA, NU, NP, NX, NOD);
228  }
229 }
230 
231 
233 {
234  LOG( LVL_DEBUG ) << "Integrator: setInputSystem... " << endl;
235  if( NX1 > 0 ) {
236  mat1 = formMatrix( M11, A11 );
237 
238  uint i, j;
239  DMatrix Amat = A11;
240  for( j = 1; j < numStages; j++ ) {
241  Amat.appendRows(A11);
242  }
243  DMatrix sol = mat1*Amat;
244  sensMat = sol;
245 
246  DMatrix Bmat = B11;
247  for( j = 1; j < numStages; j++ ) {
248  Bmat.appendRows(B11);
249  }
250  DMatrix solb = mat1*Bmat;
251  sensMat.appendCols(solb);
252 
253  for( j = 0; j < numStages; j++ ) {
254  DMatrix solc = mat1.getCols(j*NX1,(j+1)*NX1-1)*C11;
255  sensMat.appendCols(solc);
256  }
257 
258 // // sensMat: numStages*NX x NX+NU+numStages*NF
259 // sensMat.
260  for( i = 0; i < numStages*NX; i++ ) {
261  for( j = 0; j < NX+NU+numStages*NF; j++ ) {
262  if( fabs(sensMat(i,j)) < 1e-9 ) sensMat(i,j) = 0.0;
263  }
264  }
265 
266  // SENS_INPUT:
267  DifferentialState dummy;
268  dummy.clearStaticCounters();
269 
270  DifferentialState sX("", NX,NX+NU);
271 
272  DMatrix u_mat = zeros<double>(numStages*NX,NX);
273  u_mat.appendCols(sensMat.getCols(NX,NX+NU-1));
274  OutputFcn sens_input_;
275  sens_input_ << sensMat.getCols(0,NX-1)*sX+u_mat;
276 
277  sens_input.init(sens_input_, "acado_sens_input", NX*(NX+NU), NXA, NU);
278 
279  // SENS_FEEDBACK:
280  DifferentialState dummy2;
281  dummy2.clearStaticCounters();
282 
283  DifferentialState sF("", numStages*NF,NX+NU);
284 
285  OutputFcn sens_fdb_;
286  sens_fdb_ << sensMat.getCols(NX+NU,NX+NU+numStages*NF-1)*sF;
287 
288  sens_fdb.init(sens_fdb_, "acado_sens_fdb", numStages*NF*(NX+NU), NXA, NU);
289 
290  sensMat = sensMat.getCols(NX+NU,NX+NU+numStages*NF-1);
291  }
292  LOG( LVL_DEBUG ) << "done!" << endl;
293 
294  return SUCCESSFUL_RETURN;
295 }
296 
297 
299 {
300  LOG( LVL_DEBUG ) << "Integrator: prepareInputSystem... " << endl;
301  if( NX1 > 0 ) {
302  DMatrix mat1 = formMatrix( M11, A11 );
303 // rk_mat1 = ExportVariable( "rk_mat1", mat1, STATIC_CONST_REAL );
304 // code.addDeclaration( rk_mat1 );
305 // // TODO: Ask Milan why this does NOT work properly !!
306 // rk_mat1 = ExportVariable( "rk_mat1", numStages*NX1, numStages*NX1, STATIC_CONST_REAL, ACADO_LOCAL );
307 
308  uint j;
309  DMatrix Amat = A11;
310  for( j = 1; j < numStages; j++ ) {
311  Amat.appendRows(A11);
312  }
313  DMatrix sol = mat1*Amat;
314  DMatrix sens = sol;
315 
316  DMatrix Bmat = B11;
317  for( j = 1; j < numStages; j++ ) {
318  Bmat.appendRows(B11);
319  }
320  DMatrix solb = mat1*Bmat;
321  sens.appendCols(solb);
322 
323  for( j = 0; j < numStages; j++ ) {
324  DMatrix solc = mat1.getCols(j*NX1,(j+1)*NX1-1)*C11;
325  sens.appendCols(solc);
326  }
327  if( NDX2 > 0 ) {
328  rk_dk1 = ExportVariable( "rk_dk1", sens, STATIC_CONST_REAL );
329  code.addDeclaration( rk_dk1 );
330  // TODO: Ask Milan why this does NOT work properly !!
331  rk_dk1 = ExportVariable( "rk_dk1", numStages*NX1, NX1+NU+numStages*NF, STATIC_CONST_REAL, ACADO_LOCAL );
332  }
333  }
334  LOG( LVL_DEBUG ) << "done!" << endl;
335 
336  return SUCCESSFUL_RETURN;
337 }
338 
339 
341 {
342  int sensGen;
343  get( DYNAMIC_SENSITIVITY, sensGen );
344  int mode;
345  get( IMPLICIT_INTEGRATOR_MODE, mode );
346 // int liftMode;
347 // get( LIFTED_INTEGRATOR_MODE, liftMode );
350 // if( liftMode != 1 && liftMode != 4 ) ACADOERROR( RET_NOT_IMPLEMENTED_YET );
351 // if( (ExportSensitivityType)sensGen == INEXACT && liftMode != 4 ) ACADOERROR( RET_INVALID_OPTION );
352 
353  int liftMode = 1;
354  // liftMode == 1 --> EXACT LIFTING
355  // liftMode == 2 --> INEXACT LIFTING
356  if( (ExportSensitivityType)sensGen == INEXACT ) liftMode = 2;
357 
359 
360  int useOMP;
361  get(CG_USE_OPENMP, useOMP);
362  if ( useOMP ) {
364  }
365 
366  if( NX1 == 0 || NF == 0 ) return ACADOERROR( RET_INVALID_OPTION );
367 
368  code.addFunction( lin_input );
369  code.addStatement( "\n\n" );
370  code.addFunction( feedb );
371  code.addStatement( "\n\n" );
372 
373  solver->getCode( code );
374  code.addLinebreak(2);
375 
376  // export RK scheme
377  uint run5, run6, run7;
378  std::string tempString;
379 
382 
383  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
384  DMatrix tmp = AA;
385  ExportVariable Ah( "Ah_mat", tmp*=h, STATIC_CONST_REAL );
386  code.addDeclaration( Ah );
387  code.addLinebreak( 2 );
388  // TODO: Ask Milan why this does NOT work properly !!
390 
391  DVector BB( bb );
392  ExportVariable Bh( "Bh_mat", DMatrix( BB*=h ) );
393 
394  DVector CC( cc );
395  ExportVariable C;
396  if( timeDependant ) {
397  C = ExportVariable( "C_mat", DMatrix( CC*=(1.0/grid.getNumIntervals()) ), STATIC_CONST_REAL );
398  code.addDeclaration( C );
399  code.addLinebreak( 2 );
401  }
402 
403  code.addComment(std::string("Fixed step size:") + toString(h));
404 
405  ExportVariable determinant( "det", 1, 1, REAL, ACADO_LOCAL, true );
406  integrate.addDeclaration( determinant );
407 
408  ExportIndex i( "i" );
409  ExportIndex j( "j" );
410  ExportIndex k( "k" );
411  ExportIndex run( "run" );
412  ExportIndex run1( "run1" );
413  ExportIndex tmp_index1("tmp_index1");
414  ExportIndex tmp_index2("tmp_index2");
415  ExportIndex tmp_index3("tmp_index3");
416  ExportIndex k_index("k_index");
417  ExportIndex shooting_index("shoot_index");
418  ExportVariable tmp_meas("tmp_meas", 1, outputGrids.size(), INT, ACADO_LOCAL);
419 
420  ExportVariable numInt( "numInts", 1, 1, INT );
421  if( !equidistantControlGrid() ) {
422  ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
423  code.addDeclaration( numStepsV );
424  code.addLinebreak( 2 );
425  integrate.addStatement( std::string( "int " ) + numInt.getName() + " = " + numStepsV.getName() + "[" + rk_index.getName() + "];\n" );
426  }
427 
428  prepareOutputEvaluation( code );
429 
430  integrate.addIndex( i );
431  integrate.addIndex( j );
432  integrate.addIndex( k );
433  integrate.addIndex( run );
434  integrate.addIndex( run1 );
435  integrate.addIndex( tmp_index1 );
436  integrate.addIndex( tmp_index2 );
437  integrate.addIndex( tmp_index3 );
438  integrate.addIndex( shooting_index );
439  integrate.addIndex( k_index );
440  ExportVariable time_tmp( "time_tmp", 1, 1, REAL, ACADO_LOCAL, true );
441  integrate << shooting_index.getFullName() << " = " << rk_index.getFullName() << ";\n";
443  if( NU > 0 || NOD > 0 ) {
446  }
448 // if( liftMode == 1 || liftMode == 4 ) {
449  integrate.addStatement( rk_delta.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) - rk_Xprev.getRow(shooting_index) );
450  integrate.addStatement( rk_Xprev.getRow(shooting_index) == rk_eta.getCols( 0,NX ) );
451 
454 // }
455 
456  // integrator loop:
457  ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
458  ExportStatementBlock *loop;
459  if( equidistantControlGrid() ) {
460  loop = &tmpLoop;
461  }
462  else {
463  loop = &integrate;
464  loop->addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
465  }
466 
467  // if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
468  // Set rk_diffsPrev:
469  loop->addStatement( std::string("if( run > 0 ) {\n") );
470  if( NX > 0 ) {
471  ExportForLoop loopTemp1( i,0,NX );
472  loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX ) == rk_eta.getCols( i*NX+NX+NXA,i*NX+NX+NXA+NX ) );
473  if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,NX,NX+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1),i*NU+(NX+NXA)*(NX+1)+NU ) );
474  loop->addStatement( loopTemp1 );
475  }
476  loop->addStatement( std::string("}\nelse{\n") );
477  DMatrix eyeM = eye<double>(NX);
478  eyeM.appendCols(zeros<double>(NX,NU));
479  loop->addStatement( rk_diffsPrev1 == eyeM );
480  loop->addStatement( std::string("}\n") );
481  // }
482 
483  loop->addStatement( k_index == (shooting_index*grid.getNumIntervals()+run)*NF );
484 
485  // FIRST update using term from optimization variables:
486 // if( liftMode == 1 || (liftMode == 4 && (ExportSensitivityType)sensGen == INEXACT) ) {
487  ExportForLoop loopTemp1( i,0,NF );
488  loopTemp1.addStatement( j == k_index+i );
489  loopTemp1.addStatement( tmp_index1 == j*(NX+NU) );
490  ExportForLoop loopTemp2( run1,0,numStages );
491  loopTemp2.addStatement( rk_kkk.getElement( j,run1 ) += rk_delta*rk_diffK.getSubMatrix( tmp_index1,tmp_index1+NX+NU,run1,run1+1 ) );
492  loopTemp1.addStatement( loopTemp2 );
493  loop->addStatement( loopTemp1 );
494 // }
495 
496  // PART 1: The linear input system
497  prepareInputSystem( code );
498  code.addStatement( "\n\n" );
499  code.addFunction( sens_input );
500  code.addStatement( "\n\n" );
501  code.addFunction( sens_fdb );
502  code.addStatement( "\n\n" );
503 
504  solveInputSystem( loop, i, run1, j, k_index, Ah );
505  evaluateAllStatesImplicitSystem( loop, k_index, Ah, C, run1, j, tmp_index1 );
506 
507  // propagate sensitivities:
508  loop->addFunctionCall( sens_input.getName(), rk_diffsPrev1, rk_dk1_tmp );
509 
510  // IF INEXACT: CONDENSE DIFFK INTO RK_DK1:
511  if( liftMode == 2 ) {
512  ExportForLoop loopKsens02( j,0,NF );
513  loopKsens02.addStatement( tmp_index1 == (k_index+j)*(NX+NU) );
514  for( run6 = 0; run6 < numStages; run6++ ) {
515  loopKsens02.addStatement( rk_sensF.getRow(run6*NF+j) == rk_diffK.getSubMatrix(tmp_index1,tmp_index1+NX+NU,run6,run6+1).getTranspose() );
516  }
517  loop->addStatement( loopKsens02 );
518  loop->addFunctionCall( sens_fdb.getName(), rk_sensF, rk_dk2_tmp );
519 
520  ExportForLoop loopKsens04( i,0,NX );
521  for( run5 = 0; run5 < numStages; run5++ ) {
522  loopKsens04.addStatement( rk_dk2_tmp.getSubMatrix(run5*NX+i,run5*NX+i+1,0,NX+NU) += rk_dk1_tmp.getRow(run5*NX+i) );
523  }
524  loop->addStatement( loopKsens04 );
525  }
526 
527  // PART 2: The static nonlinear system
528  if( liftMode == 2 ) {
529  loop->addStatement( std::string("if(") + run.getName() + " == 0) {\n" );
530  }
531  DMatrix eyeMF = eye<double>(numStages*NF);
532  loop->addStatement( rk_A == eyeMF );
533  if( liftMode == 2 ) loop->addStatement( std::string("}\n") );
534  ExportForLoop loopF( i,0,numStages );
535  loopF.addStatement( rk_seed.getCols(0,NX+NF) == rk_stageValues.getCols(i*(NX+NF),(i+1)*(NX+NF)) );
536 
538 
539  DMatrix zeroV = zeros<double>(1,NX*(NX+NU));
540  loopF.addStatement( rk_seed.getCols(NX+NF,NX+NF+NX*(NX+NU)) == zeroV );
541 // }
542  loopF.addStatement( rk_seed.getCols(NX+NF,NX+NF+NX*(NX+NU)) == rk_diffsPrev1.makeRowVector() );
543  ExportForLoop loopF1( j,0,NX );
544  for( run5 = 0; run5 < numStages; run5++ ) {
545  if( liftMode == 1 ) {
546  loopF1.addStatement( rk_seed.getCols(NX+NF+j*(NX+NU),NX+NF+(j+1)*(NX+NU)) += Ah.getElement(i,run5)*rk_dk1_tmp.getSubMatrix(run5*NX+j,run5*NX+j+1,0,NX+NU) );
547  } else {
548  loopF1.addStatement( rk_seed.getCols(NX+NF+j*(NX+NU),NX+NF+(j+1)*(NX+NU)) += Ah.getElement(i,run5)*rk_dk2_tmp.getSubMatrix(run5*NX+j,run5*NX+j+1,0,NX+NU) );
549  }
550  }
551  loopF.addStatement( loopF1 );
552  for( run5 = 0; run5 < numStages; run5++ ) {
553  loopF.addStatement( rk_seed.getCol(NX+NF+NX*(NX+NU)+run5) == Ah.getElement(i,run5) );
554 // }
555  }
556 // }
557 
558  if( NDX2 > 0 ) {
559  ExportForLoop loopF12( j,0,NX );
560  if( liftMode == 1 ) {
561  loopF12.addStatement( tmp_index1 == i*NX+j );
562  loopF12.addStatement( rk_seed.getCols(NX+NF+NX*(NX+NU)+numStages+j*(NX+NU),NX+NF+NX*(NX+NU)+numStages+(j+1)*(NX+NU)) == rk_dk1_tmp.getSubMatrix(tmp_index1,tmp_index1+1,0,NX+NU) );
563  } else {
564  loopF12.addStatement( tmp_index1 == i*NX+j );
565  loopF12.addStatement( rk_seed.getCols(NX+NF+NX*(NX+NU)+numStages+j*(NX+NU),NX+NF+NX*(NX+NU)+numStages+(j+1)*(NX+NU)) == rk_dk2_tmp.getSubMatrix(tmp_index1,tmp_index1+1,0,NX+NU) );
566  }
567  loopF.addStatement( loopF12 );
568  for( run6 = 0; run6 < NX; run6++ ) {
569  for( run7 = 0; run7 < numStages*NF; run7++ ) {
570  loopF.addStatement( rk_seed.getCol(NX+NF+NX*(NX+NU)+numStages+NX*(NX+NU)+run6*(numStages*NF)+run7) == rk_dk1.getElement(i*NX+run6,NX+NU+run7) );
571  }
572  }
573  }
574 
576  for( run5 = 0; run5 < NF; run5++ ) {
577  loopF.addStatement( rk_b.getRow(i*NF+run5) == rk_diffsTemp2.getSubMatrix(i,i+1,run5*(1+NX+NU+numStages*NF),run5*(1+NX+NU+numStages*NF)+1+NX+NU) );
578  }
579  loopF.addStatement( rk_b.getSubMatrix(i*NF,(i+1)*NF,0,1) -= rk_kkk.getSubMatrix(k_index,k_index+NF,i,i+1) );
580 
581  // INEXACT UPDATE OF RHS SENSITIVITIES
582  if( liftMode == 2 ) {
583  ExportForLoop loopFsens( j,0,NF );
584  loopFsens.addStatement( tmp_index1 == (k_index+j)*(NX+NU) );
585  loopFsens.addStatement( tmp_index2 == i*NF+j );
586  loopFsens.addStatement( rk_b.getSubMatrix(tmp_index2,tmp_index2+1,1,1+NX+NU) -= rk_diffK.getSubMatrix(tmp_index1,tmp_index1+NX+NU,i,i+1).getTranspose() );
587  loopF.addStatement( loopFsens );
588  }
589 
590  if( liftMode == 2 ) {
591  loopF.addStatement( std::string("if(") + run.getName() + " == 0) {\n" );
592  }
593  for( run5 = 0; run5 < NF; run5++ ) {
594  loopF.addStatement( rk_A.getRow(i*NF+run5) -= rk_diffsTemp2.getSubMatrix(i,i+1,run5*(1+NX+NU+numStages*NF)+1+NX+NU,(run5+1)*(1+NX+NU+numStages*NF)) );
595  }
596  if( liftMode == 2 ) loopF.addStatement( std::string("}\n") );
597  loop->addStatement( loopF );
598 
599  // call the linear solver:
600  if( liftMode == 2 ) {
601  loop->addStatement( std::string("if(") + run.getName() + " == 0) {\n" );
602  }
603  loop->addStatement( determinant.getFullName() + " = " + ExportStatement::fcnPrefix + "_" + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
604  if( liftMode == 2 ) loop->addStatement( std::string("}\n") );
605  loop->addFunctionCall( solver->getNameSolveReuseFunction(),rk_A.getAddress(0,0),rk_b.getAddress(0,0),rk_auxSolver.getAddress(0,0) );
606 
607  // update the F variables:
608  for( run5 = 0; run5 < numStages; run5++ ) {
609  loop->addStatement( rk_kkk.getSubMatrix(k_index,k_index+NF,run5,run5+1) += rk_b.getSubMatrix(run5*NF,(run5+1)*NF,0,1) );
610  }
611  ExportForLoop loopFsens( i,0,NF );
612  loopFsens.addStatement( tmp_index1 == (k_index+i)*(NX+NU) );
613  for( run5 = 0; run5 < numStages; run5++ ) {
614  if( liftMode == 1 ) {
615  loopFsens.addStatement( rk_diffK.getSubMatrix(tmp_index1,tmp_index1+NX+NU,run5,run5+1) == rk_b.getSubMatrix(run5*NF+i,run5*NF+i+1,1,1+NX+NU).getTranspose() );
616  } else {
617  loopFsens.addStatement( rk_diffK.getSubMatrix(tmp_index1,tmp_index1+NX+NU,run5,run5+1) += rk_b.getSubMatrix(run5*NF+i,run5*NF+i+1,1,1+NX+NU).getTranspose() );
618  }
619  }
620  loop->addStatement( loopFsens );
621 
622 
623  // PART 3: Condensing of the F variables to provide integrator output
624  // update the K variables:
625  for( run5 = 0; run5 < numStages; run5++ ) {
626  loop->addStatement( rk_kTemp.getRows(run5*NX,(run5+1)*NX) += sensMat.getRows(run5*NX,(run5+1)*NX-1)*rk_b.getCol(0) );
627  }
628 
629  ExportForLoop loopKsens03( j,0,NF );
630  loopKsens03.addStatement( tmp_index2 == (k_index+j)*(NX+NU) );
631  for( run6 = 0; run6 < numStages; run6++ ) {
632  loopKsens03.addStatement( rk_sensF.getRow(run6*NF+j) == rk_diffK.getSubMatrix(tmp_index2,tmp_index2+NX+NU,run6,run6+1).getTranspose() );
633  }
634  loop->addStatement( loopKsens03 );
635 
636  loop->addFunctionCall( sens_fdb.getName(), rk_sensF, rk_dk2_tmp );
637  for( run5 = 0; run5 < numStages; run5++ ) {
638  ExportForLoop loopKsens( i,0,NX );
639  loopKsens.addStatement( rk_dk1_tmp.getRow(run5*NX+i) += rk_dk2_tmp.getRow(run5*NX+i) );
640  loop->addStatement( loopKsens );
641  }
642 
643  // update rk_diffsNew with the new sensitivities:
644  ExportForLoop loopKsens1( i,0,NX );
645  ExportForLoop loopKsens2( j,0,NX+NU );
646  loopKsens2.addStatement( rk_diffsNew1.getElement( i,j ) == rk_diffsPrev1.getElement( i,j ) );
647  for( run5 = 0; run5 < numStages; run5++ ) {
648  loopKsens2.addStatement( rk_diffsNew1.getElement( i,j ) += Bh(run5)*rk_dk1_tmp.getElement( run5*NX+i,j ) );
649  }
650  loopKsens1.addStatement( loopKsens2 );
651  loop->addStatement( loopKsens1 );
652 
653 
654  // update rk_eta:
655  for( run6 = 0; run6 < numStages; run6++ ) {
656  for( run5 = 0; run5 < NX; run5++ ) {
657  loop->addStatement( rk_eta.getCol( run5 ) += rk_kTemp.getRow( run6*NX+run5 )*Bh(run6) );
658  }
659  }
660 
661  updateInputSystem(loop, i, j, tmp_index2);
662 
663  loop->addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
664 
665  // end of the integrator loop.
666  if( !equidistantControlGrid() ) {
667  loop->addStatement( "}\n" );
668  }
669  else {
670  integrate.addStatement( *loop );
671  }
672 
673 // integrate.addStatement( std::string( "if( " ) + determinant.getFullName() + " < 1e-12 ) {\n" );
674 // integrate.addStatement( error_code == 2 );
675 // integrate.addStatement( std::string( "} else if( " ) + determinant.getFullName() + " < 1e-6 ) {\n" );
676 // integrate.addStatement( error_code == 1 );
677 // integrate.addStatement( std::string( "} else {\n" ) );
679 // integrate.addStatement( std::string( "}\n" ) );
680 
681  code.addFunction( integrate );
682  code.addLinebreak( 2 );
683 
684  return SUCCESSFUL_RETURN;
685 }
686 
687 
689 {
690  if( NX1 > 0 ) {
691  ExportForLoop loop( index1,0,numStages );
692  loop.addStatement( rk_xxx.getCols(0,NX) == rk_eta.getCols(0,NX) );
693  loop.addStatement( rk_xxx.getCols(NX,NX+NF) == rk_kkk.getSubMatrix(k_index,k_index+NF,index1,index1+1).getTranspose() );
695  block->addStatement(loop);
696 
697  block->addStatement( rk_kTemp == mat1*rk_rhsTemp );
698  }
699 
700  return SUCCESSFUL_RETURN;
701 }
702 
703 
705 {
706  ExportForLoop loop0( stage,0,numStages );
707  ExportForLoop loop1( i, 0, NX );
708  loop1.addStatement( rk_stageValues.getCol( stage*(NX+NF)+i ) == rk_eta.getCol( i ) );
709  for( uint j = 0; j < numStages; j++ ) {
710  loop1.addStatement( rk_stageValues.getCol( stage*(NX+NF)+i ) += Ah.getElement(stage,j)*rk_kTemp.getRow( j*NX+i ) );
711  }
712  loop0.addStatement( loop1 );
713 
714  ExportForLoop loop3( i, 0, NF );
715  loop3.addStatement( tmp_index == k_index + i );
716  loop3.addStatement( rk_stageValues.getCol( stage*(NX+NF)+NX+i ) == rk_kkk.getElement( tmp_index,stage ) );
717  loop0.addStatement( loop3 );
718  block->addStatement( loop0 );
719 
720  return SUCCESSFUL_RETURN;
721 }
722 
723 
725 {
727 
728 // int liftMode;
729 // get( LIFTED_INTEGRATOR_MODE, liftMode );
730 
731  NVARS2 = 0;
732  NVARS3 = 0;
733  diffsDim = (NX+NXA)*(NX+NU);
734  inputDim = (NX+NXA)*(NX+NU+1) + NU + NOD;
735 
736  int useOMP;
737  get(CG_USE_OPENMP, useOMP);
738  ExportStruct structWspace;
739  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
740 
741  uint timeDep = 0;
742  if( timeDependant ) timeDep = 1;
743 
744  rk_ttt = ExportVariable( "rk_ttt", 1, 1, REAL, structWspace, true );
745  rk_xxx = ExportVariable( "rk_xxx", 1, NX+NF+NX+NU+NOD+timeDep, REAL, structWspace );
746  rk_rhsTemp = ExportVariable( "rk_rhsTemp", numStages*NX, 1, REAL, structWspace );
747  rk_kTemp = ExportVariable( "rk_kTemp", numStages*NX, 1, REAL, structWspace );
748  rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
749  rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
750  integrate = ExportFunction( "integrate", rk_eta );
753 
754  rk_eta.setDoc( "Working array of size " + toString( rk_eta.getDim() ) + " to pass the input values and return the results." );
755  reset_int.setDoc( "The internal memory of the integrator can be reset." );
756  rk_index.setDoc( "Number of the shooting interval." );
757  error_code.setDoc( "Status code of the integrator." );
758  integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
759  integrate.addLinebreak( ); // TO MAKE SURE IT GETS EXPORTED
760 
761  // setup linear solver:
762  int solverType;
764 
765  if ( solver )
766  delete solver;
767  solver = 0;
768 
769  switch( (LinearAlgebraSolver) solverType ) {
770  case GAUSS_LU:
772  solver->init( NF*numStages, NX+NU+1 );
773  solver->setReuse( true ); // IFTR method
774  solver->setup();
776  break;
777  default:
778  return ACADOERROR( RET_INVALID_OPTION );
779  }
780 
781  rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", NX, NX+NU, REAL, structWspace );
782  rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX, NX+NU, REAL, structWspace );
783 
784  rk_seed = ExportVariable( "rk_seed", 1, 2*NX+NF+NX*(NX+NU)+numStages+NDX2*(NX+NU+numStages*NF)+NU+NOD+timeDep, REAL, structWspace );
785  rk_diffsTemp2 = ExportVariable( "rk_diffsTemp", numStages, NF*(1+NX+NU+numStages*NF), REAL, structWspace );
786  rk_stageValues = ExportVariable( "rk_stageValues", 1, numStages*(NX+NF), REAL, structWspace );
788 // if( liftMode == 1 || liftMode == 4 ) {
789  rk_diffK = ExportVariable( "rk_diffKtraj", N*grid.getNumIntervals()*NF*(NX+NU), numStages, REAL, ACADO_VARIABLES );
790 // }
791 // else {
792 // return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
793 // }
794 
795  rk_Xprev = ExportVariable( "rk_Xprev", N, NX, REAL, ACADO_VARIABLES );
796  rk_Uprev = ExportVariable( "rk_Uprev", N, NU, REAL, ACADO_VARIABLES );
797  rk_delta = ExportVariable( "rk_delta", 1, NX+NU, REAL, ACADO_WORKSPACE );
798 
799  rk_A = ExportVariable( "rk_A", numStages*NF, numStages*NF, REAL, structWspace );
800  rk_b = ExportVariable( "rk_b", numStages*NF, 1+NX+NU, REAL, structWspace );
801 
802  rk_dk1_tmp = ExportVariable( "rk_dk1_tmp", numStages*NX1, NX+NU, REAL, ACADO_WORKSPACE );
803  rk_dk2_tmp = ExportVariable( "rk_dk2_tmp", numStages*NX1, NX+NU, REAL, ACADO_WORKSPACE );
804 
805  rk_sensF = ExportVariable( "rk_sensF", numStages*NF, NX+NU, REAL, structWspace );
806 
807  return SUCCESSFUL_RETURN;
808 }
809 
810 
811 
812 // PROTECTED:
813 
814 
816 
817 // end of file.
ExportVariable rk_diffsPrev1
#define LOG(level)
Just define a handy macro for getting the logger.
Lowest level, the debug level.
bool isEmpty() const
Definition: matrix.hpp:193
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)
Allows to export a tailored lifted implicit Runge-Kutta integrator with forward sensitivity generatio...
Allows to setup and evaluate output functions based on SymbolicExpressions.
Definition: output_fcn.hpp:55
returnValue initializeDDMatrix()
Definition: irk_export.cpp:864
ExportVariable getGlobalExportVariable() const
double getFirstTime() const
LinearAlgebraSolver
virtual ExportVariable getAuxVariable() const
ExportVariable getTranspose() const
GenericMatrix & appendCols(const GenericMatrix &_arg)
Definition: matrix.cpp:83
Expression getRow(const uint &rowIdx) const
Definition: expression.cpp:533
std::vector< Grid > outputGrids
UserInteraction * userInteraction
int getNDX() const
Definition: function.cpp:217
returnValue get(OptionsName name, int &value) const
Definition: options.cpp:69
ExportVariable rk_index
virtual ExportVariable getGlobalExportVariable(const uint factor) const
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
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)
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
virtual returnValue solveInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &k_index, const ExportVariable &Ah)
Allows to export code of a for-loop.
Expression getCols(const uint &colIdx1, const uint &colIdx2) const
Definition: expression.cpp:582
string toString(T const &value)
GenericMatrix & appendRows(const GenericMatrix &_arg)
Definition: matrix.cpp:62
ImplicitIntegratorMode
ExportVariable getElement(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
ExportVariable rk_diffsTemp2
ForwardIRKExport & operator=(const ForwardIRKExport &arg)
#define CLOSE_NAMESPACE_ACADO
int getNXA() const
Definition: function.cpp:212
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
GenericMatrix< double > DMatrix
Definition: matrix.hpp:457
Defines a scalar-valued index variable to be used for exporting code.
virtual returnValue copy(const ImplicitRungeKuttaExport &arg)
GenericMatrix getCols(unsigned _start, unsigned _end) const
Definition: matrix.hpp:246
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
virtual returnValue getCode(ExportStatementBlock &code)
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
returnValue prepareInputSystem(ExportStatementBlock &code)
virtual returnValue setDoc(const std::string &_doc)
std::string commonHeaderName
ExportVariable rk_eta
ExportStruct
virtual ExportFunction & doc(const std::string &_doc)
int getNP() const
Definition: function.cpp:233
static std::string fcnPrefix
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
Allows to export Gaussian elimination for solving linear systems of specific dimensions.
ExportVariable makeRowVector() const
const std::string getNameSolveReuseFunction()
ExportVariable reset_int
Encapsulates all user interaction for setting options, logging data and plotting results.
Allows to export code of an arbitrary function.
virtual uint getDim() const
virtual DMatrix formMatrix(const DMatrix &mass, const DMatrix &jacobian)
Definition: irk_export.cpp:549
returnValue addStatement(const ExportStatement &_statement)
virtual returnValue updateInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index)
ExportFunction integrate
returnValue setReuse(const bool &reuse)
std::string getFullName() const
unsigned getNumRows() const
Definition: matrix.hpp:185
returnValue addLinebreak(uint num=1)
FeedbackLiftedIRKExport(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
unsigned getNumCols() const
Definition: matrix.hpp:189
uint getNumIntervals() const
virtual returnValue setup()=0
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
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
double getLastTime() const
ExportVariable rk_ttt
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
Allows to export a tailored implicit Runge-Kutta integrator with forward sensitivity generation for f...
returnValue addFunction(const ExportFunction &_function)
ExportVariable rk_diffsNew1
virtual returnValue clear()
virtual returnValue setNonlinearFeedback(const DMatrix &C, const Expression &feedb)
virtual returnValue getCode(ExportStatementBlock &code)=0
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Allows to export code for a block of statements.
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)
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)
#define ACADOERROR(retval)
virtual bool equidistantControlGrid() const
Defines a matrix-valued variable to be used for exporting code.
FeedbackLiftedIRKExport & operator=(const FeedbackLiftedIRKExport &arg)
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
uint getDim() const


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