controller.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 
00033 #include <acado/controller/controller.hpp>
00034 
00035 
00036 // #define SIM_DEBUG
00037 
00038 
00039 BEGIN_NAMESPACE_ACADO
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 Controller::Controller( ) : SimulationBlock( BN_CONTROLLER )
00046 {
00047         setupOptions( );
00048         setupLogging( );
00049 
00050         controlLaw = 0;
00051         estimator   = 0;
00052         referenceTrajectory = 0;
00053         
00054         isEnabled = BT_TRUE;
00055         
00056         setStatus( BS_NOT_INITIALIZED );
00057 }
00058 
00059 
00060 Controller::Controller( ControlLaw& _controlLaw,
00061                                                 Estimator& _estimator,
00062                                                 ReferenceTrajectory& _referenceTrajectory
00063                                                 ) : SimulationBlock( BN_CONTROLLER )
00064 {
00065         setupOptions( );
00066         setupLogging( );
00067 
00068         if ( _controlLaw.isDefined( ) == BT_TRUE )
00069         {
00070                 controlLaw = &_controlLaw;
00071                 setStatus( BS_NOT_INITIALIZED );
00072         }
00073         else
00074                 controlLaw = 0;
00075 
00076         if ( _estimator.isDefined( ) == BT_TRUE )
00077                 estimator = &_estimator;
00078         else
00079                 estimator = 0;
00080 
00081         if ( _referenceTrajectory.isDefined( ) == BT_TRUE )
00082                 referenceTrajectory = &_referenceTrajectory;
00083         else
00084                 referenceTrajectory = 0;
00085         
00086         isEnabled = BT_TRUE;
00087         
00088         setStatus( BS_NOT_INITIALIZED );
00089 }
00090 
00091 
00092 Controller::Controller( ControlLaw& _controlLaw,
00093                                                 ReferenceTrajectory& _referenceTrajectory
00094                                                 ) : SimulationBlock( BN_CONTROLLER )
00095 {
00096         setupOptions( );
00097         setupLogging( );
00098 
00099         if ( _controlLaw.isDefined( ) == BT_TRUE )
00100         {
00101                 controlLaw = &_controlLaw;
00102                 setStatus( BS_NOT_INITIALIZED );
00103         }
00104         else
00105                 controlLaw = 0;
00106 
00107         estimator = 0;
00108 
00109         if ( _referenceTrajectory.isDefined( ) == BT_TRUE )
00110                 referenceTrajectory = &_referenceTrajectory;
00111         else
00112                 referenceTrajectory = 0;
00113         
00114         isEnabled = BT_TRUE;
00115         
00116         setStatus( BS_NOT_INITIALIZED );
00117 }
00118 
00119 
00120 Controller::Controller( const Controller& rhs ) : SimulationBlock( rhs )
00121 {
00122         if ( rhs.controlLaw != 0 )
00123                 controlLaw = rhs.controlLaw;
00124         else
00125                 controlLaw = 0;
00126 
00127         if ( rhs.estimator != 0 )
00128                 estimator = rhs.estimator;
00129         else
00130                 estimator = 0;
00131 
00132         if ( rhs.referenceTrajectory != 0 )
00133                 referenceTrajectory = rhs.referenceTrajectory;
00134         else
00135                 referenceTrajectory = 0;
00136         
00137         isEnabled = rhs.isEnabled;
00138 }
00139 
00140 
00141 Controller::~Controller( )
00142 {
00143 //      if ( controlLaw != 0 )
00144 //              delete controlLaw;
00145 // 
00146 //      if ( estimator != 0 )
00147 //              delete estimator;
00148 // 
00149 //      if ( referenceTrajectory != 0 )
00150 //              delete referenceTrajectory;
00151 }
00152 
00153 
00154 Controller& Controller::operator=( const Controller& rhs )
00155 {
00156         if ( this != &rhs )
00157         {
00158 //              if ( controlLaw != 0 )
00159 //                      delete controlLaw;
00160 // 
00161 //              if ( estimator != 0 )
00162 //                      delete estimator;
00163 // 
00164 //              if ( referenceTrajectory != 0 )
00165 //                      delete referenceTrajectory;
00166 
00167                 SimulationBlock::operator=( rhs );
00168 
00169                 if ( rhs.controlLaw != 0 )
00170                         controlLaw = rhs.controlLaw;
00171                 else
00172                         controlLaw = 0;
00173 
00174                 if ( rhs.estimator != 0 )
00175                         estimator = rhs.estimator;
00176                 else
00177                         estimator = 0;
00178 
00179                 if ( rhs.referenceTrajectory != 0 )
00180                         referenceTrajectory = rhs.referenceTrajectory;
00181                 else
00182                         referenceTrajectory = 0;
00183                 
00184                 isEnabled = rhs.isEnabled;
00185         }
00186 
00187     return *this;
00188 }
00189 
00190 
00191 
00192 returnValue Controller::setControlLaw(  ControlLaw& _controlLaw
00193                                                                                 )
00194 {
00195         if ( _controlLaw.isDefined( ) == BT_TRUE )
00196         {
00197                 if ( controlLaw == 0 )
00198                         controlLaw = &_controlLaw;
00199                 else
00200                         *controlLaw = _controlLaw;
00201 
00202                 setStatus( BS_NOT_INITIALIZED );
00203         }
00204 
00205         return SUCCESSFUL_RETURN;
00206 }
00207 
00208 
00209 returnValue Controller::setEstimator(   Estimator& _estimator
00210                                                                                 )
00211 {
00212         if ( _estimator.isDefined( ) == BT_TRUE )
00213         {
00214                 if ( estimator == 0 )
00215                         estimator = &_estimator;
00216                 else
00217                         *estimator = _estimator;
00218 
00219                 if ( getStatus( ) > BS_NOT_INITIALIZED )
00220                         setStatus( BS_NOT_INITIALIZED );
00221         }
00222         
00223         return SUCCESSFUL_RETURN;
00224 }
00225 
00226 
00227 returnValue Controller::setReferenceTrajectory( ReferenceTrajectory& _referenceTrajectory
00228                                                                                                 )
00229 {
00230         if ( _referenceTrajectory.isDefined( ) == BT_TRUE )
00231         {
00232                 if ( referenceTrajectory == 0 )
00233                         referenceTrajectory = &_referenceTrajectory;
00234                 else
00235                         *referenceTrajectory = _referenceTrajectory;
00236 
00237                 if ( getStatus( ) > BS_NOT_INITIALIZED )
00238                         setStatus( BS_NOT_INITIALIZED );
00239         }
00240 
00241         return SUCCESSFUL_RETURN;
00242 }
00243 
00244 
00245 
00246 returnValue Controller::initializeAlgebraicStates(      const VariablesGrid& _xa_init
00247                                                                                                         )
00248 {
00249         if ( controlLaw != 0 )
00250                 controlLaw->initializeAlgebraicStates( _xa_init.getVector(0) );
00251 
00252         return SUCCESSFUL_RETURN;
00253 }
00254 
00255 
00256 returnValue Controller::initializeAlgebraicStates(      const char* fileName
00257                                                                                                         )
00258 {
00259         VariablesGrid tmp;
00260         tmp.read( fileName );
00261 
00262         if ( tmp.isEmpty( ) == BT_TRUE )
00263                 return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00264 
00265         return initializeAlgebraicStates( tmp );
00266 }
00267 
00268 
00269 
00270 returnValue Controller::init(   double startTime,
00271                                                                 const DVector& _x0,
00272                                                                 const DVector& _p,
00273                                                                 const VariablesGrid& _yRef
00274                                                                 )
00275 {
00276         if ( controlLaw == 0 )
00277                 return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
00278 
00279         /* 1) Initialize all sub-blocks. */
00280         /* a) Estimator */
00281         DVector xEst( _x0 );
00282         DVector pEst( _p );
00283         DVector xaEst, uEst, wEst;
00284 
00285         if ( estimator != 0 )
00286         {
00287                 if ( estimator->init( startTime,_x0,_p ) != SUCCESSFUL_RETURN )
00288                         return ACADOERROR( RET_CONTROLLER_INIT_FAILED );
00289 
00290                 if ( estimator->getOutputs( xEst,xaEst,uEst,pEst,wEst ) != SUCCESSFUL_RETURN )
00291                         return ACADOERROR( RET_CONTROLLER_INIT_FAILED );
00292         }
00293 
00294         /* b) Reference trajectory */
00295         if ( ( referenceTrajectory != 0 ) && ( _yRef.isEmpty( ) == BT_TRUE ) )
00296                 if ( referenceTrajectory->init( startTime,xEst,xaEst,uEst,pEst,wEst ) != SUCCESSFUL_RETURN )
00297                         return ACADOERROR( RET_CONTROLLER_INIT_FAILED );
00298                 
00299         VariablesGrid yRef( _yRef );
00300         getCurrentReference( startTime,yRef );
00301         #ifdef SIM_DEBUG
00302         yRef.print( "yRef init" );
00303         #endif
00304 
00305         /* c) Control law */
00306 //      _x0.print( "x0 init" );
00307         if ( controlLaw->init( startTime,_x0,_p,yRef ) != SUCCESSFUL_RETURN )
00308                 return ACADOERROR( RET_CONTROLLER_INIT_FAILED );
00309 
00310 
00311         /* 2) Consistency checks. */
00312         if ( estimator != 0 )
00313         {
00314                 if ( estimator->getNX( ) != controlLaw->getNX( ) )
00315                         return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
00316 
00317                 if ( estimator->getNXA( ) != controlLaw->getNXA( ) )
00318                         return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
00319 
00320                 if ( estimator->getNU( ) != controlLaw->getNU( ) )
00321                         return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
00322 
00323                 if ( estimator->getNP( ) != controlLaw->getNP( ) )
00324                         return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
00325 
00326                 if ( estimator->getNW( ) != controlLaw->getNW( ) )
00327                         return ACADOERROR( RET_BLOCK_DIMENSION_MISMATCH );
00328         }
00329 
00330         realClock.reset( );
00331         setStatus( BS_READY );
00332 
00333         return SUCCESSFUL_RETURN;
00334 }
00335 
00336 
00337 
00338 returnValue Controller::step(   double currentTime,
00339                                                                 const DVector& _y,
00340                                                                 const VariablesGrid& _yRef
00341                                                                 )
00342 {
00343         /* Consistency check. */
00344         if ( getStatus( ) != BS_READY )
00345                 return ACADOERROR( RET_BLOCK_NOT_READY );
00346 
00347         if ( controlLaw == 0 )
00348                 return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
00349 
00350         /* Do nothing if controller is disabled */
00351         if ( isEnabled == BT_FALSE )
00352         {
00353                 setLast( LOG_TIME_CONTROLLER,0.0 );
00354                 setLast( LOG_TIME_CONTROL_LAW,0.0 );
00355                 setLast( LOG_TIME_ESTIMATOR,0.0 );
00356                 return SUCCESSFUL_RETURN;
00357         }
00358 
00359         if ( feedbackStep( currentTime,_y,_yRef ) != SUCCESSFUL_RETURN )
00360                 return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
00361 
00362         double nextTime = currentTime + controlLaw->getSamplingTime( );
00363                 
00364         if ( preparationStep( nextTime,_yRef ) != SUCCESSFUL_RETURN )
00365                 return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
00366 
00367         return SUCCESSFUL_RETURN;
00368 }
00369 
00370 
00371 returnValue Controller::step(   double currentTime,
00372                                                                 uint dim,
00373                                                                 const double* const _y,
00374                                                                 const VariablesGrid& _yRef
00375                                                                 )
00376 {
00377         /* Convert double array to vector and call standard step routine. */
00378         DVector tmp( dim,_y );
00379         return step( currentTime,tmp,_yRef );
00380 }
00381 
00382 
00383 returnValue Controller::obtainEstimates(        double currentTime,
00384                                                                                         const DVector& _y,
00385                                                                                         DVector& xEst,
00386                                                                                         DVector& pEst
00387                                                                                         )
00388 {
00389         /* 1) Call Estimator */
00390         RealClock clock;
00391         DVector xaEst, uEst, wEst;
00392 
00393         clock.reset();
00394         clock.start();
00395         
00396         if ( estimator != 0 )
00397         {
00398                 if ( estimator->step( currentTime,_y ) != SUCCESSFUL_RETURN )
00399                         return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
00400 
00401                 if ( estimator->getOutputs( xEst,xaEst,uEst,pEst,wEst ) != SUCCESSFUL_RETURN )
00402                         return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
00403         }
00404         else
00405         {
00406                 /* If no estimator is specified, assume full state feedback. */
00407                 xEst = _y;
00408         }
00409 
00410         clock.stop();
00411         setLast(LOG_TIME_ESTIMATOR, clock.getTime());
00412 
00413         // step internal reference trajectory
00414         if ( referenceTrajectory != 0 )
00415         {
00416                 if ( referenceTrajectory->step( currentTime,_y,xEst,xaEst,uEst,pEst,wEst ) != SUCCESSFUL_RETURN )
00417                         return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
00418         }
00419         
00420         return SUCCESSFUL_RETURN;
00421 }
00422 
00423 
00424 returnValue Controller::feedbackStep(   double currentTime,
00425                                                                                 const DVector& _y,
00426                                                                                 const VariablesGrid& _yRef
00427                                                                                 )
00428 {
00429         realClock.reset( );
00430         
00431         if ( controlLaw == 0 )
00432                 return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
00433 
00434         /* Do nothing if controller is disabled */
00435         if ( isEnabled == BT_FALSE )
00436                 return SUCCESSFUL_RETURN;
00437 
00438         // start real runtime measurement
00439         realClock.start( );
00440         
00441         DVector xEst, pEst;
00442 
00443         /* 1) Call Estimator */
00444         if ( obtainEstimates( currentTime,_y,xEst,pEst ) != SUCCESSFUL_RETURN )
00445                 return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
00446 
00447         /* 2) Evaluate reference trajectory */
00448         VariablesGrid yRef( _yRef );
00449         getCurrentReference( currentTime,yRef );
00450         #ifdef SIM_DEBUG
00451         yRef.print( "yRef feedback" );
00452         #endif
00453 
00454         controlLawClock.reset();
00455         controlLawClock.start();
00456         
00457         /* 3) Perform feedback step of control law */
00458         if ( controlLaw->feedbackStep( currentTime,xEst,pEst,yRef ) != SUCCESSFUL_RETURN )
00459                 return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
00460 
00461         controlLawClock.stop();
00462         realClock.stop( );
00463 
00464         #ifdef SIM_DEBUG
00465         DVector uTmp;
00466         getU( uTmp );
00467         uTmp.print("u(0) after feedbackStep");
00468         #endif
00469 
00470         return SUCCESSFUL_RETURN;
00471 }
00472 
00473 
00474 returnValue Controller::preparationStep(        double nextTime,
00475                                                                                         const VariablesGrid& _yRef
00476                                                                                         )
00477 {
00478         if ( controlLaw == 0 )
00479                 return ACADOERROR( RET_NO_CONTROLLAW_SPECIFIED );
00480 
00481         /* Do nothing if controller is disabled */
00482         if ( isEnabled == BT_FALSE )
00483                 return SUCCESSFUL_RETURN;
00484 
00485         realClock.start();
00486         controlLawClock.start();
00487 
00488         /* 1) Evaluate reference trajectory */
00489         VariablesGrid yRef( _yRef );
00490         getCurrentReference( nextTime,yRef );
00491         #ifdef SIM_DEBUG
00492         yRef.print( "yRef preparation" );
00493         #endif
00494 
00495         /* 2) Perform preparation step of control law */
00496         if ( controlLaw->preparationStep( nextTime,yRef ) != SUCCESSFUL_RETURN )
00497                 return ACADOERROR( RET_CONTROLLER_STEP_FAILED );
00498 
00499         controlLawClock.stop();
00500         setLast(LOG_TIME_CONTROL_LAW, controlLawClock.getTime());
00501         
00502         // stop real runtime measurement
00503         realClock.stop();
00504         setLast(LOG_TIME_CONTROLLER, realClock.getTime());
00505 
00506 
00507         #ifdef SIM_DEBUG
00508         DVector uTmp;
00509         getU( uTmp );
00510         uTmp.print("u(0) after preparationStep");
00511         #endif
00512 
00513         return SUCCESSFUL_RETURN;
00514 }
00515 
00516 
00517 
00518 double Controller::getNextSamplingInstant(      double currentTime
00519                                                                                         )
00520 {
00521         double nextControlLaw = INFTY;
00522         double nextEstimator  = INFTY;
00523 
00524         if ( controlLaw != 0 )
00525         {
00526                 if ( acadoIsInteger( (currentTime-0.0) / getSamplingTimeControlLaw( ) ) == BT_TRUE )
00527                         nextControlLaw = currentTime + getSamplingTimeControlLaw( );
00528                 else
00529                         nextControlLaw = ceil( (currentTime-0.0) / getSamplingTimeControlLaw( ) ) * getSamplingTimeControlLaw( );
00530         }
00531 
00532         if ( estimator != 0 )
00533         {
00534                 if ( acadoIsInteger( (currentTime-0.0) / getSamplingTimeEstimator( ) ) == BT_TRUE )
00535                         nextEstimator = currentTime + getSamplingTimeEstimator( );
00536                 else
00537                         nextEstimator = ceil( (currentTime-0.0) / getSamplingTimeEstimator( ) ) * getSamplingTimeEstimator( );
00538         }
00539 
00540         return acadoMin( nextControlLaw,nextEstimator );
00541 }
00542 
00543 
00544 
00545 //
00546 // PROTECTED MEMBER FUNCTIONS:
00547 //
00548 
00549 
00550 returnValue Controller::setupOptions( )
00551 {
00552         addOption( USE_REFERENCE_PREDICTION,defaultUseReferencePrediction );
00553 
00554         return SUCCESSFUL_RETURN;
00555 }
00556 
00557 
00558 returnValue Controller::setupLogging( )
00559 {
00560         LogRecord tmp(LOG_AT_EACH_ITERATION, PS_DEFAULT);
00561 
00562         tmp.addItem( LOG_FEEDBACK_CONTROL );
00563         tmp.addItem( LOG_TIME_CONTROLLER );
00564         tmp.addItem( LOG_TIME_ESTIMATOR );
00565         tmp.addItem( LOG_TIME_CONTROL_LAW );
00566 
00567         addLogRecord( tmp );
00568 
00569         return SUCCESSFUL_RETURN;
00570 }
00571 
00572 
00573 returnValue Controller::getCurrentReference(    double tStart,
00574                                                                                                 VariablesGrid& _yRef
00575                                                                                                 ) const
00576 {
00577         double tEnd = tStart + controlLaw->getLengthPredictionHorizon( );
00578 
00579         // if no external reference trajectory is given, evaluate internal one
00580         if ( _yRef.isEmpty( ) == BT_TRUE )
00581         {
00582                 if ( referenceTrajectory != 0 )
00583                         referenceTrajectory->getReference( tStart,tEnd, _yRef );
00584         }
00585 
00586         // if prediction shall not be used, only use first value
00587         int useReferencePrediction = 0;
00588         get( USE_REFERENCE_PREDICTION,useReferencePrediction );
00589         
00590         if ( (BooleanType)useReferencePrediction == BT_FALSE )
00591         {
00592                 DVector firstVector = _yRef.getFirstVector( );
00593                 Grid predictionGrid( tStart,tEnd );
00594                 _yRef.init( firstVector,predictionGrid );
00595         }
00596 
00597         return SUCCESSFUL_RETURN;
00598 }
00599 
00600 
00601 CLOSE_NAMESPACE_ACADO
00602 
00603 // end of file.


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