SwitchedDiscreteLinearSystem.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 SwitchedDiscreteLinearSystem : public DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef typename std::shared_ptr<DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> LinearSystemPtr;
    typedef Switched<LinearSystemPtr> SwitchedLinearSystems;

    typedef DiscreteControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR> Base;
    typedef typename Base::time_t time_t;

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

    typedef StateMatrix<STATE_DIM, SCALAR> state_matrix_t;                              
    typedef StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> state_control_matrix_t;  


    SwitchedDiscreteLinearSystem(const SwitchedLinearSystems& switchedLinearSystems,
        const DiscreteModeSequence& discreteModeSequence,
        const SYSTEM_TYPE& type = SYSTEM_TYPE::GENERAL)
        : DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>(type),
          switchedLinearSystems_(switchedLinearSystems),
          discreteModeSequence_(discreteModeSequence){};

    SwitchedDiscreteLinearSystem(const SwitchedDiscreteLinearSystem& arg)
        : DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>(arg), discreteModeSequence_(arg.discreteModeSequence_)
    {
        switchedLinearSystems_.clear();
        for (auto& subSystem : arg.switchedLinearSystems_)
        {
            switchedLinearSystems_.emplace_back(subSystem->clone());
        }
    };

    virtual ~SwitchedDiscreteLinearSystem(){};

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


    virtual void propagateControlledDynamics(const state_vector_t& state,
        const time_t n,
        const control_vector_t& control,
        state_vector_t& stateNext) override
    {
        auto mode = discreteModeSequence_.getPhaseFromTime(n);
        switchedLinearSystems_[mode]->propagateControlledDynamics(state, n, control, stateNext);
    };


    virtual void getAandB(const state_vector_t& x,
        const control_vector_t& u,
        const state_vector_t& x_next,
        const int n,
        size_t subSteps,
        state_matrix_t& A,
        state_control_matrix_t& B) override
    {
        auto mode = discreteModeSequence_.getPhaseFromTime(n);
        switchedLinearSystems_[mode]->getAandB(x, u, x_next, n, subSteps, A, B);
    };

    using DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>::getAandB;

protected:
    SwitchedLinearSystems switchedLinearSystems_;  
    DiscreteModeSequence discreteModeSequence_;    
};

}  // namespace core
}  // namespace ct