integrator_runge_kutta.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/matrix_vector/matrix_vector.hpp>
00036 #include <acado/symbolic_expression/symbolic_expression.hpp>
00037 #include <acado/function/function_.hpp>
00038 #include <acado/function/differential_equation.hpp>
00039 #include <acado/integrator/integrator.hpp>
00040 #include <acado/integrator/integrator_runge_kutta.hpp>
00041 
00042 using namespace std;
00043 
00044 BEGIN_NAMESPACE_ACADO
00045 
00046 
00047 //
00048 // PUBLIC MEMBER FUNCTIONS:
00049 //
00050 
00051 IntegratorRK::IntegratorRK( )
00052              :Integrator( ){
00053 
00054     initializeVariables();
00055 }
00056 
00057 
00058 IntegratorRK::IntegratorRK( int dim_ , double power_)
00059              :Integrator( ){
00060 
00061     int run1;
00062 
00063     initializeVariables();
00064     dim       = dim_  ;
00065     err_power = power_;
00066 
00067     // BUTCHER TABLEAU:
00068     // ----------------
00069     A  = new double*[dim];
00070     b4 = new double [dim];
00071     b5 = new double [dim];
00072     c  = new double [dim];
00073 
00074     for( run1 = 0; run1 < dim; run1++ ){
00075         A[run1] = new double[dim];
00076     }
00077 }
00078 
00079 
00080 IntegratorRK::IntegratorRK( const DifferentialEquation& rhs_, int dim_, double power_ )
00081              :Integrator( ){
00082 
00083     int run1;
00084 
00085     dim       = dim_  ;
00086     err_power = power_;
00087 
00088     // BUTCHER TABLEAU:
00089     // ----------------
00090     A  = new double*[dim];
00091     b4 = new double [dim];
00092     b5 = new double [dim];
00093     c  = new double [dim];
00094 
00095     for( run1 = 0; run1 < dim; run1++ ){
00096         A[run1] = new double[dim];
00097     }
00098 
00099     init( rhs_ );
00100 }
00101 
00102 
00103 IntegratorRK::IntegratorRK( const IntegratorRK& arg )
00104              :Integrator( arg ){
00105 
00106     constructAll( arg );
00107 }
00108 
00109 
00110 IntegratorRK::~IntegratorRK( ){
00111 
00112     deleteAll();
00113 }
00114 
00115 
00116 returnValue IntegratorRK::init( const DifferentialEquation &rhs_ ){
00117 
00118     rhs = new DifferentialEquation( rhs_ );
00119     m   = rhs->getDim ();
00120     ma  = 0;
00121     mn  = rhs->getN   ();
00122     mu  = rhs->getNU  ();
00123     mui = rhs->getNUI ();
00124     mp  = rhs->getNP  ();
00125     mpi = rhs->getNPI ();
00126     mw  = rhs->getNW  ();
00127 
00128     allocateMemory();
00129 
00130     return SUCCESSFUL_RETURN;
00131 }
00132 
00133 
00134 void IntegratorRK::initializeVariables(){
00135 
00136     dim = 0; A = 0; b4 = 0; b5 = 0; c = 0;
00137     eta4 = 0; eta5 = 0; eta4_ = 0; eta5_ = 0;
00138     k = 0; k2 = 0; l = 0; l2 = 0; x = 0;
00139 
00140     G = 0; etaG = 0;
00141     G2 = 0; G3 = 0; etaG2 = 0; etaG3 = 0;
00142 
00143     H = 0; etaH = 0; H2 = 0; H3 = 0;
00144     etaH2 = 0; etaH3 = 0;
00145 
00146     maxAlloc  = 0;
00147     err_power = 1.0;
00148 }
00149 
00150 
00151 void IntegratorRK::allocateMemory( ){
00152 
00153     int run1, run2;
00154 
00155     if( 0 != rhs->getNXA() ){
00156         ACADOERROR(RET_RK45_CAN_NOT_TREAT_DAE);
00157         ASSERT(1 == 0);
00158     }
00159 
00160     if( 0 != rhs->getNDX() ){
00161         ACADOERROR(RET_RK45_CAN_NOT_TREAT_DAE);
00162         ASSERT(1 == 0);
00163     }
00164 
00165 
00166     if( m < 1 ){
00167         ACADOERROR(RET_TRIVIAL_RHS);
00168         ASSERT(1 == 0);
00169     }
00170 
00171     // RK-ALGORITHM:
00172     // -------------
00173     eta4  = new double [m];
00174     eta5  = new double [m];
00175     eta4_ = new double [m];
00176     eta5_ = new double [m];
00177 
00178     for( run1 = 0; run1 < m; run1++ ){
00179 
00180         eta4 [run1] = 0.0;
00181         eta5 [run1] = 0.0;
00182         eta4_[run1] = 0.0;
00183         eta5_[run1] = 0.0;
00184     }
00185 
00186     k     = new double*[dim];
00187     k2    = new double*[dim];
00188     l     = new double*[dim];
00189     l2    = new double*[dim];
00190     x     = new double [rhs->getNumberOfVariables() + 1 + m];
00191 
00192     for( run1 = 0; run1 < rhs->getNumberOfVariables() + 1 + m; run1++ ){
00193         x[run1] = 0.0;
00194     }
00195 
00196     t     = 0.0;
00197 
00198 
00199     for( run1 = 0; run1 < dim; run1++ ){
00200         k    [run1] = new double[m];
00201         k2   [run1] = new double[m];
00202 
00203         for( run2 = 0; run2 < m; run2++ ){
00204             k [run1][run2] = 0.0;
00205             k2[run1][run2] = 0.0;
00206         }
00207 
00208         l    [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
00209         l2   [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
00210 
00211         for( run2 = 0; run2 < rhs->getNumberOfVariables() + 1 + m; run2++ ){
00212             l [run1][run2] = 0.0;
00213             l2[run1][run2] = 0.0;
00214         }
00215     }
00216 
00217     // INTERNAL INDEX LISTS:
00218     // ---------------------
00219     diff_index = new int[m];
00220 
00221     for( run1 = 0; run1 < m; run1++ ){
00222         diff_index[run1] = rhs->getStateEnumerationIndex( run1 );
00223         if( diff_index[run1] == rhs->getNumberOfVariables() ){
00224             diff_index[run1] = diff_index[run1] + 1 + run1;
00225         }
00226     }
00227 
00228     control_index       = new int[mu ];
00229 
00230     for( run1 = 0; run1 < mu; run1++ ){
00231         control_index[run1] = rhs->index( VT_CONTROL, run1 );
00232     }
00233 
00234     parameter_index     = new int[mp ];
00235 
00236     for( run1 = 0; run1 < mp; run1++ ){
00237         parameter_index[run1] = rhs->index( VT_PARAMETER, run1 );
00238     }
00239 
00240     int_control_index   = new int[mui];
00241 
00242     for( run1 = 0; run1 < mui; run1++ ){
00243         int_control_index[run1] = rhs->index( VT_INTEGER_CONTROL, run1 );
00244     }
00245 
00246     int_parameter_index = new int[mpi];
00247 
00248     for( run1 = 0; run1 < mpi; run1++ ){
00249         int_parameter_index[run1] = rhs->index( VT_INTEGER_PARAMETER, run1 );
00250     }
00251 
00252     disturbance_index   = new int[mw ];
00253 
00254     for( run1 = 0; run1 < mw; run1++ ){
00255         disturbance_index[run1] = rhs->index( VT_DISTURBANCE, run1 );
00256     }
00257 
00258     time_index = rhs->index( VT_TIME, 0 );
00259 
00260     diff_scale.init(m);
00261     for( run1 = 0; run1 < m; run1++ )
00262         diff_scale(run1) = rhs->scale( VT_DIFFERENTIAL_STATE, run1 );
00263 
00264 
00265     // SENSITIVITIES:
00266     // --------------
00267     G          = NULL;
00268     etaG       = NULL;
00269 
00270     G2         = NULL;
00271     G3         = NULL;
00272     etaG2      = NULL;
00273     etaG3      = NULL;
00274 
00275     H          = NULL;
00276     etaH       = NULL;
00277 
00278     H2         = NULL;
00279     H3         = NULL;
00280     etaH2      = NULL;
00281     etaH3      = NULL;
00282 
00283 
00284     // STORAGE:
00285     // --------
00286     maxAlloc = 1;
00287 }
00288 
00289 
00290 
00291 void IntegratorRK::deleteAll(){
00292 
00293     int run1;
00294 
00295 
00296     // BUTCHER-
00297     // TABLEAU:
00298     // ----------
00299     for( run1 = 0; run1 < dim; run1++ ){
00300         delete[] A[run1];
00301     }
00302 
00303     delete[] A;
00304     delete[] b4;
00305     delete[] b5;
00306     delete[] c ;
00307 
00308 
00309     // RK-ALGORITHM:
00310     // -------------
00311     if( eta4 != NULL ){
00312         delete[] eta4;
00313     }
00314     if( eta4_ != NULL ){
00315         delete[] eta4_;
00316     }
00317     if( eta5 != NULL ){
00318         delete[] eta5;
00319     }
00320     if( eta5_ != NULL ){
00321         delete[] eta5_;
00322     }
00323 
00324     for( run1 = 0; run1 < dim; run1++ ){
00325       if( k[run1]  != NULL )
00326           delete[] k[run1] ;
00327       if( k2[run1] != NULL )
00328           delete[] k2[run1];
00329       if( l[run1]  != NULL )
00330           delete[] l[run1] ;
00331       if( l2[run1] != NULL )
00332           delete[] l2[run1];
00333     }
00334 
00335     if( k != NULL )
00336         delete[] k ;
00337 
00338     if( k2!= NULL )
00339         delete[] k2;
00340 
00341     if( l != NULL )
00342         delete[] l ;
00343 
00344     if( l2!= NULL )
00345         delete[] l2;
00346 
00347     if( x != NULL )
00348         delete[] x;
00349 
00350 
00351     // SENSITIVITIES:
00352     // ----------------------------------------
00353 
00354     if( G  != NULL )
00355         delete[] G;
00356 
00357     if( etaG  != NULL )
00358         delete[] etaG;
00359 
00360     if( G2  != NULL )
00361         delete[] G2;
00362 
00363     if( G3  != NULL )
00364         delete[] G3;
00365 
00366     if( etaG2  != NULL )
00367         delete[] etaG2;
00368 
00369     if( etaG3  != NULL )
00370         delete[] etaG3;
00371 
00372 
00373     // ----------------------------------------
00374 
00375     if( H  != NULL )
00376         delete[] H;
00377 
00378     if( etaH  != NULL )
00379         delete[] etaH;
00380 
00381     if( H2  != NULL )
00382         delete[] H2;
00383 
00384     if( H3  != NULL )
00385         delete[] H3;
00386 
00387     if( etaH2  != NULL )
00388         delete[] etaH2;
00389 
00390     if( etaH3  != NULL )
00391         delete[] etaH3;
00392 }
00393 
00394 
00395 IntegratorRK& IntegratorRK::operator=( const IntegratorRK& arg ){
00396 
00397     if ( this != &arg ){
00398         deleteAll();
00399         Integrator::operator=( arg );
00400         constructAll( arg );
00401     }
00402 
00403     return *this;
00404 }
00405 
00406 
00407 void IntegratorRK::constructAll( const IntegratorRK& arg ){
00408 
00409     int run1, run2;
00410 
00411     rhs = new DifferentialEquation( *arg.rhs );
00412 
00413     m   = arg.m              ;
00414     ma  = arg.ma             ;
00415     mn  = arg.mn             ;
00416     mu  = arg.mu             ;
00417     mui = arg.mui            ;
00418     mp  = arg.mp             ;
00419     mpi = arg.mpi            ;
00420     mw  = arg.mw             ;
00421 
00422 
00423     if( m < 1 ){
00424         ACADOERROR(RET_TRIVIAL_RHS);
00425         ASSERT(1 == 0);
00426     }
00427 
00428     // BUTCHER TABLEAU:
00429     // ----------------
00430     dim = arg.dim;
00431     A  = new double*[dim];
00432     b4 = new double [dim];
00433     b5 = new double [dim];
00434     c  = new double [dim];
00435 
00436     for( run1 = 0; run1 < dim; run1++ ){
00437 
00438         b4[run1] = arg.b4[run1];
00439         b5[run1] = arg.b5[run1];
00440         c [run1] = arg.c [run1];
00441 
00442         A[run1] = new double[dim];
00443         for( run2 = 0; run2 < dim; run2++ )
00444             A[run1][run2] = arg.A[run1][run2];
00445     }
00446 
00447     // RK-ALGORITHM:
00448     // -------------
00449     eta4  = new double [m];
00450     eta5  = new double [m];
00451     eta4_ = new double [m];
00452     eta5_ = new double [m];
00453 
00454     for( run1 = 0; run1 < m; run1++ ){
00455 
00456         eta4 [run1] = arg.eta4 [run1];
00457         eta5 [run1] = arg.eta5 [run1];
00458         eta4_[run1] = arg.eta4_[run1];
00459         eta5_[run1] = arg.eta5_[run1];
00460     }
00461 
00462     k     = new double*[dim];
00463     k2    = new double*[dim];
00464     l     = new double*[dim];
00465     l2    = new double*[dim];
00466     x     = new double [rhs->getNumberOfVariables() + 1 + m];
00467 
00468     for( run1 = 0; run1 < rhs->getNumberOfVariables() + 1 + m; run1++ ){
00469         x[run1] = arg.x[run1];
00470     }
00471 
00472     t     = arg.t;
00473 
00474 
00475     for( run1 = 0; run1 < dim; run1++ ){
00476         k    [run1] = new double[m];
00477         k2   [run1] = new double[m];
00478 
00479         for( run2 = 0; run2 < m; run2++ ){
00480             k [run1][run2] = arg.k [run1][run2];
00481             k2[run1][run2] = arg.k2[run1][run2];
00482         }
00483 
00484         l    [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
00485         l2   [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
00486 
00487         for( run2 = 0; run2 < rhs->getNumberOfVariables() + 1 + m; run2++ ){
00488             l [run1][run2] = arg.l [run1][run2];
00489             l2[run1][run2] = arg.l2[run1][run2];
00490         }
00491     }
00492 
00493 
00494     // SETTINGS:
00495     // ---------
00496     h    = (double*)calloc(arg.maxAlloc,sizeof(double));
00497     for( run1 = 0; run1 < arg.maxAlloc; run1++ ){
00498        h[run1] = arg.h[run1];
00499     }
00500     hini = arg.hini;
00501     hmin = arg.hmin;
00502     hmax = arg.hmax;
00503 
00504     tune  = arg.tune;
00505     TOL   = arg.TOL;
00506 
00507     err_power = arg.err_power;
00508 
00509 
00510     // INTERNAL INDEX LISTS:
00511     // ---------------------
00512     diff_index = new int[m];
00513 
00514     for( run1 = 0; run1 < m; run1++ ){
00515         diff_index[run1] = arg.diff_index[run1];
00516     }
00517 
00518     ddiff_index = 0;
00519     alg_index   = 0;
00520 
00521     control_index       = new int[mu ];
00522 
00523     for( run1 = 0; run1 < mu; run1++ ){
00524         control_index[run1] = arg.control_index[run1];
00525     }
00526 
00527     parameter_index     = new int[mp ];
00528 
00529     for( run1 = 0; run1 < mp; run1++ ){
00530         parameter_index[run1] = arg.parameter_index[run1];
00531     }
00532 
00533     int_control_index   = new int[mui];
00534 
00535     for( run1 = 0; run1 < mui; run1++ ){
00536         int_control_index[run1] = arg.int_control_index[run1];
00537     }
00538 
00539     int_parameter_index = new int[mpi];
00540 
00541     for( run1 = 0; run1 < mpi; run1++ ){
00542         int_parameter_index[run1] = arg.int_parameter_index[run1];
00543     }
00544 
00545     disturbance_index   = new int[mw ];
00546 
00547     for( run1 = 0; run1 < mw; run1++ ){
00548         disturbance_index[run1] = arg.disturbance_index[run1];
00549     }
00550 
00551     time_index = arg.time_index;
00552 
00553 
00554     // OTHERS:
00555     // -------
00556     maxNumberOfSteps = arg.maxNumberOfSteps;
00557     count            = arg.count           ;
00558     count2           = arg.count2          ;
00559     count3           = arg.count3          ;
00560 
00561     diff_scale = arg.diff_scale;
00562 
00563 
00564     // PRINT-LEVEL:
00565     // ------------
00566     PrintLevel = arg.PrintLevel;
00567 
00568 
00569     // SENSITIVITIES:
00570     // ---------------
00571     nFDirs     = 0   ;
00572     nBDirs     = 0   ;
00573 
00574     nFDirs2    = 0   ;
00575     nBDirs2    = 0   ;
00576 
00577     G          = NULL;
00578     etaG       = NULL;
00579 
00580     G2         = NULL;
00581     G3         = NULL;
00582     etaG2      = NULL;
00583     etaG3      = NULL;
00584 
00585     H          = NULL;
00586     etaH       = NULL;
00587 
00588     H2         = NULL;
00589     H3         = NULL;
00590     etaH2      = NULL;
00591     etaH3      = NULL;
00592 
00593 
00594     // THE STATE OF AGGREGATION:
00595     // -------------------------
00596     soa        = arg.soa;
00597 
00598 
00599     // STORAGE:
00600     // --------
00601     maxAlloc = arg.maxAlloc;
00602 }
00603 
00604 
00605 returnValue IntegratorRK::freezeMesh(){
00606 
00607 
00608     if( soa != SOA_UNFROZEN ){
00609        if( PrintLevel != NONE ){
00610            return ACADOWARNING(RET_ALREADY_FROZEN);
00611        }
00612        return RET_ALREADY_FROZEN;
00613     }
00614 
00615     soa = SOA_FREEZING_MESH;
00616     return SUCCESSFUL_RETURN;
00617 }
00618 
00619 
00620 
00621 returnValue IntegratorRK::freezeAll(){
00622 
00623     if( soa != SOA_UNFROZEN ){
00624        if( PrintLevel != NONE ){
00625            return ACADOWARNING(RET_ALREADY_FROZEN);
00626        }
00627        return RET_ALREADY_FROZEN;
00628     }
00629 
00630     soa = SOA_FREEZING_ALL;
00631     return SUCCESSFUL_RETURN;
00632 }
00633 
00634 
00635 returnValue IntegratorRK::unfreeze(){
00636 
00637     maxAlloc = 1;
00638     h = (double*)realloc(h,maxAlloc*sizeof(double));
00639     soa = SOA_UNFROZEN;
00640 
00641     return SUCCESSFUL_RETURN;
00642 }
00643 
00644 
00645 returnValue IntegratorRK::evaluate( const DVector &x0  ,
00646                                     const DVector &xa  ,
00647                                     const DVector &p   ,
00648                                     const DVector &u   ,
00649                                     const DVector &w   ,
00650                                     const Grid   &t_    ){
00651 
00652     int         run1;
00653     returnValue returnvalue;
00654 
00655     if( rhs == NULL ){
00656         return ACADOERROR(RET_TRIVIAL_RHS);
00657     }
00658 
00659     if( xa.getDim() != 0 )
00660         ACADOWARNING(RET_RK45_CAN_NOT_TREAT_DAE);
00661 
00662 
00663     Integrator::initializeOptions();
00664 
00665     timeInterval  = t_;
00666 
00667     xStore.init(  m, timeInterval );
00668     iStore.init( mn, timeInterval );
00669 
00670     t             = timeInterval.getFirstTime();
00671     x[time_index] = timeInterval.getFirstTime();
00672 //      printf("initial integrator t = %e\n", t );
00673 //      printf("initial integrator x[time_index] = %e\n", x[time_index] );
00674 //      printf(" with time_index = %d\n", time_index );
00675 
00676     if( soa != SOA_MESH_FROZEN && soa != SOA_MESH_FROZEN_FREEZING_ALL && soa != SOA_EVERYTHING_FROZEN  ){
00677        h[0] = hini;
00678 
00679        if( timeInterval.getLastTime() - timeInterval.getFirstTime() - h[0] < EPS ){
00680            h[0] = timeInterval.getLastTime() - timeInterval.getFirstTime();
00681 
00682            if( h[0] < 10.0*EPS )
00683                return ACADOERROR(RET_TO_SMALL_OR_NEGATIVE_TIME_INTERVAL);
00684        }
00685     }
00686 
00687     if( x0.isEmpty() == BT_TRUE ) return ACADOERROR(RET_MISSING_INPUTS);
00688 
00689 
00690     if( (int) x0.getDim() < m )
00691         return ACADOERROR(RET_INPUT_HAS_WRONG_DIMENSION);
00692 
00693     for( run1 = 0; run1 < m; run1++ ){
00694         eta4[run1]     = x0(run1);
00695         eta5[run1]     = x0(run1);
00696         xStore(0,run1) = x0(run1);
00697     }
00698 
00699     if( nFDirs != 0 ){
00700         for( run1 = 0; run1 < m; run1++ ){
00701             etaG[run1] = fseed(diff_index[run1]);
00702         }
00703     }
00704 
00705     if( mp > 0 ){
00706         if( (int) p.getDim() < mp )
00707             return ACADOERROR(RET_INPUT_HAS_WRONG_DIMENSION);
00708 
00709         for( run1 = 0; run1 < mp; run1++ ){
00710             x[parameter_index[run1]] = p(run1);
00711         }
00712     }
00713 
00714     if( mu > 0 ){
00715         if( (int) u.getDim() < mu )
00716             return ACADOERROR(RET_INPUT_HAS_WRONG_DIMENSION);
00717 
00718         for( run1 = 0; run1 < mu; run1++ ){
00719             x[control_index[run1]] = u(run1);
00720         }
00721     }
00722 
00723 
00724     if( mw > 0 ){
00725         if( (int) w.getDim() < mw )
00726             return ACADOERROR(RET_INPUT_HAS_WRONG_DIMENSION);
00727 
00728         for( run1 = 0; run1 < mw; run1++ ){
00729             x[disturbance_index[run1]] = w(run1);
00730         }
00731     }
00732 
00733 
00734     totalTime.start();
00735     nFcnEvaluations = 0;
00736 
00737 
00738      // Initialize the scaling based on the initial states:
00739      // ---------------------------------------------------
00740 
00741         double atol;
00742         get( ABSOLUTE_TOLERANCE, atol );
00743 
00744         for( run1 = 0; run1 < m; run1++ )
00745             diff_scale(run1) = fabs(eta4[run1]) + atol/TOL;
00746 
00747 
00748      // PRINTING:
00749      // ---------
00750         if( PrintLevel == HIGH || PrintLevel == MEDIUM ){
00751             acadoPrintCopyrightNotice( "IntegratorRK -- A Runge Kutta integrator." );
00752         }
00753         if( PrintLevel == HIGH ){
00754             cout << "RK: t = " << t << "\t";
00755             for( run1 = 0; run1 < m; run1++ )
00756                 cout << "x[" << run1 << "] = " << scientific << eta4[ run1 ];
00757             cout << endl;
00758         }
00759 
00760 
00761     returnvalue = RET_FINAL_STEP_NOT_PERFORMED_YET;
00762 
00763     count3 = 0;
00764     count  = 1;
00765 
00766     while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET && count <= maxNumberOfSteps ){
00767 
00768         returnvalue = step(count);
00769         count++;
00770     }
00771 
00772     count2 = count-1;
00773 
00774     for( run1 = 0; run1 < mn; run1++ )
00775         iStore( 0, run1 ) = iStore( 1, run1 );
00776 
00777     totalTime.stop();
00778 
00779     if( count > maxNumberOfSteps ){
00780         if( PrintLevel != NONE )
00781             return ACADOERROR(RET_MAX_NUMBER_OF_STEPS_EXCEEDED);
00782         return RET_MAX_NUMBER_OF_STEPS_EXCEEDED;
00783     }
00784 
00785 
00786 //         cout << "numIntSteps = %d\n",count-1);
00787 
00788 
00789     // SET THE LOGGING INFORMATION:
00790     // ----------------------------------------------------------------------------------------
00791 
00792        setLast( LOG_TIME_INTEGRATOR                              , totalTime.getTime()           );
00793        setLast( LOG_NUMBER_OF_INTEGRATOR_STEPS                   , count-1                       );
00794        setLast( LOG_NUMBER_OF_INTEGRATOR_REJECTED_STEPS          , getNumberOfRejectedSteps()    );
00795        setLast( LOG_NUMBER_OF_INTEGRATOR_FUNCTION_EVALUATIONS    , nFcnEvaluations               );
00796        setLast( LOG_NUMBER_OF_BDF_INTEGRATOR_JACOBIAN_EVALUATIONS, 0                             );
00797        setLast( LOG_TIME_INTEGRATOR_FUNCTION_EVALUATIONS         , functionEvaluation.getTime()  );
00798        setLast( LOG_TIME_BDF_INTEGRATOR_JACOBIAN_EVALUATION      , 0.0                           );
00799        setLast( LOG_TIME_BDF_INTEGRATOR_JACOBIAN_DECOMPOSITION   , 0.0                           );
00800 
00801     // ----------------------------------------------------------------------------------------
00802 
00803 
00804      // PRINTING:
00805      // ---------
00806         if( PrintLevel == MEDIUM ){
00807 
00808             if( soa == SOA_EVERYTHING_FROZEN ){
00809                 cout << "\n Results at  t =  " << t << "\t";
00810                 for( run1 = 0; run1 < m; run1++ )
00811                         cout << "x[" << run1 << "] = " << scientific << eta4[ run1 ];
00812                 cout << endl;
00813             }
00814             printIntermediateResults();
00815         }
00816         
00817         int printIntegratorProfile = 0;
00818         get( PRINT_INTEGRATOR_PROFILE,printIntegratorProfile );
00819         
00820         if ( (BooleanType)printIntegratorProfile == BT_TRUE )
00821         {
00822                 printRunTimeProfile( );
00823         }
00824         else
00825         {
00826                 if( PrintLevel == MEDIUM  || PrintLevel == HIGH )
00827                         cout << "RK: number of steps:  " << count - 1 << endl;
00828         }
00829 
00830     return returnvalue;
00831 }
00832 
00833 
00834 
00835 returnValue IntegratorRK::setProtectedForwardSeed( const DVector &xSeed,
00836                                                    const DVector &pSeed,
00837                                                    const DVector &uSeed,
00838                                                    const DVector &wSeed,
00839                                                    const int    &order  ){
00840 
00841     if( order == 2 ){
00842         return setForwardSeed2( xSeed, pSeed, uSeed, wSeed);
00843     }
00844     if( order < 1 || order > 2 ){
00845         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
00846     }
00847 
00848     if( nBDirs > 0 ){
00849         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
00850     }
00851 
00852     int run2;
00853 
00854     if( G  != NULL ){
00855         delete[] G;
00856         G = NULL;
00857     }
00858 
00859     if( etaG  != NULL ){
00860         delete[] etaG;
00861         etaG = NULL;
00862     }
00863 
00864     nFDirs = 1;
00865 
00866     fseed.init(rhs->getNumberOfVariables()+1+m);
00867     fseed.setZero();
00868 
00869     G = new double[rhs->getNumberOfVariables() + 1 + m];
00870 
00871     for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++ ){
00872         G[run2] = 0.0;
00873     }
00874 
00875     etaG = new double[m];
00876 
00877     if( xSeed.getDim() != 0 ){
00878         for( run2 = 0; run2 < m; run2++ ){
00879             fseed(diff_index[run2]) = xSeed(run2);
00880         }
00881     }
00882 
00883     if( pSeed.getDim() != 0 ){
00884         for( run2 = 0; run2 < mp; run2++ ){
00885              fseed(parameter_index[run2]) = pSeed(run2);
00886              G    [parameter_index[run2]] = pSeed(run2);
00887         }
00888     }
00889 
00890     if( uSeed.getDim() != 0 ){
00891         for( run2 = 0; run2 < mu; run2++ ){
00892             fseed(control_index[run2]) = uSeed(run2);
00893             G    [control_index[run2]] = uSeed(run2);
00894         }
00895     }
00896 
00897     if( wSeed.getDim() != 0 ){
00898 
00899         for( run2 = 0; run2 < mw; run2++ ){
00900             fseed(disturbance_index[run2]) = wSeed(run2);
00901                 G[disturbance_index[run2]] = wSeed(run2);
00902         }
00903     }
00904 
00905     return SUCCESSFUL_RETURN;
00906 }
00907 
00908 
00909 returnValue IntegratorRK::setForwardSeed2( const DVector &xSeed ,
00910                                            const DVector &pSeed ,
00911                                            const DVector &uSeed ,
00912                                            const DVector &wSeed   ){
00913 
00914     int run2;
00915 
00916     if( G2  != NULL ){
00917         delete[] G2;
00918         G2 = NULL;
00919     }
00920 
00921     if( G3  != NULL ){
00922         delete[] G3;
00923         G3 = NULL;
00924     }
00925 
00926     if( etaG2  != NULL ){
00927         delete[] etaG2;
00928         etaG2 = NULL;
00929     }
00930 
00931     if( etaG3  != NULL ){
00932         delete[] etaG3;
00933         etaG3 = NULL;
00934     }
00935 
00936     nFDirs2 = 1;
00937 
00938     fseed2.init(rhs->getNumberOfVariables() + 1 + m);
00939     fseed2.setZero();
00940 
00941     G2 = new double[rhs->getNumberOfVariables() + 1 + m];
00942     G3 = new double[rhs->getNumberOfVariables() + 1 + m];
00943 
00944     for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++ ){
00945          G2[run2] = 0.0;
00946          G3[run2] = 0.0;
00947     }
00948     etaG2 = new double[m];
00949     etaG3 = new double[m];
00950 
00951     if( xSeed.getDim() != 0 ){
00952         for( run2 = 0; run2 < m; run2++ ){
00953             fseed2(diff_index[run2]) = xSeed(run2);
00954         }
00955     }
00956 
00957     if( pSeed.getDim() != 0 ){
00958         for( run2 = 0; run2 < mp; run2++ ){
00959             fseed2(parameter_index[run2]) = pSeed(run2);
00960             G2    [parameter_index[run2]] = pSeed(run2);
00961         }
00962     }
00963 
00964     if( uSeed.getDim() != 0 ){
00965         for( run2 = 0; run2 < mu; run2++ ){
00966              fseed2(control_index[run2]) = uSeed(run2);
00967              G2    [control_index[run2]] = uSeed(run2);
00968         }
00969     }
00970 
00971     if( wSeed.getDim() != 0 ){
00972         for( run2 = 0; run2 < mw; run2++ ){
00973             fseed2(disturbance_index[run2]) = wSeed(run2);
00974             G2    [disturbance_index[run2]] = wSeed(run2);
00975         }
00976     }
00977     return SUCCESSFUL_RETURN;
00978 }
00979 
00980 
00981 returnValue IntegratorRK::setProtectedBackwardSeed( const DVector &seed, const int &order ){
00982 
00983     if( order == 2 ){
00984         return setBackwardSeed2(seed);
00985     }
00986     if( order < 1 || order > 2 ){
00987         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
00988     }
00989 
00990     if( nFDirs > 0 ){
00991         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
00992     }
00993 
00994     int run2;
00995 
00996     if( H  != NULL ){
00997         delete[] H;
00998         H = NULL;
00999     }
01000 
01001     if( etaH  != NULL ){
01002         delete[] etaH;
01003         etaH = NULL;
01004     }
01005 
01006     nBDirs = 1;
01007 
01008     bseed.init( m );
01009     bseed.setZero();
01010 
01011     H = new double[m];
01012 
01013     etaH = new double[rhs->getNumberOfVariables()+1+m];
01014     for( run2 = 0; run2 < rhs->getNumberOfVariables()+1+m; run2++ ){
01015         etaH[run2] = 0.0;
01016     }
01017 
01018     if( seed.getDim() != 0 ){
01019         for( run2 = 0; run2 < m; run2++ ){
01020             bseed(run2) = seed(run2);
01021         }
01022     }
01023 
01024     return SUCCESSFUL_RETURN;
01025 }
01026 
01027 
01028 
01029 returnValue IntegratorRK::setBackwardSeed2( const DVector &seed ){
01030 
01031     int run2;
01032 
01033     if( H2  != NULL ){
01034         delete[] H2;
01035         H2 = NULL;
01036     }
01037 
01038     if( H3  != NULL ){
01039         delete[] H3;
01040         H3 = NULL;
01041     }
01042 
01043     if( etaH2  != NULL ){
01044         delete[] etaH2;
01045         etaH2 = NULL;
01046     }
01047 
01048     if( etaH3  != NULL ){
01049         delete[] etaH3;
01050         etaH3 = NULL;
01051     }
01052 
01053 
01054     nBDirs2 = 1;
01055 
01056     bseed2.init(m);
01057     bseed2.setZero();
01058 
01059     H2 = new double[m];
01060     H3 = new double[m];
01061 
01062     etaH2 = new double[rhs->getNumberOfVariables()+1+m];
01063     etaH3 = new double[rhs->getNumberOfVariables()+1+m];
01064 
01065     if( seed.getDim() != 0 ){
01066          for( run2 = 0; run2 < m; run2++ ){
01067              bseed2(run2) = seed(run2);
01068          }
01069     }
01070 
01071     return SUCCESSFUL_RETURN;
01072 }
01073 
01074 
01075 returnValue IntegratorRK::evaluateSensitivities(){
01076 
01077     int         run1, run2 ;
01078     returnValue returnvalue;
01079 
01080     if( rhs == NULL ){
01081         return ACADOERROR(RET_TRIVIAL_RHS);
01082     }
01083 
01084     if( soa != SOA_EVERYTHING_FROZEN ){
01085         return ACADOERROR(RET_NOT_FROZEN);
01086     }
01087 
01088 
01089     if( nFDirs != 0 ){
01090         t = timeInterval.getFirstTime();
01091         dxStore.init( m, timeInterval );
01092         for( run1 = 0; run1 < m; run1++ ){
01093             etaG[run1] = fseed(diff_index[run1]);
01094         }
01095     }
01096 
01097     if( nBDirs != 0 ){
01098         for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++){
01099             etaH[run2] = 0.0;
01100         }
01101     }
01102 
01103     if( nBDirs != 0 ){
01104         for( run1 = 0; run1 < m; run1++ ){
01105             etaH[diff_index[run1]] = bseed(run1);
01106         }
01107     }
01108 
01109     if( nFDirs2 != 0 ){
01110         t = timeInterval.getFirstTime();
01111         ddxStore.init( m, timeInterval );
01112         for( run1 = 0; run1 < m; run1++ ){
01113             etaG2[run1] = fseed2(diff_index[run1]);
01114             etaG3[run1] = 0.0;
01115         }
01116     }
01117 
01118     if( nBDirs2 != 0 ){
01119         for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++){
01120             etaH2[run2] = 0.0;
01121             etaH3[run2] = 0.0;
01122         }
01123     }
01124 
01125     if( nBDirs2 != 0 ){
01126         for( run1 = 0; run1 < m; run1++ ){
01127             etaH2[diff_index[run1]] = bseed2(run1);
01128         }
01129     }
01130 
01131     if( PrintLevel == HIGH ){
01132         printIntermediateResults();
01133     }
01134 
01135     returnvalue = RET_FINAL_STEP_NOT_PERFORMED_YET;
01136 
01137 
01138     if( nBDirs > 0 || nBDirs2 > 0 ){
01139 
01140         int oldCount = count;
01141 
01142         count--;
01143         while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET && count >= 1 ){
01144 
01145             returnvalue = step( count );
01146             count--;
01147         }
01148 
01149         if( count == 0 && (returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET ||
01150                            returnvalue == SUCCESSFUL_RETURN   )            ){
01151 
01152 
01153             if( PrintLevel == MEDIUM ){
01154                 printIntermediateResults();
01155             }
01156             count = oldCount;
01157 
01158             return SUCCESSFUL_RETURN;
01159         }
01160         count = oldCount;
01161     }
01162     else{
01163 
01164         count = 1;
01165         while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET &&
01166                count <= maxNumberOfSteps ){
01167 
01168             returnvalue = step(count);
01169             count++;
01170         }
01171 
01172         if( nBDirs2 == 0 && nFDirs != 0 )
01173             for( run1 = 0; run1 < m; run1++ )
01174                 dxStore( 0, run1 ) = dxStore( 1, run1 );
01175 
01176         if( nFDirs2 != 0 )
01177             for( run1 = 0; run1 < m; run1++ )
01178                 ddxStore( 0, run1 ) = ddxStore( 1, run1 );
01179 
01180         if( count > maxNumberOfSteps ){
01181             if( PrintLevel != NONE )
01182                 return ACADOERROR(RET_MAX_NUMBER_OF_STEPS_EXCEEDED);
01183             return RET_MAX_NUMBER_OF_STEPS_EXCEEDED;
01184         }
01185 
01186         if( PrintLevel == MEDIUM ){
01187             printIntermediateResults();
01188         }
01189     }
01190     return returnvalue;
01191 }
01192 
01193 
01194 returnValue IntegratorRK::step(int number_){
01195 
01196     int run1;
01197     double E = EPS;
01198 
01199     if( soa == SOA_EVERYTHING_FROZEN || soa == SOA_MESH_FROZEN || soa == SOA_MESH_FROZEN_FREEZING_ALL ){
01200         h[0] = h[number_];
01201     }
01202 
01203     if( soa == SOA_FREEZING_MESH ||
01204         soa == SOA_MESH_FROZEN   ||
01205         soa == SOA_MESH_FROZEN_FREEZING_ALL ||
01206         soa == SOA_UNFROZEN      ){
01207 
01208         if( nFDirs > 0 ){
01209             E = determineEta45(0);
01210         }
01211         else{
01212             E = determineEta45();
01213         }
01214     }
01215     if( soa == SOA_FREEZING_ALL ){
01216         E = determineEta45(dim*number_);
01217     }
01218 
01219 
01220     if( soa != SOA_EVERYTHING_FROZEN && soa != SOA_MESH_FROZEN && soa != SOA_MESH_FROZEN_FREEZING_ALL ){
01221 
01222         int number_of_rejected_steps = 0;
01223 
01224         if( E < 0.0 ){
01225             return ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01226         }
01227 
01228         // REJECT THE STEP IF GIVEN TOLERANCE IS NOT ACHIEVED:
01229         // ---------------------------------------------------
01230         while( E >= TOL*h[0] ){
01231 
01232             if( PrintLevel == HIGH ){
01233                 cout << "STEP REJECTED: error estimate           = " << scientific << E << endl
01234                          << "               required local tolerance = " << TOL*h[0] << endl;
01235             }
01236 
01237             number_of_rejected_steps++;
01238 
01239             for( run1 = 0; run1 < m; run1++ ){
01240                 eta4[run1] = eta4_[run1];
01241             }
01242             if( h[0] <= hmin + EPS ){
01243                 return ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01244             }
01245             h[0] = 0.5*h[0];
01246             if( h[0] < hmin ){
01247                 h[0] = hmin;
01248             }
01249 
01250             if( soa == SOA_FREEZING_MESH ||
01251                 soa == SOA_UNFROZEN      ){
01252 
01253                 if( nFDirs > 0 ){
01254                     E = determineEta45(0);
01255                 }
01256                 else{
01257                     E = determineEta45();
01258                 }
01259             }
01260             if( soa == SOA_FREEZING_ALL ){
01261                 E = determineEta45(dim*number_);
01262             }
01263 
01264             if( E < 0.0 ){
01265                 return ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01266             }
01267         }
01268 
01269         count3 += number_of_rejected_steps;
01270     }
01271 
01272     // PROCEED IF THE STEP IS ACCEPTED:
01273     // --------------------------------
01274 
01275        double *etaG_  = new double[m];
01276        double *etaG3_ = new double[m];
01277 
01278 
01279      // compute forward derivatives if requested:
01280      // ------------------------------------------
01281 
01282      if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ){
01283 
01284          for( run1 = 0; run1 < m; run1++ )
01285              etaG_[run1]  = etaG[run1];
01286 
01287          if( nBDirs != 0 ){
01288              return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
01289          }
01290 
01291          if( soa == SOA_FREEZING_ALL || soa == SOA_EVERYTHING_FROZEN ){
01292              determineEtaGForward(dim*number_);
01293          }
01294          else{
01295              determineEtaGForward(0);
01296          }
01297      }
01298      if( nBDirs > 0 ){
01299 
01300          if( soa != SOA_EVERYTHING_FROZEN ){
01301              return ACADOERROR(RET_NOT_FROZEN);
01302          }
01303          if( nFDirs != 0 || nBDirs2 != 0 || nFDirs2 != 0 ){
01304              return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
01305          }
01306          determineEtaHBackward(dim*number_);
01307      }
01308      if( nFDirs2 > 0 ){
01309 
01310          for( run1 = 0; run1 < m; run1++ )
01311              etaG3_[run1]  = etaG3[run1];
01312 
01313          if( soa != SOA_EVERYTHING_FROZEN ){
01314              return ACADOERROR(RET_NOT_FROZEN);
01315          }
01316          if( nBDirs != 0 || nBDirs2 != 0 || nFDirs != 1 ){
01317              return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
01318          }
01319          determineEtaGForward2(dim*number_);
01320      }
01321      if( nBDirs2 > 0 ){
01322 
01323          if( soa != SOA_EVERYTHING_FROZEN ){
01324              return ACADOERROR(RET_NOT_FROZEN);
01325          }
01326          if( nBDirs != 0 || nFDirs2 != 0 || nFDirs != 1 ){
01327              return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
01328          }
01329 
01330          determineEtaHBackward2(dim*number_);
01331      }
01332 
01333 
01334      // increase the time:
01335      // ----------------------------------------------
01336 
01337      if( nBDirs > 0 || nBDirs2 > 0 ){
01338 
01339          t = t - h[0];
01340      }
01341      else{
01342 
01343          t = t + h[0];
01344      }
01345 //      printf("t = %e\n",t );
01346 
01347      // PRINTING:
01348      // ---------
01349      if( PrintLevel == HIGH ){
01350          cout << "RK: t = " << scientific << t << "  h = " << h[ 0 ] << "  ";
01351          printIntermediateResults();
01352      }
01353 
01354 
01355      // STORAGE:
01356      // --------
01357 
01358      if( soa == SOA_FREEZING_MESH || soa == SOA_FREEZING_ALL || soa == SOA_MESH_FROZEN_FREEZING_ALL ){
01359 
01360          if( number_ >= maxAlloc){
01361 
01362              maxAlloc = 2*maxAlloc;
01363              h = (double*)realloc(h,maxAlloc*sizeof(double));
01364          }
01365          h[number_] = h[0];
01366      }
01367 
01368      int i1 = timeInterval.getFloorIndex( t-h[0] );
01369      int i2 = timeInterval.getFloorIndex( t      );
01370      int jj;
01371 
01372      for( jj = i1+1; jj <= i2; jj++ ){
01373 
01374          if( nFDirs == 0 && nBDirs  == 0 && nFDirs2 == 0 && nBDirs == 0 ) interpolate( jj, eta4_ , k[0], eta4 ,   xStore );
01375          if( nFDirs  > 0 && nBDirs2 == 0 && nFDirs2 == 0                ) interpolate( jj, etaG_ , k[0], etaG ,  dxStore );
01376          if( nFDirs2 > 0                                                ) interpolate( jj, etaG3_, k[0], etaG3, ddxStore );
01377 
01378          for( run1 = 0; run1 < mn; run1++ )
01379              iStore( jj, run1 ) = x[rhs->index( VT_INTERMEDIATE_STATE, run1 )];
01380      }
01381 
01382      delete[] etaG_ ;
01383      delete[] etaG3_;
01384 
01385 
01386      if( nBDirs == 0 || nBDirs2 == 0 ){
01387 
01388      // Stop the algorithm if  t >= te:
01389      // ----------------------------------------------
01390         if( t >= timeInterval.getLastTime() - EPS ){
01391             x[time_index] = timeInterval.getLastTime();
01392             for( run1 = 0; run1 < m; run1++ ){
01393                                 if ( acadoIsNaN( eta4[run1] ) == BT_TRUE )
01394                                         return ACADOERROR( RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45 );
01395                 x[diff_index[run1]] = eta4[run1];
01396             }
01397 
01398             if( soa == SOA_FREEZING_MESH ){
01399                 soa = SOA_MESH_FROZEN;
01400             }
01401             if( soa == SOA_FREEZING_ALL || soa == SOA_MESH_FROZEN_FREEZING_ALL ){
01402                 soa = SOA_EVERYTHING_FROZEN;
01403             }
01404             
01405             return SUCCESSFUL_RETURN;
01406         }
01407      }
01408 
01409 
01410      if( soa != SOA_EVERYTHING_FROZEN && soa != SOA_MESH_FROZEN && soa != SOA_MESH_FROZEN_FREEZING_ALL ){
01411 
01412 
01413      // recompute the scaling based on the actual states:
01414      // -------------------------------------------------
01415 
01416         double atol;
01417         get( ABSOLUTE_TOLERANCE, atol );
01418 
01419         for( run1 = 0; run1 < m; run1++ )
01420             diff_scale(run1) = fabs(eta4[run1]) + atol/TOL;
01421 
01422 
01423 
01424      // apply a numeric stabilization of the step size control:
01425      // -------------------------------------------------------
01426         double Emin = 1e-3*sqrt(TOL)*pow(hini, ((1.0/err_power)+1.0)/2.0 );
01427 
01428         if( E < Emin     ) E = Emin    ;
01429         if( E < 10.0*EPS ) E = 10.0*EPS;
01430 
01431 
01432      // determine the new step size:
01433      // ----------------------------------------------
01434         h[0] = h[0]*pow( tune*(TOL*h[0]/E), err_power );
01435 
01436         if( h[0] > hmax ){
01437           h[0] = hmax;
01438         }
01439         if( h[0] < hmin ){
01440           h[0] = hmin;
01441         }
01442 
01443         if( t + h[0] >= timeInterval.getLastTime() ){
01444           h[0] = timeInterval.getLastTime()-t;
01445         }
01446         
01447 //         printf( "t = %e,  stepsize = %e\n", t,h[0] );
01448     }
01449 
01450     return RET_FINAL_STEP_NOT_PERFORMED_YET;
01451 }
01452 
01453 
01454 
01455 returnValue IntegratorRK::stop(){
01456 
01457     return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
01458 }
01459 
01460 
01461 returnValue IntegratorRK::getProtectedX( DVector *xEnd ) const{
01462 
01463     int run1;
01464 
01465     if( (int) xEnd[0].getDim() != m )
01466         return RET_INPUT_HAS_WRONG_DIMENSION;
01467 
01468     for( run1 = 0; run1 < m; run1++ )
01469         xEnd[0](run1) = eta4[run1];
01470 
01471     return SUCCESSFUL_RETURN;
01472 }
01473 
01474 
01475 returnValue IntegratorRK::getProtectedForwardSensitivities( DMatrix *Dx, int order ) const{
01476 
01477     int run1;
01478 
01479     if( Dx == NULL ){
01480         return SUCCESSFUL_RETURN;
01481     }
01482 
01483     if( order == 1 && nFDirs2 == 0 ){
01484         for( run1 = 0; run1 < m; run1++ ){
01485             Dx[0](run1,0) = etaG[run1];
01486         }
01487     }
01488 
01489     if( order == 2 ){
01490         for( run1 = 0; run1 < m; run1++ ){
01491             Dx[0](run1,0) = etaG3[run1];
01492         }
01493     }
01494 
01495     if( order == 1 && nFDirs2 > 0 ){
01496         for( run1 = 0; run1 < m; run1++ ){
01497             Dx[0](run1,0) = etaG2[run1];
01498         }
01499     }
01500 
01501     if( order < 1 || order > 2 ){
01502         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
01503     }
01504 
01505     return SUCCESSFUL_RETURN;
01506 }
01507 
01508 
01509 returnValue IntegratorRK::getProtectedBackwardSensitivities( DVector &Dx_x0,
01510                                                              DVector &Dx_p ,
01511                                                              DVector &Dx_u ,
01512                                                              DVector &Dx_w ,
01513                                                              int order      ) const{
01514 
01515     int run2;
01516 
01517     if( order == 1 && nBDirs2 == 0 ){
01518 
01519         if( Dx_x0.getDim() != 0 ){
01520             for( run2 = 0; run2 < m; run2++ )
01521                 Dx_x0(run2) = etaH[diff_index[run2]];
01522         }
01523         if( Dx_p.getDim() != 0 ){
01524             for( run2 = 0; run2 < mp; run2++ ){
01525                 Dx_p(run2) = etaH[parameter_index[run2]];
01526             }
01527         }
01528         if( Dx_u.getDim() != 0 ){
01529             for( run2 = 0; run2 < mu; run2++ ){
01530                 Dx_u(run2) = etaH[control_index[run2]];
01531             }
01532         }
01533         if( Dx_w.getDim() != 0 ){
01534             for( run2 = 0; run2 < mw; run2++ ){
01535                 Dx_w(run2) = etaH[disturbance_index[run2]];
01536             }
01537         }
01538     }
01539 
01540     if( order == 1 && nBDirs2 > 0 ){
01541 
01542         if( Dx_x0.getDim() != 0 ){
01543             for( run2 = 0; run2 < m; run2++ ){
01544                 Dx_x0(run2) = etaH2[diff_index[run2]];
01545             }
01546         }
01547         if( Dx_u.getDim() != 0 ){
01548             for( run2 = 0; run2 < mu; run2++ ){
01549                 Dx_u(run2) = etaH2[control_index[run2]];
01550             }
01551         }
01552         if( Dx_p.getDim() != 0 ){
01553             for( run2 = 0; run2 < mp; run2++ ){
01554                 Dx_p(run2) = etaH2[parameter_index[run2]];
01555             }
01556         }
01557         if( Dx_w.getDim() != 0 ){
01558             for( run2 = 0; run2 < mw; run2++ ){
01559                  Dx_w(run2) = etaH2[disturbance_index[run2]];
01560             }
01561         }
01562     }
01563 
01564 
01565     if( order == 2 ){
01566 
01567         if( Dx_x0.getDim() != 0 ){
01568             for( run2 = 0; run2 < m; run2++ ){
01569                 Dx_x0(run2) = etaH3[diff_index[run2]   ];
01570             }
01571         }
01572         if( Dx_u.getDim() != 0 ){
01573             for( run2 = 0; run2 < mu; run2++ ){
01574                Dx_u(run2) = etaH3[control_index[run2]];
01575             }
01576         }
01577         if( Dx_p.getDim() != 0 ){
01578             for( run2 = 0; run2 < mp; run2++ ){
01579                  Dx_p(run2) = etaH3[parameter_index[run2]];
01580             }
01581         }
01582         if( Dx_w.getDim() != 0 ){
01583             for( run2 = 0; run2 < mw; run2++ ){
01584                  Dx_w(run2) = etaH3[disturbance_index[run2]];
01585             }
01586         }
01587     }
01588 
01589     if( order < 1 || order > 2 ){
01590 
01591         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
01592     }
01593 
01594     return SUCCESSFUL_RETURN;
01595 }
01596 
01597 
01598 int IntegratorRK::getNumberOfSteps() const{
01599 
01600     return count2;
01601 }
01602 
01603 int IntegratorRK::getNumberOfRejectedSteps() const{
01604 
01605     return count3;
01606 }
01607 
01608 
01609 double IntegratorRK::getStepSize() const{
01610 
01611     return h[0];
01612 }
01613 
01614 
01615 returnValue IntegratorRK::setDxInitialization( double *dx0 ){
01616 
01617     return SUCCESSFUL_RETURN;
01618 }
01619 
01620 //
01621 // PROTECTED MEMBER FUNCTIONS:
01622 //
01623 
01624 
01625 double IntegratorRK::determineEta45(){
01626 
01627     int run1, run2, run3;
01628     double E;
01629 
01630     // determine k:
01631     // -----------------------------------------------
01632        for( run1 = 0; run1 < dim; run1++ ){
01633            x[time_index] = t + c[run1]*h[0];
01634            for( run2 = 0; run2 < m; run2++ ){
01635                x[diff_index[run2]] = eta4[run2];
01636                for( run3 = 0; run3 < run1; run3++ ){
01637                    x[diff_index[run2]] = x[diff_index[run2]] +
01638                                          A[run1][run3]*h[0]*k[run3][run2];
01639                }
01640            }
01641            functionEvaluation.start();
01642 
01643            if( rhs[0].evaluate( 0, x, k[run1] ) != SUCCESSFUL_RETURN ){
01644                ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01645                return -1.0;
01646            }
01647 
01648            functionEvaluation.stop();
01649            nFcnEvaluations++;
01650        }
01651 
01652     // save previous eta4:
01653     // ----------------------------------------------
01654 
01655        for( run1 = 0; run1 < m; run1++ ){
01656            eta4_[run1]  = eta4[run1];
01657            eta5 [run1]  = eta4[run1]; // TODO: check if eta4 is correct here!?
01658                    //printf( "%e\n",eta4[run1] );
01659        }
01660 
01661     // determine eta4 and eta5:
01662     // ----------------------------------------------
01663 
01664        for( run1 = 0; run1 < dim; run1++ ){
01665            for( run2 = 0; run2 < m; run2++ ){
01666                eta4[run2] = eta4[run2] + b4[run1]*h[0]*k[run1][run2];
01667                eta5[run2] = eta5[run2] + b5[run1]*h[0]*k[run1][run2];
01668            }
01669        }
01670 
01671     // determine local error estimate E:
01672     // ----------------------------------------------
01673 
01674        E = EPS;
01675        for( run2 = 0; run2 < m; run2++ ){
01676            if( (eta4[run2]-eta5[run2])/diff_scale(run2) >= E  ){
01677                E = (eta4[run2]-eta5[run2])/diff_scale(run2);
01678            }
01679            if( (eta4[run2]-eta5[run2])/diff_scale(run2) <= -E ){
01680                E = (-eta4[run2]+eta5[run2])/diff_scale(run2);
01681            }
01682        }
01683 
01684     return E;
01685 }
01686 
01687 
01688 
01689 double IntegratorRK::determineEta45( int number_ ){
01690 
01691     int run1, run2, run3;
01692     double E;
01693 
01694     // determine k:
01695     // -----------------------------------------------
01696        for( run1 = 0; run1 < dim; run1++ ){
01697            x[time_index] = t + c[run1]*h[0];
01698            for( run2 = 0; run2 < m; run2++ ){
01699                x[diff_index[run2]] = eta4[run2];
01700                for( run3 = 0; run3 < run1; run3++ ){
01701                    x[diff_index[run2]] = x[diff_index[run2]] +
01702                                          A[run1][run3]*h[0]*k[run3][run2];
01703                }
01704            }
01705            functionEvaluation.start();
01706 
01707            if( rhs[0].evaluate( number_+run1, x, k[run1] ) != SUCCESSFUL_RETURN ){
01708                ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01709                return -1.0;
01710            }
01711 
01712            functionEvaluation.stop();
01713            nFcnEvaluations++;
01714        }
01715 
01716     // save previous eta4:
01717     // ----------------------------------------------
01718 
01719        for( run1 = 0; run1 < m; run1++ ){
01720            eta4_[run1]  = eta4[run1];
01721            eta5 [run1]  = eta4[run1];
01722        }
01723 
01724     // determine eta4 and eta5:
01725     // ----------------------------------------------
01726        for( run1 = 0; run1 < dim; run1++ ){
01727            for( run2 = 0; run2 < m; run2++ ){
01728                eta4[run2] = eta4[run2] + b4[run1]*h[0]*k[run1][run2];
01729                eta5[run2] = eta5[run2] + b5[run1]*h[0]*k[run1][run2];
01730            }
01731        }
01732 
01733     // determine local error estimate E:
01734     // ----------------------------------------------
01735 
01736        E = EPS;
01737        for( run2 = 0; run2 < m; run2++ ){
01738            if( (eta4[run2]-eta5[run2])/diff_scale(run2) >= E  )
01739                E = (eta4[run2]-eta5[run2])/diff_scale(run2);
01740            if( (eta4[run2]-eta5[run2])/diff_scale(run2) <= -E )
01741                E = (-eta4[run2]+eta5[run2])/diff_scale(run2);
01742        }
01743 
01744     return E;
01745 }
01746 
01747 
01748 void IntegratorRK::determineEtaGForward( int number_ ){
01749 
01750     int run1, run2, run3;
01751 
01752     // determine k:
01753     // -----------------------------------------------
01754        for( run1 = 0; run1 < dim; run1++ ){
01755            for( run2 = 0; run2 < m; run2++ ){
01756                G[diff_index[run2]] = etaG[run2];
01757                for( run3 = 0; run3 < run1; run3++ ){
01758                    G[diff_index[run2]] = G[diff_index[run2]] +
01759                                                A[run1][run3]*h[0]*k[run3][run2];
01760                }
01761            }
01762            if( rhs[0].AD_forward( number_+run1, G, k[run1] ) != SUCCESSFUL_RETURN ){
01763                ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01764                return;
01765            }
01766        }
01767 
01768     // determine etaG:
01769     // ----------------------------------------------
01770        for( run1 = 0; run1 < dim; run1++ ){
01771            for( run2 = 0; run2 < m; run2++ ){
01772                etaG[run2] = etaG[run2] + b4[run1]*h[0]*k[run1][run2];
01773            }
01774        }
01775 }
01776 
01777 
01778 
01779 void IntegratorRK::determineEtaGForward2( int number_ ){
01780 
01781     int run1, run2, run3;
01782 
01783     // determine k:
01784     // -----------------------------------------------
01785        for( run1 = 0; run1 < dim; run1++ ){
01786            for( run2 = 0; run2 < m; run2++ ){
01787                G2[diff_index[run2]] = etaG2[run2];
01788                G3[diff_index[run2]] = etaG3[run2];
01789                for( run3 = 0; run3 < run1; run3++ ){
01790                    G2[diff_index[run2]] = G2[diff_index[run2]] +
01791                                                 A[run1][run3]*h[0]*k[run3][run2];
01792                    G3[diff_index[run2]] = G3[diff_index[run2]] +
01793                                                 A[run1][run3]*h[0]*k2[run3][run2];
01794                }
01795            }
01796 
01797            if( rhs[0].AD_forward2( number_+run1, G2, G3, k[run1], k2[run1] )
01798                != SUCCESSFUL_RETURN ){
01799                ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01800                return;
01801            }
01802        }
01803 
01804     // determine etaG2:
01805     // ----------------------------------------------
01806 
01807        for( run1 = 0; run1 < dim; run1++ ){
01808            for( run2 = 0; run2 < m; run2++ ){
01809                etaG2[run2] = etaG2[run2] + b4[run1]*h[0]*k[run1][run2];
01810                etaG3[run2] = etaG3[run2] + b4[run1]*h[0]*k2[run1][run2];
01811            }
01812        }
01813 }
01814 
01815 
01816 
01817 void IntegratorRK::determineEtaHBackward( int number_ ){
01818 
01819     int run1, run2, run3;
01820     const int ndir = rhs->getNumberOfVariables() + 1 + m;
01821 
01822         for( run1 = 0; run1 < dim; run1++ ){
01823             for( run2 = 0; run2 < ndir; run2++ ){
01824                 l[run1][run2] = 0.0;
01825             }
01826         }
01827         for( run1 = dim-1; run1 >= 0; run1--){
01828              for( run2 = 0; run2 < m; run2++ ){
01829                  H[run2] = b4[run1]*h[0]*etaH[diff_index[run2]];
01830                  for( run3 = run1+1; run3 < dim; run3++ ){
01831                       H[run2] = H[run2] + A[run3][run1]*h[0]*l[run3][diff_index[run2]];
01832                  }
01833              }
01834 
01835              if( rhs[0].AD_backward( number_+run1, H, l[run1] )!= SUCCESSFUL_RETURN ){
01836                  ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01837                  return;
01838              }
01839         }
01840 
01841     // determine etaH:
01842     // ----------------------------------------------
01843        for( run1 = 0; run1 < dim; run1++ ){
01844            for( run2 = 0; run2 < ndir; run2++ ){
01845                etaH[run2] = etaH[run2] + l[run1][run2];
01846            }
01847        }
01848 }
01849 
01850 
01851 void IntegratorRK::determineEtaHBackward2( int number_ ){
01852 
01853     int run1, run2, run3;
01854     const int ndir = rhs->getNumberOfVariables() + 1 + m;
01855 
01856     for( run1 = 0; run1 < dim; run1++ ){
01857         for( run2 = 0; run2 < ndir; run2++ ){
01858             l [run1][run2] = 0.0;
01859             l2[run1][run2] = 0.0;
01860         }
01861     }
01862 
01863     for( run1 = dim-1; run1 >= 0; run1--){
01864          for( run2 = 0; run2 < m; run2++ ){
01865              H2[run2] = b4[run1]*h[0]*etaH2[diff_index[run2]];
01866              H3[run2] = b4[run1]*h[0]*etaH3[diff_index[run2]];
01867              for( run3 = run1+1; run3 < dim; run3++ ){
01868                   H2[run2] = H2[run2] + A[run3][run1]*h[0]*l[run3][diff_index[run2]];
01869                   H3[run2] = H3[run2] + A[run3][run1]*h[0]*l2[run3][diff_index[run2]];
01870              }
01871          }
01872          if( rhs[0].AD_backward2( number_+run1, H2, H3, l[run1], l2[run1] )
01873              != SUCCESSFUL_RETURN ){
01874              ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01875               return;
01876          }
01877     }
01878 
01879     // determine etaH:
01880     // ----------------------------------------------
01881 
01882     for( run1 = 0; run1 < dim; run1++ ){
01883         for( run2 = 0; run2 < ndir; run2++ ){
01884             etaH2[run2] = etaH2[run2] + l [run1][run2];
01885             etaH3[run2] = etaH3[run2] + l2[run1][run2];
01886         }
01887     }
01888 }
01889 
01890 
01891 void IntegratorRK::printIntermediateResults(){
01892 
01893     int run1, run2;
01894 
01895         if( soa != SOA_EVERYTHING_FROZEN ){
01896             for( run1 = 0; run1 < m; run1++ ){
01897                 cout << "x[" << run1 << "] = " << eta4[run1] << "  ";
01898             }
01899             cout << endl;
01900         }
01901         else{
01902 
01903             cout << endl;
01904         }
01905 
01906         // Forward Sensitivities:
01907         // ----------------------
01908 
01909         if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ){
01910             cout << "RK: Forward Sensitivities:\n";
01911             for( run1 = 0; run1 < m; run1++ ){
01912                 cout << scientific << etaG[run1] << "  ";
01913             }
01914             cout << endl;
01915         }
01916 
01917 
01918         // 2nd Order Forward Sensitivities:
01919         // ---------------------------------
01920 
01921         if( nFDirs2 > 0 ){
01922 
01923             cout << "RK: First Order Forward Sensitivities:\n";
01924             for( run1 = 0; run1 < m; run1++ ){
01925                 cout << scientific << etaG2[run1] << "  ";
01926             }
01927             cout << endl;
01928 
01929             cout << "RK: Second Order Forward Sensitivities:\n";
01930             for( run1 = 0; run1 < m; run1++ ){
01931                 cout << scientific << etaG3[run1] << "  ";
01932             }
01933             cout << endl;
01934         }
01935 
01936         // Backward Sensitivities:
01937         // -----------------------
01938 
01939         if( nBDirs > 0 ){
01940 
01941             cout << "RK: Backward Sensitivities:\n";
01942 
01943             cout << "w.r.t. the states:\n" << scientific;
01944             for( run2 = 0; run2 < m; run2++ ){
01945                 cout << etaH[diff_index[run2]] << "  ";
01946             }
01947             cout << endl;
01948 
01949             if( mu > 0 ){
01950                 cout << "w.r.t. the controls:\n" << scientific;
01951                 for( run2 = 0; run2 < mu; run2++ ){
01952                         cout << etaH[control_index[run2]] << "  ";
01953                 }
01954                 cout << endl;
01955             }
01956             if( mp > 0 ){
01957                 cout << "w.r.t. the parameters:\n" << scientific;;
01958                 for( run2 = 0; run2 < mp; run2++ ){
01959                         cout << etaH[parameter_index[run2]] << "  ";
01960                 }
01961                 cout << endl;
01962             }
01963             if( mw > 0 ){
01964                 cout << "w.r.t. the disturbances:\n" << scientific;;
01965                 for( run2 = 0; run2 < mw; run2++ ){
01966                         cout << etaH[disturbance_index[run2]] << "  ";
01967                 }
01968                 cout << endl;
01969             }
01970         }
01971 
01972 
01973         // 2nd order Backward Sensitivities:
01974         // ---------------------------------
01975 
01976         if( nBDirs2 > 0 ){
01977 
01978             cout << "RK: First order Backward Sensitivities:\n";
01979 
01980             cout << "w.r.t. the states:\n" << scientific;;
01981             for( run2 = 0; run2 < m; run2++ ){
01982                 cout << etaH2[diff_index[run2]] << "  ";
01983             }
01984             cout << endl;
01985 
01986             if( mu > 0 ){
01987                 cout << "w.r.t. the controls:\n" << scientific;;
01988                 for( run2 = 0; run2 < mu; run2++ ){
01989                         cout << etaH2[control_index[run2]] << "  ";
01990                 }
01991                 cout << endl;
01992             }
01993             if( mp > 0 ){
01994                 cout << "w.r.t. the parameters:\n" << scientific;;
01995                 for( run2 = 0; run2 < mp; run2++ ){
01996                         cout << etaH2[parameter_index[run2]] << "  ";
01997                 }
01998                 cout << endl;
01999             }
02000             if( mw > 0 ){
02001                 cout << "w.r.t. the disturbances:\n" << scientific;;
02002                 for( run2 = 0; run2 < mw; run2++ ){
02003                         cout << etaH2[disturbance_index[run2]] << "  ";
02004                 }
02005                 cout << endl;
02006             }
02007 
02008             cout << "RK: Second order Backward Sensitivities:\n";
02009 
02010             cout << "w.r.t. the states:\n" << scientific;
02011             for( run2 = 0; run2 < m; run2++ ){
02012                 cout << etaH3[diff_index[run2]] << "  ";
02013             }
02014             cout << endl;
02015 
02016             if( mu > 0 ){
02017                 cout << "w.r.t. the controls:\n" << scientific;
02018                 for( run2 = 0; run2 < mu; run2++ ){
02019                         cout << etaH3[control_index[run2]] << "  ";
02020                 }
02021                 cout << "\n";
02022             }
02023 
02024             if( mp > 0 ){
02025                 cout << "w.r.t. the parameters:\n" << scientific;
02026                 for( run2 = 0; run2 < mp; run2++ ){
02027                         cout << etaH3[parameter_index[run2]] << "  ";
02028                 }
02029                 cout << "\n";
02030             }
02031 
02032             if( mw > 0 ){
02033                 cout << "w.r.t. the disturbances:\n" << scientific;
02034                 for( run2 = 0; run2 < mw; run2++ ){
02035                         cout << etaH3[disturbance_index[run2]] << "  ";
02036                 }
02037                 cout << "\n";
02038             }
02039      }
02040 }
02041 
02042 void IntegratorRK::interpolate( int jj, double *e1, double *d1, double *e2, VariablesGrid &poly ){
02043 
02044     int run1;
02045 
02046     for( run1 = 0; run1 < m; run1++ ){
02047 
02048         double cc = e1[run1];
02049         double bb = d1[run1];
02050         double aa = (e2[run1] - bb*h[0] - cc)/(h[0]*h[0]);
02051 
02052         double tt = timeInterval.getTime(jj) - t + h[0];
02053 
02054         poly( jj, run1 ) = aa*tt*tt + bb*tt + cc;
02055     }
02056 }
02057 
02058 
02059 int IntegratorRK::getDim() const{
02060 
02061     return m;
02062 }
02063 
02064 
02065 CLOSE_NAMESPACE_ACADO
02066 
02067 
02068 // end of file.


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