DiscreteConstraintBase.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/Core>

namespace ct {
namespace optcon {
namespace tpl {

template <typename SCALAR>
class DiscreteConstraintBase
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    using VectorXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>;

    DiscreteConstraintBase() = default;
    virtual ~DiscreteConstraintBase() = default;
    virtual VectorXs eval() = 0;

    virtual VectorXs evalSparseJacobian() = 0;

    virtual size_t getConstraintSize() = 0;

    virtual size_t getNumNonZerosJacobian() = 0;

    virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) = 0;


    virtual void genSparsityPatternHessian(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec)
    {
        throw std::runtime_error(
            "genSparsityPatternHessian() for DiscreteConstraintBase not implemented. Use Hessian approximation.");
    }

    virtual void sparseHessianValues(const Eigen::VectorXd& optVec,
        const Eigen::VectorXd& lambda,
        Eigen::VectorXd& sparseHes)
    {
        throw std::runtime_error(
            "sparseHessianValues() for DiscreteConstraintBase not implemented. Use Hessian approximation.");
    }


    virtual VectorXs getLowerBound() = 0;

    virtual VectorXs getUpperBound() = 0;

    Eigen::VectorXi& iRowHessian() { return iRowHessian_; }
    Eigen::VectorXi& jColHessian() { return jColHessian_; }
protected:
    size_t genDiagonalIndices(const size_t col_start,
        const size_t num_elements,
        Eigen::VectorXi& iRow_vec,
        Eigen::VectorXi& jCol_vec,
        const size_t indexNumber);

    size_t genBlockIndices(const size_t col_start,
        const size_t num_rows,
        const size_t num_cols,
        Eigen::VectorXi& iRow_vec,
        Eigen::VectorXi& jCol_vec,
        const size_t indexNumber);

    Eigen::VectorXi iRowHessian_;
    Eigen::VectorXi jColHessian_;
};

template <typename SCALAR>
inline size_t DiscreteConstraintBase<SCALAR>::genDiagonalIndices(const size_t col_start,
    const size_t num_elements,
    Eigen::VectorXi& iRow_vec,
    Eigen::VectorXi& jCol_vec,
    const size_t indexNumber)
{
    Eigen::VectorXi new_row_indices;
    Eigen::VectorXi new_col_indices;
    new_row_indices.resize(num_elements);
    new_col_indices.resize(num_elements);

    size_t count = 0;

    for (size_t i = 0; i < num_elements; ++i)
    {
        new_row_indices(count) = i;
        new_col_indices(count) = col_start + i;
        count++;
    }

    assert(count == num_elements);

    iRow_vec.segment(indexNumber, num_elements) = new_row_indices;
    jCol_vec.segment(indexNumber, num_elements) = new_col_indices;

    return count;
}

template <typename SCALAR>
inline size_t DiscreteConstraintBase<SCALAR>::genBlockIndices(const size_t col_start,
    const size_t num_rows,
    const size_t num_cols,
    Eigen::VectorXi& iRow_vec,
    Eigen::VectorXi& jCol_vec,
    const size_t indexNumber)
{
    size_t num_gen_indices = num_rows * num_cols;

    Eigen::VectorXi new_row_indices;
    Eigen::VectorXi new_col_indices;
    new_row_indices.resize(num_gen_indices);
    new_col_indices.resize(num_gen_indices);

    size_t count = 0;

    for (size_t row = 0; row < num_rows; ++row)
    {
        for (size_t col = col_start; col < col_start + num_cols; ++col)
        {
            new_row_indices(count) = row;
            new_col_indices(count) = col;
            count++;
        }
    }

    assert(count == num_gen_indices);

    iRow_vec.segment(indexNumber, num_gen_indices) = new_row_indices;
    jCol_vec.segment(indexNumber, num_gen_indices) = new_col_indices;

    return num_gen_indices;
}
}

using DiscreteConstraintBase = tpl::DiscreteConstraintBase<double>;

}  // namespace optcon
}  // namespace ct