Nlp.h

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/


#pragma once

#include <Eigen/Sparse>
#include <ct/core/math/DerivativesCppadJIT.h>
#include <ct/optcon/nlp/OptVector.h>
#include <ct/optcon/nlp/DiscreteConstraintContainerBase.h>
#include <ct/optcon/nlp/DiscreteCostEvaluatorBase.h>

namespace ct {
namespace optcon {
namespace tpl {


template <typename SCALAR>
class Nlp
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using VectorXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>;
    using VectorXi = Eigen::Matrix<int, Eigen::Dynamic, 1>;
    using MapVecXs = Eigen::Map<VectorXs>;
    using MapVecXi = Eigen::Map<VectorXi>;
    using MapConstVecXs = Eigen::Map<const VectorXs>;

    Nlp() = default;
    virtual ~Nlp() = default;
    virtual void updateProblem() = 0;

    SCALAR evaluateCostFun()
    {
        if (!costEvaluator_)
            throw std::runtime_error("Error in evaluateCostFun. Costevaluator not initialized");

        return costEvaluator_->eval();
    }


    void evaluateCostGradient(const size_t n, MapVecXs& grad)
    {
        if (!costEvaluator_)
            throw std::runtime_error("Error in evaluateCostGradient. Costevaluator not initialized");

        costEvaluator_->evalGradient(n, grad);
    }

    void evaluateConstraints(MapVecXs& values)
    {
        if (!constraints_)
            throw std::runtime_error("Error in evaluateConstraints. Constraints not initialized");

        constraints_->evalConstraints(values);
    }

    void evaluateConstraintJacobian(const int nele_jac, MapVecXs& jac)
    {
        if (!constraints_)
            throw std::runtime_error("Error in evaluateConstraintJacobian. Constraints not initialized");

        constraints_->evalSparseJacobian(jac, nele_jac);
    }

    void evaluateHessian(const int nele_hes, MapVecXs& hes, const SCALAR obj_fac, MapConstVecXs& lambda)
    {
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)

        // store objective-factor in matrix to ensure compatibility with other derivative-classes
        Eigen::Matrix<double, 1, 1> omega;
        omega << obj_fac;

        // evaluate Hessian values
        Eigen::VectorXd hessianCostValues, hessianConstraintsValues;

        costEvaluator_->sparseHessianValues(optVariables_->getOptimizationVars(), omega, hessianCostValues);

        hessianConstraintsValues = constraints_->sparseHessianValues(optVariables_->getOptimizationVars(), lambda);


        // collect all Hessian values (from both constraints and cost) in one vector of triplets
        // here, we re-use the the sparsity information collected in getNonZeroHessianCount()
        std::vector<Eigen::Triplet<SCALAR>, Eigen::aligned_allocator<Eigen::Triplet<SCALAR>>> triplets;
        triplets.reserve(iRowHessianCost_.rows() + iRowHessianConstraints_.rows());

        for (int i = 0; i < iRowHessianCost_.rows(); i++)
        {
            triplets.push_back(Eigen::Triplet<SCALAR>(iRowHessianCost_(i), jColHessianCost_(i), hessianCostValues(i)));
        }
        for (int i = 0; i < iRowHessianConstraints_.rows(); i++)
        {
            triplets.push_back(Eigen::Triplet<SCALAR>(
                iRowHessianConstraints_(i), jColHessianConstraints_(i), hessianConstraintsValues(i)));
        }

        // setting the sparse Hessian from Eigen-triplets automatically resolves (e.g. adds) entries at same indices
        Hessian_eval_.setFromTriplets(triplets.begin(), triplets.end());

        // IPOPT and SNOPT only expect a triangular Hessian matrix (symmetry)
        // enforce triangular view
        // todo: is there a nicer in-place conversion to triangularView?
        Hessian_eval_ = Hessian_eval_.template triangularView<Eigen::Lower>();

        hes = Eigen::Map<Eigen::VectorXd>(Hessian_eval_.valuePtr(), nele_hes, 1);

#else
        throw std::runtime_error(
            "Nlp::evaluateHessian() only available for Eigen 3.3 and newer. Please change solver settings to NOT use "
            "exact Hessians");
#endif
    }

    void getSparsityPatternJacobian(const int nele_jac, MapVecXi& iRow, MapVecXi& jCol) const
    {
        if (!constraints_)
            throw std::runtime_error("Error in getSparsityPatternJacobian. Constraints not initialized");

        iRow.setZero();
        jCol.setZero();

        constraints_->getSparsityPattern(iRow, jCol, nele_jac);
    }

    void getSparsityPatternHessian(const int nele_hes, MapVecXi& iRow, MapVecXi& jCol) const
    {
        // todo this implicitly assumes that getNonZeroHessianCount() got called before. Assert this!
        iRow = iRowHessian_;
        jCol = jColHessian_;
    }

    size_t getConstraintsCount() const
    {
        if (!constraints_)
            throw std::runtime_error("Error in getConstraintsCount. Constraints not initialized");

        return constraints_->getConstraintsCount();
    }

    size_t getNonZeroJacobianCount() const
    {
        if (!constraints_)
            throw std::runtime_error("Error in getNonZeroJacobianCount. Constraints not initialized");

        return constraints_->getNonZerosJacobianCount();
    }

    size_t getNonZeroHessianCount()
    {
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)

        // the sparse Eigen-matrices need to be resized properly, which happens in this step.
        // todo: need to assert that getNonZeroHessianCount() gets called before the first call to a Hessian evaluation.
        Hessian_eval_.resize(optVariables_->size(), optVariables_->size());
        Hessian_sparsity_.resize(optVariables_->size(), optVariables_->size());

        iRowHessianCost_.setZero();
        jColHessianCost_.setZero();
        iRowHessianConstraints_.setZero();
        jColHessianConstraints_.setZero();


        // evaluate sparsity patterns
        constraints_->getSparsityPatternHessian(
            iRowHessianConstraints_, jColHessianConstraints_, optVariables_->size());

        costEvaluator_->getSparsityPatternHessian(iRowHessianCost_, jColHessianCost_);


        // collect all Hessian sparsity values (from both constraints and cost) in one vector of triplets
        std::vector<Eigen::Triplet<SCALAR>, Eigen::aligned_allocator<Eigen::Triplet<SCALAR>>> triplets;
        triplets.reserve(iRowHessianCost_.rows() + iRowHessianConstraints_.rows());

        for (int i = 0; i < iRowHessianCost_.rows(); ++i)
            triplets.push_back(Eigen::Triplet<SCALAR>(iRowHessianCost_(i), jColHessianCost_(i), SCALAR(1.0)));

        for (int i = 0; i < iRowHessianConstraints_.rows(); ++i)
            triplets.push_back(
                Eigen::Triplet<SCALAR>(iRowHessianConstraints_(i), jColHessianConstraints_(i), SCALAR(1.0)));

        // setting the sparse Hessian from Eigen-triplets automatically resolves (e.g. adds) entries at same indices
        Hessian_sparsity_.setFromTriplets(triplets.begin(), triplets.end());

        // IPOPT and SNOPT only expect a triangular Hessian matrix (symmetry)
        // enforce triangular view
        // todo: is there a nicer in-place conversion to triangularView?
        Hessian_sparsity_ = Hessian_sparsity_.template triangularView<Eigen::Lower>();

        // lastly, collect and combine the sparsity as filled into the helper-matrix and store in Eigen::Vectors.
        std::vector<int> iRowHessianStdVec;
        std::vector<int> jColHessianStdVec;
        for (int k = 0; k < Hessian_sparsity_.outerSize(); ++k)
        {
            for (typename Eigen::SparseMatrix<SCALAR>::InnerIterator it(Hessian_sparsity_, k); it; ++it)
            {
                iRowHessianStdVec.push_back(it.row());
                jColHessianStdVec.push_back(it.col());
            }
        }
        iRowHessian_ = Eigen::Map<Eigen::VectorXi>(iRowHessianStdVec.data(), iRowHessianStdVec.size(), 1);
        jColHessian_ = Eigen::Map<Eigen::VectorXi>(jColHessianStdVec.data(), jColHessianStdVec.size(), 1);

        // the number of non-zero elements is equal to the number rows
        size_t nonZerosHessian = iRowHessian_.rows();
        return nonZerosHessian;
#else
        throw std::runtime_error(
            "Nlp::getNonZeroHessianCount() only available for Eigen 3.3 and newer. Please change solver settings to "
            "NOT use "
            "exact Hessians or upgrade Eigen version.");
        return 0;
#endif
    }

    void getConstraintBounds(MapVecXs& lowerBound, MapVecXs& upperBound, const size_t m) const
    {
        if (!constraints_)
            throw std::runtime_error("Error in getConstraintBounds. Constraints not initialized");

        constraints_->getBounds(lowerBound, upperBound);
    }

    size_t getVarCount() const
    {
        if (!optVariables_)
            throw std::runtime_error("Error in getVarCount. Optvariables not initialized");

        return optVariables_->size();
    }

    void getVariableBounds(MapVecXs& lowerBound, MapVecXs& upperBound, const size_t n) const
    {
        if (!optVariables_)
            throw std::runtime_error("Error in getVariableBounds. Optvariables not initialized");

        optVariables_->getLowerBounds(lowerBound);
        optVariables_->getUpperBounds(upperBound);
    }

    void extractOptimizationVars(const MapConstVecXs& x, bool isNew)
    {
        if (!optVariables_)
            throw std::runtime_error("Error in extractOptimizationVars. Optvariables not initialized");

        if (isNew)
        {
            optVariables_->setOptimizationVars(x);
            updateProblem();
        }
    }

    void getInitialGuess(const size_t n, MapVecXs& x) const
    {
        if (!optVariables_)
            throw std::runtime_error("Error in getOptimizationVars. Optvariables not initialized");

        optVariables_->getInitialGuess(n, x);
    }

    void getOptimizationMultState(const size_t n, MapVecXs& xMul, MapVecXi& xState) const
    {
        if (!optVariables_)
            throw std::runtime_error("Error in getOptimizationMultState. Optvariables not initialized");

        optVariables_->getOptimizationMultState(n, xMul, xState);
    }

    void getConstraintsMultState(const size_t m, MapVecXs& zMul, MapVecXi& zState) const
    {
        if (!optVariables_)
            throw std::runtime_error("Error in getConstraintsMultState. Optvariables not initialized");

        optVariables_->getConstraintsMultState(m, zMul, zState);
    }

    void getBoundMultipliers(size_t n, MapVecXs& zLow, MapVecXs& zUp) const
    {
        if (!optVariables_)
            throw std::runtime_error("Error in getBoundMultipliers. Optvariables not initialized");

        optVariables_->getBoundMultipliers(n, zLow, zUp);
    }

    void getLambdaVars(size_t m, MapVecXs& lambda) const
    {
        if (!optVariables_)
            throw std::runtime_error("Error in getLambdaVars. Optvariables not initialized");

        optVariables_->getLambdaVars(m, lambda);
    }

    void extractIpoptSolution(const MapConstVecXs& x,
        const MapConstVecXs& zL,
        const MapConstVecXs& zU,
        const MapConstVecXs& lambda)
    {
        if (!optVariables_)
            throw std::runtime_error("Error in extractIpoptSolution. Optvariables not initialized");

        optVariables_->setNewIpoptSolution(x, zL, zU, lambda);
    }

    void extractSnoptSolution(const MapVecXs& x,
        const MapVecXs& xMul,
        const MapVecXi& xState,
        const MapVecXs& fMul,
        const MapVecXi& fState)
    {
        if (!optVariables_)
            throw std::runtime_error("Error in extractSnoptSolution. Optvariables not initialized");

        optVariables_->setNewSnoptSolution(x, xMul, xState, fMul, fState);
    }


protected:
    std::shared_ptr<DiscreteCostEvaluatorBase<SCALAR>> costEvaluator_;

    std::shared_ptr<OptVector<SCALAR>> optVariables_;

    std::shared_ptr<DiscreteConstraintContainerBase<SCALAR>> constraints_;

#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
    Eigen::SparseMatrix<SCALAR> Hessian_eval_;
    Eigen::SparseMatrix<SCALAR> Hessian_sparsity_;  // this is just a helper data structure
#endif

    Eigen::VectorXi iRowHessianCost_, iRowHessianConstraints_, jColHessianCost_, jColHessianConstraints_;

    Eigen::VectorXi iRowHessian_, jColHessian_;
};
}  // namespace tpl

using Nlp = tpl::Nlp<double>;

}  // namespace optcon
}  // namespace ct