process.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 
00034 #include <acado/process/process.hpp>
00035 
00036 #ifdef WIN32
00037 #define round( value ) floor( value + 0.5 )
00038 #endif
00039 
00040 BEGIN_NAMESPACE_ACADO
00041 
00042 
00043 // #define SIM_DEBUG
00044 
00045 
00046 //
00047 // PUBLIC MEMBER FUNCTIONS:
00048 //
00049 
00050 Process::Process( ) : SimulationBlock( BN_PROCESS )
00051 {
00052         setupOptions( );
00053         setupLogging( );
00054 
00055         nDynSys        = 0;
00056         dynamicSystems = 0;
00057 
00058         integrationMethod = 0;
00059 
00060         actuator = 0;
00061         sensor   = 0;
00062 
00063         processDisturbance = 0;
00064 
00065         lastTime = 0.0;
00066 
00067         setStatus( BS_NOT_INITIALIZED );
00068 }
00069 
00070 
00071 Process::Process(       const DynamicSystem& _dynamicSystem,
00072                                         IntegratorType _integratorType
00073                                         ) : SimulationBlock( BN_PROCESS )
00074 {
00075         setupOptions( );
00076         setupLogging( );
00077 
00078         nDynSys        = 0;
00079         dynamicSystems = 0;
00080 
00081         integrationMethod = 0;
00082 
00083         actuator = 0;
00084         sensor   = 0;
00085 
00086         processDisturbance = 0;
00087 
00088         if ( _dynamicSystem.getNumDynamicEquations( ) > 0 )
00089         {
00090                 returnValue returnvalue = setDynamicSystem( _dynamicSystem,_integratorType );
00091                 ASSERT( returnvalue == SUCCESSFUL_RETURN );
00092         }
00093 
00094         lastTime = 0.0;
00095 
00096         setStatus( BS_NOT_INITIALIZED );
00097 }
00098 
00099 
00100 Process::Process( const Process& rhs ) : SimulationBlock( rhs )
00101 {
00102         x  = rhs.x;
00103         xa = rhs.xa;
00104 
00105         if ( rhs.dynamicSystems != 0 )
00106         {
00107                 nDynSys = rhs.nDynSys;
00108                 dynamicSystems = (DynamicSystem**) calloc( rhs.nDynSys,sizeof(DynamicSystem*) );
00109                 for( uint i=0; i<rhs.nDynSys; ++i )
00110                         dynamicSystems[i] = new DynamicSystem( *(rhs.dynamicSystems[i]) );
00111         }
00112         else
00113         {
00114                 nDynSys        = 0;
00115                 dynamicSystems = 0;
00116         }
00117 
00118         if ( rhs.integrationMethod != 0 )
00119                 integrationMethod = new ShootingMethod( *(rhs.integrationMethod) );
00120         else
00121                 integrationMethod = 0;
00122 
00123         if ( rhs.actuator != 0 )
00124                 actuator = new Actuator( *(rhs.actuator) );
00125         else
00126                 actuator = 0;
00127 
00128         if ( rhs.sensor != 0 )
00129                 sensor = new Sensor( *(rhs.sensor) );
00130         else
00131                 sensor = 0;
00132 
00133         if ( rhs.processDisturbance != 0 )
00134                 processDisturbance = new Curve( *(rhs.processDisturbance) );
00135         else
00136                 processDisturbance = 0;
00137 
00138         y = rhs.y;
00139 
00140         lastTime = rhs.lastTime;
00141 }
00142 
00143 
00144 Process::~Process( )
00145 {
00146         clear( );
00147 }
00148 
00149 
00150 Process& Process::operator=( const Process& rhs )
00151 {
00152     if ( this != &rhs )
00153     {
00154                 clear( );
00155 
00156                 SimulationBlock::operator=( rhs );
00157 
00158                 x  = rhs.x;
00159                 xa = rhs.xa;
00160 
00161                 if ( rhs.dynamicSystems != 0 )
00162                 {
00163                         nDynSys = rhs.nDynSys;
00164                         dynamicSystems = (DynamicSystem**) calloc( rhs.nDynSys,sizeof(DynamicSystem*) );
00165                         for( uint i=0; i<rhs.nDynSys; ++i )
00166                                 dynamicSystems[i] = new DynamicSystem( *(rhs.dynamicSystems[i]) );
00167                 }
00168                 else
00169                 {
00170                         nDynSys        = 0;
00171                         dynamicSystems = 0;
00172                 }
00173 
00174                 if ( rhs.integrationMethod != 0 )
00175                         integrationMethod = new ShootingMethod( *(rhs.integrationMethod) );
00176                 else
00177                         integrationMethod = 0;
00178         
00179                 if ( rhs.actuator != 0 )
00180                         actuator = new Actuator( *(rhs.actuator) );
00181                 else
00182                         actuator = 0;
00183 
00184                 if ( rhs.sensor != 0 )
00185                         sensor = new Sensor( *(rhs.sensor) );
00186                 else
00187                         sensor = 0;
00188 
00189                 if ( rhs.processDisturbance != 0 )
00190                         processDisturbance = new Curve( *(rhs.processDisturbance) );
00191                 else
00192                         processDisturbance = 0;
00193 
00194                 y = rhs.y;
00195 
00196                 lastTime = rhs.lastTime;
00197     }
00198 
00199     return *this;
00200 }
00201 
00202 
00203 
00204 returnValue Process::setDynamicSystem(  const DynamicSystem& _dynamicSystem,
00205                                                                                 IntegratorType _integratorType
00206                                                                                 )
00207 {
00208         if ( _dynamicSystem.hasImplicitSwitches( ) == BT_TRUE )
00209                 return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00210 
00211         // setup simulation algorithm
00212         if ( integrationMethod != 0 )
00213                 delete integrationMethod;
00214 
00215         integrationMethod = new ShootingMethod( this );
00216 
00217     Grid dummy( 0.0, 1.0 );   // bad idea ... QUICK HACK
00218 
00219         addStage( _dynamicSystem, dummy, _integratorType );
00220 
00221 
00222         integratorType = _integratorType;   // weird hack -- make clean later.
00223 
00224 // NOTE: THE "integratorType" can
00225 //          change during the use of the process, it might be better to load the integrator type from the options
00226 //          every time the routine "simulate" is called. This enables the user to change options online.
00227 // (SORRY, THIS IS NOT YET IMPLEMENTED - AT THE MOMENT THE  "integratorType"  GETS LOST.)
00228 
00229         y.init( _dynamicSystem.getNumOutputs( ),1 );
00230         y.setZero( );
00231 
00232         setStatus( BS_NOT_INITIALIZED );
00233 
00234         return SUCCESSFUL_RETURN;
00235 }
00236 
00237 
00238 returnValue Process::addDynamicSystemStage(     const DynamicSystem& _dynamicSystem,
00239                                                                                         IntegratorType _integratorType
00240                                                                                         )
00241 {
00242         // setup simulation algorithm
00243         if ( integrationMethod == 0 )
00244                 return setDynamicSystem( _dynamicSystem,_integratorType );
00245 
00246         if ( getNumStages( ) == 0 )
00247                 return setDynamicSystem( _dynamicSystem,_integratorType );
00248         else
00249                 return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00250 }
00251 
00252 
00253 
00254 returnValue Process::setActuator(       const Actuator& _actuator
00255                                                                         )
00256 {
00257         if ( _actuator.isDefined( ) == BT_TRUE )
00258         {
00259                 if ( actuator != 0 )
00260                         *actuator = _actuator;
00261                 else
00262                         actuator = new Actuator( _actuator );
00263 
00264                 setStatus( BS_NOT_INITIALIZED );
00265         }
00266 
00267         return SUCCESSFUL_RETURN;
00268 }
00269 
00270 
00271 returnValue Process::setSensor( const Sensor& _sensor
00272                                                                 )
00273 {
00274         if ( _sensor.isDefined( ) == BT_TRUE )
00275         {
00276                 if ( sensor != 0 )
00277                         *sensor = _sensor;
00278                 else
00279                         sensor = new Sensor( _sensor );
00280 
00281                 setStatus( BS_NOT_INITIALIZED );
00282         }
00283 
00284         return SUCCESSFUL_RETURN;
00285 }
00286 
00287 
00288 
00289 returnValue Process::setProcessDisturbance(     const Curve& _processDisturbance
00290                                                                                         )
00291 {
00292         if ( processDisturbance != 0 )
00293                 *processDisturbance = _processDisturbance;
00294         else
00295                 processDisturbance = new Curve( _processDisturbance );
00296 
00297         setStatus( BS_NOT_INITIALIZED );
00298 
00299         return SUCCESSFUL_RETURN;
00300 }
00301 
00302 
00303 returnValue Process::setProcessDisturbance(     const VariablesGrid& _processDisturbance
00304                                                                                         )
00305 {
00306         if ( _processDisturbance.isEmpty( ) == BT_TRUE )
00307                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00308 
00309     Curve _processDisturbanceCurve;
00310     _processDisturbanceCurve.add( _processDisturbance );
00311 
00312         return setProcessDisturbance( _processDisturbanceCurve );
00313 }
00314 
00315 
00316 returnValue Process::setProcessDisturbance(     const char* _processDisturbance
00317                                                                                         )
00318 {
00319     VariablesGrid _processDisturbanceFromFile;
00320     _processDisturbanceFromFile.read( _processDisturbance );
00321 
00322         if ( _processDisturbanceFromFile.isEmpty( ) == BT_TRUE )
00323                 return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00324 
00325         return setProcessDisturbance( _processDisturbanceFromFile );
00326 }
00327 
00328 
00329 
00330 returnValue Process::initializeStartValues(     const DVector& _xStart,
00331                                                                                         const DVector& _xaStart
00332                                                                                         )
00333 {
00334         x = _xStart;
00335 
00336         if ( _xaStart.getDim( ) > 0 )
00337                 xa = _xaStart;
00338 
00339         setStatus( BS_NOT_INITIALIZED );
00340 
00341         return SUCCESSFUL_RETURN;
00342 }
00343 
00344 
00345 returnValue Process::initializeAlgebraicStates( const DVector& _xaStart
00346                                                                                                 )
00347 {
00348         return initializeStartValues( x,_xaStart );
00349 }
00350 
00351 
00352 
00353 returnValue Process::init(      double _startTime,
00354                                                         const DVector& _xStart,
00355                                                         const DVector& _uStart,
00356                                                         const DVector& _pStart
00357                                                         )
00358 {
00359         DVector uStart;
00360         DVector pStart;
00361 
00362         /* 1) Assign values */
00363         lastTime = _startTime;
00364 
00365         if ( _xStart.getDim( ) > 0 )
00366                 x = _xStart;
00367         else
00368         {
00369                 if ( x.getDim() == 0 )
00370                 {
00371                         x.init( getNX() );
00372                         x.setZero( );
00373                 }
00374         }
00375 
00376         if ( ( xa.getDim( ) == 0 ) && ( getNXA( ) > 0 ) )
00377         {
00378                 xa.init( getNXA() );
00379                 xa.setZero( );
00380         }
00381 
00382         if ( _uStart.getDim( ) > 0 )
00383                 uStart = _uStart;
00384         else
00385         {
00386                 uStart.init( getNU() );
00387                 uStart.setZero( );
00388         }
00389 
00390         if ( _pStart.getDim( ) > 0 )
00391                 pStart = _pStart;
00392         else
00393         {
00394                 pStart.init( getNP() );
00395                 pStart.setZero( );
00396         }
00397         
00398 
00399         #ifdef SIM_DEBUG
00400         uStart.print("uStart(0)");
00401         #endif
00402 
00403 
00404         DynamicSystem*  dynSys  = dynamicSystems[0];
00405         OutputFcn     outputFcn = dynSys->getOutputFcn( );
00406         
00407         /* 2) Consistency checks. */
00408         if ( getNumStages( ) == 0 )
00409                 return ACADOERROR( RET_NO_DYNAMICSYSTEM_SPECIFIED );
00410 
00411         if ( getNX( ) != x.getDim( ) )
00412                 return ACADOERROR( RET_DIFFERENTIAL_STATE_DIMENSION_MISMATCH );
00413 
00414         if ( getNXA( ) != xa.getDim( ) )
00415                 return ACADOERROR( RET_ALGEBRAIC_STATE_DIMENSION_MISMATCH );
00416 
00417         if ( hasActuator( ) == BT_TRUE )
00418         {
00419                 if ( actuator->getNU( ) != getNU( ) )
00420                         return ACADOERROR( RET_CONTROL_DIMENSION_MISMATCH );
00421 
00422                 if ( actuator->getNP( ) != getNP( ) )
00423                         return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
00424 
00425                 if ( dynSys->isDiscretized( ) == BT_TRUE )
00426                 {
00427                         if ( acadoIsInteger( actuator->getSamplingTime( ) / dynSys->getSampleTime( ) ) == BT_FALSE )
00428                                 return ACADOERROR( RET_INCOMPATIBLE_ACTUATOR_SAMPLING_TIME );
00429                 }
00430         }
00431 
00432         if ( hasSensor( ) == BT_TRUE )
00433         {
00434                 if ( sensor->getNY( ) != getNY( ) )
00435                         return ACADOERROR( RET_OUTPUT_DIMENSION_MISMATCH );
00436 
00437                 if ( dynSys->isDiscretized( ) == BT_TRUE )
00438                 {
00439                         if ( acadoIsInteger( sensor->getSamplingTime( ) / dynSys->getSampleTime( ) ) == BT_FALSE )
00440                                 return ACADOERROR( RET_INCOMPATIBLE_SENSOR_SAMPLING_TIME );
00441                 }
00442         }
00443 
00444         if ( hasProcessDisturbance( ) == BT_TRUE )
00445         {
00446                 for( uint i=0; i<getNumStages( ); ++i )
00447                         if ( (int) getNW( i ) > processDisturbance->getDim( ) )
00448                                 return ACADOERROR( RET_DISTURBANCE_DIMENSION_MISMATCH );
00449 
00450                 if ( processDisturbance->isInTimeDomain( _startTime ) == BT_FALSE )
00451                         return ACADOERROR( RET_WRONG_DISTURBANCE_HORIZON );
00452         }
00453         else
00454         {
00455                 if ( getNW( ) > 0 )
00456                         return ACADOERROR( RET_DISTURBANCE_DIMENSION_MISMATCH );
00457         }
00458 
00459 
00460         /* 3) Initialize all sub-blocks. */
00461         if ( hasActuator( ) == BT_TRUE )
00462                 if ( actuator->init( _startTime,_uStart,_pStart ) != SUCCESSFUL_RETURN )
00463                         return ACADOERROR( RET_PROCESS_INIT_FAILED );
00464 
00465         if ( hasSensor( ) == BT_TRUE )
00466                 if ( sensor->init( _startTime ) != SUCCESSFUL_RETURN )
00467                         return ACADOERROR( RET_PROCESS_INIT_FAILED );
00468 
00469 
00470         /* 4) Evalute output function with initial values */
00471         Grid currentGrid( 1 );
00472         currentGrid.setTime( _startTime );
00473 
00474         // get process disturbances
00475         DVector _wStart;
00476 
00477         if ( hasProcessDisturbance( ) == BT_TRUE )
00478         {
00479                 if ( processDisturbance->evaluate( _startTime,_wStart ) != SUCCESSFUL_RETURN )
00480                         return ACADOERROR( RET_PROCESS_INIT_FAILED );
00481         }
00482 
00483         if ( outputFcn.isDefined( ) == BT_TRUE )
00484         {
00485                 // y = outputFcn(x,xa,p,u,w)
00486                 y.init( dynSys->getNumOutputs( ),currentGrid,VT_OUTPUT );
00487 
00488                 DVector yTmp( dynSys->getNumOutputs( ) );
00489  //     yTmp = outputFcn.evaluate( 0, _startTime, x, xa, _pStart, _uStart, _wStart );
00490 
00491                 y.setVector( 0,yTmp );
00492         }
00493         else
00494         {
00495                 // y = x
00496                 y.init( _xStart.getDim( ),currentGrid,VT_OUTPUT );
00497                 y.setVector( 0,_xStart );
00498         }
00499 
00500 
00501         setStatus( BS_READY );
00502 
00503         return SUCCESSFUL_RETURN;
00504 }
00505 
00506 
00507 
00508 returnValue Process::step(      const VariablesGrid& _u,
00509                                                         const VariablesGrid& _p
00510                                                         )
00511 {
00512         /* Consistency checks. */
00513         if ( getStatus( ) != BS_READY )
00514                 return ACADOERROR( RET_BLOCK_NOT_READY );
00515 
00516         if ( checkInputConsistency( _u,_p ) != SUCCESSFUL_RETURN )
00517                 return ACADOERROR( RET_PROCESS_STEP_FAILED );
00518 
00519         if ( acadoIsEqual( _u.getFirstTime( ),lastTime ) == BT_FALSE )
00520                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00521 
00522 
00523         /* 0) Log original process inputs */
00524         if ( _u.isEmpty( ) == BT_FALSE )
00525                 setLast( LOG_NOMINAL_CONTROLS,_u );
00526 
00527         if ( _p.isEmpty( ) == BT_FALSE )
00528                 setLast( LOG_NOMINAL_PARAMETERS,_p );
00529 
00530 
00531         /* 1) Get actuator outputs. */
00532         VariablesGrid uAct = _u;
00533         VariablesGrid pAct = _p;
00534 
00535         if ( hasActuator( ) == BT_TRUE )
00536         {
00537                 if ( actuator->step( uAct,pAct ) != SUCCESSFUL_RETURN )
00538                         return ACADOERROR( RET_PROCESS_STEP_FAILED );
00539         }
00540 
00541 //      printf("uAct:\n" );
00542 //      uAct.print();
00543 //      printf("pAct:\n" );
00544 //      pAct.print();
00545 
00546         /* 2) Get process disturbances. */
00547         VariablesGrid _w;
00548 
00549         if ( hasProcessDisturbance( ) == BT_TRUE )
00550         {
00551                 if ( processDisturbance->evaluate( _u.getFirstTime( ),_u.getLastTime( ),_w ) != SUCCESSFUL_RETURN )
00552                         return ACADOERROR( RET_PROCESS_STEP_FAILED_DISTURBANCE );
00553         }
00554 //      _w.print( "read w" );
00555 
00556         /* 3) Call SimulationByIntegration and evaluate output function. */
00557         Grid commonGrid, tmpGrid;
00558 
00559         uAct.getGrid( commonGrid );
00560         
00561         if ( hasProcessDisturbance( ) == BT_TRUE )
00562         {
00563                 _w.getGrid( tmpGrid );
00564                 commonGrid & tmpGrid;
00565         }
00566 //      commonGrid.print();
00567 
00568         uAct.refineGrid( commonGrid );
00569         pAct.refineGrid( commonGrid );
00570 
00571         if ( hasProcessDisturbance( ) == BT_TRUE )
00572                 _w.refineGrid( commonGrid );
00573 
00574 //      _w.print( "passed w" );
00575         if ( simulate( uAct,pAct,_w ) != SUCCESSFUL_RETURN )
00576                 return ACADOERROR( RET_PROCESS_STEP_FAILED );
00577 
00578 //      y.print("ySim");
00579         
00580         /* 4) Get sensor output. */
00581         if ( hasSensor( ) == BT_TRUE )
00582         {
00583                 if ( sensor->step( y ) != SUCCESSFUL_RETURN )
00584                         return ACADOERROR( RET_PROCESS_STEP_FAILED );
00585         }
00586 
00587 //      y.print("ySens");
00588         setLast( LOG_PROCESS_OUTPUT,y );
00589         //logCollection.setLast( LOG_SAMPLED_PROCESS_OUTPUT,y );
00590 
00591         /* 5) Update lastTime. */
00592         lastTime = _u.getLastTime( );
00593 
00594         // plot results
00595         replot( PLOT_AT_END );
00596         
00597         return SUCCESSFUL_RETURN;
00598 }
00599 
00600 
00601 returnValue Process::step(      const VariablesGrid& _u,
00602                                                         const DVector& _p
00603                                                         )
00604 {
00605         VariablesGrid pTmp( _p.getDim( ),_u.getFirstTime( ),_u.getLastTime( ),_u.getNumPoints(),VT_PARAMETER );
00606         for( uint i=0; i<_u.getNumPoints(); ++i )
00607                 pTmp.setVector( i,_p );
00608 
00609         return step( _u,pTmp );
00610 }
00611 
00612 
00613 returnValue Process::step(      double startTime,
00614                                                         double endTime,
00615                                                         const DVector& _u,
00616                                                         const DVector& _p
00617                                                         )
00618 {
00619         VariablesGrid uTmp( _u.getDim( ),startTime,endTime,2,VT_CONTROL );
00620         uTmp.setVector( 0,_u );
00621         uTmp.setVector( 1,_u );
00622 
00623         VariablesGrid pTmp( _p.getDim( ),startTime,endTime,2,VT_PARAMETER );
00624         pTmp.setVector( 0,_p );
00625         pTmp.setVector( 1,_p );
00626 
00627         return step( uTmp,pTmp );
00628 }
00629 
00630 
00631 
00632 returnValue Process::run(       const VariablesGrid& _u,
00633                                                         const VariablesGrid& _p
00634                                                         )
00635 {
00636         // Ensure that process is initialised
00637         if ( getStatus( ) == BS_NOT_INITIALIZED )
00638         {
00639                 if ( init( _u.getFirstTime( ),x,_u.getFirstVector( ),_p.getFirstVector( ) ) != SUCCESSFUL_RETURN )
00640                         return ACADOERROR( RET_PROCESS_RUN_FAILED );
00641         }
00642 
00643         return step( _u,_p );
00644 }
00645 
00646 
00647 returnValue Process::run(       const VariablesGrid& _u,
00648                                                         const DVector& _p
00649                                                         )
00650 {
00651         VariablesGrid pTmp( _p.getDim( ),_u.getFirstTime( ),_u.getLastTime( ),_u.getNumPoints(),VT_PARAMETER );
00652         for( uint i=0; i<_u.getNumPoints(); ++i )
00653                 pTmp.setVector( i,_p );
00654 
00655         return run( _u,pTmp );
00656 }
00657 
00658 
00659 returnValue Process::run(       double startTime,
00660                                                         double endTime,
00661                                                         const DVector& _u,
00662                                                         const DVector& _p
00663                                                         )
00664 {
00665         VariablesGrid uTmp( _u.getDim( ),startTime,endTime,2,VT_CONTROL );
00666         uTmp.setVector( 0,_u );
00667         uTmp.setVector( 1,_u );
00668 
00669         VariablesGrid pTmp( _p.getDim( ),startTime,endTime,2,VT_PARAMETER );
00670         pTmp.setVector( 0,_p );
00671         pTmp.setVector( 1,_p );
00672 
00673         return run( uTmp,pTmp );
00674 }
00675 
00676 
00677 
00678 returnValue Process::replot(    PlotFrequency _frequency
00679                                                                 )
00680 {
00681         int controlPlotting, parameterPlotting, outputPlotting;
00682 
00683         get( CONTROL_PLOTTING,   controlPlotting   );
00684         get( PARAMETER_PLOTTING, parameterPlotting );
00685         get( OUTPUT_PLOTTING,    outputPlotting    );
00686 
00687         if ( (ProcessPlotName)controlPlotting == PLOT_NOMINAL )
00688                 plotCollection.enableNominalControls( );
00689         else
00690                 plotCollection.disableNominalControls( );
00691         
00692         if ( (ProcessPlotName)parameterPlotting == PLOT_NOMINAL )
00693                 plotCollection.enableNominalParameters( );
00694         else
00695                 plotCollection.disableNominalParameters( );
00696         
00697         if ( (ProcessPlotName)outputPlotting == PLOT_NOMINAL )
00698                 plotCollection.enableNominalOutputs( );
00699         else
00700                 plotCollection.disableNominalOutputs( );
00701 
00702         return Plotting::replot( _frequency );
00703 }
00704 
00705 
00706 
00707 //
00708 // PROTECTED MEMBER FUNCTIONS:
00709 //
00710 
00711 returnValue Process::setupOptions( )
00712 {
00713         //printf( "Process::setupOptions( ) called.\n" );
00714 
00715         addOption( SIMULATION_ALGORITHM        , defaultSimulationAlgorithm     );
00716         addOption( CONTROL_PLOTTING            , defaultControlPlotting         );
00717         addOption( PARAMETER_PLOTTING          , defaultParameterPlotting       );
00718         addOption( OUTPUT_PLOTTING             , defaultOutputPlotting          );
00719         
00720         // add integration options
00721         addOption( FREEZE_INTEGRATOR           , BT_FALSE                       );
00722         addOption( INTEGRATOR_TYPE             , INT_BDF                        );
00723         addOption( FEASIBILITY_CHECK           , defaultFeasibilityCheck        );
00724         addOption( PLOT_RESOLUTION             , defaultPlotResoltion           );
00725         
00726         // add integrator options
00727         addOption( MAX_NUM_INTEGRATOR_STEPS    , defaultMaxNumSteps             );
00728         addOption( INTEGRATOR_TOLERANCE        , defaultIntegratorTolerance     );
00729         addOption( ABSOLUTE_TOLERANCE          , 1.0e-12       );
00730         addOption( INITIAL_INTEGRATOR_STEPSIZE , defaultInitialStepsize         );
00731         addOption( MIN_INTEGRATOR_STEPSIZE     , defaultMinStepsize             );
00732         addOption( MAX_INTEGRATOR_STEPSIZE     , defaultMaxStepsize             );
00733         addOption( STEPSIZE_TUNING             , defaultStepsizeTuning          );
00734         addOption( CORRECTOR_TOLERANCE         , defaultCorrectorTolerance      );
00735         addOption( INTEGRATOR_PRINTLEVEL       , defaultIntegratorPrintlevel    );
00736         addOption( LINEAR_ALGEBRA_SOLVER       , defaultLinearAlgebraSolver     );
00737         addOption( ALGEBRAIC_RELAXATION        , defaultAlgebraicRelaxation     );
00738         addOption( RELAXATION_PARAMETER        , defaultRelaxationParameter     );
00739         addOption( PRINT_INTEGRATOR_PROFILE    , defaultprintIntegratorProfile  );
00740 
00741         return SUCCESSFUL_RETURN;
00742 }
00743 
00744 
00745 returnValue Process::setupLogging( )
00746 {
00747         LogRecord tmp(LOG_AT_EACH_ITERATION, PS_DEFAULT);
00748 
00749         tmp.addItem( LOG_DIFFERENTIAL_STATES      );
00750         tmp.addItem( LOG_ALGEBRAIC_STATES         );
00751         tmp.addItem( LOG_PARAMETERS               );
00752         tmp.addItem( LOG_CONTROLS                 );
00753         tmp.addItem( LOG_DISTURBANCES             );
00754         tmp.addItem( LOG_INTERMEDIATE_STATES      );
00755 
00756         tmp.addItem( LOG_DISCRETIZATION_INTERVALS );
00757         tmp.addItem( LOG_STAGE_BREAK_POINTS       );
00758 
00759         tmp.addItem( LOG_NOMINAL_CONTROLS );
00760         tmp.addItem( LOG_NOMINAL_PARAMETERS );
00761 
00762         tmp.addItem( LOG_SIMULATED_DIFFERENTIAL_STATES );
00763         tmp.addItem( LOG_SIMULATED_ALGEBRAIC_STATES );
00764         tmp.addItem( LOG_SIMULATED_CONTROLS );
00765         tmp.addItem( LOG_SIMULATED_PARAMETERS );
00766         tmp.addItem( LOG_SIMULATED_DISTURBANCES );
00767         tmp.addItem( LOG_SIMULATED_OUTPUT );
00768 
00769         tmp.addItem( LOG_PROCESS_OUTPUT );
00770 
00771         addLogRecord( tmp );
00772 
00773         return SUCCESSFUL_RETURN;
00774 }
00775 
00776 
00777 
00778 returnValue Process::addStage(  const DynamicSystem  &_dynamicSystem,
00779                                                                 const Grid           &stageIntervals,
00780                                                                 const IntegratorType &_integratorType
00781                                                                 )
00782 {
00783         if ( _dynamicSystem.hasImplicitSwitches( ) == BT_TRUE )
00784                 return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00785 
00786         // add dynamic system to list...
00787         ++nDynSys;
00788 
00789         dynamicSystems = (DynamicSystem**) realloc( dynamicSystems,nDynSys*sizeof(DynamicSystem*) );
00790         dynamicSystems[ nDynSys-1 ] = new DynamicSystem( _dynamicSystem );
00791 
00792         // ... and add stage to integration algorithm
00793         return integrationMethod->addStage( _dynamicSystem, stageIntervals, _integratorType );
00794 }
00795 
00796 
00797 returnValue Process::clear( )
00798 {
00799         if ( dynamicSystems != 0 )
00800         {
00801                 for( uint i=0; i<nDynSys; ++i )
00802                         delete dynamicSystems[i];
00803 
00804                 free( dynamicSystems );
00805                 dynamicSystems = 0;
00806         }
00807 
00808         nDynSys = 0;
00809         
00810         if ( integrationMethod != 0 )
00811                 delete integrationMethod;
00812 
00813         if ( actuator != 0 )
00814                 delete actuator;
00815 
00816         if ( sensor != 0 )
00817                 delete sensor;
00818 
00819         if ( processDisturbance != 0 )
00820                 delete processDisturbance;
00821 
00822         return SUCCESSFUL_RETURN;
00823 }
00824 
00825 
00826 
00827 returnValue Process::simulate(  const VariablesGrid& _u,
00828                                                                 const VariablesGrid& _p,
00829                                                                 const VariablesGrid& _w
00830                                                                 )
00831 {
00832         DynamicSystem*       dynSys    = dynamicSystems[0];
00833         DifferentialEquation diffEqn   = dynSys->getDifferentialEquation( );
00834         OutputFcn            outputFcn = dynSys->getOutputFcn( );
00835 
00836         Grid currentGrid;
00837         _u.getGrid( currentGrid );
00838 
00839         OCPiterate iter;
00840 
00841         // allocate memory for call to simulation algorithm
00842         // and initialise states with start value
00843         DVector xComponents = diffEqn.getDifferentialStateComponents( );
00844 
00845         iter.x = new VariablesGrid( (int)round( xComponents.getMax( )+1.0 ),currentGrid,VT_DIFFERENTIAL_STATE );
00846         for( uint i=0; i<xComponents.getDim( ); ++i )
00847                 iter.x->operator()( 0,(int)xComponents(i) ) = x(i);
00848 
00849 //      DVector xaComponents;
00850         if ( getNXA() > 0 )
00851         {
00852                 iter.xa = new VariablesGrid( getNXA(),currentGrid,VT_ALGEBRAIC_STATE );
00853                 iter.xa->setVector( 0,xa );
00854         }
00855 
00856         if ( ( _u.getNumPoints( ) > 0 ) && ( _u.getNumValues( ) > 0 ) )
00857                 iter.u = new VariablesGrid( _u );
00858 
00859         if ( ( _p.getNumPoints( ) > 0 ) && ( _p.getNumValues( ) > 0 ) )
00860                 iter.p = new VariablesGrid( _p );
00861 
00862         if ( ( _w.getNumPoints( ) > 0 ) && ( _w.getNumValues( ) > 0 ) )
00863                 iter.w = new VariablesGrid( _w );
00864 
00865 
00866         // simulate process...
00867 
00868 //      delete integrationMethod;
00869 //      integrationMethod = new ShootingMethod( this );
00870         ACADO_TRY( integrationMethod->clear() );
00871         ACADO_TRY( integrationMethod->addStage( *dynSys, iter.getUnionGrid(), integratorType ) );
00872 
00873 //      iter.u->print( "u" );
00874         
00875         #ifdef SIM_DEBUG
00876 //      printf("process step \n");
00877         (iter.x->getVector(0)).print("iter.x(0)");
00878         (iter.u->getVector(0)).print("iter.u(0)");
00879 //      (iter.x->getVector(1)).print("iter.x(1)");
00880 //      (iter.u->getVector(1)).print("iter.u(1)");
00881         #endif
00882         
00883         iter.enableSimulationMode();
00884         ACADO_TRY( integrationMethod->evaluate( iter ) );
00885 
00886 
00887         // calculate sampled output
00888 //      if ( calculateOutput( outputFcn,iter.x,xComponents,iter.xa,iter.p,iter.u,iter.w, &y ) != SUCCESSFUL_RETURN )
00889 //              return ACADOERROR( RET_PROCESS_STEP_FAILED );
00890 
00891 
00892         // log simulation results
00893         VariablesGrid xCont;
00894         VariablesGrid xContFull;
00895         VariablesGrid xaCont;
00896         VariablesGrid pCont;
00897         VariablesGrid uCont;
00898         VariablesGrid wCont;
00899 
00900         if ( iter.x != 0 )
00901         {
00902                 integrationMethod->getLast( LOG_DIFFERENTIAL_STATES,xContFull );
00903 
00904                 xCont.init( getNX( ),xContFull.getTimePoints( ) );
00905                 projectToComponents( xContFull,xComponents, xCont );
00906                 setLast( LOG_SIMULATED_DIFFERENTIAL_STATES,xCont );
00907         }
00908 
00909         if ( iter.xa != 0 )
00910         {
00911                 integrationMethod->getLast( LOG_ALGEBRAIC_STATES,xaCont );
00912                 setLast( LOG_SIMULATED_ALGEBRAIC_STATES,xaCont );
00913         }
00914 
00915         if ( iter.p != 0 )
00916         {
00917                 integrationMethod->getLast( LOG_PARAMETERS,pCont );
00918                 setLast(LOG_SIMULATED_PARAMETERS,
00919                                 VariablesGrid(pCont.getCoarsenedGrid(_p.getTimePoints())));
00920         }
00921 
00922         if ( iter.u != 0 )
00923         {
00924                 integrationMethod->getLast( LOG_CONTROLS,uCont );
00925                 setLast(LOG_SIMULATED_CONTROLS,
00926                                 VariablesGrid(uCont.getCoarsenedGrid(_u.getTimePoints())));
00927         }
00928 
00929         if ( iter.w != 0 )
00930         {
00931                 integrationMethod->getLast( LOG_DISTURBANCES,wCont );
00932                 setLast(LOG_SIMULATED_DISTURBANCES,
00933                                 VariablesGrid(wCont.getCoarsenedGrid(_w.getTimePoints())));
00934         }
00935 
00936 
00937         // calculate and log continuous output
00938 //      VariablesGrid yTmp;
00939 
00940         if ( calculateOutput( outputFcn,&xContFull,xComponents,&xaCont,&pCont,&uCont,&wCont, &y ) != SUCCESSFUL_RETURN )
00941                 return ACADOERROR( RET_PROCESS_STEP_FAILED );
00942         setLast( LOG_SIMULATED_OUTPUT,y );
00943 
00944 
00945         for( uint i=0; i<xComponents.getDim( ); ++i )
00946                 x(i) = iter.x->operator()( iter.x->getNumPoints( )-1,(int)xComponents(i) );
00947 
00948         if ( iter.xa != 0 )
00949                 xa = iter.xa->getLastVector( );
00950 
00951         return SUCCESSFUL_RETURN;
00952 }
00953 
00954 
00955 
00956 /* identitical to same function within the class Actuator! */
00957 returnValue Process::checkInputConsistency(     const VariablesGrid& _u,
00958                                                                                         const VariablesGrid& _p
00959                                                                                         ) const
00960 {
00961         if ( _u.getNumPoints( ) < 2 )
00962                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00963 
00964         if ( _u.getNumRows( ) != getNU( ) )
00965                 return ACADOERROR( RET_CONTROL_DIMENSION_MISMATCH );
00966 
00967         if ( _p.isEmpty( ) == BT_TRUE )
00968         {
00969                 if ( getNP( ) > 0 )
00970                         return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
00971         }
00972         else
00973         {
00974                 if ( _p.getNumPoints( ) < 2 )
00975                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00976 
00977                 if ( _p.getNumRows( ) < getNP( ) )
00978                         return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
00979 
00980                 if ( acadoIsEqual( _u.getFirstTime( ),_p.getFirstTime( ) ) == BT_FALSE )
00981                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00982 
00983                 if ( acadoIsEqual( _u.getLastTime( ),_p.getLastTime( ) ) == BT_FALSE )
00984                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00985         }
00986 
00987         return SUCCESSFUL_RETURN;
00988 }
00989 
00990 
00991 
00992 returnValue Process::calculateOutput(   OutputFcn& _outputFcn,
00993                                                                                 const VariablesGrid* _x,
00994                                                                                 const DVector& _xComponents,
00995                                                                                 const VariablesGrid* _xa,
00996                                                                                 const VariablesGrid* _p,
00997                                                                                 const VariablesGrid* _u,
00998                                                                                 const VariablesGrid* _w,
00999                                                                                 VariablesGrid* _output
01000                                                                                 ) const
01001 {
01002         if ( _output == 0 )
01003                 return ACADOERROR( RET_INVALID_ARGUMENTS );
01004 
01005         if ( _outputFcn.isDefined( ) == BT_TRUE )
01006         {
01007                 VariablesGrid pTmp = _p->getRefinedGrid( *_x );
01008                 VariablesGrid uTmp = _u->getRefinedGrid( *_x );
01009 
01010                 // y = outputFcn(x,xa,p,u,w)
01011                 _outputFcn.evaluate( _x,_xa,&pTmp,&uTmp,_w, _output );
01012         }
01013         else
01014         {
01015                 // output = x
01016                 Grid outputGrid;
01017                 _x->getGrid( outputGrid );
01018                 _output->init( _xComponents.getDim( ),outputGrid );
01019 
01020                 projectToComponents( *_x,_xComponents, *_output );
01021         }
01022 
01023         return SUCCESSFUL_RETURN;
01024 }
01025 
01026 
01027 
01028 returnValue Process::projectToComponents(       const VariablesGrid& _x,
01029                                                                                         const DVector& _xComponents,
01030                                                                                         VariablesGrid& _output
01031                                                                                         ) const
01032 {
01033         for( uint j=0; j<_x.getNumPoints( ); ++j )
01034                 for( uint i=0; i<_xComponents.getDim( ); ++i )
01035                         _output(j,i) = _x( j,(int)_xComponents(i) );
01036 
01037         return SUCCESSFUL_RETURN;
01038 }
01039 
01040 
01041 
01042 CLOSE_NAMESPACE_ACADO
01043 
01044 // end of file.


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