export_nlp_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_EXPORT_NLP_SOLVER_HPP
#define ACADO_TOOLKIT_EXPORT_NLP_SOLVER_HPP

#include <acado/utils/acado_utils.hpp>
#include <acado/matrix_vector/matrix_vector.hpp>

#include <acado/code_generation/export_algorithm_factory.hpp>
#include <acado/code_generation/integrators/integrator_export.hpp>

#include <acado/code_generation/export_cholesky_decomposition.hpp>
#include <acado/code_generation/linear_solvers/householder_qr_export.hpp>

BEGIN_NAMESPACE_ACADO

class OCP;
class Objective;

class ExportNLPSolver : public ExportAlgorithm
{
public:

        ExportNLPSolver(        UserInteraction* _userInteraction = 0,
                                                const std::string& _commonHeaderName = ""
                                                );

        virtual ~ExportNLPSolver( )
        {}

        virtual returnValue setup( ) = 0;


        returnValue setIntegratorExport(        IntegratorExportPtr const _integrator
                                                                                );

        returnValue setLevenbergMarquardt(      double _levenbergMarquardt
                                                                                );


        virtual returnValue getDataDeclarations(        ExportStatementBlock& declarations,
                                                                                                ExportStruct dataStruct = ACADO_ANY
                                                                                                ) const;

        virtual returnValue getFunctionDeclarations(    ExportStatementBlock& declarations
                                                                                                        ) const = 0;


        virtual returnValue getCode(    ExportStatementBlock& code
                                                                        ) = 0;


        virtual unsigned getNumQPvars( ) const = 0;

        bool performsSingleShooting( ) const;

        returnValue setObjective(const Objective& _objective);
        returnValue setLSQObjective(const Objective& _objective);
        returnValue setGeneralObjective(const Objective& _objective);

        returnValue setConstraints(const OCP& _ocp);

        unsigned getNumComplexConstraints( void );

    unsigned getNumPathConstraints( void );

        unsigned weightingMatricesType( void ) const;

        bool initialStateFixed( ) const;

        bool usingLinearTerms() const;

protected:

        virtual returnValue setupSimulation( void );

        virtual returnValue setupVariables( ) = 0;

        virtual returnValue setupMultiplicationRoutines( ) = 0;

        virtual returnValue setupEvaluation( ) = 0;

        virtual returnValue setupConstraintsEvaluation( ) = 0;

        returnValue setupAuxiliaryFunctions();

        virtual returnValue setupGetObjective();

        virtual returnValue setupGetLSQObjective();

        virtual returnValue setupGetGeneralObjective();

        returnValue setupArrivalCostCalculation();

        virtual returnValue setupInitialization();

protected:

        IntegratorExportPtr integrator;

        ExportFunction modelSimulation;

        ExportVariable state;
        ExportVariable x;
        ExportVariable z;
        ExportVariable u;
        ExportVariable od;
        ExportVariable d;

        ExportVariable evGx; // stack of sensitivities w.r.t. x
        ExportVariable evGu; // stack of sensitivities w.r.t. u

        double levenbergMarquardt;

        ExportVariable y, yN, Dy, DyN;

        // lagrange multipliers
        ExportVariable mu;

        ExportVariable objg, objS, objSEndTerm;
        ExportVariable objEvFx, objEvFu, objEvFxEnd; // aliasing
        ExportVariable objEvFxx, objEvFxu, objEvFuu, objEvFxxEnd; // aliasing

        ExportVariable objAuxVar, objValueIn, objValueOut;
        ExportAcadoFunction evaluateStageCost;
        ExportAcadoFunction evaluateTerminalCost;

        ExportVariable Q1, Q2;
        ExportVariable R1, R2;
        ExportVariable S1;
        ExportVariable QN1, QN2;

        ExportVariable objSlx, objSlu;

        bool diagonalH, diagonalHN;

        VariablesGrid uBounds;
        VariablesGrid xBounds;
        unsigned dimPacH;
        ExportAcadoFunction evaluatePathConstraints;
        ExportVariable conAuxVar;
        ExportVariable conValueIn;
        ExportVariable conValueOut;

        DVector lbPathConValues, ubPathConValues;

        ExportVariable pacEvH;
        ExportVariable pacEvHx, pacEvHu, pacEvHxd;
        ExportVariable pacEvDDH;
        unsigned dimPocH;
        std::vector< std::shared_ptr< ExportAcadoFunction > > evaluatePointConstraints;
        DVector lbPointConValues, ubPointConValues;

        std::vector< DVector > pocLbStack, pocUbStack;

        ExportVariable pocEvH;
        ExportVariable pocEvHx, pocEvHu, pocEvHxd;
        ExportFunction initialize;

        ExportFunction shiftStates;
        ExportFunction shiftControls;
        ExportFunction getObjective;
        ExportFunction initializeNodes;
        ExportFunction updateArrivalCost;

        ExportCholeskyDecomposition cholObjS;
        ExportCholeskyDecomposition cholSAC;

        ExportHouseholderQR acSolver;

        ExportVariable acA, acb, acP, acTmp;
        // acWL and acVL are assumed to be upper triangular matrices
        ExportVariable acWL, acVL, acHx, acHu, acXx, acXu, acXTilde, acHTilde;

        // Older stuff; TODO make this more unique
        ExportVariable SAC, xAC, DxAC;

        ExportFunction regularizeHessian;
        ExportFunction regularization;
private:
        returnValue setupResidualVariables();
        returnValue setupObjectiveLinearTerms(const Objective& _objective);
};

enum ExportNLPType
{
        GAUSS_NEWTON_CONDENSED,
        GAUSS_NEWTON_CN2,
        GAUSS_NEWTON_BLOCK_QPDUNES,
        GAUSS_NEWTON_BLOCK_FORCES,
        GAUSS_NEWTON_CN2_FACTORIZATION,
        GAUSS_NEWTON_FORCES,
        GAUSS_NEWTON_QPDUNES,
        GAUSS_NEWTON_HPMPC,
    GAUSS_NEWTON_GENERIC,
        EXACT_HESSIAN_CN2,
        EXACT_HESSIAN_QPDUNES
};

typedef ExportAlgorithmFactory<ExportNLPSolver, ExportNLPType> NLPSolverFactory;

typedef std::shared_ptr< ExportNLPSolver > ExportNLPSolverPtr;

CLOSE_NAMESPACE_ACADO

#endif  // ACADO_TOOLKIT_EXPORT_NLP_SOLVER_HPP