external_packages/qpOASES-3.2.0/src/Bounds.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of qpOASES.
3  *
4  * qpOASES -- An Implementation of the Online Active Set Strategy.
5  * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
6  * Christian Kirches et al. All rights reserved.
7  *
8  * qpOASES is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU Lesser General Public
10  * License as published by the Free Software Foundation; either
11  * version 2.1 of the License, or (at your option) any later version.
12  *
13  * qpOASES is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16  * See the GNU Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with qpOASES; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21  *
22  */
23 
24 
36 #include <qpOASES/Bounds.hpp>
37 
38 
40 
41 
42 /*****************************************************************************
43  * P U B L I C *
44  *****************************************************************************/
45 
46 
47 /*
48  * B o u n d s
49  */
51 {
52 }
53 
54 
55 /*
56  * B o u n d s
57  */
58 Bounds::Bounds( int_t _n ) : SubjectTo( _n )
59 {
60  init( _n );
61 }
62 
63 
64 /*
65  * B o u n d s
66  */
67 Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs )
68 {
69  copy( rhs );
70 }
71 
72 
73 /*
74  * ~ B o u n d s
75  */
77 {
78  clear( );
79 }
80 
81 
82 /*
83  * o p e r a t o r =
84  */
86 {
87  if ( this != &rhs )
88  {
89  clear( );
90  SubjectTo::operator=( rhs );
91  copy( rhs );
92  }
93 
94  return *this;
95 }
96 
97 
98 
99 /*
100  * i n i t
101  */
103  )
104 {
105  if ( _n < 0 )
107 
108  clear( );
109 
110  if ( _n >= 0 )
111  {
112  freee.init( _n );
113  fixed.init( _n );
114  }
115 
116  return SubjectTo::init( _n );
117 }
118 
119 
120 
121 /*
122  * s e t u p B o u n d
123  */
125  )
126 {
127  /* consistency check */
128  if ( ( number < 0 ) || ( number >= n ) )
130 
131  /* Add bound index to respective index list. */
132  switch ( _status )
133  {
134  case ST_INACTIVE:
135  if ( this->addIndex( this->getFree( ),number,_status ) != SUCCESSFUL_RETURN )
137  break;
138 
139  case ST_LOWER:
140  if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
142  break;
143 
144  case ST_UPPER:
145  if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
147  break;
148 
149  default:
151  }
152 
153  return SUCCESSFUL_RETURN;
154 }
155 
156 
157 /*
158  * s e t u p A l l F r e e
159  */
161 {
162  return setupAll( ST_INACTIVE );
163 }
164 
165 
166 /*
167  * s e t u p A l l L o w e r
168  */
170 {
171  return setupAll( ST_LOWER );
172 }
173 
174 
175 /*
176  * s e t u p A l l U p p e r
177  */
179 {
180  return setupAll( ST_UPPER );
181 }
182 
183 
184 /*
185  * m o v e F i x e d T o F r e e
186  */
188 {
189  /* consistency check */
190  if ( ( number < 0 ) || ( number >= n ) )
192 
193  /* Move index from indexlist of fixed variables to that of free ones. */
194  if ( this->removeIndex( this->getFixed( ),number ) != SUCCESSFUL_RETURN )
196 
197  if ( this->addIndex( this->getFree( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
199 
200  return SUCCESSFUL_RETURN;
201 }
202 
203 
204 /*
205  * m o v e F r e e T o F i x e d
206  */
208  )
209 {
210  /* consistency check */
211  if ( ( number < 0 ) || ( number >= n ) )
213 
214  /* Move index from indexlist of free variables to that of fixed ones. */
215  if ( this->removeIndex( this->getFree( ),number ) != SUCCESSFUL_RETURN )
217 
218  if ( this->addIndex( this->getFixed( ),number,_status ) != SUCCESSFUL_RETURN )
220 
221  return SUCCESSFUL_RETURN;
222 }
223 
224 
225 /*
226  * f l i p F i x e d
227  */
229 {
230  /* consistency check */
231  if ( ( number < 0 ) || ( number >= n ) )
233 
234  if ( status != 0 )
235  switch (status[number])
236  {
237  case ST_LOWER: status[number] = ST_UPPER; break;
238  case ST_UPPER: status[number] = ST_LOWER; break;
239  default: return THROWERROR( RET_MOVING_BOUND_FAILED );
240  }
241 
242  return SUCCESSFUL_RETURN;
243 }
244 
245 
246 /*
247  * s w a p F r e e
248  */
249 returnValue Bounds::swapFree( int_t number1, int_t number2
250  )
251 {
252  /* consistency check */
253  if ( ( number1 < 0 ) || ( number1 >= n ) || ( number2 < 0 ) || ( number2 >= n ) )
255 
256  /* Swap index within indexlist of free variables. */
257  return this->swapIndex( this->getFree( ),number1,number2 );
258 }
259 
260 
261 /*
262  * s h i f t
263  */
265 {
266  int_t i;
267 
268  /* consistency check */
269  if ( ( offset == 0 ) || ( n <= 1 ) )
270  return SUCCESSFUL_RETURN;
271 
272  if ( ( offset < 0 ) || ( offset > n/2 ) )
274 
275  if ( ( n % offset ) != 0 )
277 
278 
279  /* 1) Shift types and status. */
280  for( i=0; i<n-offset; ++i )
281  {
282  setType( i,getType( i+offset ) );
283  setStatus( i,getStatus( i+offset ) );
284  }
285 
286  /* 2) Construct shifted index lists of free and fixed variables. */
287  Indexlist shiftedFreee( n );
288  Indexlist shiftedFixed( n );
289 
290  for( i=0; i<n; ++i )
291  {
292  switch ( getStatus( i ) )
293  {
294  case ST_INACTIVE:
295  if ( shiftedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
297  break;
298 
299  case ST_LOWER:
300  if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
302  break;
303 
304  case ST_UPPER:
305  if ( shiftedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
307  break;
308 
309  default:
311  }
312  }
313 
314  /* 3) Assign shifted index list. */
315  freee = shiftedFreee;
316  fixed = shiftedFixed;
317 
318  return SUCCESSFUL_RETURN;
319 }
320 
321 
322 /*
323  * r o t a t e
324  */
326 {
327  int_t i;
328 
329  /* consistency check */
330  if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
331  return SUCCESSFUL_RETURN;
332 
333  if ( ( offset < 0 ) || ( offset > n ) )
335 
336 
337  /* 1) Rotate types and status. */
338  SubjectToType* typeTmp = new SubjectToType[offset];
339  SubjectToStatus* statusTmp = new SubjectToStatus[offset];
340 
341  for( i=0; i<offset; ++i )
342  {
343  typeTmp[i] = getType( i );
344  statusTmp[i] = getStatus( i );
345  }
346 
347  for( i=0; i<n-offset; ++i )
348  {
349  setType( i,getType( i+offset ) );
350  setStatus( i,getStatus( i+offset ) );
351  }
352 
353  for( i=n-offset; i<n; ++i )
354  {
355  setType( i,typeTmp[i-n+offset] );
356  setStatus( i,statusTmp[i-n+offset] );
357  }
358 
359  delete[] statusTmp; delete[] typeTmp;
360 
361  /* 2) Construct shifted index lists of free and fixed variables. */
362  Indexlist rotatedFreee( n );
363  Indexlist rotatedFixed( n );
364 
365  for( i=0; i<n; ++i )
366  {
367  switch ( getStatus( i ) )
368  {
369  case ST_INACTIVE:
370  if ( rotatedFreee.addNumber( i ) != SUCCESSFUL_RETURN )
372  break;
373 
374  case ST_LOWER:
375  if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
377  break;
378 
379  case ST_UPPER:
380  if ( rotatedFixed.addNumber( i ) != SUCCESSFUL_RETURN )
382  break;
383 
384  default:
386  }
387  }
388 
389  /* 3) Assign shifted index list. */
390  freee = rotatedFreee;
391  fixed = rotatedFixed;
392 
393  return SUCCESSFUL_RETURN;
394 }
395 
396 
397 /*
398  * p r i n t
399  */
401 {
402  if ( n == 0 )
403  return SUCCESSFUL_RETURN;
404 
405  #ifndef __SUPPRESSANYOUTPUT__
406 
407  char myPrintfString[MAX_STRING_LENGTH];
408 
409  int_t nFR = getNFR( );
410  int_t nFX = getNFX( );
411 
412  int_t* FR_idx;
413  getFree( )->getNumberArray( &FR_idx );
414 
415  int_t* FX_idx;
416  getFixed( )->getNumberArray( &FX_idx );
417 
418  snprintf( myPrintfString,MAX_STRING_LENGTH,"Bounds object comprising %d variables (%d free, %d fixed):\n",(int)n,(int)nFR,(int)nFX );
419  myPrintf( myPrintfString );
420 
421  REFER_NAMESPACE_QPOASES print( FR_idx,nFR,"free " );
422  REFER_NAMESPACE_QPOASES print( FX_idx,nFX,"fixed" );
423 
424  #endif /* __SUPPRESSANYOUTPUT__ */
425 
426  return SUCCESSFUL_RETURN;
427 }
428 
429 
430 
431 /*****************************************************************************
432  * P R O T E C T E D *
433  *****************************************************************************/
434 
435 /*
436  * c l e a r
437  */
439 {
440  return SUCCESSFUL_RETURN;
441 }
442 
443 
444 /*
445  * c o p y
446  */
447 returnValue Bounds::copy( const Bounds& rhs
448  )
449 {
450  freee = rhs.freee;
451  fixed = rhs.fixed;
452 
453  return SUCCESSFUL_RETURN;
454 }
455 
456 
457 
458 /*
459  * s e t u p A l l
460  */
462 {
463  int_t i;
464 
465  /* 1) Place unbounded variables at the beginning of the index list of free variables. */
466  for( i=0; i<n; ++i )
467  {
468  if ( getType( i ) == ST_UNBOUNDED )
469  {
470  if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
472  }
473  }
474 
475  /* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
476  for( i=0; i<n; ++i )
477  {
478  if ( getType( i ) == ST_BOUNDED )
479  {
480  if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
482  }
483  }
484 
485  /* 3) Place implicitly fixed variables at the end of the index list of free variables. */
486  for( i=0; i<n; ++i )
487  {
488  if ( getType( i ) == ST_EQUALITY )
489  {
490  if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
492  }
493  }
494 
495  /* 4) Moreover, add all bounds of unknown type. */
496  for( i=0; i<n; ++i )
497  {
498  if ( getType( i ) == ST_UNKNOWN || getType( i ) == ST_DISABLED )
499  {
500  if ( setupBound( i,_status ) != SUCCESSFUL_RETURN )
502  }
503  }
504 
505  return SUCCESSFUL_RETURN;
506 }
507 
508 
510 
511 
512 /*
513  * end of file
514  */
returnValue setType(int i, SubjectToType value)
SubjectToStatus status[NVMAX]
Definition: Bounds.h:61
#define ST_LOWER
int n
Definition: Bounds.h:58
Indexlist * getFree()
virtual returnValue rotate(int offset)
Allows to pass back messages to the calling function.
Base class for managing working sets of bounds and constraints.
returnValue setupAll(SubjectToStatus _status)
returnValue myPrintf(const char *s)
#define ST_INACTIVE
returnValue setupBound(int _number, SubjectToStatus _status)
returnValue swapIndex(Indexlist *const indexlist, int number1, int number2)
returnValue addIndex(Indexlist *const indexlist, int newnumber, SubjectToStatus newstatus)
returnValue copy(const Bounds &rhs)
SubjectToStatus getStatus(int i) const
virtual returnValue shift(int offset)
returnValue removeIndex(Indexlist *const indexlist, int removenumber)
void rhs(const real_t *x, real_t *f)
SubjectToType getType(int i) const
int getNFR()
int getNFX()
returnValue setStatus(int i, SubjectToStatus value)
Manages working sets of bounds (= box constraints).
#define ST_UPPER
Indexlist * getFixed()
returnValue moveFreeToFixed(int _number, SubjectToStatus _status)


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