box_constraint.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 
34 #include <acado/constraint/box_constraint.hpp>
35 
36 
37 BEGIN_NAMESPACE_ACADO
38 
39 
40 
41 //
42 // PUBLIC MEMBER FUNCTIONS:
43 //
44 
45 
46 BoxConstraint::BoxConstraint( ){
47 
48  nb = 0;
49  var = 0;
50  index = 0;
51  blb = 0;
52  bub = 0;
53 
54  residuumXL = 0;
55  residuumXU = 0;
56  residuumXAL = 0;
57  residuumXAU = 0;
58  residuumPL = 0;
59  residuumPU = 0;
60  residuumUL = 0;
61  residuumUU = 0;
62  residuumWL = 0;
63  residuumWU = 0;
64 }
65 
66 
67 returnValue BoxConstraint::init( const Grid& grid_ ){
68 
69  deleteAll();
70 
71  grid = grid_;
72 
73  nb = 0;
74  var = 0;
75  index = 0;
76  blb = 0;
77  bub = 0;
78 
79  residuumXL = new DMatrix[grid.getNumPoints()];
80  residuumXU = new DMatrix[grid.getNumPoints()];
81  residuumXAL = new DMatrix[grid.getNumPoints()];
82  residuumXAU = new DMatrix[grid.getNumPoints()];
83  residuumPL = new DMatrix[1 ];
84  residuumPU = new DMatrix[1 ];
85  residuumUL = new DMatrix[grid.getNumPoints()];
86  residuumUU = new DMatrix[grid.getNumPoints()];
87  residuumWL = new DMatrix[grid.getNumPoints()];
88  residuumWU = new DMatrix[grid.getNumPoints()];
89 
90  return SUCCESSFUL_RETURN;
91 }
92 
93 
94 
95 BoxConstraint::BoxConstraint( const BoxConstraint& rhs ){
96 
97  int run1 ;
98  grid = rhs.grid;
99 
100  nb = rhs.nb;
101  if( nb > 0 ){
102  var = (VariableType*)calloc(nb,sizeof(VariableType));
103  index = (int*)calloc(nb,sizeof(int));
104  blb = (DVector**)calloc(nb,sizeof(DVector*));
105  bub = (DVector**)calloc(nb,sizeof(DVector*));
106 
107  for( run1 = 0; run1 < nb; run1++ ){
108 
109  var [run1] = rhs.var [run1];
110  index[run1] = rhs.index[run1];
111  blb [run1] = new DVector(*rhs.blb[run1]);
112  bub [run1] = new DVector(*rhs.bub[run1]);
113  }
114  }
115  else{
116  var = 0;
117  index = 0;
118  blb = 0;
119  bub = 0;
120  }
121 
122  residuumXL = new DMatrix[grid.getNumPoints()];
123  residuumXU = new DMatrix[grid.getNumPoints()];
124  residuumXAL = new DMatrix[grid.getNumPoints()];
125  residuumXAU = new DMatrix[grid.getNumPoints()];
126  residuumPL = new DMatrix[1 ];
127  residuumPU = new DMatrix[1 ];
128  residuumUL = new DMatrix[grid.getNumPoints()];
129  residuumUU = new DMatrix[grid.getNumPoints()];
130  residuumWL = new DMatrix[grid.getNumPoints()];
131  residuumWU = new DMatrix[grid.getNumPoints()];
132 }
133 
134 BoxConstraint::~BoxConstraint( ){
135 
136  deleteAll();
137 }
138 
139 
140 void BoxConstraint::deleteAll(){
141 
142  int run1;
143 
144  if( var != 0 ) free( var);
145  if( index != 0 ) free( index);
146  if( blb != 0 ){
147  for( run1 = 0; run1 < nb; run1++ ) delete blb[run1];
148  free(blb);
149  }
150  if( bub != 0 ){
151  for( run1 = 0; run1 < nb; run1++ ) delete bub[run1];
152  free(bub);
153  }
154 
155  if( residuumXL != 0 ) delete[] residuumXL ;
156  if( residuumXU != 0 ) delete[] residuumXU ;
157  if( residuumXAL != 0 ) delete[] residuumXAL;
158  if( residuumXAU != 0 ) delete[] residuumXAU;
159  if( residuumPL != 0 ) delete[] residuumPL ;
160  if( residuumPU != 0 ) delete[] residuumPU ;
161  if( residuumUL != 0 ) delete[] residuumUL ;
162  if( residuumUU != 0 ) delete[] residuumUU ;
163  if( residuumWL != 0 ) delete[] residuumWL ;
164  if( residuumWU != 0 ) delete[] residuumWU ;
165 }
166 
167 
168 BoxConstraint& BoxConstraint::operator=( const BoxConstraint& rhs ){
169 
170  int run1;
171 
172  if( this != &rhs ){
173 
174  deleteAll();
175 
176  grid = rhs.grid;
177 
178  nb = rhs.nb;
179  if( nb > 0 ){
180  var = (VariableType*)calloc(nb,sizeof(VariableType));
181  index = (int*)calloc(nb,sizeof(int));
182  blb = (DVector**)calloc(nb,sizeof(DVector*));
183  bub = (DVector**)calloc(nb,sizeof(DVector*));
184 
185  for( run1 = 0; run1 < nb; run1++ ){
186 
187  var [run1] = rhs.var [run1];
188  index[run1] = rhs.index[run1];
189  blb [run1] = new DVector(*rhs.blb[run1]);
190  bub [run1] = new DVector(*rhs.bub[run1]);
191  }
192  }
193  else{
194  var = 0;
195  index = 0;
196  blb = 0;
197  bub = 0;
198  }
199 
200  residuumXL = new DMatrix[grid.getNumPoints()];
201  residuumXU = new DMatrix[grid.getNumPoints()];
202  residuumXAL = new DMatrix[grid.getNumPoints()];
203  residuumXAU = new DMatrix[grid.getNumPoints()];
204  residuumPL = new DMatrix[1 ];
205  residuumPU = new DMatrix[1 ];
206  residuumUL = new DMatrix[grid.getNumPoints()];
207  residuumUU = new DMatrix[grid.getNumPoints()];
208  residuumWL = new DMatrix[grid.getNumPoints()];
209  residuumWU = new DMatrix[grid.getNumPoints()];
210  }
211  return *this;
212 }
213 
214 
215 
216 
217 
218 returnValue BoxConstraint::evaluateBounds( const OCPiterate& iter ){
219 
220 
221  uint run1, run2;
222 
223  const uint N = grid.getNumPoints();
224 
225  // EVALUATE BOUNDS:
226  // ----------------
227 
228  for( run1 = 0; run1 < N; run1++ ){
229 
230  if( iter.x != NULL ){
231  residuumXL[run1].init ( iter.x->getNumValues(), 1 );
232  residuumXU[run1].init ( iter.x->getNumValues(), 1 );
233 
234  for( run2 = 0; run2 < iter.x->getNumValues(); run2++ ){
235  residuumXL[run1](run2,0) = -INFTY;
236  residuumXU[run1](run2,0) = INFTY;
237  }
238  }
239  else{
240  residuumXL[run1].init ( 0, 0 );
241  residuumXU[run1].init ( 0, 0 );
242  }
243 
244  if( iter.xa != NULL ){
245  residuumXAL[run1].init ( iter.xa->getNumValues(), 1 );
246  residuumXAU[run1].init ( iter.xa->getNumValues(), 1 );
247 
248  for( run2 = 0; run2 < iter.xa->getNumValues(); run2++ ){
249  residuumXAL[run1](run2,0) = -INFTY;
250  residuumXAU[run1](run2,0) = INFTY;
251  }
252  }
253  else{
254  residuumXAL[run1].init ( 0, 0 );
255  residuumXAU[run1].init ( 0, 0 );
256  }
257 
258  if( iter.u != NULL ){
259  residuumUL[run1].init ( iter.u->getNumValues(), 1 );
260  residuumUU[run1].init ( iter.u->getNumValues(), 1 );
261 
262  for( run2 = 0; run2 < iter.u->getNumValues(); run2++ ){
263  residuumUL[run1](run2,0) = -INFTY;
264  residuumUU[run1](run2,0) = INFTY;
265  }
266  }
267  else{
268  residuumUL[run1].init ( 0, 0 );
269  residuumUU[run1].init ( 0, 0 );
270  }
271 
272  if( iter.w != NULL ){
273  residuumWL[run1].init ( iter.w->getNumValues(), 1 );
274  residuumWU[run1].init ( iter.w->getNumValues(), 1 );
275 
276  for( run2 = 0; run2 < iter.w->getNumValues(); run2++ ){
277  residuumWL[run1](run2,0) = -INFTY;
278  residuumWU[run1](run2,0) = INFTY;
279  }
280  }
281  else{
282  residuumWL[run1].init ( 0, 0 );
283  residuumWU[run1].init ( 0, 0 );
284  }
285  }
286 
287 
288  if( iter.p != NULL ){
289  residuumPL[0].init ( iter.p->getNumValues(), 1 );
290  residuumPU[0].init ( iter.p->getNumValues(), 1 );
291 
292  for( run2 = 0; run2 < iter.p->getNumValues(); run2++ ){
293  residuumPL[0](run2,0) = -INFTY;
294  residuumPU[0](run2,0) = INFTY;
295  }
296  }
297  else{
298  residuumPL[0].init ( 0, 0 );
299  residuumPU[0].init ( 0, 0 );
300  }
301 
302 
303  for( run1 = 0; (int) run1 < nb; run1++ ){
304 
305  switch( var[run1] ){
306 
307  case VT_DIFFERENTIAL_STATE:
308  if( iter.x != NULL ){
309  for( run2 = 0; run2 < N; run2++ ){
310  residuumXL[run2](index[run1],0) = blb[run1][0](run2) - iter.x->operator()(run2,index[run1]);
311  residuumXU[run2](index[run1],0) = bub[run1][0](run2) - iter.x->operator()(run2,index[run1]);
312  }
313  }
314  else ACADOERROR( RET_INVALID_ARGUMENTS );
315  break;
316 
317 
318  case VT_ALGEBRAIC_STATE:
319  if( iter.xa != NULL ){
320  for( run2 = 0; run2 < N; run2++ ){
321  residuumXAL[run2](index[run1],0) = blb[run1][0](run2) - iter.xa->operator()(run2,index[run1]);
322  residuumXAU[run2](index[run1],0) = bub[run1][0](run2) - iter.xa->operator()(run2,index[run1]);
323  }
324  }
325  else ACADOERROR( RET_INVALID_ARGUMENTS );
326  break;
327 
328 
329  case VT_PARAMETER:
330  if( iter.p != NULL ){
331  residuumPL[0](index[run1],0) = blb[run1][0](0) - iter.p->operator()(0,index[run1]);
332  residuumPU[0](index[run1],0) = bub[run1][0](0) - iter.p->operator()(0,index[run1]);
333  }
334  else ACADOERROR( RET_INVALID_ARGUMENTS );
335  break;
336 
337  case VT_CONTROL:
338  if( iter.u != NULL ){
339  for( run2 = 0; run2 < N; run2++ ){
340  residuumUL[run2](index[run1],0) = blb[run1][0](run2) - iter.u->operator()(run2,index[run1]);
341  residuumUU[run2](index[run1],0) = bub[run1][0](run2) - iter.u->operator()(run2,index[run1]);
342  }
343  }
344  else ACADOERROR( RET_INVALID_ARGUMENTS );
345  break;
346 
347  case VT_DISTURBANCE:
348  if( iter.w != NULL ){
349  for( run2 = 0; run2 < N; run2++ ){
350  residuumWL[run2](index[run1],0) = blb[run1][0](run2) - iter.w->operator()(run2,index[run1]);
351  residuumWU[run2](index[run1],0) = bub[run1][0](run2) - iter.w->operator()(run2,index[run1]);
352  }
353  }
354  else {ASSERT(1==0);ACADOERROR( RET_INVALID_ARGUMENTS );}
355  break;
356 
357 
358  default:
359  ACADOERRORTEXT(RET_NOT_IMPLEMENTED_YET,"Variables in constraints should be of type DIFFERENTIAL_STATE, ALGEBRAIC_STATE, PARAMETER, CONTROL or DISTURBANCE\n");
360  break;
361 
362  }
363  }
364 
365  return SUCCESSFUL_RETURN;
366 }
367 
368 
369 inline returnValue BoxConstraint::getBounds( const OCPiterate& iter ){
370 
371 
372  uint run1, run2;
373 
374  const uint N = grid.getNumPoints();
375 
376  for( run1 = 0; (int) run1 < nb; run1++ ){
377 
378  switch( var[run1] ){
379 
380  case VT_DIFFERENTIAL_STATE:
381  if( iter.x != NULL ){
382  for( run2 = 0; run2 < N; run2++ ){
383 
384  if( bub[run1][0](run2) - blb[run1][0](run2) < -0.5*BOUNDTOL )
385  return ACADOERROR( RET_INCONSISTENT_BOUNDS );
386 
387  iter.x->setLowerBound(run2,index[run1],blb[run1][0](run2));
388  iter.x->setUpperBound(run2,index[run1],bub[run1][0](run2));
389  }
390  }
391  else ACADOERROR( RET_INVALID_ARGUMENTS );
392  break;
393 
394 
395  case VT_ALGEBRAIC_STATE:
396  if( iter.xa != NULL ){
397  for( run2 = 0; run2 < N; run2++ ){
398 
399  if( bub[run1][0](run2) - blb[run1][0](run2) < -0.5*BOUNDTOL )
400  return ACADOERROR( RET_INCONSISTENT_BOUNDS );
401 
402  iter.xa->setLowerBound(run2,index[run1],blb[run1][0](run2));
403  iter.xa->setUpperBound(run2,index[run1],bub[run1][0](run2));
404  }
405  }
406  else ACADOERROR( RET_INVALID_ARGUMENTS );
407  break;
408 
409 
410  case VT_PARAMETER:
411  if( iter.p != NULL ){
412  for( run2 = 0; run2 < N; run2++ ){
413  if( bub[run1][0](0) - blb[run1][0](0) < -0.5*BOUNDTOL )
414  return ACADOERROR( RET_INCONSISTENT_BOUNDS );
415 
416  iter.p->setLowerBound(run2,index[run1],blb[run1][0](0));
417  iter.p->setUpperBound(run2,index[run1],bub[run1][0](0));
418  }
419  }
420  else ACADOERROR( RET_INVALID_ARGUMENTS );
421  break;
422 
423  case VT_CONTROL:
424  if( iter.u != NULL ){
425  for( run2 = 0; run2 < N; run2++ ){
426 
427  if( bub[run1][0](run2) - blb[run1][0](run2) < -0.5*BOUNDTOL )
428  return ACADOERROR( RET_INCONSISTENT_BOUNDS );
429 
430  iter.u->setLowerBound(run2,index[run1],blb[run1][0](run2));
431  iter.u->setUpperBound(run2,index[run1],bub[run1][0](run2));
432  }
433  }
434  else ACADOERROR( RET_INVALID_ARGUMENTS );
435  break;
436 
437  case VT_DISTURBANCE:
438  if( iter.w != NULL ){
439  for( run2 = 0; run2 < N; run2++ ){
440 
441  if( bub[run1][0](run2) - blb[run1][0](run2) < -0.5*BOUNDTOL )
442  return ACADOERROR( RET_INCONSISTENT_BOUNDS );
443 
444  iter.w->setLowerBound(run2,index[run1],blb[run1][0](run2));
445  iter.w->setUpperBound(run2,index[run1],bub[run1][0](run2));
446  }
447  }
448  else ACADOERROR( RET_INVALID_ARGUMENTS );
449  break;
450 
451 
452  default:
453  ACADOERROR(RET_NOT_IMPLEMENTED_YET);
454  break;
455 
456  }
457  }
458 
459  return SUCCESSFUL_RETURN;
460 }
461 
462 
463 
464 CLOSE_NAMESPACE_ACADO
465 
466 // end of file.
BoxConstraint::residuumPU
DMatrix * residuumPU
Definition: box_constraint.hpp:125
N
#define N
Definition: crane_kul_mhe_test.cpp:44
OCPiterate
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
VT_PARAMETER
Definition: acado_types.hpp:101
OCPiterate::x
VariablesGrid * x
Definition: ocp_iterate.hpp:186
GenericMatrix::init
void init(unsigned _nRows=0, unsigned _nCols=0)
Definition: matrix.hpp:135
BoxConstraint::init
returnValue init(const Grid &grid_)
Definition: box_constraint.cpp:67
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
INFTY
const double INFTY
Definition: acado_constants.hpp:65
OCPiterate::u
VariablesGrid * u
Definition: ocp_iterate.hpp:189
BoxConstraint::residuumXAU
DMatrix * residuumXAU
Definition: box_constraint.hpp:123
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
BoxConstraint::residuumUL
DMatrix * residuumUL
Definition: box_constraint.hpp:126
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
BoxConstraint
Stores and evaluates box constraints within optimal control problems.
Definition: box_constraint.hpp:59
VT_ALGEBRAIC_STATE
Definition: acado_types.hpp:98
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
Grid
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
VT_DIFFERENTIAL_STATE
Definition: acado_types.hpp:97
BoxConstraint::residuumXL
DMatrix * residuumXL
Definition: box_constraint.hpp:120
MatrixVariablesGrid::setLowerBound
returnValue setLowerBound(uint pointIdx, uint valueIdx, double _lb)
Definition: matrix_variables_grid.cpp:1376
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
BOUNDTOL
const double BOUNDTOL
Definition: acado_constants.hpp:79
VariableType
VariableType
Definition: acado_types.hpp:95
BoxConstraint::var
VariableType * var
Definition: box_constraint.hpp:115
OCPiterate::xa
VariablesGrid * xa
Definition: ocp_iterate.hpp:187
BoxConstraint::residuumPL
DMatrix * residuumPL
Definition: box_constraint.hpp:124
RET_NOT_IMPLEMENTED_YET
Definition: acado_types.hpp:848
BoxConstraint::residuumXU
DMatrix * residuumXU
Definition: box_constraint.hpp:121
MatrixVariablesGrid::setUpperBound
returnValue setUpperBound(uint pointIdx, uint valueIdx, double _ub)
Definition: matrix_variables_grid.cpp:1425
BoxConstraint::residuumXAL
DMatrix * residuumXAL
Definition: box_constraint.hpp:122
BoxConstraint::~BoxConstraint
virtual ~BoxConstraint()
Definition: box_constraint.cpp:134
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
BoxConstraint::getBounds
virtual returnValue getBounds(const OCPiterate &iter)
Definition: box_constraint.cpp:369
BoxConstraint::evaluateBounds
returnValue evaluateBounds(const OCPiterate &iter)
Definition: box_constraint.cpp:218
ASSERT
#define ASSERT(x)
Definition: acado_debugging.hpp:51
OCPiterate::p
VariablesGrid * p
Definition: ocp_iterate.hpp:188
RET_INCONSISTENT_BOUNDS
Definition: acado_types.hpp:1029
BoxConstraint::operator=
BoxConstraint & operator=(const BoxConstraint &rhs)
Definition: box_constraint.cpp:168
DVector
GenericVector< double > DVector
Definition: vector.hpp:329
Grid::getNumPoints
uint getNumPoints() const
VT_CONTROL
Definition: acado_types.hpp:99
BoxConstraint::residuumWU
DMatrix * residuumWU
Definition: box_constraint.hpp:129
OCPiterate::w
VariablesGrid * w
Definition: ocp_iterate.hpp:190
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
BoxConstraint::residuumUU
DMatrix * residuumUU
Definition: box_constraint.hpp:127
BoxConstraint::residuumWL
DMatrix * residuumWL
Definition: box_constraint.hpp:128
box_constraint.hpp
VT_DISTURBANCE
Definition: acado_types.hpp:103
ACADOERROR
#define ACADOERROR(retval)
Definition: acado_message_handling.hpp:106
ACADOERRORTEXT
#define ACADOERRORTEXT(retval, text)
Definition: acado_message_handling.hpp:110

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:29