real_time_algorithm.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/optimization_algorithm/real_time_algorithm.hpp>
00036 
00037 
00038 
00039 // #define SIM_DEBUG
00040 
00041 
00042 
00043 BEGIN_NAMESPACE_ACADO
00044 
00045 
00046 //
00047 // PUBLIC MEMBER FUNCTIONS:
00048 //
00049 
00050 RealTimeAlgorithm::RealTimeAlgorithm() : OptimizationAlgorithmBase( ), ControlLaw( )
00051 {
00052         setupOptions( );
00053         setupLogging( );
00054         
00055         x0        = 0;
00056         p0        = 0;
00057     reference = 0;
00058 
00059         set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
00060         set( USE_REALTIME_ITERATIONS,BT_TRUE );
00061         set( MAX_NUM_ITERATIONS,1 );
00062         set( GLOBALIZATION_STRATEGY,GS_FULLSTEP );
00063         set( TERMINATE_AT_CONVERGENCE,BT_FALSE );
00064         
00065         setStatus( BS_NOT_INITIALIZED );
00066 }
00067 
00068 
00069 RealTimeAlgorithm::RealTimeAlgorithm(   const OCP& ocp_,
00070                                                                                 double _samplingTime
00071                                                                                 ) : OptimizationAlgorithmBase( ocp_ ), ControlLaw( _samplingTime )
00072 {
00073         setupOptions( );
00074         setupLogging( );
00075 
00076         x0 = 0;
00077         p0 = 0;
00078     reference = 0;
00079 
00080         set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
00081         set( USE_REALTIME_ITERATIONS,BT_TRUE );
00082         set( MAX_NUM_ITERATIONS,1 );
00083         set( GLOBALIZATION_STRATEGY,GS_FULLSTEP );
00084         set( TERMINATE_AT_CONVERGENCE,BT_FALSE );
00085         
00086         setStatus( BS_NOT_INITIALIZED );
00087 }
00088 
00089 
00090 
00091 RealTimeAlgorithm::RealTimeAlgorithm( const RealTimeAlgorithm& rhs ) : OptimizationAlgorithmBase( rhs ), ControlLaw( rhs )
00092 {
00093     if( rhs.x0 != 0 ) x0 = new DVector(*rhs.x0);
00094     else              x0 = 0                  ;
00095 
00096     if( rhs.p0 != 0 ) p0 = new DVector(*rhs.p0);
00097     else              p0 = 0                  ;
00098 
00099     if( rhs.reference != 0 ) reference = new VariablesGrid(*rhs.reference);
00100     else                     reference = 0                         ;
00101 }
00102 
00103 
00104 RealTimeAlgorithm::~RealTimeAlgorithm( )
00105 {
00106         clear( );
00107 }
00108 
00109 
00110 
00111 RealTimeAlgorithm& RealTimeAlgorithm::operator=( const RealTimeAlgorithm& rhs ){
00112 
00113     if( this != &rhs ){
00114 
00115                 clear( );
00116 
00117                 OptimizationAlgorithmBase::operator=( rhs );
00118                 ControlLaw::operator=( rhs );
00119 
00120         if( rhs.x0 != 0 ) x0 = new DVector(*rhs.x0);
00121         else              x0 = 0                  ;
00122 
00123         if( rhs.p0 != 0 ) p0 = new DVector(*rhs.p0);
00124         else              p0 = 0                  ;
00125 
00126         if( rhs.reference != 0 ) reference = new VariablesGrid(*rhs.reference);
00127         else                     reference = 0                         ;
00128     }
00129     return *this;
00130 }
00131 
00132 
00133 ControlLaw* RealTimeAlgorithm::clone( ) const
00134 {
00135         return new RealTimeAlgorithm( *this );
00136 }
00137 
00138 
00139 
00140 returnValue RealTimeAlgorithm::initializeAlgebraicStates(       const VariablesGrid& _xa_init
00141                                                                                                                         )
00142 {
00143         return OptimizationAlgorithmBase::initializeAlgebraicStates( _xa_init );
00144 }
00145 
00146 
00147 returnValue RealTimeAlgorithm::initializeAlgebraicStates(       const char* fileName
00148                                                                                                                         )
00149 {
00150         return OptimizationAlgorithmBase::initializeAlgebraicStates( fileName );
00151 }
00152 
00153 
00154 returnValue RealTimeAlgorithm::initializeControls(      const VariablesGrid& _u_init
00155                                                                                                         )
00156 {
00157         return OptimizationAlgorithmBase::initializeControls( _u_init );
00158 }
00159 
00160 
00161 returnValue RealTimeAlgorithm::initializeControls(      const char* fileName
00162                                                                                                         )
00163 {
00164         return OptimizationAlgorithmBase::initializeControls( fileName );
00165 }
00166 
00167 
00168 
00169 returnValue RealTimeAlgorithm::init( )
00170 {
00171         if ( ( getStatus( ) == BS_READY ) && ( haveOptionsChanged( ) == BT_FALSE ) )
00172                 return SUCCESSFUL_RETURN;
00173 
00174         returnValue returnvalue = OptimizationAlgorithmBase::init( this );
00175 
00176         setStatus( BS_READY );
00177         declareOptionsUnchanged( );
00178 
00179         return returnvalue;
00180 }
00181 
00182 
00183 returnValue RealTimeAlgorithm::init(    double startTime,
00184                                                                                 const DVector& _x,
00185                                                                                 const DVector& _p,
00186                                                                                 const VariablesGrid& _yRef
00187                                                                                 )
00188 {
00189         /* 0) Consistency checks */
00190         int useImmediateFeedback = 0;
00191         get( USE_IMMEDIATE_FEEDBACK,useImmediateFeedback );
00192 
00193         int maxNumberOfSteps;
00194         get( MAX_NUM_ITERATIONS, maxNumberOfSteps );
00195 
00196 //      if ( ( (BooleanType)useImmediateFeedback == BT_TRUE ) && ( isInRealTimeMode( ) == BT_FALSE ) )
00197 //              return ACADOERROR( RET_NEED_TO_ACTIVATE_RTI );
00198 
00199         if ( ( (BooleanType)useImmediateFeedback == BT_TRUE ) && ( maxNumberOfSteps != 1 ) )
00200         {
00201                 set( MAX_NUM_ITERATIONS,1 );
00202                 ACADOWARNING( RET_IMMEDIATE_FEEDBACK_ONE_ITERATION );
00203         }
00204 
00205 
00206         /* 1) Initialize reference trajectory and real-time parameters. */
00207     clear( );
00208 
00209         if ( _yRef.isEmpty( ) == BT_FALSE )
00210                 reference = new VariablesGrid( _yRef );
00211 
00212     x0 = new DVector(_x);
00213 
00214         if ( _p.isEmpty( ) == BT_FALSE )
00215                 p0 = new DVector(_p);   
00216 
00217 
00218         /* 2) Initialize all sub-blocks. */
00219         if ( init( ) != SUCCESSFUL_RETURN )
00220                 return ACADOERROR( RET_CONTROLLAW_INIT_FAILED );
00221 
00222         u.init( OptimizationAlgorithmBase::getNU( ) );
00223         u.setZero( );
00224 
00225         p.init( OptimizationAlgorithmBase::getNP( ) );
00226         p.setZero( );
00227         
00228         setStatus( BS_READY );
00229 
00230         return SUCCESSFUL_RETURN;
00231 }
00232 
00233 
00234 
00235 returnValue RealTimeAlgorithm::step(    double currentTime,
00236                                                                                 const DVector& _x,
00237                                                                                 const DVector& _p,
00238                                                                                 const VariablesGrid& _yRef
00239                                                                                 )
00240 {
00241         if ( feedbackStep( currentTime,_x,_p ) != SUCCESSFUL_RETURN )
00242                 return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
00243 
00244         if ( preparationStep( currentTime+getSamplingTime(),_yRef ) != SUCCESSFUL_RETURN )
00245                 return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
00246 
00247         return SUCCESSFUL_RETURN;
00248 }
00249 
00250 
00251 returnValue RealTimeAlgorithm::feedbackStep(    double currentTime,
00252                                                                                                 const DVector &_x,
00253                                                                                                 const DVector &_p,
00254                                                                                                 const VariablesGrid& _yRef
00255                                                                                                 )
00256 {
00257         if ( getStatus( ) != BS_READY )
00258                 return ACADOERROR( RET_BLOCK_NOT_READY );
00259 
00260         if ( _x.getDim( ) != getNX() )
00261                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00262 
00263         if ( _p.getDim( ) > getNP() )
00264                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00265 
00266         nlpSolver->resetNumberOfSteps( );
00267 
00268 
00269         if ( isInRealTimeMode( ) == BT_TRUE )
00270         {
00271                 if ( ( x0 != 0 ) && ( _x.isEmpty() == BT_FALSE ) )
00272                         *x0 = _x;
00273                 
00274                 if ( ( p0 != 0 ) && ( _p.isEmpty() == BT_FALSE ) )
00275                         *p0 = _p;
00276         }
00277         
00278         
00279         if ( performFeedbackStep( currentTime,_x,_p ) != SUCCESSFUL_RETURN )
00280                 return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
00281 
00282         
00283         // if real-time mode is NOT enabled, perform usual step 
00284         // otherwise ignore current reference trajectory as it is
00285         // expected to be set during previous call to preparationStep
00286         if ( isInRealTimeMode( ) == BT_FALSE )
00287         {
00288                 // solve at currentTime
00289 //              if ( solve( currentTime,_x,_p,_yRef ) != SUCCESSFUL_RETURN )
00290 //                      return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
00291                 returnValue returnvalue;
00292 
00293                 int maxNumberOfSteps;
00294                 get( MAX_NUM_ITERATIONS, maxNumberOfSteps );
00295 
00296                 int terminateAtConvergence = 0;
00297                 get( TERMINATE_AT_CONVERGENCE,terminateAtConvergence );
00298 
00299                 while( nlpSolver->getNumberOfSteps( ) < maxNumberOfSteps )
00300                 {
00301                         returnvalue = performPreparationStep( _yRef,BT_FALSE );
00302 
00303                         if ( ( returnvalue != CONVERGENCE_ACHIEVED ) && ( returnvalue != CONVERGENCE_NOT_YET_ACHIEVED ) )
00304                                 return ACADOERROR( RET_NLP_SOLUTION_FAILED );
00305 
00306                         if ( ((BooleanType)terminateAtConvergence == BT_TRUE ) && ( returnvalue == CONVERGENCE_ACHIEVED ) )
00307                                 break;
00308                         
00309                         if ( performFeedbackStep( currentTime,_x,_p ) != SUCCESSFUL_RETURN )
00310                                 return ACADOERROR( RET_NLP_SOLUTION_FAILED );
00311                 }
00312         }
00313 
00314         return SUCCESSFUL_RETURN;
00315 }
00316 
00317 
00318 returnValue RealTimeAlgorithm::preparationStep( double nextTime,
00319                                                                                                 const VariablesGrid& _yRef
00320                                                                                                 )
00321 {
00322         returnValue returnvalue = performPreparationStep( _yRef,BT_TRUE );
00323         if ( ( returnvalue != CONVERGENCE_ACHIEVED ) && ( returnvalue != CONVERGENCE_NOT_YET_ACHIEVED ) )
00324                 return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
00325 
00326         return SUCCESSFUL_RETURN;
00327 }
00328 
00329 
00330 
00331 returnValue RealTimeAlgorithm::solve(   double startTime,
00332                                                                                 const DVector &_x,
00333                                                                                 const DVector &_p,
00334                                                                                 const VariablesGrid& _yRef
00335                                                                                 )
00336 {
00337         if ( getStatus( ) == BS_NOT_INITIALIZED )
00338         {
00339                 if ( init( startTime,_x ) != SUCCESSFUL_RETURN )
00340                         return ACADOERROR( RET_OPTALG_INIT_FAILED );
00341         }
00342 
00343         if ( getStatus( ) != BS_READY )
00344         return ACADOERROR( RET_OPTALG_INIT_FAILED );
00345 
00346         returnValue returnvalue;
00347         nlpSolver->resetNumberOfSteps( );
00348 
00349         int maxNumberOfSteps;
00350         get( MAX_NUM_ITERATIONS, maxNumberOfSteps );
00351 
00352         int terminateAtConvergence = 0;
00353         get( TERMINATE_AT_CONVERGENCE,terminateAtConvergence );
00354 
00355         while( nlpSolver->getNumberOfSteps( ) < maxNumberOfSteps )
00356         {
00357                 if ( performFeedbackStep( startTime,_x,_p ) != SUCCESSFUL_RETURN )
00358                         return ACADOERROR( RET_NLP_SOLUTION_FAILED );
00359 
00360                 if ( nlpSolver->getNumberOfSteps( ) == maxNumberOfSteps )
00361                         returnvalue = performPreparationStep( _yRef,BT_TRUE );
00362                 else
00363                         returnvalue = performPreparationStep( _yRef,BT_FALSE );
00364 
00365                 if ( ( returnvalue != CONVERGENCE_ACHIEVED ) && ( returnvalue != CONVERGENCE_NOT_YET_ACHIEVED ) )
00366                         return ACADOERROR( RET_NLP_SOLUTION_FAILED );
00367 
00368                 if ( ((BooleanType)terminateAtConvergence == BT_TRUE ) && ( returnvalue == CONVERGENCE_ACHIEVED ) )
00369                         break;
00370         }
00371 
00372         replot( PLOT_AT_END );
00373 
00374         return SUCCESSFUL_RETURN;
00375 }
00376 
00377 
00378 
00379 returnValue RealTimeAlgorithm::shift(   double timeShift
00380                                                                                 )
00381 {
00382         if ( acadoIsNegative( timeShift ) == BT_TRUE )
00383                 timeShift = getSamplingTime( );
00384         
00385 //      printf( "shift!\n" );
00386 
00387         return nlpSolver->shiftVariables( timeShift );
00388 }
00389 
00390 
00391 returnValue RealTimeAlgorithm::setReference( const VariablesGrid &ref )
00392 {
00393     if ( ( getStatus() != BS_READY ) && ( getStatus() != BS_RUNNING ) )
00394                 return ACADOERROR( RET_OPTALG_INIT_FAILED );
00395 
00396 //      printf( "new ref!\n" );
00397 //      ref.print();
00398     return nlpSolver->setReference( ref );
00399 }
00400 
00401 
00402 
00403 uint RealTimeAlgorithm::getNX( ) const
00404 {
00405         return OptimizationAlgorithmBase::getNX( );
00406 }
00407 
00408 uint RealTimeAlgorithm::getNXA( ) const
00409 {
00410         return OptimizationAlgorithmBase::getNXA( );
00411 }
00412 
00413 uint RealTimeAlgorithm::getNU( ) const
00414 {
00415         return OptimizationAlgorithmBase::getNU( );
00416 }
00417 
00418 uint RealTimeAlgorithm::getNP( ) const
00419 {
00420         return OptimizationAlgorithmBase::getNP( );
00421 }
00422 
00423 uint RealTimeAlgorithm::getNW( ) const
00424 {
00425         return OptimizationAlgorithmBase::getNW( );
00426 }
00427 
00428 
00429 uint RealTimeAlgorithm::getNY( ) const
00430 {
00431         return OptimizationAlgorithmBase::getNX( );
00432 }
00433 
00434 
00435 
00436 double RealTimeAlgorithm::getLengthPredictionHorizon( ) const
00437 {
00438         return getEndTime( ) - getStartTime( );
00439 }
00440 
00441 
00442 double RealTimeAlgorithm::getLengthControlHorizon( ) const
00443 {
00444         return getLengthPredictionHorizon( );
00445 }
00446 
00447 
00448 
00449 BooleanType RealTimeAlgorithm::isDynamic( ) const
00450 {
00451         return BT_TRUE;
00452 }
00453 
00454 
00455 BooleanType RealTimeAlgorithm::isStatic( ) const
00456 {
00457         if ( isDynamic() == BT_TRUE )
00458                 return BT_FALSE;
00459         else
00460                 return BT_TRUE;
00461 }
00462 
00463 
00464 BooleanType RealTimeAlgorithm::isInRealTimeMode( ) const
00465 {
00466         int maxNumIterations;
00467         get( MAX_NUM_ITERATIONS,maxNumIterations );
00468         
00469         if ( maxNumIterations > 1 )
00470                 return BT_FALSE;
00471 
00472         int useRealtimeIterations;
00473         get( USE_REALTIME_ITERATIONS,useRealtimeIterations );
00474 
00475         return (BooleanType)useRealtimeIterations;
00476 }
00477 
00478 
00479 
00480 //
00481 // PROTECTED MEMBER FUNCTIONS:
00482 //
00483 
00484 returnValue RealTimeAlgorithm::setupOptions( )
00485 {
00486         // add NLP solver options
00487         addOption( MAX_NUM_ITERATIONS          , defaultMaxNumIterations        );
00488         addOption( KKT_TOLERANCE               , defaultKKTtolerance            );
00489         addOption( KKT_TOLERANCE_SAFEGUARD     , defaultKKTtoleranceSafeguard   );
00490         addOption( LEVENBERG_MARQUARDT         , defaultLevenbergMarguardt      );
00491         addOption( HESSIAN_PROJECTION_FACTOR   , defaultHessianProjectionFactor );
00492         addOption( PRINTLEVEL                  , defaultPrintlevel              );
00493         addOption( PRINT_COPYRIGHT             , defaultPrintCopyright          );
00494         addOption( HESSIAN_APPROXIMATION       , defaultHessianApproximation    );
00495         addOption( DYNAMIC_HESSIAN_APPROXIMATION, defaultDynamicHessianApproximation );
00496         addOption( DYNAMIC_SENSITIVITY         , defaultDynamicSensitivity      );
00497         addOption( OBJECTIVE_SENSITIVITY       , defaultObjectiveSensitivity    );
00498         addOption( CONSTRAINT_SENSITIVITY      , defaultConstraintSensitivity   );
00499         addOption( DISCRETIZATION_TYPE         , defaultDiscretizationType      );
00500         addOption( LINESEARCH_TOLERANCE        , defaultLinesearchTolerance     );
00501         addOption( MIN_LINESEARCH_PARAMETER    , defaultMinLinesearchParameter  );
00502         addOption( MAX_NUM_QP_ITERATIONS       , defaultMaxNumQPiterations      );
00503         addOption( HOTSTART_QP                 , defaultHotstartQP              );
00504         addOption( INFEASIBLE_QP_RELAXATION    , defaultInfeasibleQPrelaxation  );
00505         addOption( INFEASIBLE_QP_HANDLING      , defaultInfeasibleQPhandling    );
00506         addOption( USE_REALTIME_ITERATIONS     , defaultUseRealtimeIterations   );
00507         addOption( USE_REALTIME_SHIFTS         , defaultUseRealtimeShifts       );
00508         addOption( USE_IMMEDIATE_FEEDBACK      , defaultUseImmediateFeedback    );
00509         addOption( TERMINATE_AT_CONVERGENCE    , defaultTerminateAtConvergence  );
00510         addOption( SPARSE_QP_SOLUTION          , defaultSparseQPsolution        );
00511         addOption( GLOBALIZATION_STRATEGY      , defaultGlobalizationStrategy   );
00512         addOption( PRINT_SCP_METHOD_PROFILE    , defaultprintSCPmethodProfile   );
00513 
00514         // add integration options
00515         addOption( FREEZE_INTEGRATOR           , defaultFreezeIntegrator        );
00516         addOption( INTEGRATOR_TYPE             , defaultIntegratorType          );
00517         addOption( FEASIBILITY_CHECK           , defaultFeasibilityCheck        );
00518         addOption( PLOT_RESOLUTION             , defaultPlotResoltion           );
00519         
00520         // add integrator options
00521         addOption( MAX_NUM_INTEGRATOR_STEPS    , defaultMaxNumSteps             );
00522         addOption( INTEGRATOR_TOLERANCE        , defaultIntegratorTolerance     );
00523         addOption( ABSOLUTE_TOLERANCE          , defaultAbsoluteTolerance       );
00524         addOption( INITIAL_INTEGRATOR_STEPSIZE , defaultInitialStepsize         );
00525         addOption( MIN_INTEGRATOR_STEPSIZE     , defaultMinStepsize             );
00526         addOption( MAX_INTEGRATOR_STEPSIZE     , defaultMaxStepsize             );
00527         addOption( STEPSIZE_TUNING             , defaultStepsizeTuning          );
00528         addOption( CORRECTOR_TOLERANCE         , defaultCorrectorTolerance      );
00529         addOption( INTEGRATOR_PRINTLEVEL       , defaultIntegratorPrintlevel    );
00530         addOption( LINEAR_ALGEBRA_SOLVER       , defaultLinearAlgebraSolver     );
00531         addOption( ALGEBRAIC_RELAXATION        , defaultAlgebraicRelaxation     );
00532         addOption( RELAXATION_PARAMETER        , defaultRelaxationParameter     );
00533         addOption( PRINT_INTEGRATOR_PROFILE    , defaultprintIntegratorProfile  );
00534 
00535         return SUCCESSFUL_RETURN;
00536 }
00537 
00538 
00539 returnValue RealTimeAlgorithm::setupLogging( )
00540 {
00541 //      LogRecord tmp( LOG_AT_EACH_ITERATION,stdout,PS_DEFAULT,BT_FALSE );
00542 // 
00543 //      tmp.addItem( LOG_NUM_NLP_ITERATIONS );
00544 // 
00545 //      addLogRecord( tmp );
00546   
00547         return SUCCESSFUL_RETURN;
00548 }
00549 
00550 
00551 returnValue RealTimeAlgorithm::clear( )
00552 {
00553         if( x0 != 0 )
00554         {
00555                 delete x0;
00556                 x0 = 0;
00557         }
00558         
00559     if( p0 != 0 )
00560         {
00561                 delete p0;
00562                 p0 = 0;
00563         }
00564         
00565     if( reference != 0 )
00566         {
00567                 delete reference;
00568                 reference = 0;
00569         }
00570 
00571         return SUCCESSFUL_RETURN;
00572 }
00573 
00574 
00575 
00576 returnValue RealTimeAlgorithm::allocateNlpSolver( Objective *F, DynamicDiscretization *G, Constraint *H )
00577 {
00578         if( nlpSolver != 0 )
00579                 delete nlpSolver;
00580 
00581         nlpSolver = new SCPmethod( this, F,G,H, isLinearQuadratic( F,G,H ) );
00582 
00583         return SUCCESSFUL_RETURN;
00584 }
00585 
00586 
00587 returnValue RealTimeAlgorithm::initializeNlpSolver( const OCPiterate& _userInit
00588                                                                                                         )
00589 {
00590         returnValue returnvalue = SUCCESSFUL_RETURN;
00591 
00592         if( x0 != 0 ) returnvalue = _userInit.x->setVector( 0,x0[0] );
00593         if( p0 != 0 ) returnvalue = _userInit.p->setAllVectors( p0[0] );
00594 
00595         if( returnvalue != SUCCESSFUL_RETURN )
00596                 return ACADOERROR(returnvalue);
00597 
00598         ACADO_TRY( nlpSolver->init( _userInit.x, _userInit.xa, _userInit.p, _userInit.u, _userInit.w ) );
00599     return SUCCESSFUL_RETURN;
00600 }
00601 
00602 
00603 returnValue RealTimeAlgorithm::initializeObjective( Objective *F )
00604 {
00605         returnValue returnvalue = SUCCESSFUL_RETURN;
00606 
00607         if ( ( reference != 0 ) && ( F != 0 ) )
00608         {
00609 //              reference->print( "refer init" );
00610                 returnvalue = F->setReference( *reference );
00611         }
00612 
00613         return returnvalue;
00614 }
00615 
00616 
00617 
00618 returnValue RealTimeAlgorithm::performFeedbackStep(     double currentTime,
00619                                                                                                         const DVector &_x,
00620                                                                                                         const DVector &_p
00621                                                                                                         )
00622 {
00623         if ( getStatus( ) == BS_NOT_INITIALIZED )
00624         {
00625         if ( init( currentTime,_x ) != SUCCESSFUL_RETURN )
00626                         return ACADOERROR( RET_OPTALG_INIT_FAILED );
00627         }
00628 
00629         if ( getStatus( ) != BS_READY )
00630         return ACADOERROR( RET_OPTALG_FEEDBACK_FAILED );
00631 
00632 
00633         if ( nlpSolver->feedbackStep( _x ) != SUCCESSFUL_RETURN ) //,_p
00634                 return ACADOERROR( RET_OPTALG_FEEDBACK_FAILED );
00635 
00636 //      #ifdef SIM_DEBUG
00637         if ( nlpSolver->getFirstControl( u ) != SUCCESSFUL_RETURN )
00638                 return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
00639 //      u.print("u after feedbackStep");
00640 //      #endif
00641 
00642         setStatus( BS_RUNNING );
00643 
00644         return SUCCESSFUL_RETURN;
00645 }
00646 
00647 
00648 returnValue RealTimeAlgorithm::performPreparationStep(  const VariablesGrid& _yRef,
00649                                                                                                                 BooleanType isLastIteration
00650                                                                                                                 )
00651 {
00652 //      _yRef.print( "yRef" );
00653         
00654         if ( getStatus( ) != BS_RUNNING )
00655         return ACADOERROR( RET_OPTALG_INIT_FAILED );
00656 
00657         int useRealTimeShifts = 0;
00658         get( USE_REALTIME_SHIFTS,useRealTimeShifts );
00659 
00660         // perform current step and check for convergence if desired
00661         returnValue returnvalueStep = nlpSolver->performCurrentStep( );
00662         
00663         if ( ( returnvalueStep != CONVERGENCE_ACHIEVED ) && ( returnvalueStep != CONVERGENCE_NOT_YET_ACHIEVED ) )
00664                 return ACADOERROR( returnvalueStep );
00665 
00666         if ( isLastIteration == BT_TRUE )
00667         {
00668                 if ( _yRef.isEmpty() == BT_FALSE )
00669                         setReference( _yRef );
00670 
00671                 if ( (BooleanType)useRealTimeShifts == BT_TRUE )
00672                         shift( );
00673         }
00674 
00675         // prepare next step
00676         int terminateAtConvergence = 0;
00677         get( TERMINATE_AT_CONVERGENCE,terminateAtConvergence );
00678 
00679         if ( ((BooleanType)terminateAtConvergence == BT_TRUE ) && ( returnvalueStep == CONVERGENCE_ACHIEVED ) )
00680         {
00681                 if ( _yRef.isEmpty() == BT_FALSE )
00682                         setReference( _yRef );
00683                 
00684                 if ( (BooleanType)useRealTimeShifts == BT_TRUE )
00685                         shift( );
00686         }
00687 
00688         returnValue returnvalue = nlpSolver->prepareNextStep( );
00689         if ( ( returnvalue != CONVERGENCE_ACHIEVED ) && ( returnvalue != CONVERGENCE_NOT_YET_ACHIEVED ) )
00690                 return ACADOERROR( returnvalue );
00691 
00692         if ( ((BooleanType)terminateAtConvergence == BT_TRUE ) && ( returnvalueStep == CONVERGENCE_ACHIEVED ) )
00693                 returnvalue = CONVERGENCE_ACHIEVED;
00694 
00695         setStatus( BS_READY );
00696 
00697         return returnvalue;
00698 }
00699 
00700 
00701 
00702 CLOSE_NAMESPACE_ACADO
00703 
00704 // end of file.


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