StateFeedbackController.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 core {


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

    typedef Controller<STATE_DIM, CONTROL_DIM, SCALAR> ContinuousBase;
    typedef DiscreteController<STATE_DIM, CONTROL_DIM, SCALAR> DiscreteBase;

    typedef typename DiscreteBase::state_vector_t state_vector_t;
    typedef typename DiscreteBase::control_vector_t control_vector_t;

    typedef StateVectorArray<STATE_DIM, SCALAR> state_vector_array_t;
    typedef ControlVectorArray<CONTROL_DIM, SCALAR> control_vector_array_t;
    typedef FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> feedback_array_t;

    StateFeedbackController() : ContinuousBase(), DiscreteBase() {}
    StateFeedbackController(const StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>& other);


    StateFeedbackController(const state_vector_array_t& x_ref,
        const control_vector_array_t& uff,
        const feedback_array_t& K,
        const SCALAR deltaT,
        const SCALAR t0 = 0.0,
        const InterpolationType& intType = ZOH);

    virtual ~StateFeedbackController(){};

    StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;


    virtual void computeControl(const state_vector_t& state, const SCALAR& t, control_vector_t& controlAction) override;


    virtual void computeControl(const state_vector_t& state, const int n, control_vector_t& controlAction) override;


    void update(const DiscreteArray<state_vector_t>& x_ref,
        const DiscreteArray<control_vector_t>& uff,
        const DiscreteArray<FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>>& K,
        const tpl::TimeArray<SCALAR>& t);

    const DiscreteArray<state_vector_t>& x_ref() const;

    const DiscreteArray<control_vector_t>& uff() const;

    const DiscreteArray<FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>>& K() const;

    const tpl::TimeArray<SCALAR>& time() const;

    StateTrajectory<STATE_DIM, SCALAR>& getReferenceStateTrajectory();

    const StateTrajectory<STATE_DIM, SCALAR>& getReferenceStateTrajectory() const;

    ControlTrajectory<CONTROL_DIM, SCALAR>& getFeedforwardTrajectory();

    const ControlTrajectory<CONTROL_DIM, SCALAR>& getFeedforwardTrajectory() const;

    FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>& getFeedbackTrajectory();

    const FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>& getFeedbackTrajectory() const;

    void extractControlTrajectory(const StateTrajectory<STATE_DIM, SCALAR>& x_traj,
        ControlTrajectory<CONTROL_DIM, SCALAR>& u_traj);

protected:
    StateTrajectory<STATE_DIM, SCALAR> x_ref_;              
    ControlTrajectory<CONTROL_DIM, SCALAR> uff_;            
    FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR> K_;  
};

}  // namespace core
}  // namespace ct