simulation_environment.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 
36 
37 #ifdef WIN32
38 #define round( value ) floor( value + 0.5 )
39 #endif
40 
41 
42 // #define SIM_DEBUG
43 
44 using namespace std;
45 
47 
48 
50 {
51  setupOptions( );
52  setupLogging( );
53 
54  startTime = 0.0;
55  endTime = 0.0;
56 
57  process = 0;
58  controller = 0;
59 
60  nSteps = 0;
61 
63 }
64 
65 
67  double _endTime,
68  Process& _process,
69  Controller& _controller
71 {
72  setupOptions( );
73  setupLogging( );
74 
75  startTime = _startTime;
76  endTime = _endTime;
77 
78  if ( _process.isDefined( ) == BT_TRUE )
79  {
80  process = &_process;
82  }
83  else
84  process = 0;
85 
86  if ( _controller.isDefined( ) == BT_TRUE )
87  controller = &_controller;
88  else
89  controller = 0;
90 
91  nSteps = 0;
92 }
93 
94 
96 {
97  startTime = rhs.startTime;
98  endTime = rhs.endTime;
99 
100  if ( rhs.process != 0 )
101  process = rhs.process;
102  else
103  process = 0;
104 
105  if ( rhs.controller != 0 )
106  controller = rhs.controller;
107  else
108  controller = 0;
109 
111 
115 
116  nSteps = rhs.nSteps;
117 }
118 
119 
121 {
122 }
123 
124 
126 {
127  if( this != &rhs )
128  {
130 
131  startTime = rhs.startTime;
132  endTime = rhs.endTime;
133 
134  if ( rhs.process != 0 )
135  process = rhs.process;
136  else
137  process = 0;
138 
139  if ( rhs.controller != 0 )
140  controller = rhs.controller;
141  else
142  controller = 0;
143 
145 
149 
150  nSteps = rhs.nSteps;
151  }
152 
153  return *this;
154 }
155 
156 
157 
159  )
160 {
161  if ( _process.isDefined( ) == BT_TRUE )
162  {
163  if ( process == 0 )
164  process = &_process;
165  else
166  *process = _process;
167  }
168 
169  if ( _process.isDefined( ) == BT_TRUE )
171 
172  return SUCCESSFUL_RETURN;
173 }
174 
175 
177  )
178 {
179  if ( _controller.isDefined( ) == BT_TRUE )
180  {
181  if ( controller == 0 )
182  controller = &_controller;
183  else
184  *controller = _controller;
185  }
186 
187  if ( getStatus( ) > BS_NOT_INITIALIZED )
189 
190  return SUCCESSFUL_RETURN;
191 }
192 
193 
194 
196 {
197  if ( controller != 0 )
199 
200  if ( process != 0 )
202 
203  return SUCCESSFUL_RETURN;
204 }
205 
206 
208 {
209  VariablesGrid tmp;
210  tmp.read( fileName );
211 
212  if ( tmp.isEmpty( ) == BT_TRUE )
214 
215  return initializeAlgebraicStates( tmp );
216 }
217 
218 
219 
221  const DVector &p_
222  )
223 {
224  int printLevel;
225  get( PRINTLEVEL,printLevel );
226 
227  // 1) initialise all sub-blocks and evaluate process at start time
228  DVector uStart, pStart;
229  VariablesGrid yStart;
230 
231  if ( controller != 0 )
232  {
233  if ( (PrintLevel)printLevel >= HIGH )
234  cout << "--> Initializing controller ...\n";
235 
236  if ( controller->init( startTime,x0_,p_ ) != SUCCESSFUL_RETURN )
238 
239  if ( controller->getU( uStart ) != SUCCESSFUL_RETURN )
241 
242  if ( controller->getP( pStart ) != SUCCESSFUL_RETURN )
244 
245  if ( (PrintLevel)printLevel >= HIGH )
246  cout << "<-- Initializing controller done.\n";
247  }
248  else
250 
251 
252  if ( process != 0 )
253  {
254  if ( (PrintLevel)printLevel >= HIGH )
255  cout << "--> Initializing process ...\n";
256 
257  if ( process->init( startTime,x0_,uStart,pStart ) != SUCCESSFUL_RETURN )
259 
260  if ( process->getY( yStart ) != SUCCESSFUL_RETURN )
262 
263  if ( (PrintLevel)printLevel >= HIGH )
264  cout << "<-- Initializing process done.\n";
265  }
266  else
268 
270 
271 
272  // 2) consistency checks
273  if ( ( process != 0 ) && ( controller != 0 ) )
274  {
275  if ( process->getNY( ) != controller->getNY( ) )
277 
278 // printf( "%d %d\n",process->getNU( ),controller->getNU( ) );
279 
280  if ( process->getNU( ) != controller->getNU( ) )
282 
283  if ( process->getNP( ) > controller->getNP( ) )
285  }
286 
287 
288  // initialise history...
289  if ( getNU( ) > 0 )
290  feedbackControl. add( startTime-10.0*EPS,startTime,uStart );
291 
292  if ( getNP( ) > 0 )
294  if ( getNY( ) > 0 )
295  processOutput. add( startTime-10.0*EPS,startTime,yStart.getVector(0) );
296 
297  // ... and update block status
298  setStatus( BS_READY );
299  return SUCCESSFUL_RETURN;
300 }
301 
302 
303 
305 {
306  /* Consistency check. */
307  if ( getStatus( ) != BS_READY )
309 
310 
311  ++nSteps;
312  printf( "\n*** SIMULATION LOOP NO. %d (starting at time %.3f) ***\n",nSteps,simulationClock.getTime( ) );
313 
314  /* Perform one single simulation loop */
315  DVector u, p;
316  DVector uPrevious, pPrevious;
317 
318  if ( getNU( ) > 0 )
320 
321  if ( getNP( ) > 0 )
323 
325  DVector yPrevious;
326 
327  if ( getNY( ) > 0 )
329 
330 
331  // step controller
332 // yPrevious.print("controller input y");
333 
334  int printLevel;
335  get( PRINTLEVEL,printLevel );
336 
337  if ( (PrintLevel)printLevel >= HIGH )
338  cout << "--> Calling controller ...\n";
339 
340 // yPrevious.print( "yPrevious" );
341 
342  if ( controller->step( simulationClock.getTime( ),yPrevious ) != SUCCESSFUL_RETURN )
344 /* if ( controller->feedbackStep( simulationClock.getTime( ),yPrevious ) != SUCCESSFUL_RETURN )
345  return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
346 
347  double nextTime = simulationClock.getTime( );
348  nextTime += controller->getSamplingTimeControlLaw( );
349 
350  if ( controller->preparationStep( nextTime ) != SUCCESSFUL_RETURN )
351  return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );*/
352 
353  if ( (PrintLevel)printLevel >= HIGH )
354  cout << "<-- Calling controller done.\n";
355 
357  double nextSamplingInstant = controller->getNextSamplingInstant( simulationClock.getTime( ) );
358  nextSamplingInstant = round( nextSamplingInstant * 1.0e6 ) / 1.0e6;
359 
360  // obtain new controls and parameters
361  if ( controller->getU( u ) != SUCCESSFUL_RETURN )
363 
364  #ifdef SIM_DEBUG
365  uPrevious.print("previous u(0)");
366  u.print("u(0) from controller");
367  #endif
368 
369  if ( controller->getP( p ) != SUCCESSFUL_RETURN )
371 
373  {
374  simulationClock.init( nextSamplingInstant );
375  return SUCCESSFUL_RETURN;
376  }
377 
378  if ( (PrintLevel)printLevel >= HIGH )
379  cout << "--> Simulating process ...\n";
380 
381  if ( fabs( compDelay ) < 100.0*EPS )
382  {
383  // step process without computational delay
384 // if ( process->step( simulationClock.getTime( ),nextSamplingInstant,uPrevious,pPrevious ) != SUCCESSFUL_RETURN )
385 // return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
386  if ( process->step( simulationClock.getTime( ),nextSamplingInstant,u,p ) != SUCCESSFUL_RETURN )
388 
389  // Obtain current process output
390  if ( process->getY( y ) != SUCCESSFUL_RETURN )
392 
393 // y.print("process output y");
394 
395  // update history
396 // if ( getNU( ) > 0 )
397 // feedbackControl. add( simulationClock.getTime( ),nextSamplingInstant,uPrevious );
398 // if ( getNP( ) > 0 )
399 // feedbackParameter.add( simulationClock.getTime( ),nextSamplingInstant,pPrevious );
400  if ( getNU( ) > 0 )
401  feedbackControl. add( simulationClock.getTime( ),nextSamplingInstant,u );
402  if ( getNP( ) > 0 )
403  feedbackParameter.add( simulationClock.getTime( ),nextSamplingInstant,p );
404  if ( getNY( ) > 0 )
405  processOutput. add( y,IM_LINEAR );
406  }
407  else
408  {
409  // step process WITH computational delay
410  if ( simulationClock.getTime( )+compDelay > nextSamplingInstant )
412 
413  Grid delayGrid( 3 );
414  delayGrid.setTime( simulationClock.getTime( ) );
415  delayGrid.setTime( simulationClock.getTime( )+compDelay );
416  delayGrid.setTime( nextSamplingInstant );
417 
418  VariablesGrid uDelayed( u.getDim( ),delayGrid,VT_CONTROL );
419  uDelayed.setVector( 0,uPrevious );
420  uDelayed.setVector( 1,u );
421  uDelayed.setVector( 2,u );
422 
423  VariablesGrid pDelayed( p.getDim( ),delayGrid,VT_PARAMETER );
424  pDelayed.setVector( 0,pPrevious );
425  pDelayed.setVector( 1,p );
426  pDelayed.setVector( 2,p );
427 
428  if ( process->step( uDelayed,pDelayed ) != SUCCESSFUL_RETURN )
430 
431  // Obtain current process output
432  if ( process->getY( y ) != SUCCESSFUL_RETURN )
434 
435  // update history
436  if ( getNU( ) > 0 )
437  feedbackControl. add( uDelayed,IM_CONSTANT );
438 
439  if ( getNP( ) > 0 )
440  feedbackParameter.add( pDelayed,IM_CONSTANT );
441 
442  if ( getNY( ) > 0 )
443  processOutput. add( y,IM_LINEAR );
444  }
445  if ( (PrintLevel)printLevel >= HIGH )
446  cout << "<-- Simulating process done.\n";
447 
448  // update simulation clock
449  simulationClock.init( nextSamplingInstant );
450 
451  return SUCCESSFUL_RETURN;
452 }
453 
454 
456  )
457 {
458  if ( ( nextTime < startTime ) || ( nextTime > endTime ) )
460 
461  while ( nextTime >= simulationClock.getTime( ) )
462  {
463  returnValue returnvalue = step( );
464 
465  if ( returnvalue != SUCCESSFUL_RETURN )
466  return returnvalue;
467  }
468 
469 // MatrixVariablesGrid runtimes;
470 // controller->getAll( LOG_TIME_CONTROL_LAW,runtimes );
471 // runtimes.print( "runtime",PS_MATLAB );
472 
473  return SUCCESSFUL_RETURN;
474 }
475 
476 
478 {
479  return step( endTime );
480 }
481 
482 
483 
484 // PROTECTED FUCNTIONS:
485 // --------------------
486 
488 {
493 
494  return SUCCESSFUL_RETURN;
495 }
496 
498 {
499 // LogRecord tmp( LOG_AT_EACH_ITERATION,stdout,PS_DEFAULT );
500 //
501 // tmp.addItem( LOG_FEEDBACK_CONTROL );
502 //
503 // addLogRecord( tmp );
504 
505  return SUCCESSFUL_RETURN;
506 }
507 
508 
510  ) const
511 {
512  int simulateComputationalDelay;
513  get( SIMULATE_COMPUTATIONAL_DELAY,simulateComputationalDelay );
514 
515  if ( (BooleanType)simulateComputationalDelay == BT_TRUE )
516  {
518  return 0.0;
519 
520  double computationalDelayFactor, computationalDelayOffset;
521  get( COMPUTATIONAL_DELAY_FACTOR,computationalDelayFactor );
522  get( COMPUTATIONAL_DELAY_OFFSET,computationalDelayOffset );
523 
524  return acadoMax( 0.0, controllerRuntime*computationalDelayFactor + computationalDelayOffset );
525  }
526  else
527  return 0.0;
528 }
529 
530 
531 
532 
534 
535 // end of file.
SimulationEnvironment & operator=(const SimulationEnvironment &rhs)
returnValue setTime(double _time)
Definition: grid.cpp:307
const int defaultPrintlevel
Calculates the control inputs of the Process based on the Process outputs.
Definition: controller.hpp:71
const int defaultSimulateComputationalDelay
returnValue setController(Controller &_controller)
returnValue getY(VariablesGrid &_y) const
int acadoMax(const int x, const int y)
Definition: acado_utils.cpp:64
virtual returnValue setupLogging()
BooleanType acadoIsEqual(double x, double y, double TOL)
Definition: acado_utils.cpp:88
returnValue evaluate(double t, double *result) const
Definition: curve.cpp:314
returnValue setStatus(BlockStatus _status)
BEGIN_NAMESPACE_ACADO const double EPS
BooleanType isDefined() const
returnValue initializeAlgebraicStates(const VariablesGrid &_xa_init)
Definition: controller.cpp:246
returnValue add(double tStart, double tEnd, const DVector constant)
Definition: curve.cpp:143
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Allows to pass back messages to the calling function.
const double defaultComputationalDelayFactor
uint getNU(uint stageIdx=0) const
virtual returnValue init(double _initialTime)
Definition: clock.cpp:76
uint getNP() const
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
BooleanType isEmpty() const
uint getNY() const
virtual returnValue step(const VariablesGrid &_u, const VariablesGrid &_p=emptyVariablesGrid)
Definition: process.cpp:508
#define CLOSE_NAMESPACE_ACADO
virtual returnValue setupOptions()
returnValue init(const DVector &x0_, const DVector &p_=emptyConstVector)
BlockStatus getStatus() const
virtual returnValue step(double currentTime, const DVector &_y, const VariablesGrid &_yRef=emptyConstVariablesGrid)
Definition: controller.cpp:338
returnValue initializeAlgebraicStates(const DVector &_xaStart)
Definition: process.cpp:345
returnValue initializeAlgebraicStates(const VariablesGrid &_xa_init)
const double defaultComputationalDelayOffset
returnValue setVector(uint pointIdx, const DVector &_values)
virtual returnValue init(double startTime=0.0, const DVector &_x0=emptyConstVector, const DVector &_p=emptyConstVector, const VariablesGrid &_yRef=emptyConstVariablesGrid)
Definition: controller.cpp:270
virtual returnValue print(std::ostream &stream=std::cout, const std::string &name=DEFAULT_LABEL, const std::string &startString=DEFAULT_START_STRING, const std::string &endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const std::string &colSeparator=DEFAULT_COL_SEPARATOR, const std::string &rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: vector.cpp:97
uint getNP() const
uint getNP(uint stageIdx=0) const
double determineComputationalDelay(double controllerRuntime) const
void rhs(const real_t *x, real_t *f)
PrintLevel
double getNextSamplingInstant(double currentTime)
Definition: controller.cpp:518
returnValue read(std::istream &stream)
uint getNY() const
uint getNU() const
#define BT_TRUE
Definition: acado_types.hpp:47
Base class for building-blocks of the SimulationEnvironment.
Allows to run closed-loop simulations of dynamic systems.
DVector getVector(uint pointIdx) const
#define ACADOWARNING(retval)
Simulates the process to be controlled based on a dynamic model.
Definition: process.hpp:71
#define BEGIN_NAMESPACE_ACADO
uint getNY(uint stageIdx=0) const
double getPreviousRealRuntime()
returnValue setProcess(Process &_process)
SimulationBlock & operator=(const SimulationBlock &rhs)
returnValue getU(DVector &_u) const
returnValue addOption(OptionsName name, int value)
Definition: options.cpp:301
uint getNU() const
returnValue getP(DVector &_p) const
#define ACADOERROR(retval)
virtual returnValue getTime(double &_elapsedTime)
Definition: clock.cpp:92
virtual returnValue init(double _startTime=0.0, const DVector &_xStart=emptyConstVector, const DVector &_uStart=emptyConstVector, const DVector &_pStart=emptyConstVector)
Definition: process.cpp:353


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