QProblemB.hpp
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 
00037 #ifndef QPOASES_QPROBLEMB_HPP
00038 #define QPOASES_QPROBLEMB_HPP
00039 
00040 
00041 #include <Bounds.hpp>
00042 
00043 
00044 
00045 class SolutionAnalysis;
00046 
00055 class QProblemB
00056 {
00057         /* allow SolutionAnalysis class to access private members */
00058         friend class SolutionAnalysis;
00059         
00060         /*
00061          *      PUBLIC MEMBER FUNCTIONS
00062          */
00063         public:
00065                 QProblemB( );
00066 
00068                 QProblemB(      int _nV                                         
00069                                         );
00070 
00072                 QProblemB(      const QProblemB& rhs    
00073                                         );
00074 
00076                 ~QProblemB( );
00077 
00079                 QProblemB& operator=(   const QProblemB& rhs    
00080                                                                 );
00081 
00082 
00086                 returnValue reset( );
00087 
00088 
00101                 returnValue init(       const real_t* const _H,                 
00102                                                         const real_t* const _g,                 
00103                                                         const real_t* const _lb,                
00105                                                         const real_t* const _ub,                
00107                                                         int& nWSR,                                              
00109                                                         const real_t* const yOpt = 0,   
00110                                                         real_t* const cputime = 0               
00111                                                         );
00112 
00113 
00127                 returnValue hotstart(   const real_t* const g_new,      
00128                                                                 const real_t* const lb_new,     
00130                                                                 const real_t* const ub_new,     
00132                                                                 int& nWSR,                                      
00134                                                                 real_t* const cputime           
00135                                                                 );
00136 
00137 
00140                 inline returnValue getH(        real_t* const _H        
00141                                                                         ) const;
00142 
00145                 inline returnValue getG(        real_t* const _g        
00146                                                                         ) const;
00147 
00150                 inline returnValue getLB(       real_t* const _lb       
00151                                                                         ) const;
00152 
00156                 inline returnValue getLB(       int number,             
00157                                                                         real_t& value   
00158                                                                         ) const;
00159 
00162                 inline returnValue getUB(       real_t* const _ub       
00163                                                                         ) const;
00164 
00168                 inline returnValue getUB(       int number,             
00169                                                                         real_t& value   
00170                                                                         ) const;
00171 
00172 
00175                 inline returnValue getBounds(   Bounds* const _bounds   
00176                                                                                 ) const;
00177 
00178 
00181                 inline int getNV( ) const;
00182 
00185                 inline int getNFR( );
00186 
00189                 inline int getNFX( );
00190 
00193                 inline int getNFV( ) const;
00194 
00197                 int getNZ( );
00198 
00199 
00203                 real_t getObjVal( ) const;
00204 
00207                 real_t getObjVal(       const real_t* const _x  
00208                                                         ) const;
00209 
00213                 returnValue getPrimalSolution(  real_t* const xOpt                      
00214                                                                                 ) const;
00215 
00219                 returnValue getDualSolution(    real_t* const yOpt      
00220                                                                                 ) const;
00221 
00222 
00225                 inline QProblemStatus getStatus( ) const;
00226 
00227 
00231                 inline BooleanType isInitialised( ) const;
00232 
00236                 inline BooleanType isSolved( ) const;
00237 
00241                 inline BooleanType isInfeasible( ) const;
00242 
00246                 inline BooleanType isUnbounded( ) const;
00247 
00248 
00251                 inline PrintLevel getPrintLevel( ) const;
00252 
00255                 returnValue setPrintLevel(      PrintLevel _printlevel  
00256                                                                         );
00257 
00258 
00261                 inline HessianType getHessianType( ) const;
00262 
00265                 inline returnValue setHessianType(      HessianType _hessianType 
00266                                                                                         );
00267 
00268 
00269         /*
00270          *      PROTECTED MEMBER FUNCTIONS
00271          */
00272         protected:
00276                 returnValue checkForIdentityHessian( );
00277 
00281                 returnValue setupSubjectToType( );
00282 
00287                 returnValue setupCholeskyDecomposition( );
00288 
00289 
00300                 returnValue solveInitialQP(     const real_t* const xOpt,                       
00302                                                                         const real_t* const yOpt,                       
00304                                                                         const Bounds* const guessedBounds,      
00306                                                                         int& nWSR,                                                      
00308                                                                         real_t* const cputime                           
00309                                                                         );
00310 
00311 
00317                 returnValue obtainAuxiliaryWorkingSet(  const real_t* const xOpt,                       
00319                                                                                                 const real_t* const yOpt,                       
00321                                                                                                 const Bounds* const guessedBounds,      
00322                                                                                                 Bounds* auxiliaryBounds                         
00324                                                                                                 ) const;
00325 
00333                 returnValue setupAuxiliaryWorkingSet(   const Bounds* const auxiliaryBounds,    
00334                                                                                                 BooleanType setupAfresh                                 
00336                                                                                                 );
00337 
00340                 returnValue setupAuxiliaryQPsolution(   const real_t* const xOpt,                       
00342                                                                                                 const real_t* const yOpt                        
00344                                                                                                 );
00345 
00350                 returnValue setupAuxiliaryQPgradient( );
00351 
00357                 returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation   
00358                                                                                         );
00359 
00360 
00364                 returnValue addBound(   int number,                                     
00365                                                                 SubjectToStatus B_status,       
00366                                                                 BooleanType updateCholesky      
00367                                                                 );
00368 
00373                 returnValue removeBound(        int number,                                     
00374                                                                         BooleanType updateCholesky      
00375                                                                         );
00376 
00377 
00381                 returnValue backsolveR( const real_t* const b,  
00382                                                                 BooleanType transposed, 
00383                                                                 real_t* const a                 
00384                                                                 );
00385 
00390                 returnValue backsolveR( const real_t* const b,          
00391                                                                 BooleanType transposed,         
00392                                                                 BooleanType removingBound,      
00393                                                                 real_t* const a                         
00394                                                                 );
00395 
00396 
00399                 returnValue hotstart_determineDataShift(const int* const FX_idx,        
00400                                                                                                 const real_t* const g_new,      
00401                                                                                                 const real_t* const lb_new,     
00402                                                                                                 const real_t* const ub_new,     
00403                                                                                                 real_t* const delta_g,          
00404                                                                                                 real_t* const delta_lb,         
00405                                                                                                 real_t* const delta_ub,         
00406                                                                                                 BooleanType& Delta_bB_isZero
00407                                                                                                 );
00408 
00409 
00414                 BooleanType areBoundsConsistent(        const real_t* const delta_lb,           
00415                                                                                         const real_t* const delta_ub            
00416                                                                                         ) const;
00417 
00418 
00422                 returnValue setupQPdata(        const real_t* const _H,         
00423                                                                         const real_t* const _g,         
00424                                                                         const real_t* const _lb,        
00426                                                                         const real_t* const _ub         
00428                                                                         );
00429 
00430 
00433                 inline returnValue setH(        const real_t* const H_new       
00434                                                                         );
00435 
00438                 inline returnValue setG(        const real_t* const g_new       
00439                                                                         );
00440 
00443                 inline returnValue setLB(       const real_t* const lb_new      
00444                                                                         );
00445 
00449                 inline returnValue setLB(       int number,             
00450                                                                         real_t value    
00451                                                                         );
00452 
00455                 inline returnValue setUB(       const real_t* const ub_new      
00456                                                                         );
00457 
00461                 inline returnValue setUB(       int number,             
00462                                                                         real_t value    
00463                                                                         );
00464 
00465 
00468                 inline void computeGivens(      real_t xold,    
00469                                                                         real_t yold,    
00470                                                                         real_t& xnew,   
00471                                                                         real_t& ynew,   
00472                                                                         real_t& c,              
00473                                                                         real_t& s               
00474                                                                         ) const;
00475 
00478                 inline void applyGivens(        real_t c,               
00479                                                                         real_t s,               
00480                                                                         real_t xold,    
00482                                                                         real_t yold,    
00484                                                                         real_t& xnew,   
00486                                                                         real_t& ynew    
00488                                                                         ) const;
00489 
00490 
00491         /*
00492          *      PRIVATE MEMBER FUNCTIONS
00493          */
00494         private:
00498                 returnValue hotstart_determineStepDirection(const int* const FR_idx,            
00499                                                                                                         const int* const FX_idx,                
00500                                                                                                         const real_t* const delta_g,    
00501                                                                                                         const real_t* const delta_lb,   
00502                                                                                                         const real_t* const delta_ub,   
00503                                                                                                         BooleanType Delta_bB_isZero,    
00504                                                                                                         real_t* const delta_xFX,                
00505                                                                                                         real_t* const delta_xFR,                
00506                                                                                                         real_t* const delta_yFX                 
00507                                                                                                         );
00508 
00511                 returnValue hotstart_determineStepLength(       const int* const FR_idx,                
00512                                                                                                         const int* const FX_idx,                
00513                                                                                                         const real_t* const delta_lb,   
00514                                                                                                         const real_t* const delta_ub,   
00515                                                                                                         const real_t* const delta_xFR,  
00516                                                                                                         const real_t* const delta_yFX,  
00517                                                                                                         int& BC_idx,                                    
00518                                                                                                         SubjectToStatus& BC_status              
00519                                                                                                         );
00520 
00527                 returnValue hotstart_performStep(       const int* const FR_idx,                        
00528                                                                                         const int* const FX_idx,                        
00529                                                                                         const real_t* const delta_g,            
00530                                                                                         const real_t* const delta_lb,           
00531                                                                                         const real_t* const delta_ub,           
00532                                                                                         const real_t* const delta_xFX,          
00533                                                                                         const real_t* const delta_xFR,          
00534                                                                                         const real_t* const delta_yFX,          
00535                                                                                         int BC_idx,                                             
00536                                                                                         SubjectToStatus BC_status                       
00537                                                                                         );
00538 
00539 
00540                 #ifdef PC_DEBUG  /* Define print functions only for debugging! */
00541 
00544                 returnValue printIteration(     int iteration,                          
00545                                                                         int BC_idx,                             
00546                                                                         SubjectToStatus BC_status       
00547                                                                         );
00548 
00549                 #endif  /* PC_DEBUG */
00550 
00551 
00557                 returnValue checkKKTconditions( );
00558 
00559 
00560         /*
00561          *      PROTECTED MEMBER VARIABLES
00562          */
00563         protected:
00564                 real_t H[NVMAX*NVMAX];          
00565                 real_t g[NVMAX];                        
00566                 real_t lb[NVMAX];                       
00567                 real_t ub[NVMAX];                       
00569                 Bounds bounds;                          
00571                 real_t R[NVMAX*NVMAX];          
00573                 real_t x[NVMAX];                        
00574                 real_t y[NVMAX+NCMAX];          
00576                 real_t tau;                                     
00578                 QProblemStatus status;          
00580                 BooleanType infeasible;         
00581                 BooleanType unbounded;          
00583                 HessianType hessianType;        
00585                 PrintLevel printlevel;          
00587                 int count;                                      
00588 };
00589 
00590 
00591 #include <QProblemB.ipp>
00592 
00593 #endif  /* QPOASES_QPROBLEMB_HPP */
00594 
00595 
00596 /*
00597  *      end of file
00598  */


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:59:51