dynamic_system.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 
00033 #include <acado/dynamic_system/dynamic_system.hpp>
00034 
00035 
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 //
00041 // PUBLIC MEMBER FUNCTIONS:
00042 //
00043 
00044 DynamicSystem::DynamicSystem( )
00045 {
00046         nDiffEqn   = 0;
00047         nSwitchFcn = 0;
00048 
00049         diffEqn = 0;
00050         outputFcn = 0;
00051 
00052         switchFcn = 0;
00053         selectFcn = 0;
00054 }
00055 
00056 
00057 DynamicSystem::DynamicSystem(   const DifferentialEquation& _diffEqn
00058                                                                 )
00059 {
00060     OutputFcn _outputFcn;
00061 
00062         nDiffEqn   = 1;
00063         nSwitchFcn = 0;
00064 
00065         diffEqn    = (DifferentialEquation**) calloc( 1,sizeof(DifferentialEquation*) );
00066         diffEqn[0] = _diffEqn.clone();
00067 
00068         outputFcn    = (OutputFcn**) calloc( 1,sizeof(OutputFcn*) );
00069         outputFcn[0] = new OutputFcn( _outputFcn );
00070 
00071         switchFcn = 0;
00072         selectFcn = 0;
00073 }
00074 
00075 
00076 DynamicSystem::DynamicSystem(   const DifferentialEquation& _diffEqn,
00077                                                                 const OutputFcn& _outputFcn
00078                                                                 )
00079 {
00080         nDiffEqn   = 1;
00081         nSwitchFcn = 0;
00082 
00083         diffEqn    = (DifferentialEquation**) calloc( 1,sizeof(DifferentialEquation*) );
00084         diffEqn[0] = _diffEqn.clone();
00085 
00086         outputFcn    = (OutputFcn**) calloc( 1,sizeof(OutputFcn*) );
00087         outputFcn[0] = new OutputFcn( _outputFcn );
00088 
00089         switchFcn = 0;
00090         selectFcn = 0;
00091 }
00092 
00093 
00094 DynamicSystem::DynamicSystem( const DynamicSystem& rhs )
00095 {
00096         nDiffEqn   = rhs.nDiffEqn;
00097         nSwitchFcn = rhs.nSwitchFcn;
00098 
00099         if ( ( rhs.diffEqn != 0 ) && ( rhs.outputFcn != 0 ) )
00100         {
00101                 diffEqn = (DifferentialEquation**) calloc( nDiffEqn,sizeof(DifferentialEquation*) );
00102                 for( uint i=0; i<nDiffEqn; ++i )
00103                         diffEqn[i] = (rhs.diffEqn[i])->clone();
00104 
00105                 outputFcn = (OutputFcn**) calloc( nDiffEqn,sizeof(OutputFcn*) );
00106                 for( uint i=0; i<nDiffEqn; ++i )
00107                         outputFcn[i] = new OutputFcn( *(rhs.outputFcn[i]) );
00108         }
00109         else
00110         {
00111                 nDiffEqn = 0;
00112 
00113                 diffEqn = 0;
00114                 outputFcn = 0;
00115         }
00116 
00117         if ( rhs.switchFcn != 0 )
00118         {
00119                 switchFcn = (Function**) calloc( nSwitchFcn,sizeof(Function*) );
00120                 for( uint i=0; i<nSwitchFcn; ++i )
00121                         switchFcn[i] = new Function( *(rhs.switchFcn[i]) );
00122         }
00123         else
00124         {
00125                 switchFcn = 0;
00126                 nSwitchFcn = 0;
00127         }
00128 
00129         if ( rhs.selectFcn != 0 )
00130                 selectFcn = new Function( *(rhs.selectFcn) );
00131         else
00132                 selectFcn = 0;
00133 }
00134 
00135 
00136 DynamicSystem::~DynamicSystem( )
00137 {
00138         if ( diffEqn != 0 )
00139         {
00140                 for( uint i=0; i<nDiffEqn; ++i )
00141                         delete diffEqn[i];
00142                 free( diffEqn );
00143         }
00144 
00145         if ( outputFcn != 0 )
00146         {
00147                 for( uint i=0; i<nDiffEqn; ++i )
00148                         delete outputFcn[i];
00149                 free( outputFcn );
00150         }
00151 
00152         if ( switchFcn != 0 )
00153         {
00154                 for( uint i=0; i<nSwitchFcn; ++i )
00155                         delete switchFcn[i];
00156                 free( switchFcn );
00157         }
00158 
00159         if ( selectFcn != 0 )
00160                 delete selectFcn;
00161 }
00162 
00163 
00164 DynamicSystem& DynamicSystem::operator=( const DynamicSystem& rhs )
00165 {
00166         if ( this != &rhs )
00167         {
00168                 if ( diffEqn != 0 )
00169                 {
00170                         for( uint i=0; i<nDiffEqn; ++i )
00171                                 delete diffEqn[i];
00172                         free( diffEqn );
00173                 }
00174         
00175                 if ( outputFcn != 0 )
00176                 {
00177                         for( uint i=0; i<nDiffEqn; ++i )
00178                                 delete outputFcn[i];
00179                         free( outputFcn );
00180                 }
00181         
00182                 if ( switchFcn != 0 )
00183                 {
00184                         for( uint i=0; i<nSwitchFcn; ++i )
00185                                 delete switchFcn[i];
00186                         free( switchFcn );
00187                 }
00188         
00189                 if ( selectFcn != 0 )
00190                         delete selectFcn;
00191 
00192 
00193                 nDiffEqn   = rhs.nDiffEqn;
00194                 nSwitchFcn = rhs.nSwitchFcn;
00195         
00196                 if ( ( rhs.diffEqn != 0 ) && ( rhs.outputFcn != 0 ) )
00197                 {
00198                         diffEqn = (DifferentialEquation**) calloc( nDiffEqn,sizeof(DifferentialEquation*) );
00199                         for( uint i=0; i<nDiffEqn; ++i )
00200                                 diffEqn[i] = (rhs.diffEqn[i])->clone();
00201         
00202                         outputFcn = (OutputFcn**) calloc( nDiffEqn,sizeof(OutputFcn*) );
00203                         for( uint i=0; i<nDiffEqn; ++i )
00204                                 outputFcn[i] = new OutputFcn( *(rhs.outputFcn[i]) );
00205                 }
00206                 else
00207                 {
00208                         nDiffEqn = 0;
00209         
00210                         diffEqn = 0;
00211                         outputFcn = 0;
00212                 }
00213         
00214                 if ( rhs.switchFcn != 0 )
00215                 {
00216                         switchFcn = (Function**) calloc( nSwitchFcn,sizeof(Function*) );
00217                         for( uint i=0; i<nSwitchFcn; ++i )
00218                                 switchFcn[i] = new Function( *(rhs.switchFcn[i]) );
00219                 }
00220                 else
00221                 {
00222                         switchFcn = 0;
00223                         nSwitchFcn = 0;
00224                 }
00225         
00226                 if ( rhs.selectFcn != 0 )
00227                         selectFcn = new Function( *(rhs.selectFcn) );
00228                 else
00229                         selectFcn = 0;
00230         }
00231 
00232         return *this;
00233 }
00234 
00235 
00236 
00237 returnValue DynamicSystem::addSubsystem( const DifferentialEquation& _diffEqn )
00238 {
00239         OutputFcn emptyOutputFcn;
00240         return addSubsystem( _diffEqn,emptyOutputFcn );
00241 }
00242 
00243 
00244 returnValue DynamicSystem::addSubsystem(        const DifferentialEquation& _diffEqn,
00245                                                                                         const OutputFcn& _outputFcn
00246                                                                                         )
00247 {
00248         if ( isConsistentDiffEqn( _diffEqn ) == BT_FALSE )
00249                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00250 
00251         if ( isConsistentOutputFcn( _outputFcn ) == BT_FALSE )
00252                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00253 
00254         ++nDiffEqn;
00255 
00256         diffEqn = (DifferentialEquation**) realloc( diffEqn,nDiffEqn*sizeof(DifferentialEquation*) );
00257         diffEqn[ nDiffEqn-1 ] = _diffEqn.clone();
00258 
00259         outputFcn = (OutputFcn**) realloc( outputFcn,nDiffEqn*sizeof(OutputFcn*) );
00260         outputFcn[ nDiffEqn-1 ] = new OutputFcn( _outputFcn );
00261 
00262         //return SUCCESSFUL_RETURN;
00263         return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00264 }
00265 
00266 
00267 
00268 returnValue DynamicSystem::addSwitchFunction(   const Function& _switchFcn
00269                                                                                                 )
00270 {
00271         ++nSwitchFcn;
00272 
00273         switchFcn = (Function**) realloc( switchFcn,nSwitchFcn*sizeof(Function*) );
00274         switchFcn[ nSwitchFcn-1 ] = new Function( _switchFcn );
00275 
00276         //return SUCCESSFUL_RETURN;
00277         return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00278 }
00279 
00280 
00281 
00282 returnValue DynamicSystem::setSelectFunction(   const Function& _selectFcn
00283                                                                                                 )
00284 {
00285         if ( selectFcn == 0 )
00286                 selectFcn = new Function( _selectFcn );
00287         else
00288                 *selectFcn = _selectFcn;
00289 
00290         //return SUCCESSFUL_RETURN;
00291         return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00292 }
00293 
00294 
00295 
00296 //
00297 // PROTECTED MEMBER FUNCTIONS:
00298 //
00299 
00300 BooleanType DynamicSystem::isConsistentDiffEqn( const DifferentialEquation& _diffEqn
00301                                                                                                 ) const
00302 {
00303         if ( nDiffEqn == 0 )
00304                 return BT_TRUE;
00305 
00306         if ( diffEqn[0]->getNumDynamicEquations( ) != _diffEqn.getNumDynamicEquations( ) )
00307                 return BT_FALSE;
00308 
00309         if ( diffEqn[0]->getNumAlgebraicEquations( ) != _diffEqn.getNumAlgebraicEquations( ) )
00310                 return BT_FALSE;
00311 
00312         if ( diffEqn[0]->isDiscretized( ) != _diffEqn.isDiscretized( ) )
00313                 return BT_FALSE;
00314 
00315         return BT_TRUE;
00316 }
00317 
00318 
00319 BooleanType DynamicSystem::isConsistentOutputFcn(       const OutputFcn& _outputFcn
00320                                                                                                         ) const
00321 {
00322         if ( nDiffEqn == 0 )
00323                 return BT_TRUE;
00324 
00325         if ( outputFcn[0]->getDim( ) != _outputFcn.getDim( ) )
00326                 return BT_FALSE;
00327 
00328         if ( ( diffEqn[0]->isODE( ) == BT_TRUE ) && ( _outputFcn.getNXA( ) > 0 ) )
00329                 return BT_FALSE;
00330 
00331         return BT_TRUE;
00332 }
00333 
00334 
00335 
00336 CLOSE_NAMESPACE_ACADO
00337 
00338 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:58:07