ConstraintBase.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 {

template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
class ConstraintBase
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;

    typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
    typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;

    typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
    typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;

    ConstraintBase(std::string name = "Unnamed");

    ConstraintBase(const ConstraintBase& arg);

    virtual ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const = 0;

    virtual ~ConstraintBase();

    virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) = 0;

#ifdef CPPADCG
    virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
        const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
        const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
        ct::core::ADCGScalar t);
#endif

    virtual size_t getConstraintSize() const = 0;


    virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t);

    virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t);

    virtual VectorXs getLowerBound() const;

    virtual VectorXs getUpperBound() const;

    void getName(std::string& constraintName) const;

    void setName(const std::string constraintName);

    virtual size_t getNumNonZerosJacobianState() const;

    virtual size_t getNumNonZerosJacobianInput() const;

    virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t);

    virtual VectorXs jacobianInputSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t);


    virtual void sparsityPatternState(Eigen::VectorXi& rows, Eigen::VectorXi& cols);

    virtual void sparsityPatternInput(Eigen::VectorXi& rows, Eigen::VectorXi& cols);


protected:
    VectorXs lb_;  
    VectorXs ub_;  

    static void genDiagonalIndices(const size_t num_elements, Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec);

    static void genSparseDiagonalIndices(const Eigen::VectorXi& diag_sparsity,
        Eigen::VectorXi& iRow_vec,
        Eigen::VectorXi& jCol_vec);

    static void genBlockIndices(const size_t num_rows,
        const size_t num_cols,
        Eigen::VectorXi& iRow_vec,
        Eigen::VectorXi& jCol_vec);

private:
    std::string name_;
};

}  // namespace optcon
}  // namespace ct