integration_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 
00027 
00035 #include <acado/dynamic_discretization/integration_algorithm.hpp>
00036 
00037 
00038 
00039 BEGIN_NAMESPACE_ACADO
00040 
00041 
00042 
00043 //
00044 // PUBLIC MEMBER FUNCTIONS:
00045 //
00046 
00047 
00048 IntegrationAlgorithm::IntegrationAlgorithm() : UserInteraction( )
00049 {
00050         setupOptions( );
00051         setupLogging( );
00052 
00053         integrationMethod = new ShootingMethod( this );
00054         
00055         setStatus( BS_NOT_INITIALIZED );
00056 }
00057 
00058 
00059 IntegrationAlgorithm::IntegrationAlgorithm ( const IntegrationAlgorithm& arg ) : UserInteraction( arg )
00060 {
00061         // take care of userInteraction pointer!!!
00062         if ( arg.integrationMethod != 0 )
00063                 integrationMethod = new ShootingMethod( *(arg.integrationMethod) );
00064         else
00065                 integrationMethod = 0;
00066         
00067         iter = arg.iter;
00068 }
00069 
00070 
00071 IntegrationAlgorithm::~IntegrationAlgorithm( )
00072 {
00073         if ( integrationMethod != 0 )
00074                 delete integrationMethod;
00075 }
00076 
00077 
00078 IntegrationAlgorithm& IntegrationAlgorithm::operator=( const IntegrationAlgorithm& arg )
00079 {
00080         if ( this != &arg )
00081         {
00082                 if ( integrationMethod != 0 )
00083                         delete integrationMethod;
00084                 
00085                 // take care of userInteraction pointer!!!
00086                 if ( arg.integrationMethod != 0 )
00087                         integrationMethod = new ShootingMethod( *(arg.integrationMethod) );
00088                 else
00089                         integrationMethod = 0;
00090                 
00091                 iter = arg.iter;
00092         }
00093 
00094         return *this;
00095 }
00096 
00097 
00098 returnValue IntegrationAlgorithm::addStage( const DynamicSystem  &dynamicSystem_,
00099                                             const Grid           &stageIntervals,
00100                                             const IntegratorType &integratorType_ ){
00101 
00102     return integrationMethod->addStage( dynamicSystem_, stageIntervals, integratorType_ );
00103 }
00104 
00105 
00106 returnValue IntegrationAlgorithm::addTransition( const Transition& transition_ ){
00107 
00108     return integrationMethod->addTransition( transition_ );
00109 }
00110 
00111 
00112 returnValue IntegrationAlgorithm::clear(){
00113 
00114     return integrationMethod->clear( );
00115 }
00116 
00117 
00118 
00119 returnValue IntegrationAlgorithm::evaluate( VariablesGrid  *x ,
00120                                             VariablesGrid  *xa,
00121                                             VariablesGrid  *p ,
00122                                             VariablesGrid  *u ,
00123                                             VariablesGrid  *w   ){
00124 
00125     iter.init( x,xa,p,u,w );
00126     return evaluate( iter );
00127 }
00128 
00129 
00130 returnValue IntegrationAlgorithm::evaluate( OCPiterate& _iter ){
00131 
00132         if ( integrationMethod->evaluate( _iter ) != SUCCESSFUL_RETURN )
00133                 return ACADOERROR( RET_INTALG_INTEGRATION_FAILED );
00134 
00135         return plot( );
00136 }
00137 
00138 
00139 
00140 returnValue IntegrationAlgorithm::integrate(    VariablesGrid  *x ,
00141                                                                                                 VariablesGrid  *xa,
00142                                                                                                 VariablesGrid  *p ,
00143                                                                                                 VariablesGrid  *u ,
00144                                                                                                 VariablesGrid  *w
00145                                                                                                 ){
00146 
00147     return evaluate( x,xa,p,u,w );
00148 }
00149 
00150 
00151 returnValue IntegrationAlgorithm::integrate(    double       t0,
00152                                                                                                 double       tend,
00153                                                                                                 const DVector &x0,
00154                                                                                                 const DVector &xa,
00155                                                                                                 const DVector &p,
00156                                                                                                 const DVector &u,
00157                                                                                                 const DVector &w
00158                                                                                                 )
00159 {
00160         Grid grid( t0,tend,2 );
00161 
00162         VariablesGrid  xGrid( x0,grid );
00163         VariablesGrid xaGrid( xa,grid );
00164         VariablesGrid  pGrid(  p,grid );
00165         VariablesGrid  uGrid(  u,grid );
00166         VariablesGrid  wGrid(  w,grid );
00167 
00168         return integrate( &xGrid,&xaGrid,&pGrid,&uGrid,&wGrid );
00169 }
00170 
00171 
00172 returnValue IntegrationAlgorithm::integrate(    const Grid& t,
00173                                                                                                 const DVector &x0,
00174                                                                                                 const DVector &xa,
00175                                                                                                 const DVector &p,
00176                                                                                                 const DVector &u,
00177                                                                                                 const DVector &w
00178                                                                                                 )
00179 {
00180         //Grid grid( t.getFirstTime(),t.getLastTime(),2 );
00181 
00182         VariablesGrid  xGrid( x0,t );
00183         VariablesGrid xaGrid( xa,t );
00184         VariablesGrid  pGrid(  p,t );
00185         VariablesGrid  uGrid(  u,t );
00186         VariablesGrid  wGrid(  w,t );
00187 
00188         return integrate( &xGrid,&xaGrid,&pGrid,&uGrid,&wGrid );
00189 }
00190 
00191 
00192 
00193 returnValue IntegrationAlgorithm::setForwardSeed(       const BlockMatrix &xSeed_,
00194                                                                                                         const BlockMatrix &pSeed_,
00195                                                                                                         const BlockMatrix &uSeed_,
00196                                                                                                         const BlockMatrix &wSeed_
00197                                                                                                         )
00198 {
00199     return integrationMethod->setForwardSeed( xSeed_,pSeed_,uSeed_,wSeed_ );
00200 }
00201 
00202 
00203 returnValue IntegrationAlgorithm::setForwardSeed(       const DVector &xSeed,
00204                                                                                                         const DVector &pSeed,
00205                                                                                                         const DVector &uSeed,
00206                                                                                                         const DVector &wSeed
00207                                                                                                         )
00208 {
00209         BlockMatrix xSeedTmp( (DMatrix)xSeed );
00210         BlockMatrix pSeedTmp( (DMatrix)pSeed );
00211         BlockMatrix uSeedTmp( (DMatrix)uSeed );
00212         BlockMatrix wSeedTmp( (DMatrix)wSeed );
00213 
00214         return integrationMethod->setForwardSeed( xSeedTmp,pSeedTmp,uSeedTmp,wSeedTmp );
00215 }
00216 
00217 
00218 returnValue IntegrationAlgorithm::setUnitForwardSeed( )
00219 {
00220     return integrationMethod->setUnitForwardSeed( );
00221 }
00222 
00223 
00224 returnValue IntegrationAlgorithm::setBackwardSeed(      const BlockMatrix &seed
00225                                                                                                         )
00226 {
00227     return integrationMethod->setBackwardSeed( seed );
00228 }
00229 
00230 
00231 returnValue IntegrationAlgorithm::setBackwardSeed(      const DVector &seed
00232                                                                                                         )
00233 {
00234         DMatrix seedTmpMatrix( seed );
00235         seedTmpMatrix.transposeInPlace();
00236         BlockMatrix seedTmp( seedTmpMatrix );
00237 
00238         return integrationMethod->setBackwardSeed( seedTmp );
00239 }
00240 
00241 
00242 returnValue IntegrationAlgorithm::setUnitBackwardSeed( )
00243 {
00244     return integrationMethod->setUnitBackwardSeed( );
00245 }
00246 
00247 
00248 
00249 returnValue IntegrationAlgorithm::deleteAllSeeds( )
00250 {
00251     return integrationMethod->deleteAllSeeds( );
00252 }
00253 
00254 
00255 
00256 returnValue IntegrationAlgorithm::evaluateSensitivities( )
00257 {
00258     return integrationMethod->evaluateSensitivities( );
00259 }
00260 
00261 
00262 returnValue IntegrationAlgorithm::integrateSensitivities( )
00263 {
00264     return evaluateSensitivities( );
00265 }
00266 
00267 
00268 returnValue IntegrationAlgorithm::evaluateSensitivities( const BlockMatrix &seed, BlockMatrix &hessian )
00269 {
00270     return integrationMethod->evaluateSensitivities( seed,hessian );
00271 }
00272 
00273 
00274 returnValue IntegrationAlgorithm::integrateSensitivities( const BlockMatrix &seed, BlockMatrix &hessian )
00275 {
00276     return evaluateSensitivities( seed,hessian );
00277 }
00278 
00279 
00280 
00281 returnValue IntegrationAlgorithm::unfreeze()
00282 {
00283         return integrationMethod->unfreeze( );
00284 }
00285 
00286 
00287 BooleanType IntegrationAlgorithm::isAffine( ) const
00288 {
00289         return integrationMethod->isAffine( );
00290 }
00291 
00292 
00293 returnValue IntegrationAlgorithm::getX( DVector& xEnd
00294                                                                                 ) const
00295 {
00296         if ( iter.x != 0 )
00297                 xEnd = iter.x->getLastVector( );
00298 
00299         return SUCCESSFUL_RETURN;
00300 }
00301 
00302 
00303 returnValue IntegrationAlgorithm::getXA(        DVector& xaEnd
00304                                                                                         ) const
00305 {
00306         if ( iter.xa != 0 )
00307                 xaEnd = iter.xa->getLastVector( );
00308 
00309         return SUCCESSFUL_RETURN;
00310 }
00311 
00312 
00313 returnValue IntegrationAlgorithm::getX( VariablesGrid& X
00314                                                                                 ) const
00315 {
00316         if ( iter.x != 0 )
00317                 X = *(iter.x);
00318 
00319         return SUCCESSFUL_RETURN;
00320 }
00321 
00322 
00323 returnValue IntegrationAlgorithm::getXA(        VariablesGrid& XA
00324                                                                                         ) const
00325                                                         
00326 {
00327         if ( iter.xa != 0 )
00328                 XA = *(iter.xa);
00329 
00330         return SUCCESSFUL_RETURN;
00331 }
00332 
00333 
00334 
00335 returnValue IntegrationAlgorithm::getForwardSensitivities(      BlockMatrix &D
00336                                                                                                                         ) const
00337 {
00338         return integrationMethod->getForwardSensitivities( D );
00339 }
00340 
00341 
00342 returnValue IntegrationAlgorithm::getForwardSensitivities(      DVector &Dx
00343                                                                                                                         ) const
00344 {
00345         BlockMatrix DxTmp;
00346         
00347         returnValue returnvalue = integrationMethod->getForwardSensitivities( DxTmp );
00348         if ( returnvalue != SUCCESSFUL_RETURN )
00349                 return returnvalue;
00350 
00351         if ( DxTmp.isEmpty( ) == BT_FALSE )
00352         {
00353                 DMatrix DxTmpMatrix;
00354                 DxTmp.getSubBlock( 0,0,DxTmpMatrix );
00355                 
00356                 if ( DxTmpMatrix.getNumCols( ) > 0 )
00357                         Dx = DxTmpMatrix.getCol( 0 );
00358         }
00359 
00360         return SUCCESSFUL_RETURN;
00361 }
00362 
00363 
00364 
00365 returnValue IntegrationAlgorithm::getBackwardSensitivities(     BlockMatrix &D
00366                                                                                                                         ) const
00367 {
00368         return integrationMethod->getBackwardSensitivities( D );
00369 }
00370 
00371 
00372 returnValue IntegrationAlgorithm::getBackwardSensitivities(     DVector &Dx_x0,
00373                                                                                                                         DVector &Dx_p ,
00374                                                                                                                         DVector &Dx_u ,
00375                                                                                                                         DVector &Dx_w
00376                                                                                                                         ) const
00377 {
00378         BlockMatrix DxTmp;
00379         
00380         returnValue returnvalue = integrationMethod->getBackwardSensitivities( DxTmp );
00381         if ( returnvalue != SUCCESSFUL_RETURN )
00382                 return returnvalue;
00383 
00384         if ( DxTmp.isEmpty( ) == BT_FALSE )
00385         {
00386                 DMatrix DxTmpMatrix;
00387                 
00388                 DxTmp.getSubBlock( 0,0,DxTmpMatrix );
00389                 if ( DxTmpMatrix.getNumRows( ) > 0 )
00390                         Dx_x0 = DxTmpMatrix.getRow( 0 );
00391                 else
00392                         Dx_x0.init( );
00393                 
00394                 DxTmp.getSubBlock( 0,2,DxTmpMatrix );
00395                 if ( DxTmpMatrix.getNumRows( ) > 0 )
00396                         Dx_p = DxTmpMatrix.getRow( 0 );
00397                 else
00398                         Dx_p.init( );
00399 
00400                 DxTmp.getSubBlock( 0,3,DxTmpMatrix );
00401                 if ( DxTmpMatrix.getNumRows( ) > 0 )
00402                         Dx_u = DxTmpMatrix.getRow( 0 );
00403                 else
00404                         Dx_u.init( );
00405 
00406                 DxTmp.getSubBlock( 0,4,DxTmpMatrix );
00407                 if ( DxTmpMatrix.getNumRows( ) > 0 )
00408                         Dx_w = DxTmpMatrix.getRow( 0 );
00409                 else
00410                         Dx_w.init( );
00411         }
00412 
00413         return SUCCESSFUL_RETURN;
00414 }
00415 
00416 
00417 
00418 //
00419 // PROTECTED MEMBER FUNCTIONS:
00420 //
00421 
00422 returnValue IntegrationAlgorithm::setupOptions( )
00423 {
00424         //printf( "IntegrationAlgorithm::setupOptions( ) called.\n" );
00425         
00426         // add integration options
00427         addOption( FREEZE_INTEGRATOR           , BT_FALSE                       );
00428         addOption( INTEGRATOR_TYPE             , INT_BDF                        );
00429         addOption( FEASIBILITY_CHECK           , defaultFeasibilityCheck        );
00430         addOption( PLOT_RESOLUTION             , defaultPlotResoltion           );
00431         
00432         // add integrator options
00433         addOption( MAX_NUM_INTEGRATOR_STEPS    , defaultMaxNumSteps             );
00434         addOption( INTEGRATOR_TOLERANCE        , defaultIntegratorTolerance     );
00435         addOption( ABSOLUTE_TOLERANCE          , defaultAbsoluteTolerance       );
00436         addOption( INITIAL_INTEGRATOR_STEPSIZE , defaultInitialStepsize         );
00437         addOption( MIN_INTEGRATOR_STEPSIZE     , defaultMinStepsize             );
00438         addOption( MAX_INTEGRATOR_STEPSIZE     , defaultMaxStepsize             );
00439         addOption( STEPSIZE_TUNING             , defaultStepsizeTuning          );
00440         addOption( CORRECTOR_TOLERANCE         , defaultCorrectorTolerance      );
00441         addOption( INTEGRATOR_PRINTLEVEL       , defaultIntegratorPrintlevel    );
00442         addOption( LINEAR_ALGEBRA_SOLVER       , defaultLinearAlgebraSolver     );
00443         addOption( ALGEBRAIC_RELAXATION        , defaultAlgebraicRelaxation     );
00444         addOption( RELAXATION_PARAMETER        , defaultRelaxationParameter     );
00445         addOption( PRINT_INTEGRATOR_PROFILE    , defaultprintIntegratorProfile  );
00446 
00447         return SUCCESSFUL_RETURN;
00448 }
00449 
00450 
00451 returnValue IntegrationAlgorithm::setupLogging( )
00452 {
00453     LogRecord tmp( LOG_AT_END );
00454 
00455     tmp.addItem( LOG_DIFFERENTIAL_STATES      );
00456     tmp.addItem( LOG_ALGEBRAIC_STATES         );
00457     tmp.addItem( LOG_PARAMETERS               );
00458     tmp.addItem( LOG_CONTROLS                 );
00459     tmp.addItem( LOG_DISTURBANCES             );
00460     tmp.addItem( LOG_INTERMEDIATE_STATES      );
00461 
00462     tmp.addItem( LOG_DISCRETIZATION_INTERVALS );
00463     tmp.addItem( LOG_STAGE_BREAK_POINTS       );
00464 
00465     logIdx = addLogRecord( tmp );
00466 
00467         return SUCCESSFUL_RETURN;
00468 }
00469 
00470 
00471 CLOSE_NAMESPACE_ACADO
00472 
00473 // end of file.


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