SQProblemSchur.hpp

Go to the documentation of this file.

/*
 *      This file is part of qpOASES.
 *
 *      qpOASES -- An Implementation of the Online Active Set Strategy.
 *      Copyright (C) 2007-2014 by Hans Joachim Ferreau, Andreas Potschka,
 *      Christian Kirches et al. All rights reserved.
 *
 *      qpOASES is free software; you can redistribute it and/or
 *      modify it under the terms of the GNU Lesser General Public
 *      License as published by the Free Software Foundation; either
 *      version 2.1 of the License, or (at your option) any later version.
 *
 *      qpOASES is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *      See the GNU Lesser General Public License for more details.
 *
 *      You should have received a copy of the GNU Lesser General Public
 *      License along with qpOASES; if not, write to the Free Software
 *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */


#ifndef QPOASES_SQPROBLEMSCHUR_HPP
#define QPOASES_SQPROBLEMSCHUR_HPP


#include <qpOASES/SQProblem.hpp>
#include <qpOASES/SparseSolver.hpp>

#ifdef __USE_SINGLE_PRECISION__

        //#define GEQRF sgeqrf_
        //#define ORMQR sormqr_
        #define TRTRS strtrs_

        #define TRCON strcon_

#else

        //#define GEQRF dgeqrf_
        //#define ORMQR dormqr_
        #define TRTRS dtrtrs_

        #define TRCON dtrcon_

#endif /* __USE_SINGLE_PRECISION__ */

extern "C" {
        //void dgeqrf_( const unsigned long *M, const unsigned long *N, double *A, const unsigned long *LDA,
                                        //double *TAU, double *WORK, const unsigned long *LWORK, int *INFO );
        //void sgeqrf_( const unsigned long *M, const unsigned long *N, float *A, const unsigned long *LDA,
                                        //float *TAU, float *WORK, const unsigned long *LWORK, int *INFO );

        //void dormqr_( const char *SIDE, const char *TRANS, const unsigned long *M, const unsigned long *N, const unsigned long *K,
                                        //double *A, const unsigned long *LDA, double *TAU, double *C, const unsigned long *LDC,
                                        //double *WORK, const unsigned long *LWORK, int *INFO );
        //void sormqr_( const char *SIDE, const char *TRANS, const unsigned long *M, const unsigned long *N, const unsigned long *K,
                                        //float *A, const unsigned long *LDA, float *TAU, float *C, const unsigned long *LDC,
                                        //float *WORK, const unsigned long *LWORK, int *INFO );

        void dtrtrs_(   const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS,
                                        double *A, const unsigned long *LDA, double *B, const unsigned long *LDB, long *INFO );
        void strtrs_(   const char *UPLO, const char *TRANS, const char *DIAG, const unsigned long *N, const unsigned long *NRHS,
                                        float *A, const unsigned long *LDA, float *B, const unsigned long *LDB, long *INFO );

        void dtrcon_(   const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, double *A, const unsigned long *LDA,
                                        double *RCOND, double *WORK, const unsigned long *IWORK, long *INFO );
        void strcon_(   const char *NORM, const char *UPLO, const char *DIAG, const unsigned long *N, float *A, const unsigned long *LDA,
                                        float *RCOND, float *WORK, const unsigned long *IWORK, long *INFO );
}

BEGIN_NAMESPACE_QPOASES


class SQProblemSchur : public SQProblem
{
        /* allow SolutionAnalysis class to access private members */
        friend class SolutionAnalysis;

        /*
         *      PUBLIC MEMBER FUNCTIONS
         */
        public:
                SQProblemSchur( );

                SQProblemSchur( int_t _nV,                                                                      
                                                int_t _nC,                                                                      
                                                HessianType _hessianType = HST_UNKNOWN, 
                                                int_t maxSchurUpdates = 75                                      
                                                );

                SQProblemSchur( const SQProblemSchur& rhs               
                                                );

                virtual ~SQProblemSchur( );

                virtual SQProblemSchur& operator=(      const SQProblemSchur& rhs       
                                                                                        );

                virtual returnValue reset( );

                returnValue resetSchurComplement( BooleanType allowInertiaCorrection );

                inline int_t getNumFactorizations( ) const;

        /*
         *      PROTECTED MEMBER FUNCTIONS
         */
        protected:
                returnValue clear( );

                returnValue copy(       const SQProblemSchur& rhs       
                                                        );

                virtual returnValue computeProjectedCholesky( );

                virtual returnValue computeInitialCholesky( );

                virtual returnValue setupTQfactorisation( );

                virtual returnValue setupAuxiliaryQP(   SymmetricMatrix *H_new,         
                                                                                                Matrix *A_new,                          
                                                                                                const real_t *lb_new,
                                                                                                const real_t *ub_new,
                                                                                                const real_t *lbA_new,
                                                                                                const real_t *ubA_new
                                                                                                );

                virtual returnValue setupAuxiliaryWorkingSet(   const Bounds* const auxiliaryBounds,
                                                                                                                const Constraints* const auxiliaryConstraints,
                                                                                                                BooleanType setupAfresh
                                                                                                                );


                virtual returnValue addConstraint(      int_t number,                                   
                                                                                        SubjectToStatus C_status,               
                                                                                        BooleanType updateCholesky,             
                                                                                        BooleanType ensureLI = BT_TRUE  
                                                                                        );

                virtual returnValue addConstraint_checkLI(      int_t number    
                                                                                        );

                virtual returnValue addConstraint_ensureLI(     int_t number,                           
                                                                                                                SubjectToStatus C_status        
                                                                                                                );

                virtual returnValue addBound(   int_t number,                                   
                                                                                SubjectToStatus B_status,               
                                                                                BooleanType updateCholesky,             
                                                                                BooleanType ensureLI = BT_TRUE  
                                                                                );

                virtual returnValue addBound_checkLI(   int_t number    
                                                                                                );

                virtual returnValue addBound_ensureLI(  int_t number,                           
                                                                                                SubjectToStatus B_status        
                                                                                                );

                virtual returnValue removeConstraint(   int_t number,                                                   
                                                                                                BooleanType updateCholesky,                             
                                                                                                BooleanType allowFlipping = BT_FALSE,   
                                                                                                BooleanType ensureNZC = BT_FALSE                
                                                                                                );

                virtual returnValue removeBound(        int_t number,                                                   
                                                                                        BooleanType updateCholesky,                             
                                                                                        BooleanType allowFlipping = BT_FALSE,   
                                                                                        BooleanType ensureNZC = BT_FALSE                
                                                                                        );

                virtual returnValue backsolveT(         const real_t* const b,  
                                                                                        BooleanType transposed, 
                                                                                        real_t* const a                 
                                                                                        ) const;

                virtual returnValue backsolveR(         const real_t* const b,  
                                                                                        BooleanType transposed, 
                                                                                        real_t* const a                 
                                                                                        ) const;

                virtual returnValue backsolveR(         const real_t* const b,          
                                                                                        BooleanType transposed,         
                                                                                        BooleanType removingBound,      
                                                                                        real_t* const a                         
                                                                                        ) const;


                virtual returnValue determineStepDirection(     const real_t* const delta_g,    
                                                                                                                const real_t* const delta_lbA,  
                                                                                                                const real_t* const delta_ubA,  
                                                                                                                const real_t* const delta_lb,   
                                                                                                                const real_t* const delta_ub,   
                                                                                                                BooleanType Delta_bC_isZero,    
                                                                                                                BooleanType Delta_bB_isZero,    
                                                                                                                real_t* const delta_xFX,                
                                                                                                                real_t* const delta_xFR,                
                                                                                                                real_t* const delta_yAC,                
                                                                                                                real_t* const delta_yFX                 
                                                                                                                );

                virtual returnValue determineStepDirection2(    const real_t* const delta_g,    
                                                                                                                const real_t* const delta_lbA,  
                                                                                                                const real_t* const delta_ubA,  
                                                                                                                const real_t* const delta_lb,   
                                                                                                                const real_t* const delta_ub,   
                                                                                                                BooleanType Delta_bC_isZero,    
                                                                                                                BooleanType Delta_bB_isZero,    
                                                                                                                real_t* const delta_xFX,                
                                                                                                                real_t* const delta_xFR,                
                                                                                                                real_t* const delta_yAC,                
                                                                                                                real_t* const delta_yFX                 
                                                                                                                );

        /*
         *      PRIVATE MEMBER FUNCTION
         */
        private:
                returnValue addBound_checkLISchur(      int_t number,                   
                                                                                        real_t* const xiC,              
                                                                                        real_t* const xiX               
                                                                                        );

                returnValue addConstraint_checkLISchur( int_t number,           
                                                                                                        real_t* const xiC,      
                                                                                                        real_t* const xiX       
                                                                                                        );

                returnValue computeMTimes( real_t alpha, const real_t* const x, real_t beta, real_t* const y );

                returnValue computeMTransTimes( real_t alpha, const real_t* const x, real_t beta, real_t* const y );

                returnValue addToSchurComplement( int_t number, SchurUpdateType update, int_t numNonzerosM, const sparse_int_t* M_pos, const real_t* const M_vals, int_t numNonzerosN, const sparse_int_t* Npos, const real_t* const Nvals, real_t N_diag );

                returnValue deleteFromSchurComplement( int_t idx, BooleanType allowUndo = BT_FALSE );

                returnValue undoDeleteFromSchurComplement( int_t idx );

                real_t calcDetSchur( int_t idxDel );

                returnValue updateSchurQR( int_t idxDel );

                returnValue backsolveSchurQR( int_t dimS, const real_t* const rhs, int_t dimRhs, real_t* const sol );

                returnValue correctInertia();

                returnValue repairSingularWorkingSet( );

                returnValue stepCalcRhs( int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx, real_t& rhs_max, const real_t* const delta_g,
                                                                const real_t* const delta_lbA, const real_t* const delta_ubA,
                                                                const real_t* const delta_lb, const real_t* const delta_ub,
                                                                BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
                                                                real_t* const delta_xFX, real_t* const delta_xFR,
                                                                real_t* const delta_yAC, real_t* const delta_yFX
                                                                 );

                returnValue stepCalcReorder(int_t nFR, int_t nAC, int_t* FR_idx, int_t* AC_idx, int_t nFRStart, int_t nACStart,
                                                                        int_t* FR_idxStart, int_t* AC_idxStart, int_t* FR_iSort, int_t* FR_iSortStart,
                                                                        int_t* AC_iSort, int_t* AC_iSortStart, real_t* rhs);

                returnValue stepCalcBacksolveSchur( int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx,
                                                                                        int_t dim, real_t* rhs, real_t* sol );

                returnValue stepCalcReorder2(   int_t nFR, int_t nAC, int_t* FR_idx, int_t* AC_idx, int_t nFRStart, int_t nACStart,
                                                                                int_t* FR_idxStart, int_t* AC_idxStart, int_t* FR_iSort, int_t* FR_iSortStart,
                                                                                int_t* AC_iSort, int_t* AC_iSortStart, real_t* sol, real_t* const delta_xFR, real_t* const delta_yAC);

                returnValue stepCalcResid(      int_t nFR, int_t nFX, int_t nAC, int_t* FR_idx, int_t* FX_idx, int_t* AC_idx,
                                                                        BooleanType Delta_bC_isZero, real_t* const delta_xFX, real_t* const delta_xFR,
                                                                        real_t* const delta_yAC, const real_t* const delta_g,
                                                                        const real_t* const delta_lbA, const real_t* const delta_ubA, real_t& rnrm);

                returnValue stepCalcDeltayFx(   int_t nFR, int_t nFX, int_t nAC, int_t* FX_idx, const real_t* const delta_g,
                                                                                real_t* const delta_xFX, real_t* const delta_xFR, real_t* const delta_yAC, real_t* const delta_yFX);

        /*
         *      PROTECTED MEMBER VARIABLES
         */
        protected:
                SparseSolver* sparseSolver;                     
                real_t* S;                                                      
                int_t nS;                                                       
                int_t nSmax;                                            
                real_t* Q_;                                                     
                real_t* R_;                                                     
                real_t detS;                                            
                real_t rcondS;                                          
                int_t numFactorizations;                        
                int_t* schurUpdateIndex;                        
                SchurUpdateType* schurUpdate;           
                int_t M_physicallength;                         
                real_t* M_vals;                                         
                sparse_int_t* M_ir;                                     
                sparse_int_t* M_jc;                                     
                Indexlist boundsFreeStart;                      
                Indexlist constraintsActiveStart;       
};


END_NAMESPACE_QPOASES

#include <qpOASES/SQProblemSchur.ipp>

#endif  /* QPOASES_QPROBLEMSCHUR_HPP */


/*
 *      end of file
 */