optimization_algorithm_base.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/optimization_algorithm/optimization_algorithm_base.hpp>
00035 #include <acado/ocp/ocp.hpp>
00036 
00037 using namespace std;
00038 
00039 BEGIN_NAMESPACE_ACADO
00040 
00041 
00042 
00043 //
00044 // PUBLIC MEMBER FUNCTIONS:
00045 //
00046 
00047 
00048 OptimizationAlgorithmBase::OptimizationAlgorithmBase( )
00049 {
00050     ocp       = 0;
00051     nlpSolver = 0;
00052 
00053         userInit.allocateAll( );
00054 }
00055 
00056 
00057 OptimizationAlgorithmBase::OptimizationAlgorithmBase( const OCP& ocp_ )
00058 {
00059     ocp = new OCP(ocp_);
00060     nlpSolver = 0;
00061 
00062         userInit.allocateAll( );
00063 }
00064 
00065 
00066 OptimizationAlgorithmBase::OptimizationAlgorithmBase( const OptimizationAlgorithmBase& arg )
00067 {
00068     if( arg.ocp != 0 )        ocp       = new OCP(*arg.ocp)      ;
00069     else                      ocp       = 0                      ;
00070 
00071     if( arg.nlpSolver != 0 )  nlpSolver = arg.nlpSolver->clone() ;
00072     else                      nlpSolver = 0                      ;
00073 
00074         iter     = arg.iter;
00075         userInit = arg.userInit;
00076 }
00077 
00078 
00079 OptimizationAlgorithmBase::~OptimizationAlgorithmBase( )
00080 {
00081         clear( );
00082 }
00083 
00084 
00085 
00086 OptimizationAlgorithmBase& OptimizationAlgorithmBase::operator=( const OptimizationAlgorithmBase& arg ){
00087 
00088     if( this != &arg )
00089         {
00090                 clear( );
00091 
00092         if( arg.ocp != 0 )        ocp       = new OCP(*arg.ocp)      ;
00093         else                      ocp       = 0                      ;
00094 
00095         if( arg.nlpSolver != 0 )  nlpSolver = arg.nlpSolver->clone() ;
00096         else                      nlpSolver = 0                      ;
00097 
00098                 iter    = arg.iter;
00099                 userInit = arg.userInit;
00100     }
00101     return *this;
00102 }
00103 
00104 
00105 
00106 returnValue OptimizationAlgorithmBase::initializeDifferentialStates( const char* fileName , BooleanType autoinit)
00107 {
00108         VariablesGrid tmp;
00109         tmp.read( fileName );
00110         
00111         if ( tmp.isEmpty() == BT_TRUE )
00112                 return RET_FILE_CAN_NOT_BE_OPENED;
00113         
00114     return initializeDifferentialStates(tmp,autoinit);
00115 }
00116 
00117 
00118 returnValue OptimizationAlgorithmBase::initializeAlgebraicStates( const char* fileName , BooleanType autoinit)
00119 {
00120         VariablesGrid tmp;
00121         tmp.read( fileName );
00122         
00123         if ( tmp.isEmpty() == BT_TRUE )
00124                 return RET_FILE_CAN_NOT_BE_OPENED;
00125         
00126     return initializeAlgebraicStates(tmp,autoinit);
00127 }
00128 
00129 
00130 returnValue OptimizationAlgorithmBase::initializeParameters( const char* fileName)
00131 {
00132         VariablesGrid tmp;
00133         tmp.read( fileName );
00134         
00135         if ( tmp.isEmpty() == BT_TRUE )
00136                 return RET_FILE_CAN_NOT_BE_OPENED;
00137         
00138     return initializeParameters(tmp);
00139 }
00140 
00141 
00142 returnValue OptimizationAlgorithmBase::initializeControls( const char* fileName)
00143 {
00144         VariablesGrid tmp;
00145         tmp.read( fileName );
00146         
00147         if ( tmp.isEmpty() == BT_TRUE )
00148                 return RET_FILE_CAN_NOT_BE_OPENED;
00149 
00150     return initializeControls(tmp);
00151 }
00152 
00153 
00154 returnValue OptimizationAlgorithmBase::initializeDisturbances( const char* fileName)
00155 {
00156         VariablesGrid tmp;
00157         tmp.read( fileName );
00158         
00159         if ( tmp.isEmpty() == BT_TRUE )
00160                 return RET_FILE_CAN_NOT_BE_OPENED;
00161         
00162     return initializeDisturbances(tmp);
00163 }
00164 
00165 
00166 
00167 returnValue OptimizationAlgorithmBase::initializeDifferentialStates( const VariablesGrid &xd_init_  , BooleanType autoinit){
00168 
00169         if ( userInit.x != 0 ) delete userInit.x;
00170         userInit.x = new VariablesGrid( xd_init_ );
00171         if (autoinit == BT_TRUE)
00172                 userInit.x->enableAutoInit();
00173         if (autoinit == BT_FALSE)
00174                 userInit.x->disableAutoInit();
00175 
00176     return SUCCESSFUL_RETURN;
00177 }
00178 
00179 
00180 returnValue OptimizationAlgorithmBase::simulateStatesForInitialization( ){
00181     if( userInit.x  != 0 ) userInit.x ->enableAutoInit();
00182     if( userInit.xa != 0 ) userInit.xa->enableAutoInit();
00183     return SUCCESSFUL_RETURN;
00184 }
00185 
00186 
00187 returnValue OptimizationAlgorithmBase::initializeAlgebraicStates( const VariablesGrid &xa_init_  , BooleanType autoinit){
00188 
00189         if ( userInit.xa != 0 ) delete userInit.xa;
00190     userInit.xa = new VariablesGrid( xa_init_ );
00191         if (autoinit == BT_TRUE) 
00192                 userInit.xa->enableAutoInit();
00193         if (autoinit == BT_FALSE){ 
00194                   
00195                 userInit.xa->disableAutoInit();
00196                 }
00197     return SUCCESSFUL_RETURN;
00198 }
00199 
00200 
00201 returnValue OptimizationAlgorithmBase::initializeParameters ( const VariablesGrid &p_init_ ){
00202 
00203         if ( userInit.p != 0 ) delete userInit.p;
00204     userInit.p = new VariablesGrid( p_init_ );
00205     return SUCCESSFUL_RETURN;
00206 }
00207 
00208 
00209 returnValue OptimizationAlgorithmBase::initializeControls( const VariablesGrid &u_init_){
00210 
00211         if ( userInit.u != 0 ) delete userInit.u;
00212     userInit.u = new VariablesGrid( u_init_ );
00213     return SUCCESSFUL_RETURN;
00214 }
00215 
00216 
00217 returnValue OptimizationAlgorithmBase::initializeDisturbances( const VariablesGrid &w_init_){
00218 
00219         if ( userInit.w != 0 ) delete userInit.w;
00220     userInit.w = new VariablesGrid( w_init_ );
00221     return SUCCESSFUL_RETURN;
00222 }
00223 
00224 
00225 returnValue OptimizationAlgorithmBase::getDifferentialStates( VariablesGrid &xd_ ) const{
00226 
00227     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00228     return nlpSolver->getDifferentialStates( xd_ );
00229 }
00230 
00231 
00232 returnValue OptimizationAlgorithmBase::getAlgebraicStates( VariablesGrid &xa_ ) const{
00233 
00234     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00235     return nlpSolver->getAlgebraicStates( xa_ );
00236 }
00237 
00238 
00239 returnValue OptimizationAlgorithmBase::getParameters( VariablesGrid &p_  ) const
00240 {
00241         if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00242         return nlpSolver->getParameters( p_ );
00243 }
00244 
00245 
00246 returnValue OptimizationAlgorithmBase::getParameters( DVector &p_  ) const
00247 {
00248         if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00249 
00250         VariablesGrid tmp;
00251 
00252         returnValue returnvalue = nlpSolver->getParameters( tmp );
00253         if ( returnvalue != SUCCESSFUL_RETURN )
00254                 return returnvalue;
00255 
00256         p_ = tmp.getVector( 0 );
00257 
00258         return SUCCESSFUL_RETURN;
00259 }
00260 
00261 
00262 returnValue OptimizationAlgorithmBase::getControls( VariablesGrid &u_  ) const{
00263 
00264     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00265     return nlpSolver->getControls( u_ );
00266 }
00267 
00268 
00269 returnValue OptimizationAlgorithmBase::getDisturbances( VariablesGrid &w_  ) const{
00270 
00271     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00272     return nlpSolver->getDisturbances( w_ );
00273 }
00274 
00275 
00276 returnValue OptimizationAlgorithmBase::getDifferentialStates( const char* fileName ) const{
00277 
00278     returnValue returnvalue;
00279     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00280 
00281     VariablesGrid xx;
00282     returnvalue = nlpSolver->getDifferentialStates( xx );
00283     if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
00284 
00285     xx.print( fileName );
00286 
00287     return SUCCESSFUL_RETURN;
00288 }
00289 
00290 
00291 returnValue OptimizationAlgorithmBase::getAlgebraicStates( const char* fileName ) const{
00292 
00293     returnValue returnvalue;
00294     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00295 
00296     VariablesGrid xx;
00297     returnvalue = nlpSolver->getAlgebraicStates( xx );
00298     if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
00299 
00300     xx.print( fileName );
00301 
00302     return SUCCESSFUL_RETURN;
00303 }
00304 
00305 
00306 returnValue OptimizationAlgorithmBase::getParameters( const char* fileName ) const{
00307 
00308     returnValue returnvalue;
00309     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00310 
00311     VariablesGrid xx;
00312     returnvalue = nlpSolver->getParameters( xx );
00313     if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
00314 
00315     xx.print( fileName );
00316 
00317     return SUCCESSFUL_RETURN;
00318 }
00319 
00320 
00321 returnValue OptimizationAlgorithmBase::getControls( const char* fileName ) const{
00322 
00323     returnValue returnvalue;
00324     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00325 
00326     VariablesGrid xx;
00327     returnvalue = nlpSolver->getControls( xx );
00328     if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
00329 
00330     xx.print( fileName );
00331 
00332     return SUCCESSFUL_RETURN;
00333 }
00334 
00335 
00336 returnValue OptimizationAlgorithmBase::getDisturbances( const char* fileName ) const{
00337 
00338     returnValue returnvalue;
00339     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00340 
00341     VariablesGrid xx;
00342     returnvalue = nlpSolver->getDisturbances( xx );
00343     if( returnvalue != SUCCESSFUL_RETURN ) return returnvalue;
00344 
00345     xx.print( fileName );
00346 
00347     return SUCCESSFUL_RETURN;
00348 }
00349 
00350 
00351 double OptimizationAlgorithmBase::getObjectiveValue( const char* fileName ) const
00352 {
00353         ofstream stream( fileName );
00354         stream << scientific << getObjectiveValue();
00355         stream.close();
00356 
00357     return SUCCESSFUL_RETURN;
00358 }
00359 
00360 
00361 double OptimizationAlgorithmBase::getObjectiveValue() const{
00362 
00363     if( nlpSolver == 0 ) return ACADOWARNING( RET_MEMBER_NOT_INITIALISED );
00364     return nlpSolver->getObjectiveValue();
00365 }
00366 
00367 
00368 
00369 returnValue OptimizationAlgorithmBase::getSensitivitiesX(       BlockMatrix& _sens
00370                                                                                                                         ) const
00371 {
00372         return nlpSolver->getSensitivitiesX( _sens );
00373 }
00374 
00375 
00376 returnValue OptimizationAlgorithmBase::getSensitivitiesXA(      BlockMatrix& _sens
00377                                                                                                                         ) const
00378 {
00379         return nlpSolver->getSensitivitiesXA( _sens );
00380 }
00381 
00382 returnValue OptimizationAlgorithmBase::getSensitivitiesP(       BlockMatrix& _sens
00383                                                                                                                         ) const
00384 {
00385         return nlpSolver->getSensitivitiesP( _sens );
00386 }
00387 
00388 
00389 returnValue OptimizationAlgorithmBase::getSensitivitiesU(       BlockMatrix& _sens
00390                                                                                                                         ) const
00391 {
00392         return nlpSolver->getSensitivitiesU( _sens );
00393 }
00394 
00395 
00396 returnValue OptimizationAlgorithmBase::getSensitivitiesW(       BlockMatrix& _sens
00397                                                                                                                         ) const
00398 {
00399         return nlpSolver->getSensitivitiesW( _sens );
00400 }
00401 
00402 
00403 
00404 uint OptimizationAlgorithmBase::getNX( ) const
00405 {
00406         return iter.getNX( );
00407 }
00408 
00409 
00410 uint OptimizationAlgorithmBase::getNXA( ) const
00411 {
00412         return iter.getNXA( );
00413 }
00414 
00415 
00416 uint OptimizationAlgorithmBase::getNP( ) const
00417 {
00418         return iter.getNP( );
00419 }
00420 
00421 
00422 uint OptimizationAlgorithmBase::getNU( ) const
00423 {
00424         return iter.getNU( );
00425 }
00426 
00427 
00428 uint OptimizationAlgorithmBase::getNW( ) const
00429 {
00430         return iter.getNW( );
00431 }
00432 
00433 
00434 double OptimizationAlgorithmBase::getStartTime( ) const
00435 {
00436         if ( ocp != 0 )
00437                 return ocp->getStartTime( );
00438         else
00439                 return -INFTY;
00440 }
00441 
00442 
00443 double OptimizationAlgorithmBase::getEndTime( ) const
00444 {
00445         if ( ocp != 0 )
00446                 return ocp->getEndTime( );
00447         else
00448                 return -INFTY;
00449 }
00450 
00451 
00452 
00453 //
00454 // PROTECTED MEMBER FUNCTIONS:
00455 //
00456 
00457 returnValue OptimizationAlgorithmBase::clear( )
00458 {
00459         if ( ocp != 0 )
00460         {
00461                 delete ocp;
00462                 ocp = 0;
00463         }
00464 
00465     if ( nlpSolver != 0 )
00466         {
00467                 delete nlpSolver;
00468                 nlpSolver = 0;
00469         }
00470 
00471     iter.clear( );
00472 
00473         userInit.clear( );
00474         userInit.allocateAll( );
00475 
00476         return SUCCESSFUL_RETURN;
00477 }
00478 
00479 
00480 returnValue OptimizationAlgorithmBase::init(    UserInteraction* _userIteraction
00481                                                                                                 )
00482 {
00483     // EXTRACT INFORMATION PACKED IN THE DATA WRAPPER OCP:
00484     // ---------------------------------------------------
00485     Objective             *objective           ;
00486     DifferentialEquation **differentialEquation;
00487     Constraint            *constraint          ;
00488 
00489         Grid unionGrid;
00490         
00491         if ( extractOCPdata(    &objective,&differentialEquation,&constraint,
00492                                                         unionGrid 
00493                                                         ) != SUCCESSFUL_RETURN )
00494                 return ACADOERROR(RET_OPTALG_INIT_FAILED);
00495 
00496     // REFORMULATE THE OBJECTIVE IF NECESSARY:
00497     // ---------------------------------------
00498 
00499         if ( setupObjective(    objective,differentialEquation,constraint,
00500                                                         unionGrid 
00501                                                         ) != SUCCESSFUL_RETURN )
00502                 return ACADOERROR(RET_OPTALG_INIT_FAILED);
00503 
00504     // REFORMULATE THE CONSTRAINT IF NECESSARY:
00505     // ----------------------------------------
00506 
00507         if ( setupDifferentialEquation( objective,differentialEquation,constraint,
00508                                                                         unionGrid 
00509                                                                         ) != SUCCESSFUL_RETURN )
00510                 return ACADOERROR(RET_OPTALG_INIT_FAILED);
00511 
00512     // DISCRETIZE THE DIFFERENTIAL EQUATION IF NECESSARY:
00513     // --------------------------------------------------
00514 
00515     DynamicDiscretization* dynamicDiscretization = 0;
00516 
00517         if ( setupDynamicDiscretization(        _userIteraction,
00518                                                                                 objective,differentialEquation,constraint,
00519                                                                                 unionGrid,
00520                                                                                 &dynamicDiscretization
00521                                                                                 ) != SUCCESSFUL_RETURN )
00522                 return ACADOERROR(RET_OPTALG_INIT_FAILED);
00523 
00524     // SETUP OF THE NLP SOLVER:
00525     // ------------------------
00526     if ( allocateNlpSolver( objective,dynamicDiscretization,constraint ) != SUCCESSFUL_RETURN )
00527                 return ACADOERROR(RET_OPTALG_INIT_FAILED);
00528         
00529     // DETERMINE THE DIMENSIONS OF THE OPTIMIZATION VARIABLES:
00530     // -------------------------------------------------------
00531 
00532     uint nx  = 0;
00533     uint nxa = 0;
00534     uint np  = 0;
00535     uint nu  = 0;
00536     uint nw  = 0;
00537 
00538         if ( determineDimensions( objective,differentialEquation,constraint, nx,nxa,np,nu,nw ) != SUCCESSFUL_RETURN )
00539         {
00540                 if( differentialEquation  != 0 ) delete differentialEquation[0];
00541 
00542                 if( objective             != 0 ) delete   objective            ;
00543                 if( differentialEquation  != 0 ) delete[] differentialEquation ;
00544                 if( constraint            != 0 ) delete   constraint           ;
00545                 if( dynamicDiscretization != 0 ) delete   dynamicDiscretization;
00546 
00547                 return ACADOERROR(RET_OPTALG_INIT_FAILED);
00548         }
00549         
00550         if ( initializeOCPiterate( constraint,unionGrid,nx,nxa,np,nu,nw ) != SUCCESSFUL_RETURN )
00551         {
00552                 if( differentialEquation != 0 ) delete differentialEquation[0];
00553 
00554                 if( objective             != 0 ) delete   objective            ;
00555                 if( differentialEquation  != 0 ) delete[] differentialEquation ;
00556                 if( constraint            != 0 ) delete   constraint           ;
00557                 if( dynamicDiscretization != 0 ) delete   dynamicDiscretization;
00558 
00559                 return ACADOERROR(RET_OPTALG_INIT_FAILED);
00560         }
00561 
00562     // ELIMINATE EQUALITY BOUNDS: ??
00563     // --------------------------
00564 
00565     // changes the dimensions again !
00566 
00567 
00568     // DEFINE MULTIPLE SHOOTING NOTES
00569     // OR COLLOCATION NOTES IF REQUESTED:
00570     // ----------------------------------
00571 
00572        // ADAPT INITIALIZATION OF X AND XA ?
00573 
00574        if( iter.p  != 0 ) iter.p ->disableAutoInit();
00575        if( iter.u  != 0 ) iter.u ->disableAutoInit();
00576        if( iter.w  != 0 ) iter.w ->disableAutoInit();
00577 
00578 
00579     // (COLLOCATION NOT IMPLEMENTED YET)
00580 
00581 //      printf("before!!!\n");
00582 //      iter.print();
00583 
00584     // INITIALIZE THE NLP-ALGORITHM:
00585     // -----------------------------
00586         if ( initializeNlpSolver( iter ) != SUCCESSFUL_RETURN )
00587                 return ACADOERROR( RET_OPTALG_INIT_FAILED );
00588 
00589 //      printf("after!!!\n");
00590 //      iter.print();
00591         
00592     // GIVE THE TEMPORARY MEMORY FREE:
00593     // -------------------------------
00594     if( differentialEquation != 0 ) delete differentialEquation[0];
00595 
00596     if( objective             != 0 ) delete   objective            ;
00597     if( differentialEquation  != 0 ) delete[] differentialEquation ;
00598     if( constraint            != 0 ) delete   constraint           ;
00599     if( dynamicDiscretization != 0 ) delete   dynamicDiscretization;
00600 
00601     return SUCCESSFUL_RETURN;
00602 }
00603 
00604 
00605 BooleanType OptimizationAlgorithmBase::isLinearQuadratic(       Objective *F,
00606                                                                                                                         DynamicDiscretization *G,
00607                                                                                                                         Constraint *H
00608                                                                                                                         ) const
00609 {
00610 
00611     // CONVEXITY DETECTION DOES NOT WORK FOR /examples/michaelis_menten.txt
00612     // AUTOMATIC CONVEXITY DETECTION IS DISABLED AT THE MOMENT.
00613     // (PLEASE FIX !!!).
00614 
00615 //     return BT_FALSE;
00616 
00617         // ENABLES AGAIN, BUT MENTIONED BUG IS PROBABLY STILL NOT FIXED
00618 
00619     if( F != 0 ) if ( F->isQuadratic( ) == BT_FALSE ) return BT_FALSE;
00620     if( G != 0 ) if ( G->isAffine   ( ) == BT_FALSE ) return BT_FALSE;
00621     if( H != 0 ) if ( H->isAffine   ( ) == BT_FALSE ) return BT_FALSE;
00622     return BT_TRUE;
00623 }
00624 
00625 
00626 returnValue OptimizationAlgorithmBase::extractOCPdata(  Objective** objective,
00627                                                                                                                 DifferentialEquation*** differentialEquation,
00628                                                                                                                 Constraint** constraint,
00629                                                                                                                 Grid& unionGrid
00630                                                                                                                 )
00631 {
00632 
00633         returnValue returnvalue;
00634 
00635     // EXTRACT INFORMATION PACKED IN THE DATA WRAPPER OCP:
00636     // ---------------------------------------------------
00637 
00638     if( ocp->hasObjective() == BT_TRUE )  *objective = new Objective( );
00639     else                    return ACADOERROR( RET_NO_VALID_OBJECTIVE );
00640 
00641     if( ocp->hasDifferentialEquation() == BT_TRUE ){
00642         *differentialEquation = new DifferentialEquation*[1];
00643     }
00644     else *differentialEquation = 0;
00645 
00646     if( ocp->hasConstraint() == BT_TRUE )
00647         {
00648                 *constraint = new Constraint( );
00649                 ocp->getConstraint( **constraint );
00650         }
00651     else
00652         {
00653                 // CONSTRUCT A CONSTRAINT IF NOT ALLOCATED YET:
00654                 // --------------------------------------------
00655                 *constraint = new Constraint(); constraint[0]->init( unionGrid );
00656         }
00657 
00658     ocp->getObjective( **objective );
00659     returnvalue = initializeObjective( *objective );
00660 
00661     if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00662 
00663     if( ocp->hasDifferentialEquation() == BT_TRUE ){
00664 
00665         differentialEquation[0][0] = new DifferentialEquation();
00666         ocp->getModel( *differentialEquation[0][0] );
00667     }
00668 
00669 
00670     // OBTAIN THE UNION GRID:
00671     // -------------------------
00672     ocp->getGrid( unionGrid );
00673 
00674         return SUCCESSFUL_RETURN;
00675 }
00676 
00677 
00678 returnValue OptimizationAlgorithmBase::setupObjective(  Objective* objective,
00679                                                                                                                 DifferentialEquation** differentialEquation,
00680                                                                                                                 Constraint* constraint,
00681                                                                                                                 Grid unionGrid
00682                                                                                                                 )
00683 {
00684         // note that even if the differentialEquation == 0
00685         // is satisfied it might still be that the objective
00686         // allocates a differentialEquation (e.g. for the case
00687         // that the objective is an integral expression.)
00688     return objective->init( 1,0,differentialEquation,0,constraint );
00689 }
00690 
00691 
00692 returnValue OptimizationAlgorithmBase::setupDifferentialEquation(       Objective* objective,
00693                                                                                                                                         DifferentialEquation** differentialEquation,
00694                                                                                                                                         Constraint* constraint,
00695                                                                                                                                         Grid unionGrid
00696                                                                                                                                         )
00697 {
00698         if( differentialEquation != 0 ){
00699                 if( differentialEquation[0]->getNumAlgebraicEquations() != 0 ){
00700                         constraint->add( unionGrid.getNumPoints(), *differentialEquation[0] );
00701                 }
00702         }
00703 
00704         return SUCCESSFUL_RETURN;
00705 }
00706 
00707 
00708 returnValue OptimizationAlgorithmBase::setupDynamicDiscretization(      UserInteraction* _userIteraction,
00709                                                                                                                                         Objective* objective,
00710                                                                                                                                         DifferentialEquation** differentialEquation,
00711                                                                                                                                         Constraint* constraint,
00712                                                                                                                                         Grid unionGrid,
00713                                                                                                                                         DynamicDiscretization** dynamicDiscretization
00714                                                                                                                                         )
00715 {
00716 
00717     if( differentialEquation != 0 ){
00718 
00719         *dynamicDiscretization = new ShootingMethod( _userIteraction );
00720 
00721         int intType;
00722         _userIteraction->get( INTEGRATOR_TYPE, intType );
00723 
00724         if( differentialEquation[0]->getNumAlgebraicEquations() != 0 ) intType = (int) INT_BDF     ;
00725         if( differentialEquation[0]->isImplicit()    == BT_TRUE      ) intType = (int) INT_BDF     ;
00726         if( differentialEquation[0]->isDiscretized() == BT_TRUE      ) intType = (int) INT_DISCRETE;
00727 
00728         int sensType;
00729         (*dynamicDiscretization)->get( DYNAMIC_SENSITIVITY, sensType );
00730 
00731         if( differentialEquation[0]->isSymbolic() == BT_FALSE && sensType == (int) BACKWARD_SENSITIVITY ){
00732             (*dynamicDiscretization)->set( DYNAMIC_SENSITIVITY,  FORWARD_SENSITIVITY );
00733         }
00734 
00735         (*dynamicDiscretization)->addStage( *differentialEquation[0],
00736                                             unionGrid,
00737                                             (IntegratorType)intType );
00738     }
00739 
00740         return SUCCESSFUL_RETURN;
00741 }
00742 
00743 
00744 returnValue OptimizationAlgorithmBase::determineDimensions(     Objective* const _objective,
00745                                                                                                                         DifferentialEquation** const _differentialEquation,
00746                                                                                                                         Constraint* const _constraint,
00747                                                                                                                         uint& _nx,
00748                                                                                                                         uint& _nxa,
00749                                                                                                                         uint& _np,
00750                                                                                                                         uint& _nu,
00751                                                                                                                         uint& _nw
00752                                                                                                                         ) const
00753 {
00754         if( _objective != 0 )
00755         {
00756         _nx  = acadoMax( _objective->getNX (), _nx  );
00757         _nxa = acadoMax( _objective->getNXA(), _nxa );
00758         _np  = acadoMax( _objective->getNP (), _np  );
00759         _nu  = acadoMax( _objective->getNU (), _nu  );
00760         _nw  = acadoMax( _objective->getNW (), _nw  );
00761     }
00762     if( _differentialEquation != 0 )
00763         {
00764         _nx  = acadoMax( _differentialEquation[0]->getNX() , _nx  );
00765         _nxa = acadoMax( _differentialEquation[0]->getNXA(), _nxa );
00766         _np  = acadoMax( _differentialEquation[0]->getNP (), _np  );
00767         _nu  = acadoMax( _differentialEquation[0]->getNU (), _nu  );
00768         _nw  = acadoMax( _differentialEquation[0]->getNW (), _nw  );
00769     }
00770     if( _constraint != 0 )
00771         {
00772         _nx  = acadoMax( _constraint->getNX (), _nx  );
00773         _nxa = acadoMax( _constraint->getNXA(), _nxa );
00774         _np  = acadoMax( _constraint->getNP (), _np  );
00775         _nu  = acadoMax( _constraint->getNU (), _nu  );
00776         _nw  = acadoMax( _constraint->getNW (), _nw  );
00777     }
00778 
00779     if( _differentialEquation == 0 )
00780         {
00781         if( _nx > 0 )
00782                         return RET_INCOMPATIBLE_DIMENSIONS;
00783     }
00784     else
00785         {
00786         _nx = _differentialEquation[0]->getNumDynamicEquations();
00787 //         if( nxa != (uint) differentialEquation[0]->getNumAlgebraicEquations()){
00788 // 
00789 //             for( run1 = 0; run1 < numberOfStages; run1++ )
00790 //                  if( differentialEquation != 0 ) delete differentialEquation[run1];
00791 // 
00792 //             if( objective             != 0 ) delete   objective            ;
00793 //             if( differentialEquation  != 0 ) delete[] differentialEquation ;
00794 //             if( transitions           != 0 ) delete[] transitions          ;
00795 //             if( positions             != 0 ) delete[] positions            ;
00796 //             if( constraint            != 0 ) delete   constraint           ;
00797 //             if( dynamicDiscretization != 0 ) delete   dynamicDiscretization;
00798 // 
00799 //             return ACADOERROR(RET_INCOMPATIBLE_DIMENSIONS);
00800 //         }
00801     }
00802         
00803         return SUCCESSFUL_RETURN;
00804 }
00805 
00806 
00807 
00808 returnValue OptimizationAlgorithmBase::initializeOCPiterate(    Constraint* const _constraint,
00809                                                                                                                                 const Grid& _unionGrid,
00810                                                                                                                                 uint nx,
00811                                                                                                                                 uint nxa,
00812                                                                                                                                 uint np,
00813                                                                                                                                 uint nu,
00814                                                                                                                                 uint nw
00815                                                                                                                                 )
00816 {
00817   
00818         uint run1, run2;
00819 
00820     // CONSTRUCT THE OPTIMIZATION VARIABLES:
00821     // -------------------------------------
00822 
00823     iter.clear( );
00824 
00825     if( nx  > 0 ) iter.x  = new VariablesGrid( nx , _unionGrid, VT_DIFFERENTIAL_STATE );
00826     if( nxa > 0 ) iter.xa = new VariablesGrid( nxa, _unionGrid, VT_ALGEBRAIC_STATE );
00827     if( np  > 0 ) iter.p  = new VariablesGrid( np , _unionGrid, VT_PARAMETER );
00828     if( nu  > 0 ) iter.u  = new VariablesGrid( nu , _unionGrid, VT_CONTROL );
00829     if( nw  > 0 ) iter.w  = new VariablesGrid( nw , _unionGrid, VT_DISTURBANCE );
00830 
00831     // CHECK OF BOUND CONSISTENCY:
00832     // ---------------------------
00833 
00834     if( _constraint != 0 ){
00835 
00836         // the following routine initializes the bounds of the optimization
00837         // variables. If no bound is given, a lower bound will be initialized
00838         // with -INFTY while upper bounds are initialized with +INFTY. Note
00839         // that the class Constraint auto-detects bounds. Moreover, if there
00840         // are inconsistent bounds detected, i.e. if there is an upper bound
00841         // smaller that the corresponding lower bound, the routine
00842         // getBounds(...) will return with a corresponding error message which
00843         // should be captured and returned to the user.
00844 
00845         returnValue returnvalue = _constraint->getBounds( iter );
00846         if( returnvalue != SUCCESSFUL_RETURN ){
00847             return ACADOERROR(returnvalue);
00848         }
00849     }
00850 
00851     // LOAD A ROUGH INITIALIZATION FOR ALL VARIABLES FOR THE CASE
00852     // THAT NO FURTHER INITIALIZATION IS AVAILABLE:
00853     // ----------------------------------------------------------
00854 
00855     if( iter.x  != 0 ) iter.x ->initializeFromBounds();   // inititializes the variables with 0 if no bounds are
00856     if( iter.xa != 0 ) iter.xa->initializeFromBounds();   // given. if there is one bound specified this bound
00857     if( iter.p  != 0 ) iter.p ->initializeFromBounds();   // will be used as an initialization. If there is an
00858     if( iter.u  != 0 ) iter.u ->initializeFromBounds();   // upper as well as a lower bound specified, then the
00859     if( iter.w  != 0 ) iter.w ->initializeFromBounds();   // average, i.e. (lb+ub)/2, will be used as an initialization.
00860 
00861 //     printf("OptimizAlg::1 nx = %d \n", nx );
00862 //     printf("OptimizAlg::1 np = %d \n", np );
00863 //     printf("OptimizAlg::1 nu = %d \n", nu );
00864         
00865     // LOAD THE GIVEN INITIALIZATION:
00866     // ------------------------------
00867 
00868     // For the case that the user provides an initialization for one or
00869     // the other variable this additional information from the user should
00870     // be taken into account. The following routine reads the user data in a
00871     // robust way. (Even if the user provides data with wrong dimensions
00872     // the code tries to use it as reasonable as possible.)
00873 
00874     // Added: Copy auto_initialize-values of user initialization
00875 
00876 //      iter.x->print("iter.x");
00877 //      userInit.x->print("userInit.x");
00878 
00879     BooleanType ai;
00880     if( nx > 0 && userInit.x->getNumPoints() > 0 ){
00881         ai=userInit.x->getAutoInit(0);
00882         for( run1 = 0; run1 < _unionGrid.getNumPoints(); run1++ ){
00883             DVector tmp = userInit.x->linearInterpolation( _unionGrid.getTime(run1) );
00884             uint nxx = tmp.getDim();
00885             if( nxx > nx ) nxx = nx;
00886             for( run2 = 0; run2 < nxx; run2++ )
00887                 iter.x->operator()( run1, run2 ) = tmp(run2);
00888                 iter.x->setAutoInit(run1,ai);
00889         }
00890     }
00891     
00892     if( nxa > 0 && userInit.xa->getNumPoints() > 0 ){
00893         ai=userInit.xa->getAutoInit(0);
00894         for( run1 = 0; run1 < _unionGrid.getNumPoints(); run1++ ){
00895             DVector tmp = userInit.xa->linearInterpolation( _unionGrid.getTime(run1) );
00896                         uint nxx = tmp.getDim();
00897             if( nxx > nxa ) nxx = nxa;
00898             for( run2 = 0; run2 < nxx; run2++ )
00899                 iter.xa->operator()( run1, run2 ) = tmp(run2);
00900                         iter.xa->setAutoInit(run1,ai);
00901         }
00902     }   
00903         
00904     if( nu > 0 && userInit.u->getNumPoints() > 0 ){
00905         for( run1 = 0; run1 < _unionGrid.getNumPoints(); run1++ ){
00906             DVector tmp = userInit.u->linearInterpolation( _unionGrid.getTime(run1) );
00907             uint nxx = tmp.getDim();
00908             if( nxx > nu ) nxx = nu;
00909             for( run2 = 0; run2 < nxx; run2++ )
00910                 iter.u->operator()( run1, run2 ) = tmp(run2);
00911         }
00912     }
00913 
00914 
00915     // MAKE SURE THAT THE PARAMETER IS TIME-CONSTANT:
00916     // ----------------------------------------------
00917     
00918         if( np > 0 && userInit.p->getNumPoints() > 0 ){
00919                 iter.p->setAllVectors( userInit.p->getVector(0) );
00920         }
00921 
00922 //     if( np > 0 && userInit.p->getNumPoints() > 0 ){
00923 //         for( run1 = 0; run1 < _unionGrid.getNumPoints(); run1++ ){
00924 //             DVector tmp = userInit.p->getFirstVector( );
00925 //             uint nxx = tmp.getDim();
00926 //             if( nxx > np ) nxx = np;
00927 //             for( run2 = 0; run2 < nxx; run2++ )
00928 //                 iter.p->operator()( run1, run2 ) = tmp(run2);
00929 //         }
00930 //     }
00931 
00932     if( nw > 0 && userInit.w->getNumPoints() > 0 ){
00933         for( run1 = 0; run1 < _unionGrid.getNumPoints(); run1++ ){
00934             DVector tmp = userInit.w->linearInterpolation( _unionGrid.getTime(run1) );
00935             uint nxx = tmp.getDim();
00936             if( nxx > nw ) nxx = nw;
00937             for( run2 = 0; run2 < nxx; run2++ )
00938                 iter.w->operator()( run1, run2 ) = tmp(run2);
00939         }
00940     }
00941 
00942 
00943         return SUCCESSFUL_RETURN;
00944 }
00945 
00946 
00947 
00948 CLOSE_NAMESPACE_ACADO
00949 
00950 
00951 // end of file.


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