actuator.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
35 
36 
37 
39 
40 
41 
42 //
43 // PUBLIC MEMBER FUNCTIONS:
44 //
45 
47 {
48  nU = 0;
49  nP = 0;
50 }
51 
52 
54  uint _nP,
55  double _samplingTime
56  ) : TransferDevice( _nU+_nP,BN_ACTUATOR,_samplingTime )
57 {
58  nU = _nU;
59  nP = _nP;
60 }
61 
62 
64 {
65  nU = rhs.nU;
66  nP = rhs.nP;
67 }
68 
69 
71 {
72 }
73 
74 
76 {
77  if ( this != &rhs )
78  {
80 
81  nU = rhs.nU;
82  nP = rhs.nP;
83  }
84 
85  return *this;
86 }
87 
88 
90  double _noiseSamplingTime
91  )
92 {
93  if ( _noise.getDim( ) != getNU( ) )
95 
96  for( uint i=0; i<getNU( ); ++i )
97  {
98  if ( additiveNoise[i] != 0 )
99  delete additiveNoise[i];
100 
101  additiveNoise[i] = _noise.clone( i );
102  }
103 
104  noiseSamplingTimes.setAll( _noiseSamplingTime );
105 
106  return SUCCESSFUL_RETURN;
107 }
108 
109 
111  const Noise& _noise,
112  double _noiseSamplingTime
113  )
114 {
115  if ( ( idx >= getNU( ) ) || ( _noise.getDim( ) != 1 ) )
117 
118  if ( additiveNoise[idx] != 0 )
119  delete additiveNoise[idx];
120 
121  additiveNoise[idx] = _noise.clone( );
122 
123  if ( ( idx > 0 ) && ( acadoIsEqual( _noiseSamplingTime, noiseSamplingTimes(0) ) == BT_FALSE ) )
125 
126  noiseSamplingTimes.setAll( _noiseSamplingTime ); // should be changed later
127 
128  return SUCCESSFUL_RETURN;
129 }
130 
131 
133  double _noiseSamplingTime
134  )
135 {
136  if ( _noise.getDim( ) != getNP( ) )
138 
139  for( uint i=0; i<getNP( ); ++i )
140  {
141  if ( additiveNoise[ getNU()+i ] != 0 )
142  delete additiveNoise[ getNU()+i ];
143 
144  additiveNoise[ getNU()+i ] = _noise.clone( i );
145  }
146 
147  noiseSamplingTimes.setAll( _noiseSamplingTime );
148 
149  return SUCCESSFUL_RETURN;
150 }
151 
152 
154  const Noise& _noise,
155  double _noiseSamplingTime
156  )
157 {
158  if ( ( idx >= getNP( ) ) || ( _noise.getDim( ) != 1 ) )
160 
161  if ( additiveNoise[ getNU()+idx ] != 0 )
162  delete additiveNoise[ getNU()+idx ];
163 
164  additiveNoise[ getNU()+idx ] = _noise.clone( );
165 
166  if ( ( idx > 0 ) && ( acadoIsEqual( _noiseSamplingTime, noiseSamplingTimes(0) ) == BT_FALSE ) )
168 
169  noiseSamplingTimes.setAll( _noiseSamplingTime ); // should be changed later
170 
171  return SUCCESSFUL_RETURN;
172 }
173 
174 
175 
177  )
178 {
179  if ( _deadTimes.getDim( ) != getNU( ) )
181 
182  if ( _deadTimes.getMin( ) < 0.0 )
184 
185  if ( deadTimes.getDim( ) == 0 )
187 
188  for( uint i=0; i<getNU(); ++i )
189  deadTimes( i ) = _deadTimes( i );
190 
191  return SUCCESSFUL_RETURN;
192 }
193 
194 
196  )
197 {
198  if ( _deadTime < 0.0 )
200 
201  if ( deadTimes.getDim( ) == 0 )
203 
204  for( uint i=0; i<getNU(); ++i )
205  deadTimes( i ) = _deadTime;
206 
207  return SUCCESSFUL_RETURN;
208 }
209 
210 
212  double _deadTime
213  )
214 {
215  if ( idx >= getNU( ) )
217 
218  if ( _deadTime < 0.0 )
220 
221  if ( deadTimes.getDim( ) == 0 )
223 
224  deadTimes( idx ) = _deadTime;
225 
226  return SUCCESSFUL_RETURN;
227 }
228 
229 
230 
232  )
233 {
234  if ( _deadTimes.getDim( ) != getNP( ) )
236 
237  if ( _deadTimes.getMin( ) < 0.0 )
239 
240  if ( deadTimes.getDim( ) == 0 )
242 
243  for( uint i=0; i<getNP(); ++i )
244  deadTimes( getNU()+i ) = _deadTimes( i );
245 
246  return SUCCESSFUL_RETURN;
247 }
248 
249 
251  )
252 {
253  if ( _deadTime < 0.0 )
255 
256  if ( deadTimes.getDim( ) == 0 )
258 
259  for( uint i=0; i<getNP(); ++i )
260  deadTimes( getNU()+i ) = _deadTime;
261 
262  return SUCCESSFUL_RETURN;
263 }
264 
265 
267  double _deadTime
268  )
269 {
270  if ( idx >= getNP( ) )
272 
273  if ( _deadTime < 0.0 )
275 
276  if ( deadTimes.getDim( ) == 0 )
278 
279  deadTimes( getNU()+idx ) = _deadTime;
280 
281  return SUCCESSFUL_RETURN;
282 }
283 
284 
285 
286 returnValue Actuator::init( double _startTime,
287  const DVector& _startValueU,
288  const DVector& _startValueP
289  )
290 {
291  DVector tmp;
292 
293  if ( _startValueU.isEmpty( ) == BT_FALSE )
294  tmp.append( _startValueU );
295 
296  if ( _startValueP.isEmpty( ) == BT_FALSE )
297  tmp.append( _startValueP );
298 
299  if ( TransferDevice::init( _startTime,tmp ) != SUCCESSFUL_RETURN )
301 
302  return SUCCESSFUL_RETURN;
303 }
304 
305 
306 
308  VariablesGrid& _p
309  )
310 {
311  // consistency checks
312  if ( getStatus( ) != BS_READY )
314 
315  if ( checkInputConsistency( _u,_p ) != SUCCESSFUL_RETURN )
317 
320 
321  // delay inputs and store last signal
322  if ( delayActuatorInput( _u,_p ) != SUCCESSFUL_RETURN )
324 
325  // add actuator noise
326  if ( addActuatorNoise( _u,_p ) != SUCCESSFUL_RETURN )
328 
329  return SUCCESSFUL_RETURN;
330 }
331 
332 
333 
334 //
335 // PROTECTED MEMBER FUNCTIONS:
336 //
337 
338 
339 /* identitical to same function within the class Process! */
341  const VariablesGrid& _p
342  ) const
343 {
344  if ( _u.getNumPoints( ) < 2 )
346 
347  if ( _u.getNumRows( ) != getNU( ) )
349 
350  if ( _p.isEmpty( ) == BT_TRUE )
351  {
352  if ( getNP( ) > 0 )
354  }
355  else
356  {
357  if ( _p.getNumPoints( ) < 2 )
359 
360  if ( _p.getNumRows( ) != getNP( ) )
362 
363  if ( acadoIsEqual( _u.getFirstTime( ),_p.getFirstTime( ) ) == BT_FALSE )
365 
366  if ( acadoIsEqual( _u.getLastTime( ),_p.getLastTime( ) ) == BT_FALSE )
368  }
369 
370  return SUCCESSFUL_RETURN;
371 }
372 
373 
374 
376  VariablesGrid& _p
377  )
378 {
379  if ( hasDeadTime( ) == BT_FALSE )
380  {
381  // store last signal
382  DVector tmp = _u.getLastVector( );
383  if ( _p.isEmpty( ) == BT_FALSE )
384  tmp.append( _p.getLastVector( ) );
385 
386  lastSignal.init( tmp );
387  lastSignal.setTime( 0,_u.getLastTime( ) );
388 
389  return SUCCESSFUL_RETURN;
390  }
391  else
392  {
393  double startTime = _u.getFirstTime( );
394  double endTime = _u.getLastTime( );
395 
396  // determine variables grid of delayed input
397  VariablesGrid uDelayed, pDelayed;
398  if ( getDelayedInputGrids( _u,_p, uDelayed,pDelayed ) != SUCCESSFUL_RETURN )
400 
401  // store last signal
402  lastSignal = uDelayed.getTimeSubGrid( uDelayed.getFloorIndex( endTime ),uDelayed.getLastIndex( ) );
403  if ( _p.isEmpty( ) == BT_FALSE )
404  lastSignal.appendValues( pDelayed.getTimeSubGrid( pDelayed.getFloorIndex( endTime ),pDelayed.getLastIndex( ) ) );
405 
406 /* printf("u:\n");
407  _u.print();
408  printf("p:\n");
409  _p.print();
410 
411  printf("uDelayed:\n");
412  uDelayed.print();
413  printf("pDelayed:\n");
414  pDelayed.print();*/
415 //
416 // printf("last:\n");
417 // lastSignal.print();
418 
419  // crop delayed signal to current horizon
420  _u = uDelayed.getTimeSubGrid( uDelayed.getFloorIndex( startTime ),uDelayed.getFloorIndex( endTime ) );
421  if ( _p.isEmpty( ) == BT_FALSE )
422  _p = pDelayed.getTimeSubGrid( pDelayed.getFloorIndex( startTime ),pDelayed.getFloorIndex( endTime ) );
423 
424 // printf("u:\n");
425 // _u.print();
426 // printf("p:\n");
427 // _p.print();
428 
429  return SUCCESSFUL_RETURN;
430  }
431 }
432 
433 
435  const VariablesGrid& _p,
436  VariablesGrid& _uDelayed,
437  VariablesGrid& _pDelayed
438  ) const
439 {
440  // determine common time grid for delayed inputs:
441  Grid delayedInputTimeGrid = lastSignal.getTimePoints( );
442 
443  // make sure that last time instant of horizon lies within the grid
445  delayedInputTimeGrid.addTime( _u.getLastTime( ) );
446 
447 // delayedInputTimeGrid.print();
448 
449  // add grids of all delayed input components
450  for( uint i=0; i<getNU( ); ++i )
451  delayedInputTimeGrid = delayedInputTimeGrid & ( _u.getTimePoints( ).shiftTimes( deadTimes(i) ) );
452 
453 // _u.getTimePoints( ).print();
454 // _u.getTimePoints( ).shiftTimes( deadTimes(0) ).print();
455 // delayedInputTimeGrid.print();
456 
457  if ( _p.isEmpty( ) == BT_FALSE )
458  {
459  for( uint i=0; i<getNP( ); ++i )
460  delayedInputTimeGrid = delayedInputTimeGrid & ( _p.getTimePoints( ).shiftTimes( deadTimes(getNU()+i) ) );
461  }
462 
463  VariablesGrid tmp;
464 
465  // setup common variables grid for delayed inputs
466  _uDelayed.init( );
467  _pDelayed.init( );
468 
469  for( uint i=0; i<getNU( ); ++i )
470  {
471 // tmp.print("tmp");
472  tmp = lastSignal( i );
473 // tmp.print("tmp");
474  tmp.merge( _u( i ).shiftTimes( deadTimes(i) ),MM_REPLACE,BT_FALSE );
475 // tmp.print("tmp");
476  tmp.refineGrid( delayedInputTimeGrid );
477 // tmp.print("tmp");
478 
479  _uDelayed.appendValues( tmp );
480  }
481 
482  if ( _p.isEmpty( ) == BT_FALSE )
483  {
484  for( uint i=0; i<getNP( ); ++i )
485  {
486  tmp = lastSignal( getNU()+i );
487  tmp.merge( _p( i ).shiftTimes( deadTimes(getNU()+i) ),MM_REPLACE,BT_FALSE );
488  tmp.refineGrid( delayedInputTimeGrid );
489 
490  _pDelayed.appendValues( tmp );
491  }
492  }
493 
494  return SUCCESSFUL_RETURN;
495 }
496 
497 
499  VariablesGrid& _p
500  ) const
501 {
502  if ( hasNoise( ) == BT_FALSE )
503  return SUCCESSFUL_RETURN;
504 
505  // generate current noise
506  VariablesGrid currentNoise;
507 
508  if ( generateNoise( _u.getFirstTime(),_u.getLastTime(),currentNoise ) != SUCCESSFUL_RETURN )
510 
511  // determine common grid
512  Grid commonGrid, tmpGrid;
513  currentNoise.getGrid( tmpGrid );
514  _u.getGrid( commonGrid );
515  commonGrid & tmpGrid;
516 
517 
518  // adapt input grids and add noise
519  _u.refineGrid( commonGrid );
520  currentNoise.refineGrid( commonGrid );
521  _u += currentNoise.getValuesSubGrid( 0,getNU()-1 );
522 
523  if ( _p.isEmpty( ) == BT_FALSE )
524  {
525  _p.refineGrid( commonGrid );
526  _p += currentNoise.getValuesSubGrid( getNU(),getNU()+getNP()-1 );
527  }
528 
529  return SUCCESSFUL_RETURN;
530 }
531 
532 
533 
534 
536 
537 /*
538  * end of file
539  */
uint getNumPoints() const
returnValue setParameterNoise(const Noise &_noise, double _noiseSamplingTime)
Definition: actuator.cpp:132
uint getNP() const
returnValue setTime(double _time)
Definition: grid.cpp:307
DVector getLastVector() const
returnValue getDelayedInputGrids(const VariablesGrid &_u, const VariablesGrid &_p, VariablesGrid &_uDelayed, VariablesGrid &_pDelayed) const
Definition: actuator.cpp:434
returnValue delayActuatorInput(VariablesGrid &_u, VariablesGrid &_p)
Definition: actuator.cpp:375
double getLastTime() const
BooleanType acadoIsEqual(double x, double y, double TOL)
Definition: acado_utils.cpp:88
BooleanType hasNoise() const
returnValue setParameterDeadTime(uint idx, double _deadTime)
Definition: actuator.cpp:266
returnValue setParameterDeadTimes(const DVector &_deadTimes)
Definition: actuator.cpp:231
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Allows to pass back messages to the calling function.
VariablesGrid getTimeSubGrid(uint startIdx, uint endIdx) const
BooleanType isEmpty() const
uint getDim() const
Base class for generating pseudo-random noise for simulating the Process.
Definition: noise.hpp:56
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
uint nP
Definition: actuator.hpp:390
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
returnValue checkInputConsistency(const VariablesGrid &_u, const VariablesGrid &_p) const
Definition: actuator.cpp:340
#define CLOSE_NAMESPACE_ACADO
returnValue appendValues(const VariablesGrid &arg)
Actuator & operator=(const Actuator &rhs)
Definition: actuator.cpp:75
double getFirstTime() const
uint getFloorIndex(double time) const
Definition: grid.cpp:593
virtual returnValue generateNoise(double startTime, double endTime, VariablesGrid &currentNoise) const
returnValue merge(const VariablesGrid &arg, MergeMethod _mergeMethod=MM_DUPLICATE, BooleanType keepOverlap=BT_TRUE)
VariablesGrid lastSignal
BlockStatus getStatus() const
virtual returnValue init(double _startTime=0.0, const DVector &_startValue=emptyConstVector)
returnValue refineGrid(const Grid &arg, InterpolationMode mode=IM_CONSTANT)
returnValue init()
returnValue setControlDeadTime(uint idx, double _deadTime)
Definition: actuator.cpp:211
bool isEmpty() const
Definition: vector.hpp:176
void rhs(const real_t *x, real_t *f)
virtual returnValue init(double _startTime=0.0, const DVector &_startValueU=emptyConstVector, const DVector &_startValueP=emptyConstVector)
Definition: actuator.cpp:286
uint getNU() const
virtual Noise * clone() const =0
returnValue addActuatorNoise(VariablesGrid &_u, VariablesGrid &_p) const
Definition: actuator.cpp:498
uint getLastIndex() const
#define BT_TRUE
Definition: acado_types.hpp:47
GenericVector & append(const GenericVector &_arg)
Definition: vector.cpp:42
Grid & shiftTimes(double timeShift)
Definition: grid.cpp:449
returnValue addTime(double _time)
Definition: grid.cpp:339
void setAll(const T &_value)
Definition: vector.hpp:160
returnValue getGrid(Grid &_grid) const
unsigned getDim() const
Definition: vector.hpp:172
#define ACADOWARNING(retval)
uint nU
Definition: actuator.hpp:389
BooleanType hasDeadTime() const
DVector noiseSamplingTimes
#define BEGIN_NAMESPACE_ACADO
returnValue setControlNoise(const Noise &_noise, double _noiseSamplingTime)
Definition: actuator.cpp:89
#define BT_FALSE
Definition: acado_types.hpp:49
Allows to simulate the behaviour of actuators within the Process.
Definition: actuator.hpp:54
VariablesGrid getValuesSubGrid(uint startIdx, uint endIdx) const
virtual ~Actuator()
Definition: actuator.cpp:70
virtual returnValue step(VariablesGrid &_u, VariablesGrid &_p=emptyVariablesGrid)
Definition: actuator.cpp:307
Actuator()
Definition: actuator.cpp:46
TransferDevice & operator=(const TransferDevice &rhs)
T getMin() const
Definition: vector.hpp:184
#define ACADOERROR(retval)
Base class for simulating Actuator and Sensor behaviour wihtin the Process.
returnValue setControlDeadTimes(const DVector &_deadTimes)
Definition: actuator.cpp:176


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Feb 28 2022 21:31:50