erk_export.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
27 
35 
36 using namespace std;
37 
39 
40 //
41 // PUBLIC MEMBER FUNCTIONS:
42 //
43 
45  const std::string& _commonHeaderName
46  ) : RungeKuttaExport( _userInteraction,_commonHeaderName )
47 {
49 }
50 
51 
53  ) : RungeKuttaExport( arg )
54 {
55  copy( arg );
56 }
57 
58 
60 {
61  clear( );
62 }
63 
64 
66 {
67  int sensGen;
68  get( DYNAMIC_SENSITIVITY,sensGen );
70 
71  bool DERIVATIVES = ((ExportSensitivityType)sensGen != NO_SENSITIVITY);
72 
73  LOG( LVL_DEBUG ) << "Preparing to export ExplicitRungeKuttaExport... " << endl;
74 
75  // export RK scheme
76  uint rhsDim = NX*(NX+NU+1);
77  if( !DERIVATIVES ) rhsDim = NX;
78  inputDim = NX*(NX+NU+1) + NU + NOD;
79  if( !DERIVATIVES ) inputDim = NX + NU + NOD;
80  const uint rkOrder = getNumStages();
81 
82  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
83 
84  ExportVariable Ah ( "A*h", DMatrix( AA )*=h );
85  ExportVariable b4h( "b4*h", DMatrix( bb )*=h );
86 
87  rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
88  rk_eta = ExportVariable( "rk_eta", 1, inputDim );
89 
90  int useOMP;
91  get(CG_USE_OPENMP, useOMP);
92  ExportStruct structWspace;
93  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
94 
95  rk_ttt.setup( "rk_ttt", 1, 1, REAL, structWspace, true );
96  uint timeDep = 0;
97  if( timeDependant ) timeDep = 1;
98 
99  rk_xxx.setup("rk_xxx", 1, inputDim+timeDep, REAL, structWspace);
100  rk_kkk.setup("rk_kkk", rkOrder, rhsDim, REAL, structWspace);
101 
102  if ( useOMP )
103  {
104  ExportVariable auxVar;
105 
106  auxVar = getAuxVariable();
107  auxVar.setName( "odeAuxVar" );
108  auxVar.setDataStruct( ACADO_LOCAL );
109  rhs.setGlobalExportVariable( auxVar );
111  }
112 
113  ExportIndex run( "run1" );
114 
115  // setup INTEGRATE function
116  if( equidistantControlGrid() ) {
117  integrate = ExportFunction( "integrate", rk_eta, reset_int );
118  }
119  else {
120  integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
121  }
123  rk_eta.setDoc( "Working array to pass the input values and return the results." );
124  reset_int.setDoc( "The internal memory of the integrator can be reset." );
125  rk_index.setDoc( "Number of the shooting interval." );
126  error_code.setDoc( "Status code of the integrator." );
127  integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
128  integrate.addIndex( run );
129 
130  ExportVariable numInt( "numInts", 1, 1, INT );
131  if( !equidistantControlGrid() ) {
132  integrate.addStatement( std::string( "int numSteps[" ) + toString( numSteps.getDim() ) + "] = {" + toString( numSteps(0) ) );
133  uint i;
134  for( i = 1; i < numSteps.getDim(); i++ ) {
135  integrate.addStatement( std::string( ", " ) + toString( numSteps(i) ) );
136  }
137  integrate.addStatement( std::string( "};\n" ) );
138  integrate.addStatement( std::string( "int " ) + numInt.getName() + " = numSteps[" + rk_index.getName() + "];\n" );
139  }
140 
142 
143  if( DERIVATIVES ) {
144  // initialize sensitivities:
145  DMatrix idX = eye<double>( NX );
146  DMatrix zeroXU = zeros<double>( NX,NU );
147  integrate.addStatement( rk_eta.getCols( NX,NX*(1+NX) ) == idX.makeVector().transpose() );
148  integrate.addStatement( rk_eta.getCols( NX*(1+NX),NX*(1+NX+NU) ) == zeroXU.makeVector().transpose() );
149  }
150 
151  if( inputDim > rhsDim ) {
153  }
155 
156  // integrator loop
157  ExportForLoop loop;
158  if( equidistantControlGrid() ) {
159  loop = ExportForLoop( run, 0, grid.getNumIntervals() );
160  }
161  else {
162  loop = ExportForLoop( run, 0, 1 );
163  loop.addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
164  }
165 
166  for( uint run1 = 0; run1 < rkOrder; run1++ )
167  {
168  loop.addStatement( rk_xxx.getCols( 0,rhsDim ) == rk_eta.getCols( 0,rhsDim ) + Ah.getRow(run1)*rk_kkk );
169  if( timeDependant ) loop.addStatement( rk_xxx.getCol( inputDim ) == rk_ttt + ((double)cc(run1))/grid.getNumIntervals() );
171  }
172  loop.addStatement( rk_eta.getCols( 0,rhsDim ) += b4h^rk_kkk );
173  loop.addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
174  // end of integrator loop
175 
176  if( !equidistantControlGrid() ) {
177  loop.addStatement( "}\n" );
178 // loop.unrollLoop();
179  }
180  integrate.addStatement( loop );
181 
183 
184  LOG( LVL_DEBUG ) << "done" << endl;
185 
186  return SUCCESSFUL_RETURN;
187 }
188 
189 
191 {
192  int sensGen;
193  get( DYNAMIC_SENSITIVITY,sensGen );
194 
195  OnlineData dummy0;
196  Control dummy1;
197  DifferentialState dummy2;
198  AlgebraicState dummy3;
200  dummy0.clearStaticCounters();
201  dummy1.clearStaticCounters();
202  dummy2.clearStaticCounters();
203  dummy3.clearStaticCounters();
204  dummy4.clearStaticCounters();
205 
206  x = DifferentialState("", NX, 1);
208  z = AlgebraicState("", NXA, 1);
209  u = Control("", NU, 1);
210  od = OnlineData("", NOD, 1);
211 
212  if( NDX > 0 && NDX != NX ) {
213  return ACADOERROR( RET_INVALID_OPTION );
214  }
215  if( rhs_.getNumRows() != (NX+NXA) ) {
216  return ACADOERROR( RET_INVALID_OPTION );
217  }
218 
219  DifferentialEquation f, f_ODE;
220  // add usual ODE
221  f_ODE << rhs_;
222  if( f_ODE.getNDX() > 0 ) {
223  return ACADOERRORTEXT( RET_INVALID_OPTION, "No implicit systems supported when using an explicit integration method!");
224  }
225 
226  if( (ExportSensitivityType)sensGen == FORWARD ) {
227  DifferentialState Gx("", NX,NX), Gu("", NX,NU);
228  // no free parameters yet!
229  // DifferentialState Gp(NX,NP);
230 
231  f << rhs_;
232  /* if ( f.getDim() != f.getNX() )
233  return ACADOERROR( RET_ILLFORMED_ODE );*/
234 
235  // add VDE for differential states
236  f << multipleForwardDerivative( rhs_, x, Gx );
237  /* if ( f.getDim() != f.getNX() )
238  return ACADOERROR( RET_ILLFORMED_ODE );*/
239 
240 
241  // add VDE for control inputs
242  f << multipleForwardDerivative( rhs_, x, Gu ) + forwardDerivative( rhs_, u );
243  // if ( f.getDim() != f.getNX() )
244  // return ACADOERROR( RET_ILLFORMED_ODE );
245 
246  // no free parameters yet!
247  // f << forwardDerivative( rhs_, x ) * Gp + forwardDerivative( rhs_, p );
248 
249  }
250  else if( (ExportSensitivityType)sensGen != NO_SENSITIVITY ) {
251  return ACADOERROR( RET_INVALID_OPTION );
252  }
253  if( f.getNT() > 0 ) timeDependant = true;
254 
255  int matlabInterface;
256  userInteraction->get(GENERATE_MATLAB_INTERFACE, matlabInterface);
257  if( matlabInterface && (ExportSensitivityType)sensGen == FORWARD ) {
258  return rhs.init(f_ODE, "rhs", NX, 0, NU, NP, NDX, NOD)
259  & diffs_rhs.init(f, "rhs_ext", NX * (1 + NX + NU), 0, NU, NP, NDX, NOD);
260  }
261  else if( (ExportSensitivityType)sensGen == FORWARD ) {
262  return diffs_rhs.init(f, "rhs_forw", NX * (1 + NX + NU), 0, NU, NP, NDX, NOD);
263  }
264  else {
265  return diffs_rhs.init(f_ODE, "rhs", NX, 0, NU, NP, NDX, NOD);
266  }
267 
268  return SUCCESSFUL_RETURN;
269 }
270 
271 
273 
274  return ACADOERROR( RET_INVALID_OPTION );
275 }
276 
277 
279 
280  return ACADOERROR( RET_INVALID_OPTION );
281 }
282 
283 
284 returnValue ExplicitRungeKuttaExport::setLinearOutput( const DMatrix& M3, const DMatrix& A3, const std::string& _rhs3, const std::string& _diffs_rhs3 )
285 {
286  return RET_INVALID_OPTION;
287 }
288 
289 
291  ExportStruct dataStruct
292  ) const
293 {
294  if( exportRhs ) {
295  declarations.addDeclaration( getAuxVariable(),dataStruct );
296  }
297  declarations.addDeclaration( rk_ttt,dataStruct );
298  declarations.addDeclaration( rk_xxx,dataStruct );
299  declarations.addDeclaration( rk_kkk,dataStruct );
300 
301 // declarations.addDeclaration( reset_int,dataStruct );
302 
303  return SUCCESSFUL_RETURN;
304 }
305 
306 
308  ) const
309 {
310  declarations.addDeclaration( integrate );
311 
312  declarations.addDeclaration( rhs );
313  declarations.addDeclaration( diffs_rhs );
314 
315  return SUCCESSFUL_RETURN;
316 }
317 
318 
320  )
321 {
322 // int printLevel;
323 // get( PRINTLEVEL,printLevel );
324 //
325 // if ( (PrintLevel)printLevel >= HIGH )
326 // acadoPrintf( "--> Exporting %s... ",fileName.getName() );
327 
328  int useOMP;
329  get(CG_USE_OPENMP, useOMP);
330  if ( useOMP )
331  {
333 
334  code << "#pragma omp threadprivate( "
335  << getAuxVariable().getFullName() << ", "
336  << rk_xxx.getFullName() << ", "
337  << rk_ttt.getFullName() << ", "
338  << rk_kkk.getFullName()
339  << " )\n\n";
340  }
341 
342  int sensGen;
343  get( DYNAMIC_SENSITIVITY,sensGen );
344  if( exportRhs ) {
345  code.addFunction( rhs );
346  code.addFunction( diffs_rhs );
347  }
348 
349  double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
350  code.addComment(std::string("Fixed step size:") + toString(h));
351  code.addFunction( integrate );
352 
353 
354 // if ( (PrintLevel)printLevel >= HIGH )
355 // acadoPrintf( "done.\n" );
356 
357  return SUCCESSFUL_RETURN;
358 }
359 
360 
361 returnValue ExplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_, const std::vector<Expression> _rhs ) {
362 
363  return ACADOERROR( RET_INVALID_OPTION );
364 }
365 
366 
367 returnValue ExplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_,
368  const std::vector<std::string> _outputNames,
369  const std::vector<std::string> _diffs_outputNames,
370  const std::vector<uint> _dims_output ) {
371 
372  return ACADOERROR( RET_INVALID_OPTION );
373 }
374 
375 
376 returnValue ExplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_,
377  const std::vector<std::string> _outputNames,
378  const std::vector<std::string> _diffs_outputNames,
379  const std::vector<uint> _dims_output,
380  const std::vector<DMatrix> _outputDependencies ) {
381 
382  return ACADOERROR( RET_INVALID_OPTION );
383 }
384 
385 
387 {
388  ExportVariable max;
390  if( diffs_rhs.getGlobalExportVariable().getDim() > max.getDim() ) {
392  }
393  return max;
394 }
395 
396 
397 
398 // PROTECTED:
399 
400 
401 
403 
404 // end of file.
#define LOG(level)
Just define a handy macro for getting the logger.
Lowest level, the debug level.
virtual returnValue setup()
Definition: erk_export.cpp:65
#define B1
ExportVariable getRow(const ExportIndex &idx) const
Allows to export a tailored explicit Runge-Kutta integrator for fast model predictive control...
Definition: erk_export.hpp:54
ExportVariable getGlobalExportVariable() const
double getFirstTime() const
Allows to export a tailored Runge-Kutta integrator for fast model predictive control.
Definition: rk_export.hpp:54
ExportVariable & setup(const std::string &_name, uint _nRows=1, uint _nCols=1, ExportType _type=REAL, ExportStruct _dataStruct=ACADO_LOCAL, bool _callItByValue=false, const std::string &_prefix=std::string())
UserInteraction * userInteraction
ExportAcadoFunction diffs_rhs
int getNDX() const
Definition: function.cpp:217
returnValue get(OptionsName name, int &value) const
Definition: options.cpp:69
ExportVariable rk_index
virtual ExportVariable getAuxVariable() const
Definition: erk_export.cpp:386
Allows to pass back messages to the calling function.
ExportVariable rk_kkk
Definition: rk_export.hpp:189
GenericMatrix & makeVector()
Definition: matrix.cpp:124
DifferentialState x
Expression forwardDerivative(const Expression &arg1, const Expression &arg2)
uint getNumRows() const
returnValue addComment(const std::string &_comment)
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
virtual returnValue setLinearOutput(const DMatrix &M3, const DMatrix &A3, const Expression &rhs)
Definition: erk_export.cpp:278
#define A1
Allows to export code of a for-loop.
string toString(T const &value)
returnValue setName(const std::string &_name)
Definition: export_data.cpp:61
#define CLOSE_NAMESPACE_ACADO
GenericMatrix< double > DMatrix
Definition: matrix.hpp:457
const std::string getNameDiffsRHS() const
Defines a scalar-valued index variable to be used for exporting code.
virtual ~ExplicitRungeKuttaExport()
Definition: erk_export.cpp:59
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
virtual returnValue setDoc(const std::string &_doc)
ExportVariable rk_eta
ExportStruct
virtual ExportFunction & doc(const std::string &_doc)
virtual returnValue copy(const RungeKuttaExport &arg)
Definition: rk_export.cpp:122
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
Expression multipleForwardDerivative(const Expression &arg1, const Expression &arg2, const Expression &seed)
unsigned getDim() const
Definition: vector.hpp:172
ExportVariable reset_int
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
Definition: erk_export.cpp:307
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)
int getNT() const
Definition: function.cpp:251
ExportFunction integrate
std::string getFullName() const
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: erk_export.cpp:290
returnValue addLinebreak(uint num=1)
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
uint getNumIntervals() const
returnValue setGlobalExportVariable(const ExportVariable &var)
ExportVariable rk_xxx
DifferentialStateDerivative dx
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
double getLastTime() const
ExportVariable rk_ttt
#define BEGIN_NAMESPACE_ACADO
ExportVariable error_code
returnValue clearStaticCounters()
Definition: expression.hpp:398
#define BT_FALSE
Definition: acado_types.hpp:49
returnValue addFunction(const ExportFunction &_function)
ExplicitRungeKuttaExport(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
Definition: erk_export.cpp:44
virtual returnValue setDifferentialEquation(const Expression &rhs)
Definition: erk_export.cpp:190
virtual returnValue clear()
virtual returnValue setLinearInput(const DMatrix &M1, const DMatrix &A1, const DMatrix &B1)
Definition: erk_export.cpp:272
Allows to export code for a block of statements.
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
ExportVariable getCol(const ExportIndex &idx) const
returnValue init(const Function &_f, const std::string &_name="acadoFcn", const uint _numX=0, const uint _numXA=0, const uint _numU=0, const uint _numP=0, const uint _numDX=0, const uint _numOD=0)
ExportFunction & addIndex(const ExportIndex &_index)
virtual returnValue setupOutput(const std::vector< Grid > outputGrids_, const std::vector< Expression > rhs)
Definition: erk_export.cpp:361
virtual returnValue getCode(ExportStatementBlock &code)
Definition: erk_export.cpp:319
#define ACADOERROR(retval)
virtual bool equidistantControlGrid() const
Defines a matrix-valued variable to be used for exporting code.
ExportAcadoFunction rhs
#define ACADOERRORTEXT(retval, text)
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
BooleanType is_symmetric
Definition: rk_export.hpp:194


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