actuator.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/transfer_device/actuator.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 
00042 //
00043 // PUBLIC MEMBER FUNCTIONS:
00044 //
00045 
00046 Actuator::Actuator( ) : TransferDevice( BN_ACTUATOR )
00047 {
00048         nU = 0;
00049         nP = 0;
00050 }
00051 
00052 
00053 Actuator::Actuator(     uint _nU,
00054                                         uint _nP,
00055                                         double _samplingTime
00056                                         ) : TransferDevice( _nU+_nP,BN_ACTUATOR,_samplingTime )
00057 {
00058         nU = _nU;
00059         nP = _nP;
00060 }
00061 
00062 
00063 Actuator::Actuator( const Actuator& rhs ) : TransferDevice( rhs )
00064 {
00065         nU = rhs.nU;
00066         nP = rhs.nP;
00067 }
00068 
00069 
00070 Actuator::~Actuator( )
00071 {
00072 }
00073 
00074 
00075 Actuator& Actuator::operator=( const Actuator& rhs )
00076 {
00077     if ( this != &rhs )
00078     {
00079                 TransferDevice::operator=( rhs );
00080 
00081                 nU = rhs.nU;
00082                 nP = rhs.nP;
00083     }
00084 
00085     return *this;
00086 }
00087 
00088 
00089 returnValue Actuator::setControlNoise(  const Noise& _noise,
00090                                                                                 double _noiseSamplingTime
00091                                                                                 )
00092 {
00093         if ( _noise.getDim( ) != getNU( ) )
00094                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00095 
00096         for( uint i=0; i<getNU( ); ++i )
00097         {
00098                 if ( additiveNoise[i] != 0 )
00099                         delete additiveNoise[i];
00100 
00101                 additiveNoise[i] = _noise.clone( i );
00102         }
00103         
00104         noiseSamplingTimes.setAll( _noiseSamplingTime );
00105 
00106         return SUCCESSFUL_RETURN;
00107 }
00108 
00109 
00110 returnValue Actuator::setControlNoise(  uint idx,
00111                                                                                 const Noise& _noise,
00112                                                                                 double _noiseSamplingTime
00113                                                                                 )
00114 {
00115         if ( ( idx >= getNU( ) ) || ( _noise.getDim( ) != 1 ) )
00116                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00117 
00118         if ( additiveNoise[idx] != 0 )
00119                 delete additiveNoise[idx];
00120 
00121         additiveNoise[idx] = _noise.clone( );
00122 
00123         if ( ( idx > 0 ) && ( acadoIsEqual( _noiseSamplingTime, noiseSamplingTimes(0) ) == BT_FALSE ) )
00124                 ACADOWARNING( RET_NO_DIFFERENT_NOISE_SAMPLING_FOR_DISCRETE );
00125 
00126         noiseSamplingTimes.setAll( _noiseSamplingTime ); // should be changed later
00127 
00128         return SUCCESSFUL_RETURN;
00129 }
00130 
00131 
00132 returnValue Actuator::setParameterNoise(        const Noise& _noise,
00133                                                                                         double _noiseSamplingTime
00134                                                                                         )
00135 {
00136         if ( _noise.getDim( ) != getNP( ) )
00137                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00138 
00139         for( uint i=0; i<getNP( ); ++i )
00140         {
00141                 if ( additiveNoise[ getNU()+i ] != 0 )
00142                         delete additiveNoise[ getNU()+i ];
00143 
00144                 additiveNoise[ getNU()+i ] = _noise.clone( i );
00145         }
00146         
00147         noiseSamplingTimes.setAll( _noiseSamplingTime );
00148 
00149         return SUCCESSFUL_RETURN;
00150 }
00151 
00152 
00153 returnValue Actuator::setParameterNoise(        uint idx,
00154                                                                                         const Noise& _noise,
00155                                                                                         double _noiseSamplingTime
00156                                                                                         )
00157 {
00158         if ( ( idx >= getNP( ) ) || ( _noise.getDim( ) != 1 ) )
00159                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00160 
00161         if ( additiveNoise[ getNU()+idx ] != 0 )
00162                 delete additiveNoise[ getNU()+idx ];
00163 
00164         additiveNoise[ getNU()+idx ] = _noise.clone( );
00165 
00166         if ( ( idx > 0 ) && ( acadoIsEqual( _noiseSamplingTime, noiseSamplingTimes(0) ) == BT_FALSE ) )
00167                 ACADOWARNING( RET_NO_DIFFERENT_NOISE_SAMPLING_FOR_DISCRETE );
00168 
00169         noiseSamplingTimes.setAll( _noiseSamplingTime ); // should be changed later
00170 
00171         return SUCCESSFUL_RETURN;
00172 }
00173 
00174 
00175 
00176 returnValue Actuator::setControlDeadTimes(      const DVector& _deadTimes
00177                                                                                         )
00178 {
00179         if ( _deadTimes.getDim( ) != getNU( ) )
00180                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00181 
00182         if ( _deadTimes.getMin( ) < 0.0 )
00183                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00184 
00185         if ( deadTimes.getDim( ) == 0 )
00186                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00187 
00188         for( uint i=0; i<getNU(); ++i )
00189                 deadTimes( i ) = _deadTimes( i );
00190 
00191         return SUCCESSFUL_RETURN;
00192 }
00193 
00194 
00195 returnValue Actuator::setControlDeadTimes(      double _deadTime
00196                                                                                         )
00197 {
00198         if ( _deadTime < 0.0 )
00199                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00200 
00201         if ( deadTimes.getDim( ) == 0 )
00202                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00203 
00204         for( uint i=0; i<getNU(); ++i )
00205                 deadTimes( i ) = _deadTime;
00206 
00207         return SUCCESSFUL_RETURN;
00208 }
00209 
00210 
00211 returnValue Actuator::setControlDeadTime(       uint idx,
00212                                                                                         double _deadTime
00213                                                                                         )
00214 {
00215         if ( idx >= getNU( ) )
00216                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00217 
00218         if ( _deadTime < 0.0 )
00219                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00220 
00221         if ( deadTimes.getDim( ) == 0 )
00222                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00223 
00224         deadTimes( idx ) = _deadTime;
00225 
00226         return SUCCESSFUL_RETURN;
00227 }
00228 
00229 
00230 
00231 returnValue Actuator::setParameterDeadTimes(    const DVector& _deadTimes
00232                                                                                                 )
00233 {
00234         if ( _deadTimes.getDim( ) != getNP( ) )
00235                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00236 
00237         if ( _deadTimes.getMin( ) < 0.0 )
00238                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00239 
00240         if ( deadTimes.getDim( ) == 0 )
00241                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00242 
00243         for( uint i=0; i<getNP(); ++i )
00244                 deadTimes( getNU()+i ) = _deadTimes( i );
00245 
00246         return SUCCESSFUL_RETURN;
00247 }
00248 
00249 
00250 returnValue Actuator::setParameterDeadTimes(    double _deadTime
00251                                                                                                 )
00252 {
00253         if ( _deadTime < 0.0 )
00254                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00255 
00256         if ( deadTimes.getDim( ) == 0 )
00257                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00258 
00259         for( uint i=0; i<getNP(); ++i )
00260                 deadTimes( getNU()+i ) = _deadTime;
00261 
00262         return SUCCESSFUL_RETURN;
00263 }
00264 
00265 
00266 returnValue Actuator::setParameterDeadTime(     uint idx,
00267                                                                                         double _deadTime
00268                                                                                         )
00269 {
00270         if ( idx >= getNP( ) )
00271                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00272 
00273         if ( _deadTime < 0.0 )
00274                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00275 
00276         if ( deadTimes.getDim( ) == 0 )
00277                 return ACADOERROR( RET_MEMBER_NOT_INITIALISED );
00278 
00279         deadTimes( getNU()+idx ) = _deadTime;
00280 
00281         return SUCCESSFUL_RETURN;
00282 }
00283 
00284 
00285 
00286 returnValue Actuator::init(     double _startTime,
00287                                                         const DVector& _startValueU,
00288                                                         const DVector& _startValueP
00289                                                         )
00290 {
00291         DVector tmp;
00292 
00293         if ( _startValueU.isEmpty( ) == BT_FALSE )
00294                 tmp.append( _startValueU );
00295 
00296         if ( _startValueP.isEmpty( ) == BT_FALSE )
00297                 tmp.append( _startValueP );
00298 
00299         if ( TransferDevice::init( _startTime,tmp ) != SUCCESSFUL_RETURN )
00300                 return ACADOERROR( RET_ACTUATOR_INIT_FAILED );
00301 
00302         return SUCCESSFUL_RETURN;
00303 }
00304 
00305 
00306 
00307 returnValue Actuator::step(     VariablesGrid& _u,
00308                                                         VariablesGrid& _p
00309                                                         )
00310 {
00311         // consistency checks
00312         if ( getStatus( ) != BS_READY )
00313                 return ACADOERROR( RET_BLOCK_NOT_READY );
00314 
00315         if ( checkInputConsistency( _u,_p ) != SUCCESSFUL_RETURN )
00316                 return ACADOERROR( RET_ACTUATOR_STEP_FAILED );
00317 
00318         if ( acadoIsEqual( _u.getFirstTime( ),lastSignal.getLastTime( ) ) == BT_FALSE )
00319                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00320 
00321         // delay inputs and store last signal
00322         if ( delayActuatorInput( _u,_p ) != SUCCESSFUL_RETURN )
00323                 return ACADOERROR( RET_ACTUATOR_STEP_FAILED );
00324 
00325         // add actuator noise
00326         if ( addActuatorNoise( _u,_p ) != SUCCESSFUL_RETURN )
00327                 return ACADOERROR( RET_ACTUATOR_STEP_FAILED );
00328 
00329         return SUCCESSFUL_RETURN;
00330 }
00331 
00332 
00333 
00334 //
00335 // PROTECTED MEMBER FUNCTIONS:
00336 //
00337 
00338 
00339 /* identitical to same function within the class Process! */
00340 returnValue Actuator::checkInputConsistency(    const VariablesGrid& _u,
00341                                                                                                 const VariablesGrid& _p
00342                                                                                                 ) const
00343 {
00344         if ( _u.getNumPoints( ) < 2 )
00345                 return ACADOERROR( RET_INVALID_ARGUMENTS );
00346 
00347         if ( _u.getNumRows( ) != getNU( ) )
00348                 return ACADOERROR( RET_CONTROL_DIMENSION_MISMATCH );
00349 
00350         if ( _p.isEmpty( ) == BT_TRUE )
00351         {
00352                 if ( getNP( ) > 0 )
00353                         return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
00354         }
00355         else
00356         {
00357                 if ( _p.getNumPoints( ) < 2 )
00358                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00359 
00360                 if ( _p.getNumRows( ) != getNP( ) )
00361                         return ACADOERROR( RET_PARAMETER_DIMENSION_MISMATCH );
00362         
00363                 if ( acadoIsEqual( _u.getFirstTime( ),_p.getFirstTime( ) ) == BT_FALSE )
00364                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00365         
00366                 if ( acadoIsEqual( _u.getLastTime( ),_p.getLastTime( ) ) == BT_FALSE )
00367                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00368         }
00369 
00370         return SUCCESSFUL_RETURN;
00371 }
00372 
00373 
00374 
00375 returnValue Actuator::delayActuatorInput(       VariablesGrid& _u,
00376                                                                                         VariablesGrid& _p
00377                                                                                         )
00378 {
00379         if ( hasDeadTime( ) == BT_FALSE )
00380         {
00381                 // store last signal
00382                 DVector tmp = _u.getLastVector( );
00383                 if ( _p.isEmpty( ) == BT_FALSE )
00384                         tmp.append( _p.getLastVector( ) );
00385 
00386                 lastSignal.init( tmp );
00387                 lastSignal.setTime( 0,_u.getLastTime( ) );
00388         
00389                 return SUCCESSFUL_RETURN;
00390         }
00391         else
00392         {
00393                 double startTime = _u.getFirstTime( );
00394                 double endTime   = _u.getLastTime( );
00395 
00396                 // determine variables grid of delayed input
00397                 VariablesGrid uDelayed, pDelayed;
00398                 if ( getDelayedInputGrids( _u,_p, uDelayed,pDelayed ) != SUCCESSFUL_RETURN )
00399                         return ACADOERROR( RET_DELAYING_INPUTS_FAILED );
00400 
00401                 // store last signal
00402                 lastSignal = uDelayed.getTimeSubGrid( uDelayed.getFloorIndex( endTime ),uDelayed.getLastIndex( ) );
00403                 if ( _p.isEmpty( ) == BT_FALSE )
00404                         lastSignal.appendValues( pDelayed.getTimeSubGrid( pDelayed.getFloorIndex( endTime ),pDelayed.getLastIndex( ) ) );
00405 
00406 /*              printf("u:\n");
00407                 _u.print();
00408                 printf("p:\n");
00409                 _p.print();
00410                 
00411                 printf("uDelayed:\n");
00412                 uDelayed.print();
00413                 printf("pDelayed:\n");
00414                 pDelayed.print();*/
00415 // 
00416 //              printf("last:\n");
00417 //              lastSignal.print();
00418 
00419                 // crop delayed signal to current horizon
00420                 _u = uDelayed.getTimeSubGrid( uDelayed.getFloorIndex( startTime ),uDelayed.getFloorIndex( endTime ) );
00421                 if ( _p.isEmpty( ) == BT_FALSE )
00422                         _p = pDelayed.getTimeSubGrid( pDelayed.getFloorIndex( startTime ),pDelayed.getFloorIndex( endTime ) );
00423 
00424 //              printf("u:\n");
00425 //              _u.print();
00426 //              printf("p:\n");
00427 //              _p.print();
00428 
00429                 return SUCCESSFUL_RETURN;
00430         }
00431 }
00432 
00433 
00434 returnValue Actuator::getDelayedInputGrids(     const VariablesGrid& _u,
00435                                                                                         const VariablesGrid& _p,
00436                                                                                         VariablesGrid& _uDelayed,
00437                                                                                         VariablesGrid& _pDelayed
00438                                                                                         ) const
00439 {
00440         // determine common time grid for delayed inputs:
00441         Grid delayedInputTimeGrid = lastSignal.getTimePoints( );
00442 
00443         // make sure that last time instant of horizon lies within the grid
00444         if ( acadoIsEqual( lastSignal.getLastTime(),_u.getLastTime( ) ) == BT_FALSE )
00445                 delayedInputTimeGrid.addTime( _u.getLastTime( ) );
00446 
00447 //      delayedInputTimeGrid.print();
00448         
00449         // add grids of all delayed input components
00450         for( uint i=0; i<getNU( ); ++i )
00451                 delayedInputTimeGrid = delayedInputTimeGrid & ( _u.getTimePoints( ).shiftTimes( deadTimes(i) ) );
00452 
00453 //      _u.getTimePoints( ).print();
00454 //      _u.getTimePoints( ).shiftTimes( deadTimes(0) ).print();
00455 //      delayedInputTimeGrid.print();
00456         
00457         if ( _p.isEmpty( ) == BT_FALSE )
00458         {
00459                 for( uint i=0; i<getNP( ); ++i )
00460                         delayedInputTimeGrid = delayedInputTimeGrid & ( _p.getTimePoints( ).shiftTimes( deadTimes(getNU()+i) ) );
00461         }
00462 
00463         VariablesGrid tmp;
00464 
00465         // setup common variables grid for delayed inputs
00466         _uDelayed.init( );
00467         _pDelayed.init( );
00468 
00469         for( uint i=0; i<getNU( ); ++i )
00470         {
00471 //              tmp.print("tmp");
00472                 tmp = lastSignal( i );
00473 //              tmp.print("tmp");
00474                 tmp.merge( _u( i ).shiftTimes( deadTimes(i) ),MM_REPLACE,BT_FALSE );
00475 //              tmp.print("tmp");
00476                 tmp.refineGrid( delayedInputTimeGrid );
00477 //              tmp.print("tmp");
00478                 
00479                 _uDelayed.appendValues( tmp );
00480         }
00481 
00482         if ( _p.isEmpty( ) == BT_FALSE )
00483         {
00484                 for( uint i=0; i<getNP( ); ++i )
00485                 {
00486                         tmp = lastSignal( getNU()+i );
00487                         tmp.merge( _p( i ).shiftTimes( deadTimes(getNU()+i) ),MM_REPLACE,BT_FALSE );
00488                         tmp.refineGrid( delayedInputTimeGrid );
00489 
00490                         _pDelayed.appendValues( tmp );
00491                 }
00492         }
00493 
00494         return SUCCESSFUL_RETURN;
00495 }
00496 
00497 
00498 returnValue Actuator::addActuatorNoise( VariablesGrid& _u,
00499                                                                                 VariablesGrid& _p
00500                                                                                 ) const
00501 {
00502         if ( hasNoise( ) == BT_FALSE )
00503                 return SUCCESSFUL_RETURN;
00504 
00505         // generate current noise
00506         VariablesGrid currentNoise;
00507 
00508         if ( generateNoise( _u.getFirstTime(),_u.getLastTime(),currentNoise ) != SUCCESSFUL_RETURN )
00509                 return ACADOERROR( RET_GENERATING_NOISE_FAILED );
00510 
00511         // determine common grid
00512         Grid commonGrid, tmpGrid;
00513         currentNoise.getGrid( tmpGrid );
00514         _u.getGrid( commonGrid );
00515         commonGrid & tmpGrid;
00516 
00517 
00518         // adapt input grids and add noise
00519         _u.refineGrid( commonGrid );
00520         currentNoise.refineGrid( commonGrid );
00521         _u += currentNoise.getValuesSubGrid( 0,getNU()-1 );
00522 
00523         if ( _p.isEmpty( ) == BT_FALSE )
00524         {
00525                 _p.refineGrid( commonGrid );
00526                 _p += currentNoise.getValuesSubGrid( getNU(),getNU()+getNP()-1 );
00527         }
00528 
00529         return SUCCESSFUL_RETURN;
00530 }
00531 
00532 
00533 
00534 
00535 CLOSE_NAMESPACE_ACADO
00536 
00537 /*
00538  *      end of file
00539  */


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