ConstantStateFeedbackController.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 "Controller.h"
#include <ct/core/types/FeedbackMatrix.h>

namespace ct {
namespace core {


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


    ConstantStateFeedbackController()
    {
        u_ff_.setZero();
        x_ref_.setZero();
        K_.setZero();
    }



    ConstantStateFeedbackController(const ControlVector<CONTROL_DIM, SCALAR>& uff,
        const StateVector<STATE_DIM, SCALAR>& x,
        const FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>& K)
        : u_ff_(uff), x_ref_(x), K_(K)
    {
    }


    ConstantStateFeedbackController(const ConstantStateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>& other)
        : Controller<STATE_DIM, CONTROL_DIM, SCALAR>(other), u_ff_(other.u_ff_), x_ref_(other.x_ref_), K_(other.K_)
    {
    }


    ~ConstantStateFeedbackController() {}

    ConstantStateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
    {
        return new ConstantStateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
    }



    void computeControl(const StateVector<STATE_DIM, SCALAR>& x,
        const SCALAR& t,
        ControlVector<CONTROL_DIM, SCALAR>& controlAction) override
    {
        controlAction = u_ff_ + K_ * (x - x_ref_);
    }



    void updateControlLaw(const ControlVector<CONTROL_DIM, SCALAR>& uff,
        const StateVector<STATE_DIM, SCALAR>& x,
        const FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>& K =
            FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero())
    {
        u_ff_ = uff;
        x_ref_ = x;
        K_ = K;
    }


private:
    ControlVector<CONTROL_DIM, SCALAR> u_ff_;

    StateVector<STATE_DIM, SCALAR> x_ref_;

    FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR> K_;
};

}  // namespace core
}  // namespace ct