ocp_iterate.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 
33 #include <acado/function/ocp_iterate.hpp>
34 
35 
36 BEGIN_NAMESPACE_ACADO
37 
38 
39 //
40 // PUBLIC MEMBER FUNCTIONS:
41 //
42 
43 OCPiterate::OCPiterate( )
44 {
45  x = 0;
46  xa = 0;
47  p = 0;
48  u = 0;
49  w = 0;
50 
51  init( 0,0,0,0,0 );
52 }
53 
54 
55 OCPiterate::OCPiterate( const VariablesGrid* const _x,
56  const VariablesGrid* const _xa,
57  const VariablesGrid* const _p,
58  const VariablesGrid* const _u,
59  const VariablesGrid* const _w
60  )
61 {
62 
63  x = 0; xa = 0; p = 0; u = 0; w = 0;
64 
65  init( _x,_xa,_p,_u,_w );
66 }
67 
68 
69 OCPiterate::OCPiterate( const OCPiterate& rhs ){
70 
71  copy(rhs);
72 }
73 
74 
75 OCPiterate::~OCPiterate( ){
76 
77  clear( );
78 }
79 
80 
81 OCPiterate& OCPiterate::operator=( const OCPiterate& rhs ){
82 
83  if ( this != &rhs ){
84 
85  clear( ) ;
86  copy(rhs);
87  }
88  return *this;
89 }
90 
91 
92 returnValue OCPiterate::allocateAll( )
93 {
94  if ( x == 0 )
95  x = new VariablesGrid;
96 
97  if ( xa == 0 )
98  xa = new VariablesGrid;
99 
100  if ( p == 0 )
101  p = new VariablesGrid;
102 
103  if ( u == 0 )
104  u = new VariablesGrid;
105 
106  if ( w == 0 )
107  w = new VariablesGrid;
108 
109  return SUCCESSFUL_RETURN;
110 }
111 
112 returnValue OCPiterate::init( const VariablesGrid* const _x,
113  const VariablesGrid* const _xa,
114  const VariablesGrid* const _p,
115  const VariablesGrid* const _u,
116  const VariablesGrid* const _w
117  )
118 {
119  clear( );
120 
121  if ( _x != 0 )
122  x = new VariablesGrid( *_x );
123 
124  if ( _xa != 0 )
125  xa = new VariablesGrid( *_xa );
126 
127  if ( _p != 0 )
128  p = new VariablesGrid( *_p );
129 
130  if ( _u != 0 )
131  u = new VariablesGrid( *_u );
132 
133  if ( _w != 0 )
134  w = new VariablesGrid( *_w );
135 
136  return SUCCESSFUL_RETURN;
137 }
138 
139 
140 returnValue OCPiterate::clear( ){
141 
142  if( x != 0 ){ delete x ; x = 0; }
143  if( xa != 0 ){ delete xa; xa = 0; }
144  if( p != 0 ){ delete p ; p = 0; }
145  if( u != 0 ){ delete u ; u = 0; }
146  if( w != 0 ){ delete w ; w = 0; }
147 
148  inSimulationMode = BT_FALSE;
149 
150  return SUCCESSFUL_RETURN;
151 }
152 
153 
154 
155 uint OCPiterate::getNumPoints( ) const
156 {
157  int N = 0;
158 
159  if( x != 0 ) N = acadoMax( N, x ->getNumPoints( ) );
160  if( xa != 0 ) N = acadoMax( N, xa->getNumPoints( ) );
161  if( p != 0 ) N = acadoMax( N, p ->getNumPoints( ) );
162  if( u != 0 ) N = acadoMax( N, u ->getNumPoints( ) );
163  if( w != 0 ) N = acadoMax( N, w ->getNumPoints( ) );
164 
165  return N;
166 }
167 
168 
169 returnValue OCPiterate::print( ) const{
170 
171  if( x != 0 ) x ->print( "iter.x " );
172  if( xa != 0 ) xa->print( "iter.xa" );
173  if( p != 0 ) p ->print( "iter.p " );
174  if( u != 0 ) u ->print( "iter.u " );
175  if( w != 0 ) w ->print( "iter.w " );
176 
177  return SUCCESSFUL_RETURN;
178 }
179 
180 
181 
182 Grid OCPiterate::getUnionGrid() const{
183 
184  Grid tmp, unionGrid;
185 
186  if( x != 0 ){ x ->getGrid(tmp); unionGrid = unionGrid & tmp; }
187  if( xa != 0 ){ xa->getGrid(tmp); unionGrid = unionGrid & tmp; }
188  if( p != 0 ){ p ->getGrid(tmp); unionGrid = unionGrid & tmp; }
189  if( u != 0 ){ u ->getGrid(tmp); unionGrid = unionGrid & tmp; }
190  if( w != 0 ){ w ->getGrid(tmp); unionGrid = unionGrid & tmp; }
191 
192  return unionGrid;
193 }
194 
195 
196 
197 BooleanType OCPiterate::areGridsConsistent( )
198 {
199  double startTime = 0.0, endTime = 0.0;
200  BooleanType timeDefined = BT_FALSE;
201 
202  if ( ( x != 0 ) && ( x->isEmpty( ) == BT_FALSE ) )
203  {
204  startTime = x->getFirstTime( );
205  endTime = x->getLastTime( );
206  timeDefined = BT_TRUE;
207  }
208 
209  if ( ( xa != 0 ) && ( xa->isEmpty( ) == BT_FALSE ) )
210  {
211  if ( timeDefined == BT_FALSE )
212  {
213  startTime = xa->getFirstTime( );
214  endTime = xa->getLastTime( );
215  timeDefined = BT_TRUE;
216  }
217  else
218  {
219  if ( ( acadoIsEqual( startTime,xa->getFirstTime( ) ) == BT_FALSE ) ||
220  ( acadoIsEqual( endTime, xa->getLastTime( ) ) == BT_FALSE ) )
221  return BT_FALSE;
222  }
223  }
224 
225  if ( ( p != 0 ) && ( p->isEmpty( ) == BT_FALSE ) )
226  {
227  if ( timeDefined == BT_FALSE )
228  {
229  startTime = p->getFirstTime( );
230  endTime = p->getLastTime( );
231  timeDefined = BT_TRUE;
232  }
233  else
234  {
235  if ( ( acadoIsEqual( startTime,p->getFirstTime( ) ) == BT_FALSE ) ||
236  ( acadoIsEqual( endTime, p->getLastTime( ) ) == BT_FALSE ) )
237  return BT_FALSE;
238  }
239  }
240 
241  if ( ( u != 0 ) && ( u->isEmpty( ) == BT_FALSE ) )
242  {
243  if ( timeDefined == BT_FALSE )
244  {
245  startTime = u->getFirstTime( );
246  endTime = u->getLastTime( );
247  timeDefined = BT_TRUE;
248  }
249  else
250  {
251  if ( ( acadoIsEqual( startTime,u->getFirstTime( ) ) == BT_FALSE ) ||
252  ( acadoIsEqual( endTime, u->getLastTime( ) ) == BT_FALSE ) )
253  return BT_FALSE;
254  }
255  }
256 
257  if ( ( w != 0 ) && ( w->isEmpty( ) == BT_FALSE ) )
258  {
259  if ( timeDefined == BT_FALSE )
260  {
261  startTime = w->getFirstTime( );
262  endTime = w->getLastTime( );
263  timeDefined = BT_TRUE;
264  }
265  else
266  {
267  if ( ( acadoIsEqual( startTime,w->getFirstTime( ) ) == BT_FALSE ) ||
268  ( acadoIsEqual( endTime, w->getLastTime( ) ) == BT_FALSE ) )
269  return BT_FALSE;
270  }
271  }
272 
273  return BT_TRUE;
274 }
275 
276 
277 
278 returnValue OCPiterate::getInitialData( DVector &x_, DVector &xa_, DVector &p_,
279  DVector &u_, DVector &w_ ) const{
280 
281  if( x != 0 ){ x_ = x ->getVector(0); } else { x_ = emptyVector; }
282  if( xa != 0 ){ xa_ = xa->getVector(0); } else { xa_ = emptyVector; }
283  if( p != 0 ){ p_ = p ->getVector(0); } else { p_ = emptyVector; }
284  if( u != 0 ){ u_ = u ->getVector(0); } else { u_ = emptyVector; }
285  if( w != 0 ){ w_ = w ->getVector(0); } else { w_ = emptyVector; }
286 
287  return SUCCESSFUL_RETURN;
288 }
289 
290 
291 void OCPiterate::update( double t, VariablesGrid &z1, DVector &z2 ) const{
292 
293  if( z1.hasTime( t ) == BT_TRUE ){
294 
295  uint idx = z1.getFloorIndex( t );
296  if( z1.getAutoInit(idx) == BT_FALSE ){ z2 = z1.getVector(idx); }
297  else{
298 
299  DVector safeGuard = z2;
300  safeGuard.setAll( BOUNDTOL );
301 
302  if( z1.hasUpperBounds() == BT_TRUE )
303  if( z2 >= z1.getUpperBounds(idx) ) z2 = (DVector)z1.getUpperBounds(idx) - safeGuard;
304 
305  if( z1.hasLowerBounds() == BT_TRUE )
306  if( z2 <= z1.getLowerBounds(idx) ) z2 = (DVector)z1.getLowerBounds(idx) + safeGuard;
307 
308  z1.setVector( idx, z2 );
309  }
310  }
311 }
312 
313 
314 returnValue OCPiterate::updateData( double t , DVector &x_, DVector &xa_,
315  DVector &p_, DVector &u_, DVector &w_ ){
316 
317  if( x != 0 ) update( t, *x , x_ );
318  if( xa != 0 ) update( t, *xa, xa_ );
319  if( p != 0 ) update( t, *p , p_ );
320  if( u != 0 ) update( t, *u , u_ );
321  if( w != 0 ) update( t, *w , w_ );
322 
323  return SUCCESSFUL_RETURN;
324 }
325 
326 
327 
328 returnValue OCPiterate::applyStep( const BlockMatrix& bm,
329  double alpha
330  )
331 {
332  uint run1, run2;
333  DMatrix tmp;
334 
335  if( getNX() > 0 ){
336  for( run1 = 0; run1 < getNumPoints(); run1++ ){
337  bm.getSubBlock( run1, 0, tmp, getNX(), 1 );
338  for( run2 = 0; run2 < getNX(); run2++ )
339  x->operator()(run1,run2) += alpha*tmp(run2,0);
340  }
341  }
342 
343  if( getNXA() > 0 ){
344  for( run1 = 0; run1 < getNumPoints(); run1++ ){
345  bm.getSubBlock( getNumPoints()+run1, 0, tmp, getNXA(), 1 );
346  for( run2 = 0; run2 < getNXA(); run2++ )
347  xa->operator()(run1,run2) += alpha*tmp(run2,0);
348  }
349  }
350 
351  if( getNP() > 0 ){
352  for( run1 = 0; run1 < getNumPoints(); run1++ ){
353  bm.getSubBlock( 2*getNumPoints(), 0, tmp, getNP(), 1 );
354  for( run2 = 0; run2 < getNP(); run2++ )
355  p->operator()(run1,run2) += alpha*tmp(run2,0);
356  }
357  }
358 
359  if( getNU() > 0 ){
360  for( run1 = 0; run1 < getNumPoints(); run1++ )
361  {
362  bm.getSubBlock( 3*getNumPoints()+run1, 0, tmp, getNU(), 1 );
363  for( run2 = 0; run2 < getNU(); run2++ )
364  u->operator()(run1,run2) += alpha*tmp(run2,0);
365  }
366  }
367 
368  if( getNW() > 0 ){
369  for( run1 = 0; run1 < getNumPoints(); run1++ ){
370  bm.getSubBlock( 4*getNumPoints()+run1, 0, tmp, getNW(), 1 );
371  for( run2 = 0; run2 < getNW(); run2++ )
372  w->operator()(run1,run2) += alpha*tmp(run2,0);
373  }
374  }
375 
376  return SUCCESSFUL_RETURN;
377 }
378 
379 
380 
381 returnValue OCPiterate::enableSimulationMode(){
382 
383  if( x != 0 ) x ->enableAutoInit ();
384  if( xa != 0 ) xa->enableAutoInit ();
385  if( p != 0 ) p ->disableAutoInit();
386  if( u != 0 ) u ->disableAutoInit();
387  if( w != 0 ) w ->disableAutoInit();
388 
389  inSimulationMode = BT_TRUE;
390 
391  return SUCCESSFUL_RETURN;
392 }
393 
394 
395 
396 returnValue OCPiterate::shift( double timeShift,
397  DVector lastX,
398  DVector lastXA,
399  DVector lastP,
400  DVector lastU,
401  DVector lastW ){
402 
403  if ( acadoIsNegative( timeShift ) == BT_TRUE )
404  ACADOERROR( RET_INVALID_ARGUMENTS );
405 
406  double subintervalLength = x->getIntervalLength( 0 );
407 
408  if ( acadoIsEqual( timeShift,subintervalLength ) == BT_FALSE )
409  return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
410 
411 // return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
412 
413  if ( x != 0 )
414  x->shiftBackwards( lastX );
415  if ( xa != 0 )
416  xa->shiftBackwards( lastXA );
417 
418  if ( p != 0 )
419  p->shiftBackwards( lastP );
420 
421  if ( u != 0 )
422  u->shiftBackwards( lastU );
423 
424  if ( w != 0 )
425  w->shiftBackwards( lastW );
426 
427  return SUCCESSFUL_RETURN;
428 }
429 
430 
431 
432 //
433 // PROTECTED MEMBER FUNCTIONS:
434 //
435 
436 void OCPiterate::copy( const OCPiterate& rhs ){
437 
438  if( rhs.x != 0 ) x = new VariablesGrid(*rhs.x );
439  else x = 0 ;
440  if( rhs.xa != 0 ) xa = new VariablesGrid(*rhs.xa);
441  else xa = 0 ;
442  if( rhs.p != 0 ) p = new VariablesGrid(*rhs.p );
443  else p = 0 ;
444  if( rhs.u != 0 ) u = new VariablesGrid(*rhs.u );
445  else u = 0 ;
446  if( rhs.w != 0 ) w = new VariablesGrid(*rhs.w );
447  else w = 0 ;
448 
449  inSimulationMode = rhs.inSimulationMode;
450 }
451 
452 
453 
454 CLOSE_NAMESPACE_ACADO
455 
456 // end of file.
Grid::getNumPoints
uint getNumPoints() const
N
#define N
Definition: crane_kul_mhe_test.cpp:44
OCPiterate
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
BlockMatrix
Implements a very rudimentary block sparse matrix class.
Definition: block_matrix.hpp:52
VariablesGrid::shiftBackwards
VariablesGrid & shiftBackwards(DVector lastValue=emptyVector)
Definition: variables_grid.cpp:343
MatrixVariablesGrid::hasLowerBounds
BooleanType hasLowerBounds() const
Definition: matrix_variables_grid.cpp:1521
OCPiterate::getNX
uint getNX() const
OCPiterate::x
VariablesGrid * x
Definition: ocp_iterate.hpp:186
Grid::getLastTime
double getLastTime() const
OCPiterate::getUnionGrid
Grid getUnionGrid() const
Definition: ocp_iterate.cpp:182
acadoMax
int acadoMax(const int x, const int y)
Definition: acado_utils.cpp:64
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
acadoIsEqual
BooleanType acadoIsEqual(double x, double y, double TOL)
Definition: acado_utils.cpp:88
OCPiterate::getNU
uint getNU() const
OCPiterate::u
VariablesGrid * u
Definition: ocp_iterate.hpp:189
VariablesGrid::getVector
DVector getVector(uint pointIdx) const
Definition: variables_grid.cpp:306
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
acadoIsNegative
BooleanType acadoIsNegative(double x, double TOL)
Definition: acado_utils.cpp:141
MatrixVariablesGrid::getLowerBounds
DVector getLowerBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1344
VariablesGrid
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Definition: variables_grid.hpp:56
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
OCPiterate::allocateAll
returnValue allocateAll()
Definition: ocp_iterate.cpp:92
OCPiterate::~OCPiterate
virtual ~OCPiterate()
Definition: ocp_iterate.cpp:75
Grid::isEmpty
BooleanType isEmpty() const
MatrixVariablesGrid::hasUpperBounds
BooleanType hasUpperBounds() const
Definition: matrix_variables_grid.cpp:1533
ocp_iterate.hpp
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
OCPiterate::updateData
returnValue updateData(double t, DVector &x_=emptyVector, DVector &xa_=emptyVector, DVector &p_=emptyVector, DVector &u_=emptyVector, DVector &w_=emptyVector)
Definition: ocp_iterate.cpp:314
OCPiterate::update
void update(double t, VariablesGrid &z1, DVector &z2) const
Definition: ocp_iterate.cpp:291
OCPiterate::init
returnValue init(const VariablesGrid *const _x, const VariablesGrid *const _xa, const VariablesGrid *const _p, const VariablesGrid *const _u, const VariablesGrid *const _w)
Definition: ocp_iterate.cpp:112
Grid
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
OCPiterate::getNW
uint getNW() const
OCPiterate::copy
void copy(const OCPiterate &rhs)
Definition: ocp_iterate.cpp:436
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
BOUNDTOL
const double BOUNDTOL
Definition: acado_constants.hpp:79
MatrixVariablesGrid::getUpperBounds
DVector getUpperBounds(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1393
OCPiterate::clear
returnValue clear()
Definition: ocp_iterate.cpp:140
OCPiterate::getNumPoints
uint getNumPoints() const
Definition: ocp_iterate.cpp:155
OCPiterate::xa
VariablesGrid * xa
Definition: ocp_iterate.hpp:187
Grid::getFirstTime
double getFirstTime() const
OCPiterate::print
returnValue print() const
Definition: ocp_iterate.cpp:169
Grid::getFloorIndex
uint getFloorIndex(double time) const
Definition: grid.cpp:593
OCPiterate::enableSimulationMode
returnValue enableSimulationMode()
Definition: ocp_iterate.cpp:381
VariablesGrid::setVector
returnValue setVector(uint pointIdx, const DVector &_values)
Definition: variables_grid.cpp:279
OCPiterate::getInitialData
returnValue getInitialData(DVector &x_=emptyVector, DVector &xa_=emptyVector, DVector &p_=emptyVector, DVector &u_=emptyVector, DVector &w_=emptyVector) const
Definition: ocp_iterate.cpp:278
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
Grid::hasTime
BooleanType hasTime(double _time) const
Definition: grid.cpp:520
OCPiterate::operator=
OCPiterate & operator=(const OCPiterate &rhs)
Definition: ocp_iterate.cpp:81
OCPiterate::applyStep
returnValue applyStep(const BlockMatrix &bm, double alpha)
Definition: ocp_iterate.cpp:328
MatrixVariablesGrid::print
returnValue print(std::ostream &stream=std::cout, const char *const name=DEFAULT_LABEL, const char *const startString=DEFAULT_START_STRING, const char *const endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const char *const colSeparator=DEFAULT_COL_SEPARATOR, const char *const rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: matrix_variables_grid.cpp:695
BlockMatrix::getSubBlock
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
MatrixVariablesGrid::getAutoInit
BooleanType getAutoInit(uint pointIdx) const
Definition: matrix_variables_grid.cpp:1442
Grid::getIntervalLength
double getIntervalLength() const
OCPiterate::p
VariablesGrid * p
Definition: ocp_iterate.hpp:188
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
OCPiterate::getNXA
uint getNXA() const
OCPiterate::inSimulationMode
BooleanType inSimulationMode
Definition: ocp_iterate.hpp:213
OCPiterate::shift
virtual returnValue shift(double timeShift=-1.0, DVector lastX=emptyVector, DVector lastXA=emptyVector, DVector lastP=emptyVector, DVector lastU=emptyVector, DVector lastW=emptyVector)
Definition: ocp_iterate.cpp:396
GenericVector::setAll
void setAll(const T &_value)
Definition: vector.hpp:160
MatrixVariablesGrid::getGrid
returnValue getGrid(Grid &_grid) const
Definition: matrix_variables_grid.cpp:1609
OCPiterate::areGridsConsistent
BooleanType areGridsConsistent()
Definition: ocp_iterate.cpp:197
emptyVector
static DVector emptyVector
Definition: vector.hpp:335
OCPiterate::w
VariablesGrid * w
Definition: ocp_iterate.hpp:190
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
RET_NOT_YET_IMPLEMENTED
Definition: acado_types.hpp:847
OCPiterate::getNP
uint getNP() const
ACADOERROR
#define ACADOERROR(retval)
Definition: acado_message_handling.hpp:106

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