00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00034 #include <acado/transfer_device/actuator.hpp>
00035
00036
00037
00038 BEGIN_NAMESPACE_ACADO
00039
00040
00041
00042
00043
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 );
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 );
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
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
00322 if ( delayActuatorInput( _u,_p ) != SUCCESSFUL_RETURN )
00323 return ACADOERROR( RET_ACTUATOR_STEP_FAILED );
00324
00325
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
00336
00337
00338
00339
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
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
00397 VariablesGrid uDelayed, pDelayed;
00398 if ( getDelayedInputGrids( _u,_p, uDelayed,pDelayed ) != SUCCESSFUL_RETURN )
00399 return ACADOERROR( RET_DELAYING_INPUTS_FAILED );
00400
00401
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
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
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
00425
00426
00427
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
00441 Grid delayedInputTimeGrid = lastSignal.getTimePoints( );
00442
00443
00444 if ( acadoIsEqual( lastSignal.getLastTime(),_u.getLastTime( ) ) == BT_FALSE )
00445 delayedInputTimeGrid.addTime( _u.getLastTime( ) );
00446
00447
00448
00449
00450 for( uint i=0; i<getNU( ); ++i )
00451 delayedInputTimeGrid = delayedInputTimeGrid & ( _u.getTimePoints( ).shiftTimes( deadTimes(i) ) );
00452
00453
00454
00455
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
00466 _uDelayed.init( );
00467 _pDelayed.init( );
00468
00469 for( uint i=0; i<getNU( ); ++i )
00470 {
00471
00472 tmp = lastSignal( i );
00473
00474 tmp.merge( _u( i ).shiftTimes( deadTimes(i) ),MM_REPLACE,BT_FALSE );
00475
00476 tmp.refineGrid( delayedInputTimeGrid );
00477
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
00506 VariablesGrid currentNoise;
00507
00508 if ( generateNoise( _u.getFirstTime(),_u.getLastTime(),currentNoise ) != SUCCESSFUL_RETURN )
00509 return ACADOERROR( RET_GENERATING_NOISE_FAILED );
00510
00511
00512 Grid commonGrid, tmpGrid;
00513 currentNoise.getGrid( tmpGrid );
00514 _u.getGrid( commonGrid );
00515 commonGrid & tmpGrid;
00516
00517
00518
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
00539