OptVector.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

namespace ct {
namespace optcon {
namespace tpl {


template <typename SCALAR>
class OptVector
{
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>;

    OptVector() = delete;

    OptVector(const size_t n) : updateCount_(0)
    {
        x_.resize(n);
        x_.setZero();
        xInit_.resize(n);
        xLb_.resize(n);
        xUb_.resize(n);
        xLb_.setConstant(std::numeric_limits<SCALAR>::lowest());
        xUb_.setConstant(std::numeric_limits<SCALAR>::max());
        xMul_.resize(n);
        xMul_.setZero();
        xState_.resize(n);
        xState_.setZero();
        zUpper_.resize(n);
        zUpper_.setZero();
        zLow_.resize(n);
        zLow_.setZero();
    }

    void resizeConstraintVars(size_t m)
    {
        zMul_.resize(m + 1);
        zMul_.setZero();
        zState_.resize(m + 1);
        zState_.setZero();
    }

    void reset() { updateCount_ = 0; }
    virtual ~OptVector() = default;
    void resizeOptimizationVars(const size_t size)
    {
        x_.resize(size);
        xInit_.resize(size);
        xLb_.resize(size);
        xUb_.resize(size);
        lambda_.resize(size);
        zUpper_.resize(size);
        zLow_.resize(size);
    }


    void setZero()
    {
        x_.setZero();
        xInit_.setZero();
        lambda_.setZero();
        zUpper_.setZero();
        zLow_.setZero();
    }

    void setRandomInitialGuess()
    {
        x_.setRandom();
        xInit_.setRandom();
    }

    void setInitialGuess(const VectorXs& xinit)
    {
        x_ = xinit;
        xInit_ = xinit;
    }

    bool checkOptimizationVarDimension(const unsigned int n)
    {
        bool xDim = x_.size() == n ? true : false;
        bool xLDim = xLb_.size() == n ? true : false;
        bool xUDim = xUb_.size() == n ? true : false;
        return xDim && xLDim && xUDim;
    }

    void setBounds(const VectorXs& xLb, const VectorXs& xUb)
    {
        xLb_ = xLb;
        xUb_ = xUb;
    }


    void getLowerBounds(MapVecXs& x) const { x = xLb_; }
    void getUpperBounds(MapVecXs& x) const { x = xUb_; }
    void getOptimizationMultState(const size_t n, MapVecXs& xMul, MapVecXi& xState) const
    {
        assert(n == xMul_.size());
        assert(n == xState_.size());
        xMul = xMul_;
        xState = xState_;
    }

    void getConstraintsMultState(const size_t m, MapVecXs& zMul, MapVecXi& zState) const
    {
        assert(m == zMul_.size());
        assert(m == zState_.size());
        zMul = zMul_;
        zState = zState_;
    }

    size_t size() const { return x_.size(); }
    void getBoundMultipliers(size_t n, MapVecXs& low, MapVecXs& up) const
    {
        assert(n == static_cast<size_t>(zLow_.size()));
        low = zLow_;
        up = zUpper_;
    }

    void getLambdaVars(size_t m, MapVecXs& x) const
    {
        assert(m == static_cast<size_t>(lambda_.size()));
        x = lambda_;
    }

    void getOptimizationVars(size_t n, MapVecXs& x) const
    {
        assert(n == x_.size());
        x = x_;
    }

    const VectorXs& getOptimizationVars() const { return x_; }
    void getInitialGuess(size_t n, MapVecXs& x) const
    {
        assert(n == static_cast<size_t>(xInit_.size()));
        x = xInit_;
    }

    void setNewIpoptSolution(const MapConstVecXs& x,
        const MapConstVecXs& zL,
        const MapConstVecXs& zU,
        const MapConstVecXs& lambda)
    {
        x_ = x;
        zLow_ = zL;
        zUpper_ = zU;
        lambda_ = lambda;
    }

    void setNewSnoptSolution(const MapVecXs& x,
        const MapVecXs& xMul,
        const MapVecXi& xState,
        const MapVecXs& fMul,
        const MapVecXi& fState)
    {
        x_ = x;
        xMul_ = xMul;
        xState_ = xState;
        zMul_ = fMul;
        zState_ = fState;
    }

    void setOptimizationVars(const MapConstVecXs& x)
    {
        x_ = x;
        updateCount_++;
    }

    void setOptimizationVars(const VectorXs& x)
    {
        x_ = x;
        updateCount_++;
    }


    size_t getUpdateCount() const { return updateCount_; }
protected:
    VectorXs x_; 
    VectorXs xInit_;
    VectorXs xLb_; 
    VectorXs xUb_; 
    VectorXs zUpper_; 
    VectorXs zLow_;   
    VectorXs lambda_; 
    // Snopt variables
    VectorXs xMul_;   
    VectorXi xState_; 
    VectorXs zMul_;   
    VectorXi zState_; 
    size_t updateCount_; 
};
}

using OptVector = tpl::OptVector<double>;
}
}