Constraints.cpp
Go to the documentation of this file.
00001 /*
00002  *      This file is part of qpOASES.
00003  *
00004  *      qpOASES -- An Implementation of the Online Active Set Strategy.
00005  *      Copyright (C) 2007-2011 by Hans Joachim Ferreau, Andreas Potschka,
00006  *      Christian Kirches et al. All rights reserved.
00007  *
00008  *      qpOASES is free software; you can redistribute it and/or
00009  *      modify it under the terms of the GNU Lesser General Public
00010  *      License as published by the Free Software Foundation; either
00011  *      version 2.1 of the License, or (at your option) any later version.
00012  *
00013  *      qpOASES is distributed in the hope that it will be useful,
00014  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016  *      See the GNU Lesser General Public License for more details.
00017  *
00018  *      You should have received a copy of the GNU Lesser General Public
00019  *      License along with qpOASES; if not, write to the Free Software
00020  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  */
00023 
00024 
00036 #include <qpOASES/Constraints.hpp>
00037 
00038 
00039 BEGIN_NAMESPACE_QPOASES
00040 
00041 
00042 /*****************************************************************************
00043  *  P U B L I C                                                              *
00044  *****************************************************************************/
00045 
00046 
00047 /*
00048  *      C o n s t r a i n t s
00049  */
00050 Constraints::Constraints( ) : SubjectTo( )
00051 {
00052 }
00053 
00054 
00055 /*
00056  *      C o n s t r a i n t s
00057  */
00058 Constraints::Constraints( int _n ) : SubjectTo( _n )
00059 {
00060         init( _n );
00061 }
00062 
00063 
00064 /*
00065  *      C o n s t r a i n t s
00066  */
00067 Constraints::Constraints( const Constraints& rhs ) : SubjectTo( rhs )
00068 {
00069         copy( rhs );
00070 }
00071 
00072 
00073 /*
00074  *      ~ C o n s t r a i n t s
00075  */
00076 Constraints::~Constraints( )
00077 {
00078         clear( );
00079 }
00080 
00081 
00082 /*
00083  *      o p e r a t o r =
00084  */
00085 Constraints& Constraints::operator=( const Constraints& rhs )
00086 {
00087         if ( this != &rhs )
00088         {
00089                 clear( );
00090                 SubjectTo::operator=( rhs );
00091                 copy( rhs );
00092         }
00093 
00094         return *this;
00095 }
00096 
00097 
00098 /*
00099  *      i n i t
00100  */
00101 returnValue Constraints::init(  int _n
00102                                                                 )
00103 {
00104         if ( _n < 0 )
00105                 return THROWERROR( RET_INVALID_ARGUMENTS );
00106 
00107         clear( );
00108 
00109         if ( _n > 0 )
00110         {
00111                 active.init(   _n );
00112                 inactive.init( _n );
00113         }
00114 
00115         return SubjectTo::init( _n );
00116 }
00117 
00118 
00119 
00120 /*
00121  *      s e t u p C o n s t r a i n t
00122  */
00123 returnValue Constraints::setupConstraint(       int number, SubjectToStatus _status
00124                                                                                         )
00125 {
00126         /* consistency check */
00127         if ( ( number < 0 ) || ( number >= n ) )
00128                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00129 
00130         /* Add constraint index to respective index list. */
00131         switch ( _status )
00132         {
00133                 case ST_INACTIVE:
00134                         if ( this->addIndex( this->getInactive( ),number,_status ) != SUCCESSFUL_RETURN )
00135                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00136                         break;
00137 
00138                 case ST_LOWER:
00139                         if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
00140                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00141                         break;
00142 
00143                 case ST_UPPER:
00144                         if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
00145                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00146                         break;
00147 
00148                 default:
00149                         return THROWERROR( RET_INVALID_ARGUMENTS );
00150         }
00151 
00152         return SUCCESSFUL_RETURN;
00153 }
00154 
00155 
00156 /*
00157  *      s e t u p A l l I n a c t i v e
00158  */
00159 returnValue Constraints::setupAllInactive( )
00160 {
00161         return setupAll( ST_INACTIVE );
00162 }
00163 
00164 
00165 /*
00166  *      s e t u p A l l L o w e r
00167  */
00168 returnValue Constraints::setupAllLower( )
00169 {
00170         return setupAll( ST_LOWER );
00171 }
00172 
00173 
00174 /*
00175  *      s e t u p A l l U p p e r
00176  */
00177 returnValue Constraints::setupAllUpper( )
00178 {
00179         return setupAll( ST_UPPER );
00180 }
00181 
00182 
00183 /*
00184  *      m o v e A c t i v e T o I n a c t i v e
00185  */
00186 returnValue Constraints::moveActiveToInactive( int number )
00187 {
00188         /* consistency check */
00189         if ( ( number < 0 ) || ( number >= n ) )
00190                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00191 
00192         /* Move index from indexlist of active constraints to that of inactive ones. */
00193         if ( this->removeIndex( this->getActive( ),number ) != SUCCESSFUL_RETURN )
00194                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00195 
00196         if ( this->addIndex( this->getInactive( ),number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00197                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00198 
00199         return SUCCESSFUL_RETURN;
00200 }
00201 
00202 
00203 /*
00204  *      m o v e I n a c t i v e T o A c t i v e
00205  */
00206 returnValue Constraints::moveInactiveToActive(  int number, SubjectToStatus _status
00207                                                                                                 )
00208 {
00209         /* consistency check */
00210         if ( ( number < 0 ) || ( number >= n ) )
00211                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00212 
00213         /* Move index from indexlist of inactive constraints to that of active ones. */
00214         if ( this->removeIndex( this->getInactive( ),number ) != SUCCESSFUL_RETURN )
00215                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00216 
00217         if ( this->addIndex( this->getActive( ),number,_status ) != SUCCESSFUL_RETURN )
00218                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00219 
00220         return SUCCESSFUL_RETURN;
00221 }
00222 
00223 
00224 /*
00225  *      f l i p F i x e d
00226  */
00227 returnValue Constraints::flipFixed( int number )
00228 {
00229         /* consistency check */
00230         if ( ( number < 0 ) || ( number >= n ) )
00231                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00232 
00233         if ( status != 0 )
00234                 switch (status[number])
00235                 {
00236                         case ST_LOWER: status[number] = ST_UPPER; break;
00237                         case ST_UPPER: status[number] = ST_LOWER; break;
00238                         default: return THROWERROR( RET_MOVING_CONSTRAINT_FAILED );
00239                 }
00240 
00241         return SUCCESSFUL_RETURN;
00242 }
00243 
00244 
00245 /*
00246  *      s h i f t
00247  */
00248 returnValue Constraints::shift( int offset )
00249 {
00250         int i;
00251 
00252         /* consistency check */
00253         if ( ( offset == 0 ) || ( n <= 1 ) )
00254                 return SUCCESSFUL_RETURN;
00255 
00256         if ( ( offset < 0 ) || ( offset > n/2 ) )
00257                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00258 
00259         if ( ( n % offset ) != 0 )
00260                 return THROWERROR( RET_INVALID_ARGUMENTS );
00261 
00262 
00263         /* 1) Shift types and status. */
00264         for( i=0; i<n-offset; ++i )
00265         {
00266                 setType( i,getType( i+offset ) );
00267                 setStatus( i,getStatus( i+offset ) );
00268         }
00269 
00270         /* 2) Construct shifted index lists of free and fixed variables. */
00271         Indexlist shiftedActive( n );
00272         Indexlist shiftedInactive( n );
00273 
00274         for( i=0; i<n; ++i )
00275         {
00276                 switch ( getStatus( i ) )
00277                 {
00278                         case ST_INACTIVE:
00279                                 if ( shiftedInactive.addNumber( i ) != SUCCESSFUL_RETURN )
00280                                         return THROWERROR( RET_SHIFTING_FAILED );
00281                                 break;
00282 
00283                         case ST_LOWER:
00284                                 if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
00285                                         return THROWERROR( RET_SHIFTING_FAILED );
00286                                 break;
00287 
00288                         case ST_UPPER:
00289                                 if ( shiftedActive.addNumber( i ) != SUCCESSFUL_RETURN )
00290                                         return THROWERROR( RET_SHIFTING_FAILED );
00291                                 break;
00292 
00293                         default:
00294                                 return THROWERROR( RET_SHIFTING_FAILED );
00295                 }
00296         }
00297 
00298         /* 3) Assign shifted index list. */
00299         active = shiftedActive;
00300         inactive = shiftedInactive;
00301 
00302         return SUCCESSFUL_RETURN;
00303 }
00304 
00305 
00306 /*
00307  *      r o t a t e
00308  */
00309 returnValue Constraints::rotate( int offset )
00310 {
00311         int i;
00312 
00313         /* consistency check */
00314         if ( ( offset == 0 ) || ( offset == n ) || ( n <= 1 ) )
00315                 return SUCCESSFUL_RETURN;
00316 
00317         if ( ( offset < 0 ) || ( offset > n ) )
00318                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00319 
00320 
00321         /* 1) Rotate types and status. */
00322         SubjectToType*   typeTmp   = new SubjectToType[offset];
00323         SubjectToStatus* statusTmp = new SubjectToStatus[offset];
00324 
00325         for( i=0; i<offset; ++i )
00326         {
00327                 typeTmp[i] = getType( i );
00328                 statusTmp[i] = getStatus( i );
00329         }
00330 
00331         for( i=0; i<n-offset; ++i )
00332         {
00333                 setType( i,getType( i+offset ) );
00334                 setStatus( i,getStatus( i+offset ) );
00335         }
00336 
00337         for( i=n-offset; i<n; ++i )
00338         {
00339                 setType( i,typeTmp[i-n+offset] );
00340                 setStatus( i,statusTmp[i-n+offset] );
00341         }
00342 
00343         delete[] statusTmp; delete[] typeTmp;
00344 
00345         /* 2) Construct shifted index lists of free and fixed variables. */
00346         Indexlist rotatedActive( n );
00347         Indexlist rotatedInactive( n );
00348 
00349         for( i=0; i<n; ++i )
00350         {
00351                 switch ( getStatus( i ) )
00352                 {
00353                         case ST_INACTIVE:
00354                                 if ( rotatedInactive.addNumber( i ) != SUCCESSFUL_RETURN )
00355                                         return THROWERROR( RET_ROTATING_FAILED );
00356                                 break;
00357 
00358                         case ST_LOWER:
00359                                 if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
00360                                         return THROWERROR( RET_ROTATING_FAILED );
00361                                 break;
00362 
00363                         case ST_UPPER:
00364                                 if ( rotatedActive.addNumber( i ) != SUCCESSFUL_RETURN )
00365                                         return THROWERROR( RET_ROTATING_FAILED );
00366                                 break;
00367 
00368                         default:
00369                                 return THROWERROR( RET_ROTATING_FAILED );
00370                 }
00371         }
00372 
00373         /* 3) Assign shifted index list. */
00374         active = rotatedActive;
00375         inactive = rotatedInactive;
00376 
00377         return SUCCESSFUL_RETURN;
00378 }
00379 
00380 
00381 /*
00382  *      p r i n t
00383  */
00384 returnValue Constraints::print( )
00385 {
00386         if ( n == 0 )
00387                 return SUCCESSFUL_RETURN;
00388 
00389         #ifndef __XPCTARGET__
00390         #ifndef __DSPACE__
00391         char myPrintfString[160];
00392 
00393         int nIAC = getNIAC( );
00394         int nAC  = getNAC( );
00395 
00396         int* IAC_idx;
00397         getInactive( )->getNumberArray( &IAC_idx );
00398 
00399         int* AC_idx;
00400         getActive( )->getNumberArray( &AC_idx );
00401 
00402         snprintf( myPrintfString,160,"Constraints object comprising %d constraints (%d inactive, %d active):\n",n,nIAC,nAC );
00403         myPrintf( myPrintfString );
00404 
00405         REFER_NAMESPACE_QPOASES print( IAC_idx,nIAC,"inactive" );
00406         REFER_NAMESPACE_QPOASES print( AC_idx, nAC, "active  " );
00407 
00408         #endif
00409         #endif
00410 
00411         return SUCCESSFUL_RETURN;
00412 }
00413 
00414 
00415 
00416 /*****************************************************************************
00417  *  P R O T E C T E D                                                        *
00418  *****************************************************************************/
00419 
00420 /*
00421  *      c l e a r
00422  */
00423 returnValue Constraints::clear( )
00424 {
00425         return SUCCESSFUL_RETURN;
00426 }
00427 
00428 
00429 /*
00430  *      c o p y
00431  */
00432 returnValue Constraints::copy(  const Constraints& rhs
00433                                                                 )
00434 {
00435         active   = rhs.active;
00436         inactive = rhs.inactive;
00437 
00438         return SUCCESSFUL_RETURN;
00439 }
00440 
00441 
00442 
00443 /*
00444  *      s e t u p A l l
00445  */
00446 returnValue Constraints::setupAll( SubjectToStatus _status )
00447 {
00448         int i;
00449 
00450         /* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
00451         for( i=0; i<n; ++i )
00452         {
00453                 if ( getType( i ) == ST_UNBOUNDED )
00454                 {
00455                         if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
00456                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00457                 }
00458         }
00459 
00460         /* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
00461         for( i=0; i<n; ++i )
00462         {
00463                 if ( getType( i ) == ST_BOUNDED )
00464                 {
00465                         if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
00466                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00467                 }
00468         }
00469 
00470         /* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
00471         for( i=0; i<n; ++i )
00472         {
00473                 if ( getType( i ) == ST_EQUALITY )
00474                 {
00475                         if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
00476                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00477                 }
00478         }
00479 
00480         /* 4) Moreover, add all constraints of unknown type. */
00481         for( i=0; i<n; ++i )
00482         {
00483                 if ( getType( i ) == ST_UNKNOWN )
00484                 {
00485                         if ( setupConstraint( i,_status ) != SUCCESSFUL_RETURN )
00486                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00487                 }
00488         }
00489 
00490 
00491         return SUCCESSFUL_RETURN;
00492 }
00493 
00494 
00495 END_NAMESPACE_QPOASES
00496 
00497 
00498 /*
00499  *      end of file
00500  */


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:56