condensing_based_cp_solver.hpp

Go to the documentation of this file.

/*
 *    This file is part of ACADO Toolkit.
 *
 *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
 *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
 *    Milan Vukov, Rien Quirynen, KU Leuven.
 *    Developed within the Optimization in Engineering Center (OPTEC)
 *    under supervision of Moritz Diehl. All rights reserved.
 *
 *    ACADO Toolkit 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 3 of the License, or (at your option) any later version.
 *
 *    ACADO Toolkit 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 ACADO Toolkit; if not, write to the Free Software
 *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */


#ifndef ACADO_TOOLKIT_CONDENSING_BASED_CP_SOLVER_HPP
#define ACADO_TOOLKIT_CONDENSING_BASED_CP_SOLVER_HPP

#include <acado/utils/acado_utils.hpp>
#include <acado/matrix_vector/matrix_vector.hpp>
#include <acado/conic_solver/banded_cp_solver.hpp>
#include <acado/conic_solver/dense_qp_solver.hpp>



BEGIN_NAMESPACE_ACADO


class CondensingBasedCPsolver: public BandedCPsolver {


    //
    // PUBLIC MEMBER FUNCTIONS:
    //
    public:

        CondensingBasedCPsolver( );
                
        CondensingBasedCPsolver(        UserInteraction* _userInteraction,
                                                                        uint nConstraints_,
                                                                const DVector& blockDims_
                                                                );

        CondensingBasedCPsolver( const CondensingBasedCPsolver& rhs );

        virtual ~CondensingBasedCPsolver( );

        CondensingBasedCPsolver& operator=( const CondensingBasedCPsolver& rhs );


        virtual BandedCPsolver* clone() const;


        virtual returnValue init( const OCPiterate &iter_ );


        virtual returnValue prepareSolve(       BandedCP& cp
                                                                                        );

        virtual returnValue solve(      BandedCP& cp
                                                                        );

        virtual returnValue finalizeSolve(      BandedCP& cp
                                                                                        );


                inline uint getNX( ) const;
                inline uint getNXA( ) const;
                inline uint getNP( ) const;
                inline uint getNU( ) const;
                inline uint getNW( ) const;

                inline uint getNC( ) const;
                inline uint getNF( ) const;
                inline uint getNA( ) const;

                inline uint getNumPoints( ) const;


                virtual returnValue getParameters        ( DVector        &p_  ) const;
                virtual returnValue getFirstControl      ( DVector        &u0_ ) const;


        virtual returnValue getVarianceCovariance( DMatrix &var );

                
                virtual returnValue setRealTimeParameters(      const DVector& DeltaX,
                                                                                                        const DVector& DeltaP = emptyConstVector
                                                                                                        );

                inline BooleanType areRealTimeParametersDefined( ) const;


                virtual returnValue freezeCondensing( );

                virtual returnValue unfreezeCondensing( );



    //
    // PROTECTED MEMBER FUNCTIONS:
    //
    protected:

        virtual returnValue initializeCPsolver( InfeasibleQPhandling infeasibleQPhandling
                                                                                                );


                virtual returnValue solveQP(    uint maxIter,                           
                                                                                InfeasibleQPhandling infeasibleQPhandling = IQH_UNDEFINED
                                                                                );





        virtual returnValue solveCPsubproblem( );



        returnValue projectHessian( DMatrix &H_, double dampingFactor );


                // --------
                // SQP DATA
                // --------

        returnValue condense(   BandedCP& cp
                                                                );


        returnValue expand(             BandedCP& cp
                                                                );


        returnValue generateHessianBlockLine   ( uint nn, uint rowOffset, uint& rowOffset1 );
        returnValue generateConstraintBlockLine( uint nn, uint rowOffset, uint& rowOffset1 );
        returnValue generateStateBoundBlockLine( uint nn, uint rowOffset, uint& rowOffset1 );
        returnValue generateConstraintVectors  ( uint nn, uint rowOffset, uint& rowOffset1 );
        returnValue generateStateBoundVectors  ( uint nn, uint rowOffset, uint& rowOffset1 );


        returnValue generateBoundVectors     ( );
        returnValue generateObjectiveGradient( );


        returnValue initializeCondensingOperator( );
                
                returnValue computeCondensingOperator(  BandedCP& cp
                                                                                                );


        virtual returnValue setupRelaxedQPdata( InfeasibleQPhandling infeasibleQPhandling,
                                                                                                DenseCP& _denseCPrelaxed                                        
                                                                                                ) const;

        virtual returnValue setupRelaxedQPdataL1(       DenseCP& _denseCPrelaxed                                        
                                                                                                        ) const;

        virtual returnValue setupRelaxedQPdataL2(       DenseCP& _denseCPrelaxed                                        
                                                                                                        ) const;



    //
    // DATA MEMBERS:
    //
    protected:

        OCPiterate iter;
        DVector blockDims;
        uint nConstraints;

                CondensingStatus condensingStatus;


        // THE CONDENSING OPERATORS:
        // -----------------------------------------------------
        BlockMatrix   T;    
        BlockMatrix   d;    
                BlockMatrix  hT;
        // ------------------------------------------------


        // DENSE QP IN BLOCK-MATRIX FORM:
        // ----------------------------------------------------------------------

        BlockMatrix        HDense;    
        BlockMatrix        gDense;    
        BlockMatrix        ADense;    
        BlockMatrix      lbADense;    
        BlockMatrix      ubADense;    
        BlockMatrix       lbDense;    
        BlockMatrix       ubDense;    
        // ----------------------------------------------------------------------


        DenseCPsolver* cpSolver;
        DenseQPsolver* cpSolverRelaxed;

        DenseCP        denseCP;

                DVector deltaX;
                DVector deltaP;
};


CLOSE_NAMESPACE_ACADO


#include <acado/conic_solver/condensing_based_cp_solver.ipp>


#endif  // ACADO_TOOLKIT_CONDENSING_BASED_CP_SOLVER_HPP

/*
 *  end of file
 */