ControlledSystem.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 <ct/core/control/continuous_time/Controller.h>
#include "System.h"

#include <ct/core/types/arrays/MatrixArrays.h>
#include <ct/core/types/arrays/TimeArray.h>
#include <ct/core/types/trajectories/MatrixTrajectories.h>


namespace ct {
namespace core {


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

    typedef System<STATE_DIM, SCALAR> Base;
    typedef typename std::shared_ptr<ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> Ptr;
    typedef typename Base::time_t time_t;


    ControlledSystem(const SYSTEM_TYPE& type = SYSTEM_TYPE::GENERAL)
        : System<STATE_DIM, SCALAR>(type), controller_(nullptr)
    {
    }


    ControlledSystem(std::shared_ptr<ct::core::Controller<STATE_DIM, CONTROL_DIM, SCALAR>> controller,
        const SYSTEM_TYPE& type = SYSTEM_TYPE::GENERAL)
        : System<STATE_DIM, SCALAR>(type), controller_(controller)
    {
        controlAction_.setZero();
    }

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

    virtual ~ControlledSystem() {}
    virtual ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override = 0;


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


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


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

    virtual void computeDynamics(const StateVector<STATE_DIM, SCALAR>& state,
        const time_t& t,
        StateVector<STATE_DIM, SCALAR>& derivative) override
    {
        if (controller_)
            controller_->computeControl(state, t, controlAction_);
        else
            controlAction_.setZero();

        computeControlledDynamics(state, t, controlAction_, derivative);
    }


    virtual void computeControlledDynamics(const StateVector<STATE_DIM, SCALAR>& state,
        const time_t& t,
        const ControlVector<CONTROL_DIM, SCALAR>& control,
        StateVector<STATE_DIM, SCALAR>& derivative) = 0;

    ControlVector<CONTROL_DIM, SCALAR> getLastControlAction() { return controlAction_; }
protected:
    std::shared_ptr<Controller<STATE_DIM, CONTROL_DIM, SCALAR>> controller_;  

    ControlVector<CONTROL_DIM, SCALAR> controlAction_;
};
}
}