LQOCProblem.hpp

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 <int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
class LQOCProblem
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using constr_vec_t = Eigen::Matrix<SCALAR, -1, -1>;
    using constr_state_jac_t = Eigen::Matrix<SCALAR, -1, -1>;
    using constr_control_jac_t = Eigen::Matrix<SCALAR, -1, -1>;

    using constr_vec_array_t = ct::core::DiscreteArray<constr_vec_t>;
    using constr_state_jac_array_t = ct::core::DiscreteArray<constr_state_jac_t>;
    using constr_control_jac_array_t = ct::core::DiscreteArray<constr_control_jac_t>;

    using input_box_constr_vec_t = Eigen::Matrix<SCALAR, CONTROL_DIM, 1>;
    using state_box_constr_vec_t = Eigen::Matrix<SCALAR, STATE_DIM, 1>;
    using input_box_constr_array_t = ct::core::DiscreteArray<input_box_constr_vec_t>;
    using state_box_constr_array_t = ct::core::DiscreteArray<state_box_constr_vec_t>;

    using state_box_constr_sparsity_t = Eigen::Matrix<int, STATE_DIM, 1>;
    using input_box_constr_sparsity_t = Eigen::Matrix<int, CONTROL_DIM, 1>;

    using input_box_constr_sparsity_array_t = ct::core::DiscreteArray<input_box_constr_sparsity_t>;
    using state_box_constr_sparsity_array_t = ct::core::DiscreteArray<state_box_constr_sparsity_t>;

    using VectorXi = Eigen::VectorXi;

    LQOCProblem(int N = 0);

    int getNumberOfStages();

    void changeNumStages(int N);

    void setZero(const int& nGenConstr = 0);

    void setInputBoxConstraint(const int index,
        const int nConstr,
        const constr_vec_t& u_lb,
        const constr_vec_t& u_ub,
        const VectorXi& sp,
        const ct::core::ControlVector<CONTROL_DIM, SCALAR>& u_nom_abs);

    void setInputBoxConstraints(const int nConstr,
        const constr_vec_t& u_lb,
        const constr_vec_t& u_ub,
        const VectorXi& sp,
        const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_nom_abs);

    void setIntermediateStateBoxConstraint(const int index,
        const int nConstr,
        const constr_vec_t& x_lb,
        const constr_vec_t& x_ub,
        const VectorXi& sp,
        const ct::core::StateVector<STATE_DIM, SCALAR>& x_nom_abs);

    void setIntermediateStateBoxConstraints(const int nConstr,
        const constr_vec_t& x_lb,
        const constr_vec_t& x_ub,
        const VectorXi& sp,
        const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_nom_abs);

    void setTerminalBoxConstraints(const int nConstr,
        const constr_vec_t& x_lb,
        const constr_vec_t& x_ub,
        const VectorXi& sp,
        const ct::core::StateVector<STATE_DIM, SCALAR>& x_nom_abs);

    void setGeneralConstraints(const constr_vec_t& d_lb,
        const constr_vec_t& d_ub,
        const constr_state_jac_t& C,
        const constr_control_jac_t& D);

    void setFromTimeInvariantLinearQuadraticProblem(
        ct::core::DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>& linearSystem,
        ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>& costFunction,
        const ct::core::StateVector<STATE_DIM, SCALAR>& stateOffset,
        const double dt);

    bool isConstrained() const;

    bool isInputBoxConstrained() const;
    bool isStateBoxConstrained() const;
    bool isGeneralConstrained() const;

    ct::core::StateMatrixArray<STATE_DIM, SCALAR> A_;
    ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> B_;
    ct::core::StateVectorArray<STATE_DIM, SCALAR> b_;

    ct::core::ScalarArray<SCALAR> q_;

    ct::core::StateVectorArray<STATE_DIM, SCALAR> qv_;
    ct::core::StateMatrixArray<STATE_DIM, SCALAR> Q_;

    ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> rv_;
    ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR> R_;

    ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> P_;

    input_box_constr_array_t u_lb_;
    input_box_constr_array_t u_ub_;
    state_box_constr_array_t x_lb_;
    state_box_constr_array_t x_ub_;

    input_box_constr_sparsity_array_t u_I_;
    state_box_constr_sparsity_array_t x_I_;

    std::vector<int> nbu_;

    std::vector<int> nbx_;

    constr_vec_array_t d_lb_;
    constr_vec_array_t d_ub_;
    constr_state_jac_array_t C_;
    constr_control_jac_array_t D_;
    std::vector<int> ng_;


private:
    int K_;
};

}  // namespace optcon
}  // namespace ct