scp_method.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 
00034 #include <acado/nlp_solver/scp_method.hpp>
00035 #include <iomanip>
00036 #include <iostream>
00037 
00038 
00039 // #define SIM_DEBUG
00040 
00041 using namespace std;
00042 
00043 BEGIN_NAMESPACE_ACADO
00044 
00045 
00046 //
00047 // PUBLIC MEMBER FUNCTIONS:
00048 //
00049 
00050 SCPmethod::SCPmethod( ) : NLPsolver( )
00051 {
00052         setupLogging( );
00053 
00054         eval = 0;
00055         scpStep = 0;
00056         derivativeApproximation = 0;
00057 
00058         bandedCPsolver = 0;
00059         
00060         status = BS_NOT_INITIALIZED;
00061         isCP = BT_FALSE;
00062         
00063         hasPerformedStep = BT_FALSE;
00064         isInRealTimeMode = BT_FALSE;
00065         needToReevaluate = BT_FALSE;
00066 }
00067 
00068 
00069 SCPmethod::SCPmethod(   UserInteraction* _userInteraction,
00070                                                 const Objective             *objective_          ,
00071                                                 const DynamicDiscretization *dynamic_discretization_,
00072                                                 const Constraint            *constraint_,
00073                                                 BooleanType _isCP
00074                                                 ) : NLPsolver( _userInteraction )
00075 {
00076         eval = new SCPevaluation( _userInteraction,objective_,dynamic_discretization_,constraint_,_isCP );
00077         scpStep = 0;
00078         derivativeApproximation = 0;
00079 
00080         bandedCPsolver = 0;
00081 
00082         status = BS_NOT_INITIALIZED;
00083         isCP = _isCP;
00084         
00085         hasPerformedStep = BT_FALSE;
00086         isInRealTimeMode = BT_FALSE;
00087         needToReevaluate = BT_FALSE;
00088 
00089         setupLogging( );
00090 }
00091 
00092 
00093 SCPmethod::SCPmethod( const SCPmethod& rhs ) : NLPsolver( rhs )
00094 {
00095 
00096         timeLoggingIdx = rhs.timeLoggingIdx;
00097         clock = rhs.clock;
00098         clockTotalTime = rhs.clockTotalTime;
00099 
00100     iter    = rhs.iter;
00101         oldIter = rhs.oldIter;
00102 
00103     if( rhs.eval != 0 ) eval = (rhs.eval)->clone( );
00104     else                eval = 0;
00105 
00106     if( rhs.scpStep != 0 ) scpStep = (rhs.scpStep)->clone( );
00107     else                   scpStep = 0;
00108 
00109     if( rhs.derivativeApproximation != 0 ) derivativeApproximation = (rhs.derivativeApproximation)->clone( );
00110     else                                   derivativeApproximation = 0;
00111 
00112     if( rhs.bandedCPsolver != 0 ) bandedCPsolver = (rhs.bandedCPsolver)->clone( );
00113     else                          bandedCPsolver = 0;
00114 
00115         status = rhs.status;
00116         isCP   = rhs.isCP;
00117         
00118         hasPerformedStep = rhs.hasPerformedStep;
00119         isInRealTimeMode = rhs.isInRealTimeMode;
00120         needToReevaluate = rhs.needToReevaluate;
00121 }
00122 
00123 
00124 SCPmethod::~SCPmethod( )
00125 {
00126     if( eval != 0 )
00127         delete eval;
00128 
00129         if( scpStep != 0 )
00130                 delete scpStep;
00131 
00132     if( derivativeApproximation != 0 )
00133                 delete derivativeApproximation;
00134 
00135         if( bandedCPsolver != 0 )
00136                 delete bandedCPsolver;
00137 }
00138 
00139 
00140 SCPmethod& SCPmethod::operator=( const SCPmethod& rhs ){
00141 
00142     if ( this != &rhs )
00143     {
00144                 if( eval != 0 )
00145                         delete eval;
00146 
00147                 if( scpStep != 0 )
00148                         delete scpStep;
00149 
00150                 if( derivativeApproximation != 0 )
00151                         delete derivativeApproximation;
00152 
00153                 if( bandedCPsolver != 0 )
00154                         delete bandedCPsolver;
00155 
00156 
00157         NLPsolver::operator=( rhs );
00158 
00159 
00160                 timeLoggingIdx = rhs.timeLoggingIdx;
00161                 clock = rhs.clock;
00162                 clockTotalTime = rhs.clockTotalTime;
00163 
00164                 iter    = rhs.iter;
00165                 oldIter = rhs.oldIter;
00166 
00167                 if( rhs.eval != 0 ) eval = (rhs.eval)->clone( );
00168                 else                eval = 0;
00169 
00170                 if( rhs.scpStep != 0 ) scpStep = (rhs.scpStep)->clone( );
00171                 else                   scpStep = 0;
00172 
00173                 if( rhs.derivativeApproximation != 0 ) derivativeApproximation = (rhs.derivativeApproximation)->clone( );
00174                 else                                   derivativeApproximation = 0;
00175 
00176         if( rhs.bandedCPsolver != 0 ) bandedCPsolver = (rhs.bandedCPsolver)->clone( );
00177         else                          bandedCPsolver = 0;
00178 
00179                 status = rhs.status;
00180                 isCP   = rhs.isCP;
00181                 
00182                 hasPerformedStep = rhs.hasPerformedStep;
00183                 isInRealTimeMode = rhs.isInRealTimeMode;
00184                 needToReevaluate = rhs.needToReevaluate;
00185         }
00186 
00187     return *this;
00188 }
00189 
00190 
00191 NLPsolver* SCPmethod::clone( ) const
00192 {
00193         return new SCPmethod( *this );
00194 }
00195 
00196 
00197 
00198 returnValue SCPmethod::init(    VariablesGrid* x_init ,
00199                                                                 VariablesGrid* xa_init,
00200                                                                 VariablesGrid* p_init ,
00201                                                                 VariablesGrid* u_init ,
00202                                                                 VariablesGrid* w_init   )
00203 {
00204     int printC;
00205     get( PRINT_COPYRIGHT, printC );
00206 
00207     // PRINT THE HEADER:
00208     // -----------------
00209     if( printC == BT_TRUE )
00210         acadoPrintCopyrightNotice( "SCPmethod -- A Sequential Quadratic Programming Algorithm." );
00211 
00212         iter.init( x_init, xa_init, p_init, u_init, w_init );
00213 
00214 //      iter.print(); // already here different!!
00215 
00216         if ( setup( ) != SUCCESSFUL_RETURN )
00217                 return ACADOERROR( RET_NLP_INIT_FAILED );
00218 
00219 
00220         // COMPUTATION OF DERIVATIVES:
00221         // ---------------------------
00222         int printLevel;
00223         get( PRINTLEVEL,printLevel );
00224         
00225         if ( (PrintLevel)printLevel >= HIGH ) 
00226                 cout << "--> Computing initial linearization of NLP system ...\n";
00227 
00228 //      iter.print();
00229         
00230     ACADO_TRY( eval->evaluateSensitivities( iter,bandedCP ) ).changeType( RET_NLP_INIT_FAILED );
00231 
00232 //      iter.print();
00233         
00234         if ( (PrintLevel)printLevel >= HIGH ) 
00235                 cout << "<-- Computing initial linearization of NLP system done.\n";
00236 
00237         int useRealtimeIterations;
00238         get( USE_REALTIME_ITERATIONS,useRealtimeIterations );
00239 
00240         if ( (BooleanType)useRealtimeIterations == BT_TRUE )
00241         {
00242                 if ( bandedCPsolver->prepareSolve( bandedCP ) != SUCCESSFUL_RETURN )
00243                         return ACADOERROR( RET_NLP_STEP_FAILED );
00244         }
00245 
00246 
00247         // freeze condensing in case OCP is QP -- isCP is a bit misleading...
00248         if ( ( isCP == BT_TRUE ) && ( eval->hasLSQobjective( ) == BT_TRUE ) )
00249         {
00250 //              bandedCPsolver->freezeCondensing( );
00251 //              eval->freezeSensitivities( );
00252         }
00253 
00254         status = BS_READY;
00255 
00256     return SUCCESSFUL_RETURN;
00257 }
00258 
00259 
00260 returnValue SCPmethod::solve(   const DVector &x0_,
00261                                                                 const DVector &p_
00262                                                                 )
00263 {
00264         if ( ( status != BS_READY ) && ( status != BS_RUNNING ) )
00265                 return ACADOERROR( RET_INITIALIZE_FIRST );
00266 
00267         returnValue returnvalue = SUCCESSFUL_RETURN;
00268         numberOfSteps = 0;
00269 
00270         int maxNumberOfSteps;
00271         get( MAX_NUM_ITERATIONS, maxNumberOfSteps );
00272 
00273         while( numberOfSteps < maxNumberOfSteps )
00274         {
00275                 returnvalue = step( x0_,p_ );
00276                 // also increases numberOfSteps by one
00277 
00278                 if( returnvalue == CONVERGENCE_ACHIEVED )
00279                         break;
00280 
00281                 if( returnvalue != CONVERGENCE_NOT_YET_ACHIEVED )
00282                         return ACADOERROR( RET_NLP_SOLUTION_FAILED );
00283         }
00284 
00285         replot( PLOT_AT_END );
00286 
00287     if( numberOfSteps == maxNumberOfSteps )
00288         return RET_MAX_NUMBER_OF_STEPS_EXCEEDED;
00289 
00290     return returnvalue;
00291 }
00292 
00293 
00294 
00295 returnValue SCPmethod::step(    const DVector& x0_,
00296                                                                 const DVector& p_
00297                                                                 )
00298 {
00299         if ( numberOfSteps == 0 )
00300                 replot( PLOT_AT_START );
00301 
00302         if ( feedbackStep( x0_,p_ ) != SUCCESSFUL_RETURN )
00303                 return RET_NLP_STEP_FAILED;
00304 
00305         if ( performCurrentStep( ) == CONVERGENCE_ACHIEVED )
00306                 return CONVERGENCE_ACHIEVED;
00307         
00308         returnValue returnValue = prepareNextStep( );
00309 
00310         if ( ( returnValue != CONVERGENCE_ACHIEVED ) && ( returnValue != CONVERGENCE_NOT_YET_ACHIEVED ) )
00311                 return RET_NLP_STEP_FAILED;
00312         
00313         return returnValue;
00314 }
00315 
00316 
00317 returnValue SCPmethod::feedbackStep(    const DVector& x0_,
00318                                                                                 const DVector& p_
00319                                                                                 )
00320 {
00321   
00322   #ifdef SIM_DEBUG
00323   printf("START OF THE FEEDBACK STEP \n");
00324   
00325   x0_.print("x0");
00326   #endif
00327   
00328   
00329         returnValue returnvalue;
00330 
00331         if ( ( status != BS_READY ) && ( status != BS_RUNNING ) )
00332                 return ACADOERROR( RET_INITIALIZE_FIRST );
00333 
00334         clockTotalTime.reset( );
00335         clockTotalTime.start( );
00336         
00337         status = BS_RUNNING;
00338         hasPerformedStep = BT_FALSE;
00339 
00340         if ( checkForRealTimeMode( x0_,p_ ) != SUCCESSFUL_RETURN )
00341                 return ACADOERROR( RET_NLP_STEP_FAILED );
00342 
00343     int hessianMode;
00344     get( HESSIAN_APPROXIMATION,hessianMode );
00345 
00346         if ( ( isInRealTimeMode == BT_FALSE ) && ( (HessianApproximationMode)hessianMode == EXACT_HESSIAN ) )
00347         {
00348                 returnvalue = initializeHessianProjection();
00349                 if( returnvalue != SUCCESSFUL_RETURN )
00350                         return ACADOERROR( returnvalue );
00351         }
00352 
00353         //bandedCP.objectiveGradient.print();
00354         //x0_.print("x0");
00355         
00356     if ( isInRealTimeMode == BT_TRUE )
00357         {
00358                 if ( setupRealTimeParameters( x0_,p_ ) != SUCCESSFUL_RETURN )
00359                         return ACADOERROR( RET_NLP_STEP_FAILED );
00360         }
00361 
00362         int printLevel;
00363         get( PRINTLEVEL,printLevel );
00364 
00365         if ( (PrintLevel)printLevel >= HIGH ) 
00366                 cout << "--> Solving banded QP ...\n";
00367 
00368 //      iter.print();
00369 
00370         if ( bandedCPsolver->solve( bandedCP ) != SUCCESSFUL_RETURN )
00371                 return ACADOERROR( RET_NLP_STEP_FAILED );
00372 
00373 //      bandedCP.deltaX.print();
00374         
00375         if ( (PrintLevel)printLevel >= HIGH ) 
00376                 cout << "<-- Solving banded QP done.\n";
00377 
00378         ++numberOfSteps;
00379 
00380         return SUCCESSFUL_RETURN;
00381 }
00382 
00383 
00384 returnValue SCPmethod::performCurrentStep( )
00385 {
00386         returnValue returnvalue;
00387 
00388         if ( isInRealTimeMode == BT_TRUE )
00389         {
00390                 if ( bandedCPsolver->finalizeSolve( bandedCP ) != SUCCESSFUL_RETURN )
00391                         return ACADOERROR( RET_NLP_STEP_FAILED );
00392                 
00393 //              bandedCP.deltaX.print();
00394     }
00395 
00396 //     cout <<"bandedCP.dynResiduum = \n");
00397 //     bandedCP.dynResiduum.print();
00398 //     cout <<"bandedCP.lambdaDynamic = \n");
00399 //     bandedCP.lambdaDynamic.print();
00400 
00401         oldIter = iter;
00402 
00403     // Perform a globalized step:
00404     // --------------------------
00405         int printLevel;
00406         get( PRINTLEVEL,printLevel );
00407 
00408         if ( (PrintLevel)printLevel >= HIGH ) 
00409                 cout << "--> Perform globalized SQP step ...\n";
00410         
00411         clock.reset( );
00412         clock.start( );
00413 
00414         #ifdef SIM_DEBUG
00415 /*      printf("performing the current step...: old iterate \n");
00416         (iter.x->getVector(0)).print("iter.x(0)");
00417         (iter.u->getVector(0)).print("iter.u(0)");
00418         (iter.x->getVector(1)).print("iter.x(1)");
00419         (iter.u->getVector(1)).print("iter.u(1)");*/
00420         #endif
00421 
00422         returnvalue = scpStep->performStep( iter,bandedCP,eval );
00423         if( returnvalue != SUCCESSFUL_RETURN )
00424                 ACADOERROR( RET_NLP_STEP_FAILED );
00425 
00426         hasPerformedStep = BT_TRUE;
00427 
00428         clock.stop( );
00429         setLast( LOG_TIME_GLOBALIZATION,clock.getTime() );
00430 
00431         if ( (PrintLevel)printLevel >= HIGH ) 
00432                 cout << "<-- Perform globalized SQP step done.\n";
00433 
00434         printIteration( );
00435 
00436         // Check convergence criterion if no real-time iterations are performed
00437         int terminateAtConvergence = 0;
00438         get( TERMINATE_AT_CONVERGENCE,terminateAtConvergence );
00439 
00440         if ( (BooleanType)terminateAtConvergence == BT_TRUE )
00441         {
00442                 if ( checkForConvergence( ) == CONVERGENCE_ACHIEVED )
00443                 {
00444                         stopClockAndPrintRuntimeProfile( );
00445                         return CONVERGENCE_ACHIEVED;
00446                 }
00447         }
00448 
00449         if ( numberOfSteps >= 0 )
00450                 set( KKT_TOLERANCE_SAFEGUARD,0.0 );
00451         
00452         return CONVERGENCE_NOT_YET_ACHIEVED;
00453 }
00454 
00455 
00456 returnValue SCPmethod::prepareNextStep( )
00457 {
00458     returnValue returnvalue;
00459     RealClock clockLG;
00460 
00461         BlockMatrix oldLagrangeGradient;
00462         BlockMatrix newLagrangeGradient;
00463 
00464 //      cout <<"bandedCP.dynResiduum (possibly shifted) = \n");
00465 //     bandedCP.dynResiduum.print();
00466 //      cout <<"bandedCP.lambdaDynamic (possibly shifted) = \n");
00467 //     bandedCP.lambdaDynamic.print();
00468 
00469     // Coumpute the "old" Lagrange Gradient with the latest multipliers:
00470     // -----------------------------------------------------------------
00471         clockLG.reset( );
00472         clockLG.start( );
00473 
00474         returnvalue = eval->evaluateLagrangeGradient( getNumPoints(),oldIter,bandedCP, oldLagrangeGradient );
00475         if( returnvalue != SUCCESSFUL_RETURN )
00476                 ACADOERROR( RET_NLP_STEP_FAILED );
00477 
00478         clockLG.stop( );
00479         
00480         
00481     // Linearize the NLP system at the new point:
00482     // ------------------------------------------
00483         int printLevel;
00484         get( PRINTLEVEL,printLevel );
00485 
00486 
00487         #ifdef SIM_DEBUG
00488 /*      printf("preparation step \n");
00489         (iter.x->getVector(0)).print("iter.x(0)");
00490         (iter.u->getVector(0)).print("iter.u(0)");
00491         (iter.x->getVector(1)).print("iter.x(1)");
00492         (iter.u->getVector(1)).print("iter.u(1)");*/
00493         #endif
00494         
00495         if ( (PrintLevel)printLevel >= HIGH ) 
00496                 cout << "--> Computing new linearization of NLP system ...\n";
00497 
00498         clock.reset( );
00499         clock.start( );
00500 
00501         if ( needToReevaluate == BT_TRUE )
00502         {
00503                 // needs to re-evaluate due to eval->evaluate call within merit function evaluation!
00504                 eval->clearDynamicDiscretization( );
00505                 if ( eval->evaluate( iter,bandedCP ) != SUCCESSFUL_RETURN )
00506                         return ACADOERROR( RET_NLP_STEP_FAILED );
00507                 needToReevaluate = BT_FALSE;
00508         }
00509 
00510         returnvalue = eval->evaluateSensitivities( iter,bandedCP );
00511         if( returnvalue != SUCCESSFUL_RETURN )
00512                 ACADOERROR( RET_NLP_STEP_FAILED );
00513 
00514         clock.stop( );
00515         setLast( LOG_TIME_SENSITIVITIES,clock.getTime() );
00516 
00517         if ( (PrintLevel)printLevel >= HIGH ) 
00518                 cout << "<-- Computing new linearization of NLP system done.\n";
00519         //bandedCP.objectiveGradient.print();
00520         
00521 
00522     // Coumpute the "new" Lagrange Gradient with the latest multipliers:
00523     // -----------------------------------------------------------------
00524     clockLG.start( );
00525 
00526         returnvalue = eval->evaluateLagrangeGradient( getNumPoints(),iter,bandedCP, newLagrangeGradient );
00527         if( returnvalue != SUCCESSFUL_RETURN )
00528                 ACADOERROR( RET_NLP_STEP_FAILED );
00529 
00530         clockLG.stop( );
00531         setLast( LOG_TIME_LAGRANGE_GRADIENT,clockLG.getTime() );
00532 
00533         
00534     // Compute the next Hessian:
00535     // -------------------------
00536         if ( (PrintLevel)printLevel >= HIGH ) 
00537                 cout << "--> Computing or approximating Hessian matrix ...\n";
00538         
00539         clock.reset( );
00540         clock.start( );
00541 
00542         returnvalue = computeHessianMatrix( oldLagrangeGradient,newLagrangeGradient );
00543         if( returnvalue != SUCCESSFUL_RETURN )
00544                 ACADOERROR( RET_NLP_STEP_FAILED );
00545 
00546         clock.stop( );
00547         setLast( LOG_TIME_HESSIAN_COMPUTATION,clock.getTime() );
00548 
00549         if ( (PrintLevel)printLevel >= HIGH ) 
00550                 cout << "<-- Computing or approximating Hessian matrix done.\n";
00551 
00552         // CONDENSE THE KKT-SYSTEM:
00553     // ------------------------
00554     if ( isInRealTimeMode == BT_TRUE )
00555         {
00556                 if ( bandedCPsolver->prepareSolve( bandedCP ) != SUCCESSFUL_RETURN )
00557                         return ACADOERROR( RET_NLP_STEP_FAILED );
00558         }
00559 
00560         stopClockAndPrintRuntimeProfile( );
00561 
00562         return CONVERGENCE_NOT_YET_ACHIEVED;
00563 }
00564 
00565 
00566 
00567 returnValue SCPmethod::setReference( const VariablesGrid &ref )
00568 {
00569         needToReevaluate = BT_TRUE;
00570 //      printf("new reference!\n");
00571     return eval->setReference( ref );
00572 }
00573 
00574 
00575 // returnValue SCPmethod::enableNeedToReevaluate( )
00576 // {
00577 //      needToReevaluate = BT_TRUE;
00578 //      return SUCCESSFUL_RETURN;
00579 // }
00580 
00581 
00582 returnValue SCPmethod::shiftVariables(  double timeShift,
00583                                                                                 DVector  lastX,
00584                                                                                 DVector  lastXA,
00585                                                                                 DVector  lastP,
00586                                                                                 DVector  lastU,
00587                                                                                 DVector  lastW
00588                                                                                 )
00589 {
00590         #ifdef SIM_DEBUG
00591         cout << "SCPmethod::shiftVariables\n" );
00592         #endif
00593         
00594         if ( acadoIsNegative( timeShift ) == BT_TRUE )
00595                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00596 
00597 //      DMatrix tmp;
00598 //      for( uint i=1; i<iter.getNumPoints()-1; ++i )
00599 //      {
00600 //              bandedCP.lambdaDynamic.getSubBlock( i,0, tmp );
00601 //              bandedCP.lambdaDynamic.setDense( i-1,0, tmp );
00602 //      }
00603 
00604 //      printf("shifted!\n");
00605         needToReevaluate = BT_TRUE;
00606         return iter.shift( timeShift, lastX, lastXA, lastP, lastU, lastW );
00607 }
00608 
00609 
00610 
00611 returnValue SCPmethod::getVarianceCovariance( DMatrix &var )
00612 {
00613         if( eval->hasLSQobjective( ) == BT_FALSE )
00614                 return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00615 
00616         if( bandedCPsolver == 0 )
00617                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00618 
00619         return bandedCPsolver->getVarianceCovariance( var );
00620 }
00621 
00622 
00623 returnValue SCPmethod::printRuntimeProfile() const
00624 {
00625         return printLogRecord(cout, timeLoggingIdx, PRINT_LAST_ITER);
00626 }
00627 
00628 
00629 //
00630 // PROTECTED MEMBER FUNCTIONS:
00631 //
00632 
00633 
00634 returnValue SCPmethod::setupLogging( )
00635 {
00636         LogRecord tmp(LOG_AT_EACH_ITERATION, PS_PLAIN);
00637 
00638         tmp.addItem( LOG_TIME_SQP_ITERATION,         "TIME FOR THE WHOLE SQP ITERATION     [sec]" );
00639         tmp.addItem( LOG_TIME_CONDENSING,            "TIME FOR CONDENSING                  [sec]" );
00640         tmp.addItem( LOG_TIME_QP,                    "TIME FOR SOLVING THE QP              [sec]" );
00641 //      tmp.addItem( LOG_TIME_RELAXED_QP,            "TIME FOR SOLVING RELAXED QP's        [sec]" );
00642 //      tmp.addItem( LOG_TIME_EXPAND,                "TIME FOR EXPANSION                   [sec]" );
00643 //      tmp.addItem( LOG_TIME_EVALUATION,            "TIME FOR FUNCTION EVALUATIONS        [sec]" );
00644         tmp.addItem( LOG_TIME_GLOBALIZATION,         "TIME FOR GLOBALIZATION               [sec]" );
00645         tmp.addItem( LOG_TIME_SENSITIVITIES,         "TIME FOR SENSITIVITY GENERATION      [sec]" );
00646 //      tmp.addItem( LOG_TIME_LAGRANGE_GRADIENT,     "TIME FOR COMPUTING LAGRANGE GRADIENT [sec]" );
00647 //      tmp.addItem( LOG_TIME_HESSIAN_COMPUTATION,   "TIME FOR HESSIAN EVALUATION          [sec]" );
00648 
00649         timeLoggingIdx = addLogRecord( tmp );
00650 
00651         LogRecord iterationOutput(LOG_AT_EACH_ITERATION, PS_PLAIN);
00652 
00653         iterationOutput.addItem( LOG_NUM_SQP_ITERATIONS,"SQP iteration");
00654         iterationOutput.addItem( LOG_KKT_TOLERANCE,"KKT tolerance");
00655         iterationOutput.addItem( LOG_LINESEARCH_STEPLENGTH,"line search parameter");
00656         iterationOutput.addItem( LOG_OBJECTIVE_VALUE,"objective value");
00657         iterationOutput.addItem( LOG_MERIT_FUNCTION_VALUE,"merit function value");
00658         iterationOutput.addItem( LOG_IS_QP_RELAXED,"QP relaxation");
00659         iterationOutput.addItem( LOG_NUM_QP_ITERATIONS,"No. QP iterations");
00660         
00661         outputLoggingIdx = addLogRecord( iterationOutput );
00662 
00663         return SUCCESSFUL_RETURN;
00664 }
00665 
00666 
00667 returnValue SCPmethod::setup( )
00668 {
00669         // CONSISTENCY CHECKS:
00670         // -------------------
00671         if ( isCP == BT_TRUE )
00672         {
00673                 int hessianApproximation;
00674                 get( HESSIAN_APPROXIMATION,hessianApproximation );
00675 
00676                 // Gauss-Newton is exact for linear-quadratic systems
00677                 if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN )
00678                         set( HESSIAN_APPROXIMATION,GAUSS_NEWTON );
00679         }
00680 
00681 
00682     // PREPARE THE SQP ALGORITHM:
00683     // --------------------------
00684 
00685     if ( eval == 0 )
00686         return ACADOERROR( RET_UNKNOWN_BUG );
00687 
00688         if ( eval->init( iter ) != SUCCESSFUL_RETURN )
00689                 return ACADOERROR( RET_NLP_INIT_FAILED );
00690 
00691 
00692     // PREPARE THE DATA FOR THE SQP ALGORITHM:
00693     // ---------------------------------------
00694 
00695         if ( bandedCPsolver != 0 )
00696                 delete bandedCPsolver;
00697 
00698         int sparseQPsolution;
00699         get( SPARSE_QP_SOLUTION,sparseQPsolution );
00700         
00701         int printLevel;
00702         get( PRINTLEVEL,printLevel );
00703 
00704         if ( (PrintLevel)printLevel >= HIGH ) 
00705                 cout << "--> Initializing banded QP solver ...\n";
00706 
00707         if ( (SparseQPsolutionMethods)sparseQPsolution == CONDENSING )
00708         {
00709         bandedCP.lambdaConstraint.init( eval->getNumConstraintBlocks(), 1 );
00710         bandedCP.lambdaDynamic.init( getNumPoints()-1, 1 );
00711 
00712                 bandedCPsolver = new CondensingBasedCPsolver( userInteraction,eval->getNumConstraints(),eval->getConstraintBlockDims() );
00713                 bandedCPsolver->init( iter );
00714         }
00715         else
00716         {
00717                 return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00718         }
00719 
00720         if ( (PrintLevel)printLevel >= HIGH ) 
00721                 cout << "<-- Initializing banded QP solver done.\n";
00722 
00723     // INITIALIZE GLOBALIZATION STRATEGY (SCPstep):
00724     // --------------------------------------------
00725 
00726         if ( scpStep != 0 )
00727                 delete scpStep;
00728 
00729     int globalizationStrategy;
00730     get( GLOBALIZATION_STRATEGY,globalizationStrategy );
00731 
00732         switch( (GlobalizationStrategy)globalizationStrategy )
00733         {
00734                 case GS_FULLSTEP:
00735                         scpStep = new SCPstepFullstep( userInteraction );
00736                         break;
00737 
00738                 case GS_LINESEARCH:
00739                         scpStep = new SCPstepLinesearch( userInteraction );
00740                         break;
00741 
00742                 default:
00743                         return ACADOERROR( RET_UNKNOWN_BUG );
00744         }
00745 
00746 
00747         // EVALUATION OF THE NLP FUNCTIONS:
00748         // --------------------------------
00749         if ( (PrintLevel)printLevel >= HIGH ) 
00750                 cout << "--> Initial integration of dynamic system ...\n";
00751         
00752         ACADO_TRY( eval->evaluate(iter,bandedCP) ).changeType( RET_NLP_INIT_FAILED );
00753 
00754         if ( (PrintLevel)printLevel >= HIGH ) 
00755                 cout << "<-- Initial integration of dynamic system done.\n";
00756 
00757     // INITIALIZE HESSIAN MATRIX:
00758     // --------------------------
00759     int hessianMode;
00760     get( HESSIAN_APPROXIMATION,hessianMode );
00761 
00762     if( ( (HessianApproximationMode)hessianMode == GAUSS_NEWTON ) || ( (HessianApproximationMode)hessianMode == GAUSS_NEWTON_WITH_BLOCK_BFGS ) )
00763         {
00764         if( eval->hasLSQobjective( ) == BT_FALSE ){
00765 //             ACADOWARNING( RET_GAUSS_NEWTON_APPROXIMATION_NOT_SUPPORTED );
00766 //             set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE );
00767 //             get( HESSIAN_APPROXIMATION, hessianMode );
00768         }
00769     }
00770 
00771         if ( derivativeApproximation != 0 )
00772                 delete derivativeApproximation;
00773 
00774         switch( (HessianApproximationMode)hessianMode )
00775         {
00776                 case EXACT_HESSIAN:
00777                         derivativeApproximation = new ExactHessian( userInteraction );
00778                         break;
00779 
00780                 case CONSTANT_HESSIAN:
00781                         derivativeApproximation = new ConstantHessian( userInteraction );
00782                         break;
00783 
00784                 case FULL_BFGS_UPDATE:
00785                         derivativeApproximation = new BFGSupdate( userInteraction );
00786                         break;
00787 
00788                 case BLOCK_BFGS_UPDATE:
00789                         derivativeApproximation = new BFGSupdate( userInteraction,getNumPoints() );
00790                         break;
00791 
00792                 case GAUSS_NEWTON:
00793                         derivativeApproximation = new GaussNewtonApproximation( userInteraction );
00794                         break;
00795 
00796                 case GAUSS_NEWTON_WITH_BLOCK_BFGS:
00797                         derivativeApproximation = new GaussNewtonApproximationWithBFGS( userInteraction,getNumPoints() );
00798                         break;
00799 
00800                 default:
00801                         return ACADOERROR( RET_UNKNOWN_BUG );
00802         }
00803 
00804         bandedCP.hessian.init( 5*getNumPoints(), 5*getNumPoints() );
00805 
00806         if ( (PrintLevel)printLevel >= HIGH ) 
00807                 cout << "--> Initializing Hessian computations ...\n";
00808         
00809         ACADO_TRY( derivativeApproximation->initHessian( bandedCP.hessian,getNumPoints(),iter ) );
00810 
00811         if ( (PrintLevel)printLevel >= HIGH ) 
00812                 cout << "<-- Initializing Hessian computations done.\n";
00813 
00814         // SWITCH BETWEEN SINGLE- AND MULTIPLE SHOOTING:
00815         // ---------------------------------------------
00816         int discretizationMode;
00817         get( DISCRETIZATION_TYPE, discretizationMode );
00818 
00819         if( (StateDiscretizationType)discretizationMode != SINGLE_SHOOTING ){
00820                 if( iter.x  != 0 ) iter.x ->disableAutoInit();
00821                 if( iter.xa != 0 ) iter.xa->disableAutoInit();
00822         }
00823 
00824         return SUCCESSFUL_RETURN;
00825 }
00826 
00827 
00828 returnValue SCPmethod::printIterate( ) const
00829 {
00830         return iter.print( );
00831 }
00832 
00833 
00834 returnValue SCPmethod::printIteration( )
00835 {
00836         double KKTmultiplierRegularisation;
00837         get( KKT_TOLERANCE_SAFEGUARD,KKTmultiplierRegularisation );
00838         
00839         setLast( LOG_NUM_SQP_ITERATIONS, numberOfSteps );
00840         setLast( LOG_KKT_TOLERANCE, eval->getKKTtolerance( iter,bandedCP,KKTmultiplierRegularisation ) );
00841         setLast( LOG_OBJECTIVE_VALUE, eval->getObjectiveValue() );
00842 
00843         int printLevel;
00844         get( PRINTLEVEL,printLevel );
00845 
00846         if ( (PrintLevel)printLevel >= MEDIUM ) 
00847         {
00848                 if (numberOfSteps == 1 || (numberOfSteps % 10) == 0)
00849                         cout    << "sqp it | "
00850                                         << "qp its | "
00851                                         << "      kkt tol | "
00852                                         << "      obj val | "
00853                                         << "    merit val | "
00854                                         << "     ls param | "
00855                                         << endl;
00856 
00857                 DMatrix foo;
00858 
00859                 IoFormatter iof( cout );
00860 
00861                 getLast(LOG_NUM_SQP_ITERATIONS, foo);
00862                 cout << setw( 6 ) << right << (int) foo(0, 0) << " | ";
00863                 getLast(LOG_NUM_QP_ITERATIONS, foo);
00864                 cout << setw( 6 ) << right << (int) foo(0, 0) << " | ";
00865                 getLast(LOG_KKT_TOLERANCE, foo);
00866                 cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) << " | ";
00867                 getLast(LOG_OBJECTIVE_VALUE, foo);
00868                 cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) << " | ";
00869                 getLast(LOG_MERIT_FUNCTION_VALUE, foo);
00870                 cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) << " | ";
00871                 getLast(LOG_LINESEARCH_STEPLENGTH, foo);
00872                 cout << setw( 13 ) << setprecision( 6 ) << right << scientific << foo(0, 0) << " | ";
00873                 cout << endl;
00874 
00875                 // Restore cout flags
00876                 iof.reset();
00877         }
00878 
00879         replot( PLOT_AT_EACH_ITERATION );
00880 
00881         return SUCCESSFUL_RETURN;
00882 }
00883 
00884 
00885 returnValue SCPmethod::checkForConvergence( )
00886 {
00887         double tol;
00888         get( KKT_TOLERANCE,tol );
00889 
00890         // NEEDS TO BE CHECKED CARFULLY !!!
00891         double KKTmultiplierRegularisation;
00892         get(KKT_TOLERANCE_SAFEGUARD, KKTmultiplierRegularisation);
00893 
00894         if( eval->getKKTtolerance( iter,bandedCP,KKTmultiplierRegularisation ) <= tol )
00895         {
00896                 int discretizationMode;
00897                 get( DISCRETIZATION_TYPE, discretizationMode );
00898 
00899                 if( (StateDiscretizationType)discretizationMode == SINGLE_SHOOTING )
00900                 {
00901                         eval->clearDynamicDiscretization( );
00902                         if ( eval->evaluate( iter,bandedCP ) != SUCCESSFUL_RETURN )
00903                                 return ACADOERROR( RET_NLP_STEP_FAILED );
00904                 }
00905 
00906                 int printLevel;
00907                 get( PRINTLEVEL,printLevel );
00908 
00909                 if ( (PrintLevel)printLevel >= MEDIUM )
00910                 {
00911                         cout    << endl
00912                                         << "Covergence achieved. Demanded KKT tolerance is "
00913                                         << scientific << tol
00914                                         << "." << endl << endl;
00915                 }
00916                 return CONVERGENCE_ACHIEVED;
00917         }
00918         
00919         return CONVERGENCE_NOT_YET_ACHIEVED;
00920 }
00921 
00922 
00923 returnValue SCPmethod::computeHessianMatrix(    const BlockMatrix& oldLagrangeGradient,
00924                                                                                                 const BlockMatrix& newLagrangeGradient
00925                                                                                                 )
00926 {
00927         returnValue returnvalue;
00928 
00929         if ( numberOfSteps == 1 )
00930         {
00931                 returnvalue = derivativeApproximation->initScaling( bandedCP.hessian, bandedCP.deltaX, newLagrangeGradient-oldLagrangeGradient );
00932                 if( returnvalue != SUCCESSFUL_RETURN )
00933                         ACADOERROR( returnvalue );
00934         }
00935 
00936         returnvalue = derivativeApproximation->apply( bandedCP.hessian, bandedCP.deltaX, newLagrangeGradient-oldLagrangeGradient );
00937         if( returnvalue != SUCCESSFUL_RETURN )
00938                 ACADOERROR( returnvalue );
00939 
00940         return SUCCESSFUL_RETURN;
00941 }
00942 
00943 
00944 
00945 returnValue SCPmethod::initializeHessianProjection( )
00946 {
00947     // COMPUTE A HEURISTIC DAMPING FACTOR:
00948     // -----------------------------------
00949     double damping = 1.0;
00950     int run1 = 0;
00951     while( run1 < numberOfSteps ){
00952         if( run1 == 5 ) break;
00953         damping *= 0.01;
00954         ++run1;
00955     }
00956 
00957     return bandedCPsolver->set( HESSIAN_PROJECTION_FACTOR, derivativeApproximation->getHessianScaling()*damping );
00958 }
00959 
00960 
00961 
00962 returnValue SCPmethod::checkForRealTimeMode(    const DVector &x0_,
00963                                                                                                 const DVector &p_
00964                                                                                                 )
00965 {
00966         int useRealtimeIterations;
00967         get( USE_REALTIME_ITERATIONS,useRealtimeIterations );
00968         isInRealTimeMode = (BooleanType)useRealtimeIterations;
00969 
00970         if ( ( isInRealTimeMode == BT_FALSE ) && 
00971                  ( ( x0_.isEmpty( ) == BT_FALSE ) || ( p_.isEmpty( ) == BT_FALSE ) ) )
00972                 return ACADOERROR( RET_NEED_TO_ACTIVATE_RTI );
00973 
00974         return SUCCESSFUL_RETURN;
00975 }
00976 
00977 
00978 returnValue SCPmethod::setupRealTimeParameters( const DVector &x0_,
00979                                                                                                 const DVector &p_
00980                                                                                                 )
00981 {
00982         DVector deltaX;
00983         DVector deltaP;
00984 
00985         if( x0_.isEmpty( ) == BT_FALSE )
00986                 deltaX = x0_ - iter.x->getVector(0);
00987 
00988         if( p_ .isEmpty( ) == BT_FALSE )
00989                 deltaP = p_ - iter.p->getVector(0);
00990 
00991         return bandedCPsolver->setRealTimeParameters( deltaX, deltaP );
00992 }
00993 
00994 
00995 returnValue SCPmethod::stopClockAndPrintRuntimeProfile( )
00996 {
00997         clockTotalTime.stop( );
00998         setLast( LOG_TIME_SQP_ITERATION,clockTotalTime.getTime() );
00999         
01000         int printProfile;
01001         get( PRINT_SCP_METHOD_PROFILE, printProfile );
01002 
01003         if( (BooleanType) printProfile == BT_TRUE )
01004                 printRuntimeProfile();
01005         
01006         return SUCCESSFUL_RETURN;
01007 }
01008 
01009 
01010 
01011 returnValue SCPmethod::getDifferentialStates( VariablesGrid &xd_ ) const{
01012 
01013     if( iter.x == 0 )
01014                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
01015 
01016     xd_ = iter.x[0];
01017     return SUCCESSFUL_RETURN;
01018 }
01019 
01020 
01021 returnValue SCPmethod::getAlgebraicStates( VariablesGrid &xa_ ) const{
01022 
01023     if( iter.xa == 0 )
01024                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
01025     xa_ = iter.xa[0];
01026     return SUCCESSFUL_RETURN;
01027 }
01028 
01029 
01030 returnValue SCPmethod::getParameters( VariablesGrid &p_  ) const{
01031 
01032     if( iter.p == 0 )
01033                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
01034 //     p_.init( iter.p[0].getVector( 0 ),iter.p[0].getTimePoints( ) );
01035         p_ = iter.p[0];
01036     return SUCCESSFUL_RETURN;
01037 }
01038 
01039 
01040 returnValue SCPmethod::getParameters( DVector& p_  ) const{
01041 
01042         if( iter.p == 0 )
01043                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
01044 
01045         p_ = iter.p->getVector( 0 );
01046     return SUCCESSFUL_RETURN;
01047 }
01048 
01049 
01050 returnValue SCPmethod::getControls( VariablesGrid &u_  ) const{
01051 
01052     if( iter.u == 0 )
01053                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
01054 
01055     u_ = iter.u[0];
01056     return SUCCESSFUL_RETURN;
01057 }
01058 
01059 
01060 returnValue SCPmethod::getFirstControl( DVector& u0_  ) const
01061 {
01062         #ifdef SIM_DEBUG
01063         cout << "SCPmethod::getFirstControl\n";
01064         #endif
01065         
01066     if( iter.u == 0 )
01067                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
01068 
01069     u0_ = iter.u->getVector( 0 );
01070         
01071         if ( hasPerformedStep == BT_FALSE )
01072         {
01073                 DVector deltaU0( getNU() );
01074                 bandedCPsolver->getFirstControl( deltaU0 );
01075                 
01076                 u0_ += deltaU0;
01077         }
01078 
01079     return SUCCESSFUL_RETURN;
01080 }
01081 
01082 
01083 returnValue SCPmethod::getDisturbances( VariablesGrid &w_  ) const{
01084 
01085     if( iter.w == 0 )
01086                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
01087 
01088     w_ = iter.w[0];
01089     return SUCCESSFUL_RETURN;
01090 }
01091 
01092 
01093 
01094 double SCPmethod::getObjectiveValue( ) const
01095 {
01096     ASSERT( eval != 0 );
01097     return eval->getObjectiveValue();
01098 }
01099 
01100 
01101 
01102 returnValue SCPmethod::getSensitivitiesX(       BlockMatrix& _sens
01103                                                                                         ) const
01104 {
01105         return getAnySensitivities( _sens,0 );
01106 }
01107 
01108 
01109 returnValue SCPmethod::getSensitivitiesXA(      BlockMatrix& _sens
01110                                                                                         ) const
01111 {
01112         return getAnySensitivities( _sens,1 );
01113 }
01114 
01115 returnValue SCPmethod::getSensitivitiesP(       BlockMatrix& _sens
01116                                                                                         ) const
01117 {
01118         return getAnySensitivities( _sens,2 );
01119 }
01120 
01121 
01122 returnValue SCPmethod::getSensitivitiesU(       BlockMatrix& _sens
01123                                                                                         ) const
01124 {
01125         return getAnySensitivities( _sens,3 );
01126 }
01127 
01128 
01129 returnValue SCPmethod::getSensitivitiesW(       BlockMatrix& _sens
01130                                                                                         ) const
01131 {
01132         return getAnySensitivities( _sens,4 );
01133 }
01134 
01135 
01136 returnValue SCPmethod::getAnySensitivities(     BlockMatrix& _sens,
01137                                                                                         uint idx
01138                                                                                         ) const
01139 {
01140         if ( idx > 4 )
01141                 return ACADOERROR( RET_INVALID_ARGUMENTS );
01142 
01143         uint N = bandedCP.dynGradient.getNumRows();
01144         DMatrix tmp;
01145         
01146         _sens.init( N,1 );
01147         
01148         for( uint i=0; i<N; ++i )
01149         {
01150                 bandedCP.dynGradient.getSubBlock( i,idx,tmp );
01151                 _sens.setDense( i,0,tmp );
01152         }
01153         
01154         return SUCCESSFUL_RETURN;
01155 }
01156 
01157 
01158 CLOSE_NAMESPACE_ACADO
01159 
01160 // end of file.


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