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

#include <ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp>
#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>

namespace ct {
namespace optcon {

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

    typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
    typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
    typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
    typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
    typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
    typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_feedback_t;

    typedef core::StateVectorArray<STATE_DIM, SCALAR> state_vector_array_t;
    typedef core::ControlVectorArray<CONTROL_DIM, SCALAR> control_vector_array_t;
    typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> state_matrix_array_t;

    typedef ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> control_feedback_array_t;
    typedef ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> control_gain_matrix_array_t;



    FHDTLQR(std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFunction);

    ~FHDTLQR();



    void designController(const state_vector_array_t& x_trajectory,
        const control_vector_array_t& u_trajectory,
        const state_matrix_array_t& A,
        const control_gain_matrix_array_t& B,
        SCALAR dt,
        control_feedback_array_t& K,
        bool performNumericalChecks = true);



    void designController(const state_vector_array_t& x_trajectory,
        const control_vector_array_t& u_trajectory,
        std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> derivatives,
        SCALAR dt,
        control_feedback_array_t& K,
        bool performNumericalChecks = true);


private:

    void linearizeModel(const state_vector_array_t& x_trajectory,
        const control_vector_array_t& u_trajectory,
        size_t N,
        SCALAR dt,
        std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& derivatives,
        state_matrix_array_t& A,
        control_gain_matrix_array_t& B);



    void solve(const state_vector_array_t& x_trajectory,
        const control_vector_array_t& u_trajectory,
        const state_matrix_array_t& A,
        const control_gain_matrix_array_t& B,
        size_t N,
        SCALAR dt,
        control_feedback_array_t& K,
        bool performNumericalChecks = true);


    std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>>
        costFunction_;  
    DynamicRiccatiEquation<STATE_DIM, CONTROL_DIM, SCALAR> ricattiEq_;  
};

}  // namespace optcon
}  // namespace ct