differential_equation.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/symbolic_expression/lyapunov.hpp>
00039 
00040 
00041 BEGIN_NAMESPACE_ACADO
00042 
00043 
00044 
00045 
00046 //
00047 // PUBLIC MEMBER FUNCTIONS:
00048 //
00049 
00050 DifferentialEquation::DifferentialEquation( ) : Function( ){
00051 
00052     T1 = 0;
00053     T2 = 0;
00054     t1 = 0.0;
00055     t2 = 1.0;
00056     setup();
00057 }
00058 
00059 
00060 DifferentialEquation::DifferentialEquation( const double &tStart, const double &tEnd ){
00061 
00062     T1 = 0;
00063     T2 = 0;
00064     t1 = tStart;
00065     t2 = tEnd;
00066     setup();
00067 }
00068 
00069 
00070 DifferentialEquation::DifferentialEquation( const double &tStart, const Parameter &tEnd ){
00071 
00072     ASSERT( tEnd.getDim() == 1 );
00073 
00074     T1 = 0;
00075     T2 = new Parameter(tEnd);
00076     t1 = tStart;
00077     t2 = 0.0;
00078     setup();
00079 }
00080 
00081 
00082 DifferentialEquation::DifferentialEquation( const Parameter &tStart, const double &tEnd ){
00083 
00084     ASSERT( tStart.getDim() == 1 );
00085 
00086     T1 = new Parameter(tStart);
00087     T2 = 0;
00088     t1 = 0.0;
00089     t2 = tEnd;
00090     setup();
00091 }
00092 
00093 
00094 DifferentialEquation::DifferentialEquation( const Parameter &tStart, const Parameter &tEnd ){
00095 
00096     ASSERT( tStart.getDim() == 1 );
00097     ASSERT( tEnd.getDim() == 1 );
00098 
00099     T1 = new Parameter(tStart);
00100     T2 = new Parameter(tEnd);
00101     t1 = 0.0;
00102     t2 = 0.0;
00103     setup();
00104 }
00105 
00106 
00107 void DifferentialEquation::setup(){
00108 
00109     det             = DET_UNKNOWN;
00110     is_implicit     = BT_FALSE;
00111         is_discretized  = BT_FALSE;
00112 
00113     counter   = 0;
00114     component = 0;
00115         
00116         stepLength = -INFTY;
00117 }
00118 
00119 
00120 void DifferentialEquation::copy( const DifferentialEquation &arg ){
00121 
00122     det            = arg.det        ;
00123     is_implicit    = arg.is_implicit;
00124         is_discretized = arg.is_discretized;
00125 
00126     lyap = arg.lyap;
00127     counter = arg.counter;
00128 
00129     if( arg.component == 0 ){
00130           component = 0;
00131     }
00132     else{
00133         int run1;
00134         component = (int*)calloc(counter,sizeof(int));
00135         for( run1 = 0; run1 < counter; run1++ ){
00136             component[run1] = arg.component[run1];
00137         }
00138     }
00139 
00140     if( arg.T1 != 0 ) T1 = new Parameter(*arg.T1);
00141     else              T1 = 0;
00142 
00143     if( arg.T2 != 0 ) T2 = new Parameter(*arg.T2);
00144     else              T2 = 0;
00145 
00146     t1 = arg.t1;
00147     t2 = arg.t2;
00148         
00149         stepLength = arg.stepLength;
00150 }
00151 
00152 
00153 
00154 DifferentialEquation::DifferentialEquation( const DifferentialEquation& arg )
00155                      :Function( arg ){
00156 
00157     copy( arg );
00158 }
00159 
00160 
00161 
00162 DifferentialEquation::~DifferentialEquation( ){
00163 
00164     if( component != 0 )
00165         free(component);
00166 
00167     if( T1 != 0 ) delete T1;
00168     if( T2 != 0 ) delete T2;
00169 }
00170 
00171 
00172 
00173 DifferentialEquation& DifferentialEquation::operator=( const DifferentialEquation& arg ){
00174 
00175     if ( this != &arg ){
00176 
00177         if( component != 0 )
00178             free(component);
00179 
00180         if( T1 != 0 ) delete T1;
00181         if( T2 != 0 ) delete T2;
00182 
00183         Function::operator=( arg );
00184 
00185         copy( arg );
00186     }
00187     return *this;
00188 }
00189 
00190 
00191 DifferentialEquation* DifferentialEquation::clone() const{
00192 
00193     return new DifferentialEquation(*this);
00194 }
00195 
00196 
00197 
00198 DifferentialEquation& DifferentialEquation::operator<<( const Expression& arg ){
00199 
00200     uint run1;
00201 
00202     if( arg.getVariableType() == VT_DDIFFERENTIAL_STATE ){
00203 
00204         if( det == DET_DAE ){
00205             ACADOERROR(RET_INVALID_USE_OF_FUNCTION);
00206             ASSERT(1 == 0);
00207             return *this;
00208         }
00209 
00210         component = (int*)realloc(component,(counter+arg.getDim())*sizeof(int));
00211 
00212         for( run1 = 0; run1 < arg.getDim(); run1++ ){
00213             counter++;
00214             component[counter-1] = arg.getComponent(run1);
00215         }
00216 
00217         return *this;
00218     }
00219     addDifferential( arg );
00220     if( getNDX() > 0 ) is_implicit = BT_TRUE;
00221     return *this;
00222 }
00223 
00224 
00225 DifferentialEquation& DifferentialEquation::operator<<( const int& arg ){
00226 
00227     if( arg != 0 ){
00228         ACADOWARNING(RET_INVALID_USE_OF_FUNCTION);
00229         return *this;
00230     }
00231 
00232     det = DET_DAE;
00233     return *this;
00234 }
00235 
00236 
00237 DifferentialEquation& DifferentialEquation::addDifferential( const Expression& arg ){
00238 
00239     if( T1 == 0 && T2 == 0 ){
00240        Function::operator<<(arg);
00241     }
00242     if( T1 != 0 && T2 == 0 ){
00243         Function::operator<<( (t2-T1[0])*arg );
00244     }
00245     if( T1 == 0 && T2 != 0 ){
00246         Function::operator<<( (T2[0]-t1)*arg );
00247     }
00248     if( T1 != 0 && T2 != 0 ){
00249         Function::operator<<( (T2[0]-T1[0])*arg );
00250     }
00251 
00252     if( arg.isDependingOn( VT_DDIFFERENTIAL_STATE ) == BT_TRUE ){
00253 
00254         det = DET_DAE;
00255     }
00256     else{
00257         if( det != DET_DAE )
00258             det = DET_ODE;
00259     }
00260 
00261     return *this;
00262 }
00263 
00264 
00265 
00266 DifferentialEquation& DifferentialEquation::addDifferential( const double &arg ){
00267 
00268     Expression tmp;
00269     tmp = arg;
00270     return addDifferential( tmp );
00271 }
00272 
00273 
00274 DifferentialEquation& DifferentialEquation::addAlgebraic( const Expression& arg ){
00275 
00276     Function::operator<<(arg);
00277     det = DET_DAE;
00278     return *this;
00279 }
00280 
00281 
00282 DifferentialEquation& DifferentialEquation::addAlgebraic( const double &arg ){
00283 
00284     det = DET_DAE;
00285     if( fabs(arg) < EPS ) return *this;
00286     ACADOWARNING( RET_INFEASIBLE_ALGEBRAIC_CONSTRAINT );
00287     return *this;
00288 }
00289 
00290 
00291 DifferentialEquation& DifferentialEquation::addDifferential( const DVector& arg ){
00292 
00293     uint run1;
00294 
00295     for( run1 = 0; run1 < arg.getDim(); run1++ )
00296         addDifferential( arg(run1) );
00297 
00298     return *this;
00299 }
00300 
00301 
00302 DifferentialEquation& DifferentialEquation::addAlgebraic( const DVector& arg ){
00303 
00304     uint run1;
00305 
00306     for( run1 = 0; run1 < arg.getDim(); run1++ )
00307         addAlgebraic( arg(run1) );
00308 
00309     return *this;
00310 }
00311 
00312 
00313 DifferentialEquation& DifferentialEquation::addDifferential( const DMatrix& arg ){
00314 
00315     uint run1, run2;
00316 
00317     for( run1 = 0; run1 < arg.getNumRows(); run1++ )
00318         for( run2 = 0; run2 < arg.getNumCols(); run2++ )
00319             addDifferential( arg.operator()(run1,run2) );
00320 
00321     return *this;
00322 }
00323 
00324 
00325 DifferentialEquation& DifferentialEquation::addAlgebraic( const DMatrix& arg ){
00326 
00327     uint run1, run2;
00328 
00329     for( run1 = 0; run1 < arg.getNumRows(); run1++ )
00330         for( run2 = 0; run2 < arg.getNumCols(); run2++ )
00331             addAlgebraic( arg.operator()(run1,run2) );
00332 
00333     return *this;
00334 }
00335 
00336 
00337 DifferentialEquation& DifferentialEquation::operator==(const Lyapunov& arg ){
00338 
00339   lyap = arg;
00340 
00341   return operator==( arg.A*arg.P+arg.P*(arg.A).transpose()+(arg.B)*(arg.B).transpose() );
00342 }
00343 
00344 
00345 
00346 Lyapunov DifferentialEquation::getLyapunovObject( ) const{
00347  
00348   return lyap;
00349 }
00350 
00351 BooleanType DifferentialEquation::hasLyapunovEquation( ) const
00352 {
00353     if( lyap.isEmpty() == BT_TRUE ) return BT_FALSE;
00354     return BT_TRUE;
00355     }
00356 
00357 
00358 
00359 DifferentialEquation& DifferentialEquation::operator==( const Expression& arg ){
00360 
00361     if( det == DET_DAE ) return addAlgebraic(arg);
00362     return addDifferential(arg);
00363 }
00364 
00365 
00366 DifferentialEquation& DifferentialEquation::operator==( const double &arg ){
00367 
00368     if( det == DET_DAE ) return addAlgebraic(arg);
00369     return addDifferential(arg);
00370 }
00371 
00372 
00373 DifferentialEquation& DifferentialEquation::operator==( const DVector& arg ){
00374 
00375     if( det == DET_DAE ) return addAlgebraic(arg);
00376     return addDifferential(arg);
00377 }
00378 
00379 
00380 DifferentialEquation& DifferentialEquation::operator==( const DMatrix& arg ){
00381 
00382     if( det == DET_DAE ) return addAlgebraic(arg);
00383     return addDifferential(arg);
00384 }
00385 
00386 
00387 
00388 BooleanType DifferentialEquation::isDAE( ) const
00389 {
00390     if( getNumAlgebraicEquations() > 0 )  return BT_TRUE;
00391     return BT_FALSE;
00392 }
00393 
00394 
00395 BooleanType DifferentialEquation::isODE( ) const
00396 {
00397     if( getNumAlgebraicEquations() == 0 )  return BT_TRUE;
00398     return BT_FALSE;
00399 }
00400 
00401 
00402 Expression DifferentialEquation::getODEexpansion( const int &order ) const{
00403 
00404         if ( order < 0 ){ ACADOERROR( RET_INDEX_OUT_OF_BOUNDS ); return 0; }
00405         
00406         Expression rhs;
00407         getExpression(rhs);
00408         
00409         return rhs.getODEexpansion( order, component );
00410 }
00411 
00412 
00413 
00414 BooleanType DifferentialEquation::makeImplicit(){
00415 
00416     if ( is_implicit == BT_FALSE ){
00417 
00418         evaluationTree.makeImplicit( getDim() - getNumAlgebraicEquations() );
00419         is_implicit = BT_TRUE;
00420 
00421         return BT_TRUE;
00422     }
00423     return BT_FALSE;
00424 }
00425 
00426 
00427 BooleanType DifferentialEquation::isDiscretized( ) const{
00428 
00429     return is_discretized;
00430 }
00431 
00432 
00433 double DifferentialEquation::getStepLength() const{
00434 
00435     return stepLength;
00436 }
00437 
00438 
00439 CLOSE_NAMESPACE_ACADO
00440 
00441 // end of file.


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