external_packages/qpOASES-3.2.0/src/Constraints.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/Constraints.hpp>
37 
38 
40 
41 
42 /*****************************************************************************
43  * P U B L I C *
44  *****************************************************************************/
45 
46 
47 /*
48  * C o n s t r a i n t s
49  */
51 {
52 }
53 
54 
55 /*
56  * C o n s t r a i n t s
57  */
59 {
60  init( _n );
61 }
62 
63 
64 /*
65  * C o n s t r a i n t s
66  */
68 {
69  copy( rhs );
70 }
71 
72 
73 /*
74  * ~ C o n s t r a i n t 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  * i n i t
100  */
102  )
103 {
104  if ( _n < 0 )
106 
107  clear( );
108 
109  if ( _n >= 0 )
110  {
111  active.init( _n );
112  inactive.init( _n );
113  }
114 
115  return SubjectTo::init( _n );
116 }
117 
118 
119 
120 /*
121  * s e t u p C o n s t r a i n t
122  */
124  )
125 {
126  /* consistency check */
127  if ( ( number < 0 ) || ( number >= n ) )
129 
130  /* Add constraint index to respective index list. */
131  switch ( _status )
132  {
133  case ST_INACTIVE:
134  if ( this->addIndex( this->getInactive( ),number,_status ) != SUCCESSFUL_RETURN )
136  break;
137 
138  case ST_LOWER:
139  if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
141  break;
142 
143  case ST_UPPER:
144  if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
146  break;
147 
148  default:
150  }
151 
152  return SUCCESSFUL_RETURN;
153 }
154 
155 
156 /*
157  * s e t u p A l l I n a c t i v e
158  */
160 {
161  return setupAll( ST_INACTIVE );
162 }
163 
164 
165 /*
166  * s e t u p A l l L o w e r
167  */
169 {
170  return setupAll( ST_LOWER );
171 }
172 
173 
174 /*
175  * s e t u p A l l U p p e r
176  */
178 {
179  return setupAll( ST_UPPER );
180 }
181 
182 
183 /*
184  * m o v e A c t i v e T o I n a c t i v e
185  */
187 {
188  /* consistency check */
189  if ( ( number < 0 ) || ( number >= n ) )
191 
192  /* Move index from indexlist of active constraints to that of inactive ones. */
193  if ( this->removeIndex( this->getActive( ),number ) != SUCCESSFUL_RETURN )
195 
196  if ( this->addIndex( this->getInactive( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
198 
199  return SUCCESSFUL_RETURN;
200 }
201 
202 
203 /*
204  * m o v e I n a c t i v e T o A c t i v e
205  */
207  )
208 {
209  /* consistency check */
210  if ( ( number < 0 ) || ( number >= n ) )
212 
213  /* Move index from indexlist of inactive constraints to that of active ones. */
214  if ( this->removeIndex( this->getInactive( ),number ) != SUCCESSFUL_RETURN )
216 
217  if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
219 
220  return SUCCESSFUL_RETURN;
221 }
222 
223 
224 /*
225  * f l i p F i x e d
226  */
228 {
229  /* consistency check */
230  if ( ( number < 0 ) || ( number >= n ) )
232 
233  if ( status != 0 )
234  switch (status[number])
235  {
236  case ST_LOWER: status[number] = ST_UPPER; break;
237  case ST_UPPER: status[number] = ST_LOWER; break;
238  default: return THROWERROR( RET_MOVING_CONSTRAINT_FAILED );
239  }
240 
241  return SUCCESSFUL_RETURN;
242 }
243 
244 
245 /*
246  * s h i f t
247  */
249 {
250  int_t i;
251 
252  /* consistency check */
253  if ( ( offset == 0 ) || ( n <= 1 ) )
254  return SUCCESSFUL_RETURN;
255 
256  if ( ( offset < 0 ) || ( offset > n/2 ) )
258 
259  if ( ( n % offset ) != 0 )
261 
262 
263  /* 1) Shift types and status. */
264  for( i=0; i<n-offset; ++i )
265  {
266  setType( i,getType( i+offset ) );
267  setStatus( i,getStatus( i+offset ) );
268  }
269 
270  /* 2) Construct shifted index lists of free and fixed variables. */
271  Indexlist shiftedActive( n );
272  Indexlist shiftedInactive( n );
273 
274  for( i=0; i<n; ++i )
275  {
276  switch ( getStatus( i ) )
277  {
278  case ST_INACTIVE:
279  if ( shiftedInactive.addNumber( i ) != SUCCESSFUL_RETURN )
281  break;
282 
283  case ST_LOWER:
284  if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
286  break;
287 
288  case ST_UPPER:
289  if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
291  break;
292 
293  default:
295  }
296  }
297 
298  /* 3) Assign shifted index list. */
299  active = shiftedActive;
300  inactive = shiftedInactive;
301 
302  return SUCCESSFUL_RETURN;
303 }
304 
305 
306 /*
307  * r o t a t e
308  */
310 {
311  int_t i;
312 
313  /* consistency check */
314  if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
315  return SUCCESSFUL_RETURN;
316 
317  if ( ( offset < 0 ) || ( offset > n ) )
319 
320 
321  /* 1) Rotate types and status. */
322  SubjectToType* typeTmp = new SubjectToType[offset];
323  SubjectToStatus* statusTmp = new SubjectToStatus[offset];
324 
325  for( i=0; i<offset; ++i )
326  {
327  typeTmp[i] = getType( i );
328  statusTmp[i] = getStatus( i );
329  }
330 
331  for( i=0; i<n-offset; ++i )
332  {
333  setType( i,getType( i+offset ) );
334  setStatus( i,getStatus( i+offset ) );
335  }
336 
337  for( i=n-offset; i<n; ++i )
338  {
339  setType( i,typeTmp[i-n+offset] );
340  setStatus( i,statusTmp[i-n+offset] );
341  }
342 
343  delete[] statusTmp; delete[] typeTmp;
344 
345  /* 2) Construct shifted index lists of free and fixed variables. */
346  Indexlist rotatedActive( n );
347  Indexlist rotatedInactive( n );
348 
349  for( i=0; i<n; ++i )
350  {
351  switch ( getStatus( i ) )
352  {
353  case ST_INACTIVE:
354  if ( rotatedInactive.addNumber( i ) != SUCCESSFUL_RETURN )
356  break;
357 
358  case ST_LOWER:
359  if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
361  break;
362 
363  case ST_UPPER:
364  if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
366  break;
367 
368  default:
370  }
371  }
372 
373  /* 3) Assign shifted index list. */
374  active = rotatedActive;
375  inactive = rotatedInactive;
376 
377  return SUCCESSFUL_RETURN;
378 }
379 
380 
381 /*
382  * p r i n t
383  */
385 {
386  if ( n == 0 )
387  return SUCCESSFUL_RETURN;
388 
389  #ifndef __SUPPRESSANYOUTPUT__
390 
391  char myPrintfString[MAX_STRING_LENGTH];
392 
393  int_t nIAC = getNIAC( );
394  int_t nAC = getNAC( );
395 
396  int_t* IAC_idx;
397  getInactive( )->getNumberArray( &IAC_idx );
398 
399  int_t* AC_idx;
400  getActive( )->getNumberArray( &AC_idx );
401 
402  snprintf( myPrintfString,MAX_STRING_LENGTH,"Constraints object comprising %d constraints (%d inactive, %d active):\n",(int)n,(int)nIAC,(int)nAC );
403  myPrintf( myPrintfString );
404 
405  REFER_NAMESPACE_QPOASES print( IAC_idx,nIAC,"inactive" );
406  REFER_NAMESPACE_QPOASES print( AC_idx, nAC, "active " );
407 
408  #endif /* __SUPPRESSANYOUTPUT__ */
409 
410  return SUCCESSFUL_RETURN;
411 }
412 
413 
414 
415 /*****************************************************************************
416  * P R O T E C T E D *
417  *****************************************************************************/
418 
419 /*
420  * c l e a r
421  */
423 {
424  return SUCCESSFUL_RETURN;
425 }
426 
427 
428 /*
429  * c o p y
430  */
432  )
433 {
434  active = rhs.active;
435  inactive = rhs.inactive;
436 
437  return SUCCESSFUL_RETURN;
438 }
439 
440 
441 
442 /*
443  * s e t u p A l l
444  */
446 {
447  int_t i;
448 
449  /* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
450  for( i=0; i<n; ++i )
451  {
452  if ( getType( i ) == ST_UNBOUNDED )
453  {
454  if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
456  }
457  }
458 
459  /* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
460  for( i=0; i<n; ++i )
461  {
462  if ( getType( i ) == ST_BOUNDED )
463  {
464  if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
466  }
467  }
468 
469  /* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
470  for( i=0; i<n; ++i )
471  {
472  if ( getType( i ) == ST_EQUALITY )
473  {
474  if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
476  }
477  }
478 
479  /* 4) Moreover, add all constraints of unknown type. */
480  for( i=0; i<n; ++i )
481  {
482  if ( getType( i ) == ST_UNKNOWN || getType( i ) == ST_DISABLED )
483  {
484  if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
486  }
487  }
488 
489 
490  return SUCCESSFUL_RETURN;
491 }
492 
493 
495 
496 
497 /*
498  * end of file
499  */
returnValue setType(int i, SubjectToType value)
#define ST_LOWER
returnValue moveInactiveToActive(int _number, SubjectToStatus _status)
returnValue copy(const Constraints &rhs)
Allows to pass back messages to the calling function.
Base class for managing working sets of bounds and constraints.
Indexlist * getInactive()
returnValue myPrintf(const char *s)
#define ST_INACTIVE
returnValue addIndex(Indexlist *const indexlist, int newnumber, SubjectToStatus newstatus)
SubjectToStatus getStatus(int i) const
returnValue setupAll(SubjectToStatus _status)
returnValue setupConstraint(int _number, SubjectToStatus _status)
SubjectToStatus status[NCMAX]
Definition: Constraints.h:61
returnValue removeIndex(Indexlist *const indexlist, int removenumber)
void rhs(const real_t *x, real_t *f)
SubjectToType getType(int i) const
Indexlist * getActive()
returnValue setStatus(int i, SubjectToStatus value)
#define ST_UPPER


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