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  */
returnValue setParameterNoise(const Noise &_noise, double _noiseSamplingTime)
Definition: actuator.cpp:132
T getMin() const
Definition: vector.hpp:184
returnValue setTime(double _time)
Definition: grid.cpp:307
returnValue delayActuatorInput(VariablesGrid &_u, VariablesGrid &_p)
Definition: actuator.cpp:375
double getFirstTime() const
BooleanType acadoIsEqual(double x, double y, double TOL)
Definition: acado_utils.cpp:88
returnValue setParameterDeadTime(uint idx, double _deadTime)
Definition: actuator.cpp:266
returnValue addActuatorNoise(VariablesGrid &_u, VariablesGrid &_p) const
Definition: actuator.cpp:498
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 getValuesSubGrid(uint startIdx, uint endIdx) 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
BooleanType isEmpty() const
uint getNP() const
uint getLastIndex() const
#define CLOSE_NAMESPACE_ACADO
bool isEmpty() const
Definition: vector.hpp:176
BooleanType hasDeadTime() const
uint getFloorIndex(double time) const
Definition: grid.cpp:593
BooleanType hasNoise() const
BlockStatus getStatus() const
returnValue appendValues(const VariablesGrid &arg)
Actuator & operator=(const Actuator &rhs)
Definition: actuator.cpp:75
returnValue checkInputConsistency(const VariablesGrid &_u, const VariablesGrid &_p) const
Definition: actuator.cpp:340
returnValue merge(const VariablesGrid &arg, MergeMethod _mergeMethod=MM_DUPLICATE, BooleanType keepOverlap=BT_TRUE)
VariablesGrid lastSignal
unsigned getDim() const
Definition: vector.hpp:172
virtual returnValue generateNoise(double startTime, double endTime, VariablesGrid &currentNoise) const
returnValue getGrid(Grid &_grid) const
virtual returnValue init(double _startTime=0.0, const DVector &_startValue=emptyConstVector)
DVector getLastVector() const
returnValue refineGrid(const Grid &arg, InterpolationMode mode=IM_CONSTANT)
VariablesGrid getTimeSubGrid(uint startIdx, uint endIdx) const
returnValue init()
returnValue setControlDeadTime(uint idx, double _deadTime)
Definition: actuator.cpp:211
uint getNU() const
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
virtual Noise * clone() const =0
#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
uint getNumPoints() const
void setAll(const T &_value)
Definition: vector.hpp:160
returnValue getDelayedInputGrids(const VariablesGrid &_u, const VariablesGrid &_p, VariablesGrid &_uDelayed, VariablesGrid &_pDelayed) const
Definition: actuator.cpp:434
double getLastTime() const
#define ACADOWARNING(retval)
uint nU
Definition: actuator.hpp:389
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
uint getDim() 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)
#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 Jun 10 2019 12:34:27