integrator_discretized_ode.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 
00033 #include <acado/utils/acado_utils.hpp>
00034 #include <acado/matrix_vector/matrix_vector.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/function/discretized_differential_equation.hpp>
00039 #include <acado/integrator/integrator.hpp>
00040 #include <acado/integrator/integrator_runge_kutta.hpp>
00041 #include <acado/integrator/integrator_runge_kutta12.hpp>
00042 #include <acado/integrator/integrator_discretized_ode.hpp>
00043 
00044 
00045 
00046 BEGIN_NAMESPACE_ACADO
00047 
00048 
00049 //
00050 // PUBLIC MEMBER FUNCTIONS:
00051 //
00052 
00053 IntegratorDiscretizedODE::IntegratorDiscretizedODE( )
00054                          :IntegratorRK12( ){
00055 }
00056 
00057 
00058 IntegratorDiscretizedODE::IntegratorDiscretizedODE( const DifferentialEquation &rhs_ )
00059                          :IntegratorRK12( rhs_ ){
00060 
00061     if ( rhs_.isDiscretized( ) == BT_FALSE ){
00062         ACADOERROR( RET_CANNOT_TREAT_CONTINUOUS_DE );
00063         ASSERT( 1 == 0 );
00064     }
00065     if( rhs_.isDiscretized( ) == BT_FALSE ){
00066         ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00067         ASSERT( 1 == 0 );
00068     }
00069 
00070     stepLength = rhs_.getStepLength();
00071 }
00072 
00073 
00074 IntegratorDiscretizedODE::IntegratorDiscretizedODE( const IntegratorDiscretizedODE& arg )
00075                          :IntegratorRK12( arg ){
00076 
00077     stepLength = arg.stepLength;
00078 }
00079 
00080 
00081 IntegratorDiscretizedODE::~IntegratorDiscretizedODE( ){
00082 
00083 }
00084 
00085 
00086 IntegratorDiscretizedODE& IntegratorDiscretizedODE::operator=( const IntegratorDiscretizedODE& arg ){
00087 
00088     if( this != &arg ){
00089         IntegratorRK12::operator=( arg );
00090         stepLength = arg.stepLength;
00091     }
00092     return *this;
00093 }
00094 
00095 
00096 Integrator* IntegratorDiscretizedODE::clone() const{
00097 
00098     return new IntegratorDiscretizedODE(*this);
00099 }
00100 
00101 
00102 
00103 returnValue IntegratorDiscretizedODE::init( const DifferentialEquation &rhs_ )
00104 {
00105         stepLength = rhs_.getStepLength( );
00106         return IntegratorRK12::init( rhs_ );
00107 }
00108 
00109 
00110 
00111 returnValue IntegratorDiscretizedODE::step(int number){
00112 
00113     // DEFINE SOME LOCAL VARIABLES:
00114     // ----------------------------
00115 
00116     int         run1       ;   // simple run variable
00117     returnValue returnvalue;   // return value for error handling
00118 
00119 
00120     // PERFORM ONE STEP OF THE ITERATION:
00121     // ----------------------------------
00122 
00123     if( soa == SOA_FREEZING_ALL )  returnvalue = performDiscreteStep(number);
00124     else                           returnvalue = performDiscreteStep( 0    );
00125 
00126     if( returnvalue != SUCCESSFUL_RETURN ) return ACADOWARNING( returnvalue );
00127 
00128 
00129 
00130     // COMPUTE DERIVATIVES IF REQUESTED:
00131     // ---------------------------------
00132 
00133     if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ){
00134 
00135         if( soa != SOA_EVERYTHING_FROZEN ){
00136             return ACADOERROR(RET_NOT_FROZEN);
00137         }
00138         if( nBDirs != 0 ){
00139             return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
00140         }
00141         performADforwardStep(number);
00142     }
00143     if( nBDirs > 0 ){
00144 
00145         if( soa != SOA_EVERYTHING_FROZEN ){
00146             return ACADOERROR(RET_NOT_FROZEN);
00147         }
00148         if( nFDirs != 0 || nBDirs2 != 0 || nFDirs2 != 0 ){
00149             return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
00150         }
00151         performADbackwardStep(number);
00152     }
00153     if( nFDirs2 > 0 ){
00154 
00155         if( soa != SOA_EVERYTHING_FROZEN ){
00156             return ACADOERROR(RET_NOT_FROZEN);
00157         }
00158         if( nBDirs != 0 || nBDirs2 != 0 || nFDirs != 1 ){
00159             return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
00160         }
00161         performADforwardStep2(number);
00162     }
00163     if( nBDirs2 > 0 ){
00164 
00165         if( soa != SOA_EVERYTHING_FROZEN ){
00166             return ACADOERROR(RET_NOT_FROZEN);
00167         }
00168         if( nBDirs != 0 || nFDirs2 != 0 || nFDirs != 1 ){
00169             return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
00170         }
00171         performADbackwardStep2(number);
00172     }
00173 
00174 
00175      // ACTUALLY INCREASE THE TIME:
00176      // ---------------------------
00177 
00178      if( nBDirs > 0 || nBDirs2 > 0 ){
00179 
00180          t = t - stepLength;   // IN BACKWARD THE TIME IS RUNNING BACKWARDS.
00181      }
00182      else{
00183 
00184          t = t + stepLength;   // IN FORWARD MODE THE TIME IS RUNNING FORWARDS.
00185      }
00186 
00187 
00188 
00189     // STORE THE INTERMEDIATE VALUES IF THIS IS REQUESTED:
00190     // ---------------------------------------------------
00191 
00192     if( soa == SOA_FREEZING_MESH || soa == SOA_FREEZING_ALL || soa == SOA_MESH_FROZEN_FREEZING_ALL ){
00193 
00194         if( number >= maxAlloc){
00195             maxAlloc = 2*maxAlloc;
00196             h = (double*)realloc(h,maxAlloc*sizeof(double));
00197         }
00198         h[number] = stepLength;
00199     }
00200 
00201 
00202      if( nFDirs == 0 && nBDirs == 0 && nFDirs2 == 0 && nBDirs == 0 ){
00203 
00204          int i1 = timeInterval.getFloorIndex( t-h[0] );
00205          int i2 = timeInterval.getFloorIndex( t      );
00206          int jj;
00207 
00208          for( jj = i1+1; jj <= i2; jj++ ){
00209 
00210              for( run1 = 0; run1 < m; run1++ )
00211                  xStore( jj, run1 ) = eta4[run1];
00212 
00213              for( run1 = 0; run1 < mn; run1++ )
00214                  iStore( jj, run1 ) = x[rhs->index( VT_INTERMEDIATE_STATE, run1 )];
00215          }
00216      }
00217 
00218 
00219     // CHECK WHETHER THE END OF THE TIME HORIZON IS ALREADY ACHIEVED:
00220     // --------------------------------------------------------------
00221 
00222 
00223     if( nBDirs == 0 || nBDirs2 == 0 ){
00224 
00225         // Stop the algorithm if  t >= tend:
00226         // ----------------------------------------------
00227         if( t >= timeInterval.getLastTime() - 0.5*stepLength ){
00228             x[time_index] = timeInterval.getLastTime();
00229             for( run1 = 0; run1 < m; run1++ ){
00230                 x[diff_index[run1]] = eta4[run1];
00231             }
00232             if( soa == SOA_FREEZING_MESH ){
00233                 soa = SOA_MESH_FROZEN;
00234             }
00235             if( soa == SOA_FREEZING_ALL || soa == SOA_MESH_FROZEN_FREEZING_ALL ){
00236                 soa = SOA_EVERYTHING_FROZEN;
00237             }
00238 
00239             return SUCCESSFUL_RETURN;
00240         }
00241     }
00242 
00243 
00244     return RET_FINAL_STEP_NOT_PERFORMED_YET;
00245 }
00246 
00247 
00248 
00249 
00250 
00251 // PROTECTED ROUTINES:
00252 // -------------------
00253 
00254 
00255 returnValue IntegratorDiscretizedODE::performDiscreteStep( const int& number_ ){
00256 
00257     int run1;
00258 
00259     x[time_index] = t;
00260 
00261     for( run1 = 0; run1 < m; run1++ )
00262         x[diff_index[run1]] = eta4[run1];
00263 
00264     if( rhs[0].evaluate( number_, x, k[0] ) != SUCCESSFUL_RETURN )
00265         return RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45;
00266 
00267     for( run1 = 0; run1 < m; run1++ )
00268         eta4[run1] = k[0][run1];
00269 
00270     return SUCCESSFUL_RETURN;
00271 }
00272 
00273 
00274 
00275 returnValue IntegratorDiscretizedODE::performADforwardStep( const int& number_ ){
00276 
00277     int run1, run2;
00278 
00279     for( run1 = 0; run1 < nFDirs; run1++ ){
00280 
00281         for( run2 = 0; run2 < m; run2++ )
00282             G[diff_index[run2]] = etaG[run2];
00283 
00284         if( rhs[0].AD_forward( number_, G, k[0] ) != SUCCESSFUL_RETURN )
00285             return RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45;
00286 
00287         for( run2 = 0; run2 < m; run2++ )
00288             etaG[run2] = k[0][run2];
00289     }
00290 
00291     return SUCCESSFUL_RETURN;
00292 }
00293 
00294 
00295 
00296 returnValue IntegratorDiscretizedODE::performADbackwardStep( const int& number_ ){
00297 
00298     int run2;
00299     const int ndir = rhs->getNumberOfVariables() + 1 + m;
00300 
00301     for( run2 = 0; run2 < ndir; run2++ )
00302         l[0][run2] = 0.0;
00303 
00304     for( run2 = 0; run2 < m; run2++ )
00305         H[run2] = etaH[diff_index[run2]];
00306 
00307     if( rhs[0].AD_backward( number_, H, l[0] )!= SUCCESSFUL_RETURN )
00308         return RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45;
00309 
00310     for( run2 = 0; run2 < ndir; run2++ )
00311         etaH[run2] += l[0][run2];
00312 
00313     for( run2 = 0; run2 < m; run2++ )
00314         etaH[diff_index[run2]] = l[0][diff_index[run2]];
00315 
00316     return SUCCESSFUL_RETURN;
00317 }
00318 
00319 
00320 
00321 returnValue IntegratorDiscretizedODE::performADforwardStep2( const int& number_ ){
00322 
00323     int run2;
00324 
00325     for( run2 = 0; run2 < m; run2++ ){
00326         G2[diff_index[run2]] = etaG2[run2];
00327         G3[diff_index[run2]] = etaG3[run2];
00328     }
00329 
00330     if( rhs[0].AD_forward2( number_, G2, G3, k[0], k2[0] ) != SUCCESSFUL_RETURN )
00331         return RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45;
00332 
00333     for( run2 = 0; run2 < m; run2++ ){
00334         etaG2[run2] = k [0][run2];
00335         etaG3[run2] = k2[0][run2];
00336     }
00337 
00338     return SUCCESSFUL_RETURN;
00339 }
00340 
00341 
00342 
00343 returnValue IntegratorDiscretizedODE::performADbackwardStep2( const int& number_ ){
00344 
00345     int run2;
00346     const int ndir = rhs->getNumberOfVariables() + 1 + m;
00347 
00348     for( run2 = 0; run2 < ndir; run2++ ){
00349         l [0][run2] = 0.0;
00350         l2[0][run2] = 0.0;
00351     }
00352 
00353     for( run2 = 0; run2 < m; run2++ ){
00354         H2[run2] = etaH2[diff_index[run2]];
00355         H3[run2] = etaH3[diff_index[run2]];
00356     }
00357 
00358     if( rhs[0].AD_backward2( number_, H2, H3, l[0], l2[0] )!= SUCCESSFUL_RETURN )
00359         return RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45;
00360 
00361     for( run2 = 0; run2 < ndir; run2++ ){
00362         etaH2[run2] += l [0][run2];
00363         etaH3[run2] += l2[0][run2];
00364     }
00365     for( run2 = 0; run2 < m; run2++ ){
00366         etaH2[diff_index[run2]] = l [0][diff_index[run2]];
00367         etaH3[diff_index[run2]] = l2[0][diff_index[run2]];
00368     }
00369 
00370     return SUCCESSFUL_RETURN;
00371 }
00372 
00373 
00374 
00375 CLOSE_NAMESPACE_ACADO
00376 
00377 
00378 // end of file.


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