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 
35 
36 
38 
39 
40 
41 //
42 // PUBLIC MEMBER FUNCTIONS:
43 //
44 
45 
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 
68 
69  deleteAll();
70 
71  grid = grid_;
72 
73  nb = 0;
74  var = 0;
75  index = 0;
76  blb = 0;
77  bub = 0;
78 
83  residuumPL = new DMatrix[1 ];
84  residuumPU = new DMatrix[1 ];
89 
90  return SUCCESSFUL_RETURN;
91 }
92 
93 
94 
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 
126  residuumPL = new DMatrix[1 ];
127  residuumPU = new DMatrix[1 ];
132 }
133 
135 
136  deleteAll();
137 }
138 
139 
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 
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 
204  residuumPL = new DMatrix[1 ];
205  residuumPU = new DMatrix[1 ];
210  }
211  return *this;
212 }
213 
214 
215 
216 
217 
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 
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  }
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  }
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  }
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  }
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 
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 
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 )
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  }
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 )
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  }
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 )
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  }
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 )
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  }
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 )
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  }
449  break;
450 
451 
452  default:
454  break;
455 
456  }
457  }
458 
459  return SUCCESSFUL_RETURN;
460 }
461 
462 
463 
465 
466 // end of file.
DMatrix * residuumPU
#define N
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
VariablesGrid * x
void init(unsigned _nRows=0, unsigned _nCols=0)
Definition: matrix.hpp:135
returnValue init(const Grid &grid_)
const double INFTY
VariablesGrid * u
DMatrix * residuumXAU
DMatrix * residuumUL
Allows to pass back messages to the calling function.
Stores and evaluates box constraints within optimal control problems.
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
DMatrix * residuumXL
returnValue setLowerBound(uint pointIdx, uint valueIdx, double _lb)
#define CLOSE_NAMESPACE_ACADO
const double BOUNDTOL
VariableType
Definition: acado_types.hpp:95
VariableType * var
VariablesGrid * xa
DMatrix * residuumPL
DMatrix * residuumXU
returnValue setUpperBound(uint pointIdx, uint valueIdx, double _ub)
DMatrix * residuumXAL
virtual ~BoxConstraint()
void rhs(const real_t *x, real_t *f)
virtual returnValue getBounds(const OCPiterate &iter)
returnValue evaluateBounds(const OCPiterate &iter)
#define ASSERT(x)
VariablesGrid * p
BoxConstraint & operator=(const BoxConstraint &rhs)
GenericVector< double > DVector
Definition: vector.hpp:329
uint getNumPoints() const
DMatrix * residuumWU
VariablesGrid * w
#define BEGIN_NAMESPACE_ACADO
DMatrix * residuumUU
DMatrix * residuumWL
#define ACADOERROR(retval)
#define ACADOERRORTEXT(retval, text)


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