SwitchedControlledSystem.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 "ControlledSystem.h"
#include <ct/core/switching/Switching.h>

namespace ct {
namespace core {


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

    typedef typename std::shared_ptr<SwitchedControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> Ptr;
    typedef typename std::shared_ptr<ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> SystemPtr;
    typedef Switched<SystemPtr> SwitchedSystems;

    typedef System<STATE_DIM, SCALAR> Base;
    typedef typename Base::time_t time_t;



    SwitchedControlledSystem(const SwitchedSystems& switchedSystems,
        const ContinuousModeSequence& continuousModeSequence,
        const SYSTEM_TYPE& type = SYSTEM_TYPE::GENERAL)
        : ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>(type),
          switchedSystems_(switchedSystems),
          continuousModeSequence_(continuousModeSequence){};


    SwitchedControlledSystem(const SwitchedSystems& switchedSystems,
        const ContinuousModeSequence& continuousModeSequence,
        std::shared_ptr<ct::core::Controller<STATE_DIM, CONTROL_DIM, SCALAR>> controller,
        const SYSTEM_TYPE& type = SYSTEM_TYPE::GENERAL)
        : ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>(controller, type),
          switchedSystems_(switchedSystems),
          continuousModeSequence_(continuousModeSequence){};

    SwitchedControlledSystem(const SwitchedControlledSystem& arg)
        : ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>(arg), continuousModeSequence_(arg.continuousModeSequence_)
    {
        switchedSystems_.clear();
        for (auto& subSystem : arg.switchedSystems_)
        {
            switchedSystems_.emplace_back(subSystem->clone());
        }
    };

    virtual ~SwitchedControlledSystem(){};

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

    virtual void computeControlledDynamics(const StateVector<STATE_DIM, SCALAR>& state,
        const time_t& t,
        const ControlVector<CONTROL_DIM, SCALAR>& control,
        StateVector<STATE_DIM, SCALAR>& derivative) override
    {
        auto mode = continuousModeSequence_.getPhaseFromTime(t);
        switchedSystems_[mode]->computeControlledDynamics(state, t, control, derivative);
    };

protected:
    SwitchedSystems switchedSystems_;                
    ContinuousModeSequence continuousModeSequence_;  
};
}
}