DiscreteControlledSystem.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 "DiscreteSystem.h"
#include <ct/core/control/discrete_time/DiscreteController.h>

namespace ct {
namespace core {


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

    typedef typename std::shared_ptr<DiscreteControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> Ptr;

    typedef DiscreteSystem<STATE_DIM, CONTROL_DIM, SCALAR> Base;

    typedef typename Base::state_vector_t state_vector_t;
    typedef typename Base::control_vector_t control_vector_t;
    typedef typename Base::time_t time_t;


    DiscreteControlledSystem(const SYSTEM_TYPE& type = SYSTEM_TYPE::GENERAL)
        : DiscreteSystem<STATE_DIM, CONTROL_DIM, SCALAR>(type), controller_(nullptr){};


    DiscreteControlledSystem(std::shared_ptr<DiscreteController<STATE_DIM, CONTROL_DIM, SCALAR>> controller,
        const SYSTEM_TYPE& type = SYSTEM_TYPE::GENERAL)
        : DiscreteSystem<STATE_DIM, CONTROL_DIM, SCALAR>(type), controller_(controller){};

    DiscreteControlledSystem(const ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>& arg)
        : DiscreteSystem<STATE_DIM, CONTROL_DIM, SCALAR>(arg)
    {
        if (arg.controller_)
            controller_ = std::shared_ptr<DiscreteController<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.controller_->clone());
    }

    virtual ~DiscreteControlledSystem() = default;

    virtual DiscreteControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override = 0;


    void setController(const std::shared_ptr<DiscreteController<STATE_DIM, CONTROL_DIM, SCALAR>>& controller)
    {
        controller_ = controller;
    }


    void getController(std::shared_ptr<DiscreteController<STATE_DIM, CONTROL_DIM, SCALAR>>& controller) const
    {
        controller = controller_;
    }


    std::shared_ptr<DiscreteController<STATE_DIM, CONTROL_DIM, SCALAR>> getController() { return controller_; }

    virtual void propagateDynamics(const state_vector_t& state, const time_t n, state_vector_t& stateNext) override
    {
        control_vector_t controlAction;
        if (controller_)
            controller_->computeControl(state, n, controlAction);
        else
            controlAction.setZero();

        propagateControlledDynamics(state, n, controlAction, stateNext);
    }



    virtual void propagateControlledDynamics(const state_vector_t& state,
        const time_t n,
        const control_vector_t& control,
        state_vector_t& stateNext) = 0;


protected:
    std::shared_ptr<DiscreteController<STATE_DIM, CONTROL_DIM, SCALAR>> controller_;  
};
}  // namespace core
}  // namespace ct