simulation_environment.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00035 #include <acado/simulation_environment/simulation_environment.hpp>
00036 
00037 #ifdef WIN32
00038 #define round( value ) floor( value + 0.5 )
00039 #endif
00040 
00041 
00042 // #define SIM_DEBUG
00043 
00044 using namespace std;
00045 
00046 BEGIN_NAMESPACE_ACADO
00047 
00048 
00049 SimulationEnvironment::SimulationEnvironment( ) : SimulationBlock( BN_SIMULATION_ENVIRONMENT )
00050 {
00051         setupOptions( );
00052         setupLogging( );
00053 
00054         startTime = 0.0;
00055         endTime   = 0.0;
00056 
00057         process    = 0;
00058         controller = 0;
00059 
00060         nSteps = 0;
00061         
00062         setStatus( BS_NOT_INITIALIZED );
00063 }
00064 
00065 
00066 SimulationEnvironment::SimulationEnvironment(   double _startTime,
00067                                                                                                 double _endTime,
00068                                                                                                 Process& _process,
00069                                                                                                 Controller& _controller
00070                                                                                                 ) : SimulationBlock( BN_SIMULATION_ENVIRONMENT )
00071 {
00072         setupOptions( );
00073         setupLogging( );
00074         
00075         startTime = _startTime;
00076         endTime   = _endTime;
00077 
00078         if ( _process.isDefined( ) == BT_TRUE )
00079         {
00080                 process = &_process;
00081                 setStatus( BS_NOT_INITIALIZED );
00082         }
00083         else
00084                 process = 0;
00085 
00086         if ( _controller.isDefined( ) == BT_TRUE )
00087                 controller = &_controller;
00088         else
00089                 controller = 0;
00090 
00091         nSteps = 0;
00092 }
00093 
00094 
00095 SimulationEnvironment::SimulationEnvironment( const SimulationEnvironment &rhs ) : SimulationBlock( rhs )
00096 {
00097         startTime = rhs.startTime;
00098         endTime   = rhs.endTime;
00099 
00100         if ( rhs.process != 0 )
00101                 process = rhs.process;
00102         else
00103                 process = 0;
00104 
00105         if ( rhs.controller != 0 )
00106                 controller = rhs.controller;
00107         else
00108                 controller = 0;
00109 
00110         simulationClock = rhs.simulationClock;
00111 
00112         processOutput     = rhs.processOutput;
00113         feedbackControl   = rhs.feedbackControl;
00114         feedbackParameter = rhs.feedbackParameter;
00115 
00116         nSteps = rhs.nSteps;
00117 }
00118 
00119 
00120 SimulationEnvironment::~SimulationEnvironment( )
00121 {
00122 }
00123 
00124 
00125 SimulationEnvironment& SimulationEnvironment::operator=( const SimulationEnvironment &rhs )
00126 {
00127         if( this != &rhs )
00128         {
00129                 SimulationBlock::operator=( rhs );
00130                 
00131                 startTime = rhs.startTime;
00132                 endTime   = rhs.endTime;
00133         
00134                 if ( rhs.process != 0 )
00135                         process = rhs.process;
00136                 else
00137                         process = 0;
00138         
00139                 if ( rhs.controller != 0 )
00140                         controller = rhs.controller;
00141                 else
00142                         controller = 0;
00143 
00144                 simulationClock = rhs.simulationClock;
00145         
00146                 processOutput     = rhs.processOutput;
00147                 feedbackControl   = rhs.feedbackControl;
00148                 feedbackParameter = rhs.feedbackParameter;
00149 
00150                 nSteps = rhs.nSteps;
00151     }
00152 
00153     return *this;
00154 }
00155 
00156 
00157 
00158 returnValue SimulationEnvironment::setProcess(  Process& _process
00159                                                                                                 )
00160 {
00161         if ( _process.isDefined( ) == BT_TRUE )
00162         {
00163                 if ( process == 0 )
00164                         process = &_process;
00165                 else
00166                         *process = _process;
00167         }
00168 
00169         if ( _process.isDefined( ) == BT_TRUE )
00170                 setStatus( BS_NOT_INITIALIZED );
00171 
00172         return SUCCESSFUL_RETURN;
00173 }
00174 
00175 
00176 returnValue SimulationEnvironment::setController(       Controller& _controller
00177                                                                                                         )
00178 {
00179         if ( _controller.isDefined( ) == BT_TRUE )
00180         {
00181                 if ( controller == 0 )
00182                         controller = &_controller;
00183                 else
00184                         *controller = _controller;
00185         }
00186 
00187         if ( getStatus( ) > BS_NOT_INITIALIZED )
00188                 setStatus( BS_NOT_INITIALIZED );
00189 
00190         return SUCCESSFUL_RETURN;
00191 }
00192 
00193 
00194 
00195 returnValue SimulationEnvironment::initializeAlgebraicStates( const VariablesGrid& _xa_init )
00196 {
00197         if ( controller != 0 )
00198                 controller->initializeAlgebraicStates( _xa_init );
00199 
00200         if ( process != 0 )
00201                 process->initializeAlgebraicStates( _xa_init.getVector(0) );
00202 
00203         return SUCCESSFUL_RETURN;
00204 }
00205 
00206 
00207 returnValue SimulationEnvironment::initializeAlgebraicStates( const char* fileName )
00208 {
00209         VariablesGrid tmp;
00210         tmp.read( fileName );
00211         
00212         if ( tmp.isEmpty( ) == BT_TRUE )
00213                 return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00214         
00215         return initializeAlgebraicStates( tmp );
00216 }
00217 
00218 
00219 
00220 returnValue SimulationEnvironment::init(        const DVector &x0_,
00221                                                                                         const DVector &p_
00222                                                                                         )
00223 {
00224         int printLevel;
00225         get( PRINTLEVEL,printLevel );
00226 
00227         // 1) initialise all sub-blocks and evaluate process at start time
00228         DVector uStart, pStart;
00229         VariablesGrid yStart;
00230         
00231         if ( controller != 0 )
00232         {
00233                 if ( (PrintLevel)printLevel >= HIGH ) 
00234                         cout << "--> Initializing controller ...\n";
00235 
00236                 if ( controller->init( startTime,x0_,p_ ) != SUCCESSFUL_RETURN )
00237                         return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
00238 
00239                 if ( controller->getU( uStart ) != SUCCESSFUL_RETURN )
00240                         return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
00241 
00242                 if ( controller->getP( pStart ) != SUCCESSFUL_RETURN )
00243                         return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
00244                 
00245                 if ( (PrintLevel)printLevel >= HIGH ) 
00246                         cout << "<-- Initializing controller done.\n";
00247         }
00248         else
00249                 return ACADOERROR( RET_NO_CONTROLLER_SPECIFIED );
00250         
00251 
00252         if ( process != 0 )
00253         {
00254                 if ( (PrintLevel)printLevel >= HIGH ) 
00255                         cout << "--> Initializing process ...\n";
00256 
00257                 if ( process->init( startTime,x0_,uStart,pStart ) != SUCCESSFUL_RETURN )
00258                         return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
00259 
00260                 if ( process->getY( yStart ) != SUCCESSFUL_RETURN )
00261                         return ACADOERROR( RET_ENVIRONMENT_INIT_FAILED );
00262                 
00263                 if ( (PrintLevel)printLevel >= HIGH ) 
00264                         cout << "<-- Initializing process done.\n";
00265         }
00266         else
00267                 return ACADOERROR( RET_NO_PROCESS_SPECIFIED );
00268 
00269         simulationClock.init( startTime );
00270 
00271 
00272         // 2) consistency checks
00273         if ( ( process != 0 ) && ( controller != 0 ) )
00274         {
00275                 if ( process->getNY( ) != controller->getNY( ) )
00276                         return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
00277 
00278 //              printf( "%d  %d\n",process->getNU( ),controller->getNU( ) );
00279                         
00280                 if ( process->getNU( ) != controller->getNU( ) )
00281                         return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
00282 
00283                 if ( process->getNP( ) > controller->getNP( ) )
00284                         return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
00285         }
00286 
00287 
00288         // initialise history...
00289         if ( getNU( ) > 0 )
00290                 feedbackControl.  add( startTime-10.0*EPS,startTime,uStart );
00291 
00292         if ( getNP( ) > 0 )
00293                 feedbackParameter.add( startTime-10.0*EPS,startTime,pStart );
00294         if ( getNY( ) > 0 )
00295                 processOutput.    add( startTime-10.0*EPS,startTime,yStart.getVector(0) );
00296 
00297         // ... and update block status
00298         setStatus( BS_READY );
00299         return SUCCESSFUL_RETURN;
00300 }
00301 
00302 
00303 
00304 returnValue SimulationEnvironment::step( )
00305 {
00306         /* Consistency check. */
00307         if ( getStatus( ) != BS_READY )
00308                 return ACADOERROR( RET_BLOCK_NOT_READY );
00309 
00310 
00311         ++nSteps;
00312         printf( "\n*** SIMULATION LOOP NO. %d (starting at time %.3f) ***\n",nSteps,simulationClock.getTime( ) );
00313 
00314         /* Perform one single simulation loop */
00315         DVector u, p;
00316         DVector uPrevious, pPrevious;
00317 
00318         if ( getNU( ) > 0 )
00319                 feedbackControl.evaluate( simulationClock.getTime( ),uPrevious );
00320 
00321         if ( getNP( ) > 0 )
00322                 feedbackParameter.evaluate( simulationClock.getTime( ),pPrevious );
00323 
00324         VariablesGrid y;
00325         DVector yPrevious;
00326 
00327         if ( getNY( ) > 0 )
00328                 processOutput.evaluate( simulationClock.getTime( ),yPrevious );
00329 
00330 
00331         // step controller
00332 //      yPrevious.print("controller input y");
00333 
00334         int printLevel;
00335         get( PRINTLEVEL,printLevel );
00336 
00337         if ( (PrintLevel)printLevel >= HIGH ) 
00338                 cout << "--> Calling controller ...\n";
00339 
00340 //      yPrevious.print( "yPrevious" );
00341 
00342         if ( controller->step( simulationClock.getTime( ),yPrevious ) != SUCCESSFUL_RETURN )
00343                 return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00344 /*      if ( controller->feedbackStep( simulationClock.getTime( ),yPrevious ) != SUCCESSFUL_RETURN )
00345                 return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00346 
00347         double nextTime = simulationClock.getTime( );
00348         nextTime += controller->getSamplingTimeControlLaw( );
00349 
00350         if ( controller->preparationStep( nextTime ) != SUCCESSFUL_RETURN )
00351                 return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );*/
00352         
00353         if ( (PrintLevel)printLevel >= HIGH ) 
00354                 cout << "<-- Calling controller done.\n";
00355 
00356         double compDelay = determineComputationalDelay( controller->getPreviousRealRuntime( ) );
00357         double nextSamplingInstant = controller->getNextSamplingInstant( simulationClock.getTime( ) );
00358         nextSamplingInstant = round( nextSamplingInstant * 1.0e6 ) / 1.0e6;
00359 
00360         // obtain new controls and parameters
00361         if ( controller->getU( u ) != SUCCESSFUL_RETURN )
00362                 return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00363 
00364         #ifdef SIM_DEBUG
00365         uPrevious.print("previous u(0)");
00366         u.print("u(0) from controller");
00367         #endif
00368 
00369         if ( controller->getP( p ) != SUCCESSFUL_RETURN )
00370                 return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00371 
00372         if ( acadoIsEqual( simulationClock.getTime( ),endTime ) == BT_TRUE )
00373         {
00374                 simulationClock.init( nextSamplingInstant );
00375                 return SUCCESSFUL_RETURN;
00376         }
00377         
00378         if ( (PrintLevel)printLevel >= HIGH ) 
00379                 cout << "--> Simulating process ...\n";
00380 
00381         if ( fabs( compDelay ) < 100.0*EPS )
00382         {
00383                 // step process without computational delay
00384 //              if ( process->step( simulationClock.getTime( ),nextSamplingInstant,uPrevious,pPrevious ) != SUCCESSFUL_RETURN )
00385 //                      return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00386                 if ( process->step( simulationClock.getTime( ),nextSamplingInstant,u,p ) != SUCCESSFUL_RETURN )
00387                         return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00388 
00389                 // Obtain current process output
00390                 if ( process->getY( y ) != SUCCESSFUL_RETURN )
00391                         return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00392 
00393 //              y.print("process output y");
00394 
00395                 // update history
00396 //              if ( getNU( ) > 0 )
00397 //                      feedbackControl.  add( simulationClock.getTime( ),nextSamplingInstant,uPrevious );
00398 //              if ( getNP( ) > 0 )
00399 //                      feedbackParameter.add( simulationClock.getTime( ),nextSamplingInstant,pPrevious );
00400                 if ( getNU( ) > 0 )
00401                         feedbackControl.  add( simulationClock.getTime( ),nextSamplingInstant,u );
00402                 if ( getNP( ) > 0 )
00403                         feedbackParameter.add( simulationClock.getTime( ),nextSamplingInstant,p );
00404                 if ( getNY( ) > 0 )
00405                         processOutput.    add( y,IM_LINEAR );
00406         }
00407         else
00408         {
00409                 // step process WITH computational delay
00410                 if ( simulationClock.getTime( )+compDelay > nextSamplingInstant )
00411                         return ACADOERROR( RET_COMPUTATIONAL_DELAY_TOO_BIG );
00412 
00413                 Grid delayGrid( 3 );
00414                 delayGrid.setTime( simulationClock.getTime( ) );
00415                 delayGrid.setTime( simulationClock.getTime( )+compDelay );
00416                 delayGrid.setTime( nextSamplingInstant );
00417 
00418                 VariablesGrid uDelayed( u.getDim( ),delayGrid,VT_CONTROL );
00419                 uDelayed.setVector( 0,uPrevious );
00420                 uDelayed.setVector( 1,u );
00421                 uDelayed.setVector( 2,u );
00422 
00423                 VariablesGrid pDelayed( p.getDim( ),delayGrid,VT_PARAMETER );
00424                 pDelayed.setVector( 0,pPrevious );
00425                 pDelayed.setVector( 1,p );
00426                 pDelayed.setVector( 2,p );
00427 
00428                 if ( process->step( uDelayed,pDelayed ) != SUCCESSFUL_RETURN )
00429                         return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00430 
00431                 // Obtain current process output
00432                 if ( process->getY( y ) != SUCCESSFUL_RETURN )
00433                         return ACADOERROR( RET_ENVIRONMENT_STEP_FAILED );
00434 
00435                 // update history
00436                 if ( getNU( ) > 0 )
00437                         feedbackControl.  add( uDelayed,IM_CONSTANT );
00438 
00439                 if ( getNP( ) > 0 )
00440                         feedbackParameter.add( pDelayed,IM_CONSTANT );
00441 
00442                 if ( getNY( ) > 0 )
00443                         processOutput.    add( y,IM_LINEAR );
00444         }
00445         if ( (PrintLevel)printLevel >= HIGH ) 
00446                 cout <<  "<-- Simulating process done.\n";
00447 
00448         // update simulation clock
00449         simulationClock.init( nextSamplingInstant );
00450 
00451         return SUCCESSFUL_RETURN;
00452 }
00453 
00454 
00455 returnValue SimulationEnvironment::step(        double nextTime
00456                                                                                         )
00457 {
00458         if ( ( nextTime < startTime ) || ( nextTime > endTime ) )
00459                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00460 
00461         while ( nextTime >= simulationClock.getTime( ) )
00462         {
00463                 returnValue returnvalue = step( );
00464 
00465                 if ( returnvalue != SUCCESSFUL_RETURN )
00466                         return returnvalue;
00467         }
00468 
00469 //      MatrixVariablesGrid runtimes;
00470 //      controller->getAll( LOG_TIME_CONTROL_LAW,runtimes );
00471 //      runtimes.print( "runtime",PS_MATLAB );
00472 
00473         return SUCCESSFUL_RETURN;
00474 }
00475 
00476 
00477 returnValue SimulationEnvironment::run( )
00478 {
00479         return step( endTime );
00480 }
00481 
00482 
00483 
00484 // PROTECTED FUCNTIONS:
00485 // --------------------
00486 
00487 returnValue SimulationEnvironment::setupOptions( )
00488 {
00489         addOption( SIMULATE_COMPUTATIONAL_DELAY , defaultSimulateComputationalDelay );
00490         addOption( COMPUTATIONAL_DELAY_FACTOR   , defaultComputationalDelayFactor   );
00491         addOption( COMPUTATIONAL_DELAY_OFFSET   , defaultComputationalDelayOffset   );
00492         addOption( PRINTLEVEL                   , defaultPrintlevel                 );
00493 
00494         return SUCCESSFUL_RETURN;
00495 }
00496 
00497 returnValue SimulationEnvironment::setupLogging( )
00498 {
00499 //      LogRecord tmp( LOG_AT_EACH_ITERATION,stdout,PS_DEFAULT );
00500 // 
00501 //      tmp.addItem( LOG_FEEDBACK_CONTROL );
00502 // 
00503 //      addLogRecord( tmp );
00504 
00505         return SUCCESSFUL_RETURN;
00506 }
00507 
00508 
00509 double SimulationEnvironment::determineComputationalDelay(      double controllerRuntime
00510                                                                                                                         ) const
00511 {
00512         int simulateComputationalDelay;
00513         get( SIMULATE_COMPUTATIONAL_DELAY,simulateComputationalDelay );
00514 
00515         if ( (BooleanType)simulateComputationalDelay == BT_TRUE )
00516         {
00517                 ACADOWARNING( RET_COMPUTATIONAL_DELAY_NOT_SUPPORTED );
00518                 return 0.0;
00519 
00520                 double computationalDelayFactor, computationalDelayOffset;
00521                 get( COMPUTATIONAL_DELAY_FACTOR,computationalDelayFactor );
00522                 get( COMPUTATIONAL_DELAY_OFFSET,computationalDelayOffset );
00523 
00524                 return acadoMax( 0.0, controllerRuntime*computationalDelayFactor + computationalDelayOffset );
00525         }
00526         else
00527                 return 0.0;
00528 }
00529 
00530 
00531 
00532 
00533 CLOSE_NAMESPACE_ACADO
00534 
00535 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:39:06