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-2008 by Hans Joachim Ferreau et al. All rights reserved.
00006  *
00007  *      qpOASES is free software; you can redistribute it and/or
00008  *      modify it under the terms of the GNU Lesser General Public
00009  *      License as published by the Free Software Foundation; either
00010  *      version 2.1 of the License, or (at your option) any later version.
00011  *
00012  *      qpOASES is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015  *      Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with qpOASES; if not, write to the Free Software
00019  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00020  *
00021  */
00022 
00023 
00035 #include <Constraints.hpp>
00036 
00037 
00038 /*****************************************************************************
00039  *  P U B L I C                                                              *
00040  *****************************************************************************/
00041 
00042 
00043 /*
00044  *      C o n s t r a i n t s
00045  */
00046 Constraints::Constraints( ) :   SubjectTo( ),
00047                                                                 nC( 0 ),
00048                                                                 nEC( 0 ),
00049                                                                 nIC( 0 ),
00050                                                                 nUC( 0 )
00051 {
00052 }
00053 
00054 
00055 /*
00056  *      C o n s t r a i n t s
00057  */
00058 Constraints::Constraints( const Constraints& rhs ) :    SubjectTo( rhs ),
00059                                                                                                                 nC( rhs.nC ),
00060                                                                                                                 nEC( rhs.nEC ),
00061                                                                                                                 nIC( rhs.nIC ),
00062                                                                                                                 nUC( rhs.nUC )
00063 {
00064         active =   rhs.active;
00065         inactive = rhs.inactive;
00066 }
00067 
00068 
00069 /*
00070  *      ~ C o n s t r a i n t s
00071  */
00072 Constraints::~Constraints( )
00073 {
00074 }
00075 
00076 
00077 /*
00078  *      o p e r a t o r =
00079  */
00080 Constraints& Constraints::operator=( const Constraints& rhs )
00081 {
00082         if ( this != &rhs )
00083         {
00084                 SubjectTo::operator=( rhs );
00085 
00086                 nC  = rhs.nC;
00087                 nEC = rhs.nEC;
00088                 nIC = rhs.nIC;
00089                 nUC = rhs.nUC;
00090 
00091                 active =   rhs.active;
00092                 inactive = rhs.inactive;
00093         }
00094 
00095         return *this;
00096 }
00097 
00098 
00099 /*
00100  *      i n i t
00101  */
00102 returnValue Constraints::init( int n )
00103 {
00104         nC = n;
00105         nEC = 0;
00106         nIC = 0;
00107         nUC = 0;
00108 
00109         active.init( );
00110         inactive.init( );
00111 
00112         return SubjectTo::init( n );
00113 }
00114 
00115 
00116 /*
00117  *      s e t u p C o n s t r a i n t
00118  */
00119 returnValue Constraints::setupConstraint(       int _number, SubjectToStatus _status
00120                                                                                         )
00121 {
00122         /* consistency check */
00123         if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
00124                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00125         
00126         /* Add constraint index to respective index list. */
00127         switch ( _status )
00128         {
00129                 case ST_INACTIVE:
00130                         if ( this->addIndex( this->getInactive( ),_number,_status ) != SUCCESSFUL_RETURN )
00131                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00132                         break;
00133 
00134                 case ST_LOWER:
00135                         if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
00136                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00137                         break;
00138 
00139                 case ST_UPPER:
00140                         if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
00141                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00142                         break;
00143 
00144                 default:
00145                         return THROWERROR( RET_INVALID_ARGUMENTS );
00146         }
00147 
00148         return SUCCESSFUL_RETURN;
00149 }
00150 
00151 
00152 /*
00153  *      s e t u p A l l I n a c t i v e
00154  */
00155 returnValue Constraints::setupAllInactive( )
00156 {
00157         int i;
00158 
00159 
00160         /* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
00161         for( i=0; i<nC; ++i )
00162         {
00163                 if ( getType( i ) == ST_UNBOUNDED )
00164                 {
00165                         if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00166                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00167                 }
00168         }
00169 
00170         /* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
00171         for( i=0; i<nC; ++i )
00172         {
00173                 if ( getType( i ) == ST_BOUNDED )
00174                 {
00175                         if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00176                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00177                 }
00178         }
00179 
00180         /* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
00181         for( i=0; i<nC; ++i )
00182         {
00183                 if ( getType( i ) == ST_EQUALITY )
00184                 {
00185                         if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00186                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00187                 }
00188         }
00189 
00190         /* 4) Moreover, add all constraints of unknown type. */
00191         for( i=0; i<nC; ++i )
00192         {
00193                 if ( getType( i ) == ST_UNKNOWN )
00194                 {
00195                         if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00196                                 return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
00197                 }
00198         }
00199 
00200         return SUCCESSFUL_RETURN;
00201 }
00202 
00203 
00204 /*
00205  *      m o v e A c t i v e T o I n a c t i v e
00206  */
00207 returnValue Constraints::moveActiveToInactive( int _number )
00208 {
00209         /* consistency check */
00210         if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
00211                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00212 
00213         /* Move index from indexlist of active constraints to that of inactive ones. */
00214         if ( this->removeIndex( this->getActive( ),_number ) != SUCCESSFUL_RETURN )
00215                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00216 
00217         if ( this->addIndex( this->getInactive( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
00218                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00219 
00220         return SUCCESSFUL_RETURN;
00221 }
00222 
00223 
00224 /*
00225  *      m o v e I n a c t i v e T o A c t i v e
00226  */
00227 returnValue Constraints::moveInactiveToActive(  int _number, SubjectToStatus _status
00228                                                                                                 )
00229 {
00230         /* consistency check */
00231         if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
00232                 return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
00233 
00234         /* Move index from indexlist of inactive constraints to that of active ones. */
00235         if ( this->removeIndex( this->getInactive( ),_number ) != SUCCESSFUL_RETURN )
00236                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00237 
00238         if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
00239                 return THROWERROR( RET_MOVING_BOUND_FAILED );
00240 
00241         return SUCCESSFUL_RETURN;
00242 }
00243 
00244 
00245 
00246 /*
00247  *      end of file
00248  */


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