integrator.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 
00034 #include <acado/utils/acado_utils.hpp>
00035 #include <acado/symbolic_expression/symbolic_expression.hpp>
00036 #include <acado/function/function_.hpp>
00037 #include <acado/function/differential_equation.hpp>
00038 #include <acado/integrator/integrator.hpp>
00039 #include <acado/integrator/integrator_runge_kutta.hpp>
00040 #include <acado/integrator/integrator_runge_kutta12.hpp>
00041 #include <acado/integrator/integrator_runge_kutta23.hpp>
00042 #include <acado/integrator/integrator_runge_kutta45.hpp>
00043 #include <acado/integrator/integrator_runge_kutta78.hpp>
00044 #include <acado/integrator/integrator_bdf.hpp>
00045 
00046 using namespace std;
00047 
00048 BEGIN_NAMESPACE_ACADO
00049 
00050 
00051 
00052 //
00053 // PUBLIC MEMBER FUNCTIONS:
00054 //
00055 
00056 Integrator::Integrator( )
00057            :AlgorithmicBase( ){
00058 
00059     // RHS:
00060     // --------
00061        rhs = 0;
00062        m   = 0;
00063        ma  = 0;
00064        mdx = 0;
00065        md  = 0;
00066        mn  = 0;
00067        mu  = 0;
00068        mui = 0;
00069        mp  = 0;
00070        mpi = 0;
00071        mw  = 0;
00072 
00073     transition = 0;
00074 
00075 
00076     // SETTINGS:
00077     // ---------
00078     h     = (double*)calloc(1,sizeof(double));
00079     h[0]  = 0.001    ;
00080     hmin  = 0.000001 ;
00081     hmax  = 1.0e10   ;
00082 
00083     tune  = 0.5      ;
00084     TOL   = 0.000001 ;
00085 
00086 
00087     // INTERNAL INDEX LISTS:
00088     // ---------------------
00089     diff_index          = 0;
00090     ddiff_index         = 0;
00091     alg_index           = 0;
00092     control_index       = 0;
00093     parameter_index     = 0;
00094     int_control_index   = 0;
00095     int_parameter_index = 0;
00096     disturbance_index   = 0;
00097     time_index          = 0;
00098 
00099 
00100     // OTHERS:
00101     // -------
00102     count            = 0   ;
00103     count2           = 0   ;
00104     count3           = 0   ;
00105     maxNumberOfSteps = 1000;
00106 
00107 
00108     // PRINT-LEVEL:
00109     // ------------
00110     PrintLevel = LOW;
00111 
00112 
00113     // SEED DIMENSIONS:
00114     // ----------------
00115     nFDirs         = 0;
00116     nBDirs         = 0;
00117     nFDirs2        = 0;
00118     nBDirs2        = 0;
00119 
00120 
00121     // THE STATE OF AGGREGATION:
00122     // -------------------------
00123     soa        = SOA_UNFROZEN;
00124 
00125     setupOptions( );
00126     setupLogging( );
00127 }
00128 
00129 
00130 
00131 Integrator::Integrator( const Integrator &arg )
00132            :AlgorithmicBase( arg ){
00133 
00134     if( arg.transition == 0 )  transition = 0;
00135     else                       transition = new Transition( *arg.transition );
00136 }
00137 
00138 
00139 Integrator::~Integrator( ){
00140 
00141 
00142     // RHS:
00143     // ---------
00144     if( rhs != NULL )
00145         delete rhs;
00146 
00147     if( transition != 0 )
00148         delete transition;
00149 
00150 
00151     // SETTINGS:
00152     // ---------
00153     free(h);
00154 
00155 
00156     // INTERNAL INDEX LISTS:
00157     // ---------------------
00158 
00159     if( diff_index != 0 )
00160         delete[] diff_index;
00161 
00162     if( ddiff_index != 0 )
00163         delete[] ddiff_index;
00164 
00165     if( alg_index != 0 )
00166         delete[] alg_index;
00167 
00168     if( control_index != 0 )
00169         delete[] control_index;
00170 
00171     if( parameter_index != 0 )
00172         delete[] parameter_index;
00173 
00174     if( int_control_index != 0 )
00175         delete[] int_control_index;
00176 
00177     if( int_parameter_index != 0 )
00178         delete[] int_parameter_index;
00179 
00180     if( disturbance_index != 0 )
00181         delete[] disturbance_index;
00182 }
00183 
00184 
00185 returnValue Integrator::init(   const DifferentialEquation &rhs_,
00186                                                                 const Transition           &trs 
00187                                                                 )
00188 {
00189     returnValue returnvalue;
00190 
00191     returnvalue = init( rhs_ );
00192     transition = new Transition(trs);
00193 
00194     return returnvalue;
00195 }
00196 
00197 
00198 returnValue Integrator::setTransition(  const Transition &trs
00199                                                                                 )
00200 {
00201     if( transition != 0 ) delete transition;
00202     transition = new Transition(trs);
00203     return SUCCESSFUL_RETURN;
00204 }
00205 
00206 
00207 returnValue Integrator::integrate(      double t0_  ,
00208                                                                         double tend_,
00209                                                                         double *x0  ,
00210                                                                         double *xa  ,
00211                                                                         double *p   ,
00212                                                                         double *u   ,
00213                                                                         double *w    ){
00214 
00215     Grid t_( t0_, tend_, 2 );
00216     return integrate( t_, x0, xa, p, u, w );
00217 }
00218 
00219 
00220 returnValue Integrator::integrate(      const Grid  &t_,
00221                                                                         double      *x0,
00222                                                                         double      *xa,
00223                                                                         double      *p ,
00224                                                                         double      *u ,
00225                                                                         double      *w  ){
00226 
00227     if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
00228     DVector components = rhs->getDifferentialStateComponents();
00229 
00230     DVector tmpX ( components.getDim(), x0 );
00231     DVector tmpXA( rhs->getNXA()      , xa );
00232     DVector tmpP ( rhs->getNP ()      , p  );
00233     DVector tmpU ( rhs->getNU ()      , u  );
00234     DVector tmpW ( rhs->getNW ()      , w  );
00235 
00236     return integrate( t_, tmpX, tmpXA, tmpP, tmpU, tmpW );
00237 }
00238 
00239 
00240 
00241 returnValue Integrator::integrate(      double       t0_  ,
00242                                                                         double       tend_,
00243                                                                         const DVector &x0  ,
00244                                                                         const DVector &xa  ,
00245                                                                         const DVector &p   ,
00246                                                                         const DVector &u   ,
00247                                                                         const DVector &w    ){
00248 
00249     Grid t_( t0_, tend_, 2 );
00250     return integrate( t_, x0, xa, p, u, w );
00251 }
00252 
00253 
00254 
00255 returnValue Integrator::integrate(      const Grid   &t_  ,
00256                                                                         const DVector &x0  ,
00257                                                                         const DVector &xa  ,
00258                                                                         const DVector &p   ,
00259                                                                         const DVector &u   ,
00260                                                                         const DVector &w    ){
00261 
00262     int run1;
00263     returnValue returnvalue;
00264     if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
00265 
00266     DVector tmpX;
00267 
00268     DVector components = rhs->getDifferentialStateComponents();
00269 
00270     const int N = components.getDim();
00271 
00272     if( x0.getDim() != 0 ){
00273         tmpX.init( components.getDim() );
00274         for( run1 = 0; run1 < (int) components.getDim(); run1++ )
00275             tmpX(run1) = x0((int) components(run1));
00276     }
00277 
00278 
00279 //      tmpX.print( "integrator x0" );
00280 //      u.print( "integrator u0" );
00281 //      p.print( "integrator p0" );
00282     returnvalue = evaluate( tmpX, xa, p, u, w, t_ );
00283 
00284     if( returnvalue != SUCCESSFUL_RETURN )
00285         return returnvalue;
00286 
00287     xE.init(rhs->getDim());
00288     xE.setZero();
00289 
00290     DVector tmp(rhs->getDim());
00291     getProtectedX(&tmp);
00292 
00293     for( run1 = 0; run1 < N; run1++ )
00294         xE((int) components(run1)) = tmp(run1);
00295 
00296     for( run1 = N; run1 < N + ma; run1++ )
00297         xE(run1) = tmp(run1);
00298 
00299     if( transition != 0 )
00300         returnvalue = evaluateTransition( t_.getLastTime(), xE, xa, p, u, w );
00301 
00302     return returnvalue;
00303 }
00304 
00305 
00306 // ======================================================================================
00307 
00308 returnValue Integrator::setForwardSeed( const int    &order,
00309                                                                                 const DVector &xSeed,
00310                                                                                 const DVector &pSeed,
00311                                                                                 const DVector &uSeed,
00312                                                                                 const DVector &wSeed  ){
00313 
00314 
00315     int run1;
00316     if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
00317 
00318     DVector tmpX;
00319     DVector components = rhs->getDifferentialStateComponents();
00320 
00321     dP = pSeed;
00322     dU = uSeed;
00323     dW = wSeed;
00324 
00325     if( xSeed.getDim() != 0 ){
00326 
00327         tmpX.init( components.getDim() );
00328         for( run1 = 0; run1 < (int) components.getDim(); run1++ )
00329              tmpX(run1) = xSeed((int) components(run1));
00330     }
00331 
00332     return setProtectedForwardSeed( tmpX, pSeed, uSeed, wSeed, order );
00333 }
00334 
00335 
00336 // ======================================================================================
00337 
00338 returnValue Integrator::setBackwardSeed(        const int    &order,
00339                                                                                         const DVector &seed   ){
00340 
00341     dXb = seed;
00342 
00343     uint run1;
00344     DVector tmp( seed.getDim() );
00345     DVector components = rhs->getDifferentialStateComponents();
00346 
00347     if( seed.getDim() != 0 ){
00348         tmp.init( components.getDim() );
00349         for( run1 = 0; run1 < components.getDim(); run1++ )
00350              tmp(run1) = seed((int) components(run1));
00351     }
00352     return setProtectedBackwardSeed( tmp, order );
00353 }
00354 
00355 
00356 
00357 returnValue Integrator::integrateSensitivities( ){
00358 
00359     uint run1;
00360     returnValue returnvalue;
00361 
00362 
00363     if( ( nBDirs > 0 || nBDirs2 > 0 ) && transition != 0 ){
00364 
00365         int order;
00366         if( nBDirs2 > 0 ) order = 2;
00367         else              order = 1;
00368 
00369         returnvalue = diffTransitionBackward( dXb, dPb, dUb, dWb, order );
00370 
00371         setBackwardSeed( order, dXb );
00372 
00373         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00374     }
00375 
00376     returnvalue = evaluateSensitivities();
00377 
00378     if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00379 
00380 
00381     if( nBDirs > 0 || nBDirs2 > 0 ) return SUCCESSFUL_RETURN;
00382 
00383     int order = 1;
00384     if( nFDirs2 > 0 ) order = 2;
00385 
00386     DMatrix tmp( rhs->getDim(), 1 );
00387     returnvalue = getProtectedForwardSensitivities(&tmp,order);
00388 
00389     DVector components = rhs->getDifferentialStateComponents();
00390 
00391     dX.init(rhs->getDim()-ma);
00392     dX.setZero();
00393 
00394     for( run1 = 0; run1 < components.getDim(); run1++ )
00395         dX((int) components(run1)) = tmp(run1,0);
00396 
00397     if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00398 
00399     if( transition != 0 )
00400         returnvalue = diffTransitionForward( dX, dP, dU, dW, order );
00401 
00402     return returnvalue;
00403 }
00404 
00405 
00406 returnValue Integrator::getForwardSensitivities(        DVector &Dx,
00407                                                                                                         int order ) const{
00408 
00409     Dx = dX;
00410     return SUCCESSFUL_RETURN;
00411 }
00412 
00413 
00414 returnValue Integrator::getForwardSensitivities(        VariablesGrid &Dx,
00415                                                                                                         int order ) const{
00416 
00417    if( order == 1 ) Dx =  dxStore;
00418    if( order == 2 ) Dx = ddxStore;
00419 
00420    return SUCCESSFUL_RETURN;
00421 }
00422 
00423 
00424 returnValue Integrator::getBackwardSensitivities(       DVector &DX,
00425                                                                                                         DVector &DP ,
00426                                                                                                         DVector &DU ,
00427                                                                                                         DVector &DW ,
00428                                                                                                         int    order   ) const{
00429 
00430     int run2;
00431     returnValue returnvalue;
00432 
00433     DVector tmpX ( rhs->getDim() );
00434 
00435     DX.setZero();
00436     DP.setZero();
00437     DU.setZero();
00438     DW.setZero();
00439 
00440     returnvalue = getProtectedBackwardSensitivities( tmpX, DP, DU, DW, order );
00441     DVector components = rhs->getDifferentialStateComponents();
00442 
00443     for( run2 = 0; run2 < (int) components.getDim(); run2++ )
00444         DX((int) components(run2)) = tmpX(run2);
00445 
00446     for( run2 = 0; run2 < (int) dPb.getDim(); run2++ )
00447         DP(run2) += dPb(run2);
00448 
00449     for( run2 = 0; run2 < (int) dUb.getDim(); run2++ )
00450         DU(run2) += dUb(run2);
00451 
00452     for( run2 = 0; run2 < (int) dWb.getDim(); run2++ )
00453         DW(run2) += dWb(run2);
00454 
00455     return returnvalue;
00456 }
00457 
00458 
00459 BooleanType Integrator::canHandleImplicitSwitches( ) const{
00460 
00461     return BT_FALSE;
00462 }
00463 
00464 
00465 BooleanType Integrator::isDifferentialEquationDefined( ) const{
00466 
00467     if ( rhs != 0 ) return BT_TRUE ;
00468     else            return BT_FALSE;
00469 }
00470 
00471 BooleanType Integrator::isDifferentialEquationAffine( ) const
00472 {
00473     if ( rhs == 0 ) return BT_FALSE ;
00474     else            return rhs->isAffine();
00475 }
00476 
00477 
00478 double Integrator::getDifferentialEquationSampleTime( ) const
00479 {
00480         if ( rhs == 0 )
00481                 return -INFTY;
00482         else
00483                 return rhs->getStepLength( );
00484 }
00485 
00486 
00487 
00488 returnValue Integrator::deleteAllSeeds(){
00489 
00490     nBDirs  = 0;
00491     nFDirs  = 0;
00492     nBDirs2 = 0;
00493     nFDirs2 = 0;
00494 
00495     return SUCCESSFUL_RETURN;
00496 }
00497 
00498 
00499 
00500 
00501 //
00502 // PROTECTED MEMBER FUNCTIONS:
00503 //
00504 
00505 returnValue Integrator::evaluateTransition( const double time, DVector &xd, const DVector &xa,
00506                                             const DVector &p, const DVector &u, const DVector &w ){
00507 
00508     ASSERT( transition != 0 );
00509     EvaluationPoint z( *transition, xd.getDim(), xa.getDim(), p.getDim(), u.getDim(), w.getDim() );
00510     z.setT ( time );
00511     z.setX ( xd   );
00512     z.setXA( xa   );
00513     z.setP ( p    );
00514     z.setU ( u    );
00515     z.setW ( w    );
00516     xd = transition->evaluate( z );
00517     return SUCCESSFUL_RETURN;
00518 }
00519 
00520 
00521 returnValue Integrator::diffTransitionForward(       DVector &DX,
00522                                                const DVector &DP,
00523                                                const DVector &DU,
00524                                                const DVector &DW,
00525                                                const int    &order ){
00526 
00527     ASSERT( transition != 0 );
00528     EvaluationPoint z( *transition, DX.getDim(), 0, DP.getDim(), DU.getDim(), DW.getDim() );
00529     z.setX ( DX );
00530     z.setP ( DP );
00531     z.setU ( DU );
00532     z.setW ( DW );
00533 
00534     if( order == 1 ) DX = transition->AD_forward( z );
00535 
00536     if( order != 1 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00537 
00538     return SUCCESSFUL_RETURN;
00539 }
00540 
00541 
00542 returnValue Integrator::diffTransitionBackward( DVector &DX,
00543                                                 DVector &DP,
00544                                                 DVector &DU,
00545                                                 DVector &DW,
00546                                                 int    &order ){
00547 
00548     ASSERT( transition != 0 );
00549     if( order != 1 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00550 
00551     EvaluationPoint z( *transition, DX.getDim(), 0, DP.getDim(), DU.getDim(), DW.getDim() );
00552 
00553     transition->AD_backward( DX, z );
00554 
00555     DX = z.getX();
00556     DP = z.getP();
00557     DU = z.getU();
00558     DW = z.getW();
00559 
00560     return SUCCESSFUL_RETURN;
00561 }
00562 
00563 
00564 
00565 
00566 returnValue Integrator::setupOptions( )
00567 {
00568         addOption( MAX_NUM_INTEGRATOR_STEPS    , defaultMaxNumSteps             );
00569         addOption( INTEGRATOR_TOLERANCE        , defaultIntegratorTolerance     );
00570         addOption( ABSOLUTE_TOLERANCE          , defaultAbsoluteTolerance       );
00571         addOption( INITIAL_INTEGRATOR_STEPSIZE , defaultInitialStepsize         );
00572         addOption( MIN_INTEGRATOR_STEPSIZE     , defaultMinStepsize             );
00573         addOption( MAX_INTEGRATOR_STEPSIZE     , defaultMaxStepsize             );
00574         addOption( STEPSIZE_TUNING             , defaultStepsizeTuning          );
00575         addOption( CORRECTOR_TOLERANCE         , defaultCorrectorTolerance      );
00576         addOption( INTEGRATOR_PRINTLEVEL       , defaultIntegratorPrintlevel    );
00577         addOption( LINEAR_ALGEBRA_SOLVER       , defaultLinearAlgebraSolver     );
00578         addOption( ALGEBRAIC_RELAXATION        , defaultAlgebraicRelaxation     );
00579         addOption( RELAXATION_PARAMETER        , defaultRelaxationParameter     );
00580         addOption( PRINT_INTEGRATOR_PROFILE    , defaultprintIntegratorProfile  );
00581         
00582         return SUCCESSFUL_RETURN;
00583 }
00584 
00585 
00586 
00587 void Integrator::initializeOptions(){
00588 
00589     get( MAX_NUM_INTEGRATOR_STEPS         , maxNumberOfSteps  );
00590     get( INTEGRATOR_TOLERANCE  , TOL               );
00591     get( INITIAL_INTEGRATOR_STEPSIZE      , hini              );
00592     get( MIN_INTEGRATOR_STEPSIZE          , hmin              );
00593     get( MAX_INTEGRATOR_STEPSIZE          , hmax              );
00594     get( STEPSIZE_TUNING       , tune              );
00595     get( INTEGRATOR_PRINTLEVEL , PrintLevel        );
00596     get( LINEAR_ALGEBRA_SOLVER , las               );
00597 }
00598 
00599 returnValue Integrator::setupLogging( ){
00600 
00601     LogRecord tmp(LOG_AT_EACH_ITERATION, PS_DEFAULT);
00602 
00603     tmp.addItem( LOG_TIME_INTEGRATOR,                              "INTEGRATION TIME                 [sec]:  ");
00604     tmp.addItem( LOG_NUMBER_OF_INTEGRATOR_STEPS,                   "NUMBER OF STEPS                       :  ");
00605     tmp.addItem( LOG_NUMBER_OF_INTEGRATOR_REJECTED_STEPS,          "NUMBER OF REJECTED STEPS              :  ");
00606     tmp.addItem( LOG_NUMBER_OF_INTEGRATOR_FUNCTION_EVALUATIONS,    "NUMBER OF RHS EVALUATIONS             :  ");
00607     tmp.addItem( LOG_NUMBER_OF_BDF_INTEGRATOR_JACOBIAN_EVALUATIONS,"NUMBER OF JACOBIAN EVALUATIONS        :  ");
00608     tmp.addItem( LOG_TIME_INTEGRATOR_FUNCTION_EVALUATIONS,         "TIME FOR RHS EVALUATIONS         [sec]:  ");
00609     tmp.addItem( LOG_TIME_BDF_INTEGRATOR_JACOBIAN_EVALUATION,      "TIME FOR JACOBIAN EVALUATIONS    [sec]:  ");
00610     tmp.addItem( LOG_TIME_BDF_INTEGRATOR_JACOBIAN_DECOMPOSITION,   "TIME FOR JACOBIAN DECOMPOSITIONS [sec]:  ");
00611 
00612     outputLoggingIdx = addLogRecord( tmp );
00613 
00614     return SUCCESSFUL_RETURN;
00615 }
00616 
00617 
00618 int Integrator::getDimX() const{
00619 
00620     return getDim();
00621 }
00622 
00623 
00624 returnValue Integrator::printRunTimeProfile() const{
00625 
00626         return printLogRecord(cout, outputLoggingIdx, PRINT_LAST_ITER);
00627 }
00628 
00629 
00630 CLOSE_NAMESPACE_ACADO
00631 
00632 // end of file.


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