SystemDiscretizer.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 "DiscreteControlledSystem.h"
#include "../continuous_time/ControlledSystem.h"
#include <ct/core/control/continuous_time/ConstantController.h>

#include <ct/core/integration/Integrator.h>
#include <ct/core/integration/IntegratorSymplectic.h>
#include <ct/core/integration/EventHandlers/SubstepRecorder.h>

#define SYMPLECTIC_ENABLED                   \
    template <size_t V, size_t P, size_t ST> \
    typename std::enable_if<(V > 0 && P > 0 && (V + P == ST)), void>::type
#define SYMPLECTIC_DISABLED                  \
    template <size_t V, size_t P, size_t ST> \
    typename std::enable_if<(V <= 0 || P <= 0 || (V + P != ST)), void>::type


namespace ct {
namespace core {


template <size_t STATE_DIM,
    size_t CONTROL_DIM,
    size_t P_DIM = STATE_DIM / 2,
    size_t V_DIM = STATE_DIM / 2,
    typename SCALAR = double>
class SystemDiscretizer : public DiscreteControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>
{
public:
    // convenience typedefs
    typedef DiscreteControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR> Base;
    typedef typename Base::time_t time_t;

    using IntegratorPtr = std::shared_ptr<Integrator<STATE_DIM, SCALAR>>;
    using IntegratorSymplecticEulerPtr =
        std::shared_ptr<ct::core::IntegratorSymplecticEuler<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>;
    using IntegratorSymplecticRkPtr =
        std::shared_ptr<ct::core::IntegratorSymplecticRk<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>;

    using ContinuousSystemPtr = std::shared_ptr<ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>>;
    using ContinuousConstantControllerPtr = std::shared_ptr<ConstantController<STATE_DIM, CONTROL_DIM, SCALAR>>;

    using SubstepRecorderPtr = std::shared_ptr<ct::core::SubstepRecorder<STATE_DIM, CONTROL_DIM, SCALAR>>;

    using StateVectorArray = ct::core::StateVectorArray<STATE_DIM, SCALAR>;
    using StateVectorArrayPtr = std::shared_ptr<StateVectorArray>;
    using ControlVectorArray = ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>;
    using ControlVectorArrayPtr = std::shared_ptr<ControlVectorArray>;

    SystemDiscretizer();


    SystemDiscretizer(const SCALAR& dt,
        const ct::core::IntegrationType& integratorType = ct::core::IntegrationType::RK4,
        const int& K_sim = 1);


    SystemDiscretizer(ContinuousSystemPtr system,
        const SCALAR& dt,
        const ct::core::IntegrationType& integratorType = ct::core::IntegrationType::RK4,
        const int& K_sim = 1);

    SystemDiscretizer(const SystemDiscretizer& arg);

    virtual ~SystemDiscretizer();

    virtual SystemDiscretizer<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>* clone() const override;

    void initialize();

    void setIntegrationType(const ct::core::IntegrationType& integratorType);

    void setParameters(const SCALAR& dt, const int& K_sim = 1);

    void changeContinuousTimeSystem(ContinuousSystemPtr newSystem);


    virtual void propagateControlledDynamics(const StateVector<STATE_DIM, SCALAR>& state,
        const time_t n,
        const ControlVector<CONTROL_DIM, SCALAR>& control,
        StateVector<STATE_DIM, SCALAR>& stateNext) override;

    const StateVectorArrayPtr& getSubstates() const;

    const ControlVectorArrayPtr& getSubcontrols() const;

protected:
    SYMPLECTIC_ENABLED initializeSymplecticIntegrator();

    SYMPLECTIC_DISABLED initializeSymplecticIntegrator();

    SYMPLECTIC_ENABLED integrateSymplectic(ct::core::StateVector<STATE_DIM, SCALAR>& x0,
        const double& t,
        const size_t& steps,
        const double& dt_sim) const;

    SYMPLECTIC_DISABLED integrateSymplectic(ct::core::StateVector<STATE_DIM, SCALAR>& x0,
        const double& t,
        const size_t& steps,
        const double& dt_sim) const;

    SCALAR getSimulationTimestep();

    SCALAR dt_;

    int K_sim_;

    SCALAR dt_sim_;

    ct::core::IntegrationType integratorType_;

    ContinuousSystemPtr cont_time_system_;

    ContinuousConstantControllerPtr cont_constant_controller_;

    IntegratorPtr integrator_;

    IntegratorSymplecticEulerPtr integratorEulerSymplectic_;

    IntegratorSymplecticRkPtr integratorRkSymplectic_;

    SubstepRecorderPtr substepRecorder_;
};


}  // namespace core
}  // namespace ct

#undef SYMPLECTIC_ENABLED
#undef SYMPLECTIC_DISABLED