1 /*
2 * This file is part of qpOASES.
3 *
4 * qpOASES -- An Implementation of the Online Active Set Strategy.
5 * Copyright (C) 2007-2017 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
25 /**
26 * \file src/Constraints.cpp
27 * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
28 * \version 3.2
29 * \date 2007-2017
30 *
31 * Implementation of the Constraints class designed to manage working sets of
32 * constraints within a QProblem.
33 */
34
35
36 #include <qpOASES/Constraints.hpp>
37
38
39 BEGIN_NAMESPACE_QPOASES
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 */
Constraints()50 Constraints::Constraints( ) : SubjectTo( )
51 {
52 }
53
54
55 /*
56 * C o n s t r a i n t s
57 */
Constraints(int_t _n)58 Constraints::Constraints( int_t _n ) : SubjectTo( _n )
59 {
60 init( _n );
61 }
62
63
64 /*
65 * C o n s t r a i n t s
66 */
Constraints(const Constraints & rhs)67 Constraints::Constraints( const Constraints& rhs ) : SubjectTo( rhs )
68 {
69 copy( rhs );
70 }
71
72
73 /*
74 * ~ C o n s t r a i n t s
75 */
~Constraints()76 Constraints::~Constraints( )
77 {
78 clear( );
79 }
80
81
82 /*
83 * o p e r a t o r =
84 */
operator =(const Constraints & rhs)85 Constraints& Constraints::operator=( const Constraints& rhs )
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 */
init(int_t _n)101 returnValue Constraints::init( int_t _n
102 )
103 {
104 if ( _n < 0 )
105 return THROWERROR( RET_INVALID_ARGUMENTS );
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 */
setupConstraint(int_t number,SubjectToStatus _status)123 returnValue Constraints::setupConstraint( int_t number, SubjectToStatus _status
124 )
125 {
126 /* consistency check */
127 if ( ( number < 0 ) || ( number >= n ) )
128 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
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 )
135 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
136 break;
137
138 case ST_LOWER:
139 if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
140 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
141 break;
142
143 case ST_UPPER:
144 if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
145 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
146 break;
147
148 default:
149 return THROWERROR( RET_INVALID_ARGUMENTS );
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 */
setupAllInactive()159 returnValue Constraints::setupAllInactive( )
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 */
setupAllLower()168 returnValue Constraints::setupAllLower( )
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 */
setupAllUpper()177 returnValue Constraints::setupAllUpper( )
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 */
moveActiveToInactive(int_t number)186 returnValue Constraints::moveActiveToInactive( int_t number )
187 {
188 /* consistency check */
189 if ( ( number < 0 ) || ( number >= n ) )
190 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
191
192 /* Move index from indexlist of active constraints to that of inactive ones. */
193 if ( this->removeIndex( this->getActive( ),number ) != SUCCESSFUL_RETURN )
194 return THROWERROR( RET_MOVING_BOUND_FAILED );
195
196 if ( this->addIndex( this->getInactive( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
197 return THROWERROR( RET_MOVING_BOUND_FAILED );
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 */
moveInactiveToActive(int_t number,SubjectToStatus _status)206 returnValue Constraints::moveInactiveToActive( int_t number, SubjectToStatus _status
207 )
208 {
209 /* consistency check */
210 if ( ( number < 0 ) || ( number >= n ) )
211 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
212
213 /* Move index from indexlist of inactive constraints to that of active ones. */
214 if ( this->removeIndex( this->getInactive( ),number ) != SUCCESSFUL_RETURN )
215 return THROWERROR( RET_MOVING_BOUND_FAILED );
216
217 if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
218 return THROWERROR( RET_MOVING_BOUND_FAILED );
219
220 return SUCCESSFUL_RETURN;
221 }
222
223
224 /*
225 * f l i p F i x e d
226 */
flipFixed(int_t number)227 returnValue Constraints::flipFixed( int_t number )
228 {
229 /* consistency check */
230 if ( ( number < 0 ) || ( number >= n ) )
231 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
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 */
shift(int_t offset)248 returnValue Constraints::shift( int_t offset )
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 ) )
257 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
258
259 if ( ( n % offset ) != 0 )
260 return THROWERROR( RET_INVALID_ARGUMENTS );
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 )
280 return THROWERROR( RET_SHIFTING_FAILED );
281 break;
282
283 case ST_LOWER:
284 if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
285 return THROWERROR( RET_SHIFTING_FAILED );
286 break;
287
288 case ST_UPPER:
289 if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
290 return THROWERROR( RET_SHIFTING_FAILED );
291 break;
292
293 default:
294 return THROWERROR( RET_SHIFTING_FAILED );
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 */
rotate(int_t offset)309 returnValue Constraints::rotate( int_t offset )
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 ) )
318 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
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 )
355 return THROWERROR( RET_ROTATING_FAILED );
356 break;
357
358 case ST_LOWER:
359 if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
360 return THROWERROR( RET_ROTATING_FAILED );
361 break;
362
363 case ST_UPPER:
364 if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
365 return THROWERROR( RET_ROTATING_FAILED );
366 break;
367
368 default:
369 return THROWERROR( RET_ROTATING_FAILED );
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 */
print()384 returnValue Constraints::print( )
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 */
clear()422 returnValue Constraints::clear( )
423 {
424 return SUCCESSFUL_RETURN;
425 }
426
427
428 /*
429 * c o p y
430 */
copy(const Constraints & rhs)431 returnValue Constraints::copy( const Constraints& rhs
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 */
setupAll(SubjectToStatus _status)445 returnValue Constraints::setupAll( SubjectToStatus _status )
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 )
455 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
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 )
465 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
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 )
475 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
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 )
485 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
486 }
487 }
488
489
490 return SUCCESSFUL_RETURN;
491 }
492
493
494 END_NAMESPACE_QPOASES
495
496
497 /*
498 * end of file
499 */
500