integrator_discretized_ode.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 
43 
44 
45 
47 
48 
49 //
50 // PUBLIC MEMBER FUNCTIONS:
51 //
52 
54  :IntegratorRK12( ){
55 }
56 
57 
59  :IntegratorRK12( rhs_ ){
60 
61  if ( rhs_.isDiscretized( ) == BT_FALSE ){
63  ASSERT( 1 == 0 );
64  }
65  if( rhs_.isDiscretized( ) == BT_FALSE ){
67  ASSERT( 1 == 0 );
68  }
69 
70  stepLength = rhs_.getStepLength();
71 }
72 
73 
75  :IntegratorRK12( arg ){
76 
77  stepLength = arg.stepLength;
78 }
79 
80 
82 
83 }
84 
85 
87 
88  if( this != &arg ){
90  stepLength = arg.stepLength;
91  }
92  return *this;
93 }
94 
95 
97 
98  return new IntegratorDiscretizedODE(*this);
99 }
100 
101 
102 
104 {
105  stepLength = rhs_.getStepLength( );
106  return IntegratorRK12::init( rhs_ );
107 }
108 
109 
110 
112 
113  // DEFINE SOME LOCAL VARIABLES:
114  // ----------------------------
115 
116  int run1 ; // simple run variable
117  returnValue returnvalue; // return value for error handling
118 
119 
120  // PERFORM ONE STEP OF THE ITERATION:
121  // ----------------------------------
122 
123  if( soa == SOA_FREEZING_ALL ) returnvalue = performDiscreteStep(number);
124  else returnvalue = performDiscreteStep( 0 );
125 
126  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOWARNING( returnvalue );
127 
128 
129 
130  // COMPUTE DERIVATIVES IF REQUESTED:
131  // ---------------------------------
132 
133  if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ){
134 
135  if( soa != SOA_EVERYTHING_FROZEN ){
136  return ACADOERROR(RET_NOT_FROZEN);
137  }
138  if( nBDirs != 0 ){
140  }
141  performADforwardStep(number);
142  }
143  if( nBDirs > 0 ){
144 
145  if( soa != SOA_EVERYTHING_FROZEN ){
146  return ACADOERROR(RET_NOT_FROZEN);
147  }
148  if( nFDirs != 0 || nBDirs2 != 0 || nFDirs2 != 0 ){
150  }
151  performADbackwardStep(number);
152  }
153  if( nFDirs2 > 0 ){
154 
155  if( soa != SOA_EVERYTHING_FROZEN ){
156  return ACADOERROR(RET_NOT_FROZEN);
157  }
158  if( nBDirs != 0 || nBDirs2 != 0 || nFDirs != 1 ){
160  }
161  performADforwardStep2(number);
162  }
163  if( nBDirs2 > 0 ){
164 
165  if( soa != SOA_EVERYTHING_FROZEN ){
166  return ACADOERROR(RET_NOT_FROZEN);
167  }
168  if( nBDirs != 0 || nFDirs2 != 0 || nFDirs != 1 ){
170  }
171  performADbackwardStep2(number);
172  }
173 
174 
175  // ACTUALLY INCREASE THE TIME:
176  // ---------------------------
177 
178  if( nBDirs > 0 || nBDirs2 > 0 ){
179 
180  t = t - stepLength; // IN BACKWARD THE TIME IS RUNNING BACKWARDS.
181  }
182  else{
183 
184  t = t + stepLength; // IN FORWARD MODE THE TIME IS RUNNING FORWARDS.
185  }
186 
187 
188 
189  // STORE THE INTERMEDIATE VALUES IF THIS IS REQUESTED:
190  // ---------------------------------------------------
191 
193 
194  if( number >= maxAlloc){
195  maxAlloc = 2*maxAlloc;
196  h = (double*)realloc(h,maxAlloc*sizeof(double));
197  }
198  h[number] = stepLength;
199  }
200 
201 
202  if( nFDirs == 0 && nBDirs == 0 && nFDirs2 == 0 && nBDirs == 0 ){
203 
204  int i1 = timeInterval.getFloorIndex( t-h[0] );
205  int i2 = timeInterval.getFloorIndex( t );
206  int jj;
207 
208  for( jj = i1+1; jj <= i2; jj++ ){
209 
210  for( run1 = 0; run1 < m; run1++ )
211  xStore( jj, run1 ) = eta4[run1];
212 
213  for( run1 = 0; run1 < mn; run1++ )
214  iStore( jj, run1 ) = x[rhs->index( VT_INTERMEDIATE_STATE, run1 )];
215  }
216  }
217 
218 
219  // CHECK WHETHER THE END OF THE TIME HORIZON IS ALREADY ACHIEVED:
220  // --------------------------------------------------------------
221 
222 
223  if( nBDirs == 0 || nBDirs2 == 0 ){
224 
225  // Stop the algorithm if t >= tend:
226  // ----------------------------------------------
227  if( t >= timeInterval.getLastTime() - 0.5*stepLength ){
229  for( run1 = 0; run1 < m; run1++ ){
230  x[diff_index[run1]] = eta4[run1];
231  }
232  if( soa == SOA_FREEZING_MESH ){
234  }
237  }
238 
239  return SUCCESSFUL_RETURN;
240  }
241  }
242 
243 
245 }
246 
247 
248 
249 
250 
251 // PROTECTED ROUTINES:
252 // -------------------
253 
254 
256 
257  int run1;
258 
259  x[time_index] = t;
260 
261  for( run1 = 0; run1 < m; run1++ )
262  x[diff_index[run1]] = eta4[run1];
263 
264  if( rhs[0].evaluate( number_, x, k[0] ) != SUCCESSFUL_RETURN )
266 
267  for( run1 = 0; run1 < m; run1++ )
268  eta4[run1] = k[0][run1];
269 
270  return SUCCESSFUL_RETURN;
271 }
272 
273 
274 
276 
277  int run1, run2;
278 
279  for( run1 = 0; run1 < nFDirs; run1++ ){
280 
281  for( run2 = 0; run2 < m; run2++ )
282  G[diff_index[run2]] = etaG[run2];
283 
284  if( rhs[0].AD_forward( number_, G, k[0] ) != SUCCESSFUL_RETURN )
286 
287  for( run2 = 0; run2 < m; run2++ )
288  etaG[run2] = k[0][run2];
289  }
290 
291  return SUCCESSFUL_RETURN;
292 }
293 
294 
295 
297 
298  int run2;
299  const int ndir = rhs->getNumberOfVariables() + 1 + m;
300 
301  for( run2 = 0; run2 < ndir; run2++ )
302  l[0][run2] = 0.0;
303 
304  for( run2 = 0; run2 < m; run2++ )
305  H[run2] = etaH[diff_index[run2]];
306 
307  if( rhs[0].AD_backward( number_, H, l[0] )!= SUCCESSFUL_RETURN )
309 
310  for( run2 = 0; run2 < ndir; run2++ )
311  etaH[run2] += l[0][run2];
312 
313  for( run2 = 0; run2 < m; run2++ )
314  etaH[diff_index[run2]] = l[0][diff_index[run2]];
315 
316  return SUCCESSFUL_RETURN;
317 }
318 
319 
320 
322 
323  int run2;
324 
325  for( run2 = 0; run2 < m; run2++ ){
326  G2[diff_index[run2]] = etaG2[run2];
327  G3[diff_index[run2]] = etaG3[run2];
328  }
329 
330  if( rhs[0].AD_forward2( number_, G2, G3, k[0], k2[0] ) != SUCCESSFUL_RETURN )
332 
333  for( run2 = 0; run2 < m; run2++ ){
334  etaG2[run2] = k [0][run2];
335  etaG3[run2] = k2[0][run2];
336  }
337 
338  return SUCCESSFUL_RETURN;
339 }
340 
341 
342 
344 
345  int run2;
346  const int ndir = rhs->getNumberOfVariables() + 1 + m;
347 
348  for( run2 = 0; run2 < ndir; run2++ ){
349  l [0][run2] = 0.0;
350  l2[0][run2] = 0.0;
351  }
352 
353  for( run2 = 0; run2 < m; run2++ ){
354  H2[run2] = etaH2[diff_index[run2]];
355  H3[run2] = etaH3[diff_index[run2]];
356  }
357 
358  if( rhs[0].AD_backward2( number_, H2, H3, l[0], l2[0] )!= SUCCESSFUL_RETURN )
360 
361  for( run2 = 0; run2 < ndir; run2++ ){
362  etaH2[run2] += l [0][run2];
363  etaH3[run2] += l2[0][run2];
364  }
365  for( run2 = 0; run2 < m; run2++ ){
366  etaH2[diff_index[run2]] = l [0][diff_index[run2]];
367  etaH3[diff_index[run2]] = l2[0][diff_index[run2]];
368  }
369 
370  return SUCCESSFUL_RETURN;
371 }
372 
373 
374 
376 
377 
378 // end of file.
virtual IntegratorRK12 & operator=(const IntegratorRK12 &arg)
virtual double getStepLength() const
returnValue performADforwardStep(const int &number_)
virtual BooleanType isDiscretized() const
short int m
Definition: integrator.hpp:620
int * diff_index
Definition: integrator.hpp:653
Allows to pass back messages to the calling function.
virtual returnValue init(const DifferentialEquation &rhs_)
Grid timeInterval
Definition: integrator.hpp:648
#define CLOSE_NAMESPACE_ACADO
returnValue performADforwardStep2(const int &number_)
Abstract base class for all kinds of algorithms for integrating differential equations (ODEs or DAEs)...
Definition: integrator.hpp:61
VariablesGrid xStore
Definition: integrator.hpp:711
uint getFloorIndex(double time) const
Definition: grid.cpp:593
returnValue performADbackwardStep(const int &number_)
returnValue performDiscreteStep(const int &number_)
StateOfAggregation soa
Definition: integrator.hpp:688
virtual returnValue step(int number)
virtual returnValue evaluate(const DVector &x0, const DVector &xa, const DVector &p, const DVector &u, const DVector &w, const Grid &t_)
double * h
Definition: integrator.hpp:640
returnValue performADbackwardStep2(const int &number_)
Implements the Runge-Kutta-12 scheme for integrating ODEs.
int index(VariableType variableType_, int index_) const
Definition: function.cpp:176
VariablesGrid iStore
Definition: integrator.hpp:714
#define ASSERT(x)
short int mn
Definition: integrator.hpp:623
virtual IntegratorDiscretizedODE & operator=(const IntegratorDiscretizedODE &arg)
double getLastTime() const
#define ACADOWARNING(retval)
#define BEGIN_NAMESPACE_ACADO
#define BT_FALSE
Definition: acado_types.hpp:49
virtual returnValue init(const DifferentialEquation &rhs_)
virtual Integrator * clone() const
int getNumberOfVariables() const
Definition: function.cpp:264
Implements a scheme for evaluating discretized ODEs.
#define ACADOERROR(retval)
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
DifferentialEquation * rhs
Definition: integrator.hpp:619


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