Integrator.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 <type_traits>
#include <functional>
#include <cmath>

#include "EventHandler.h"
#include "Observer.h"
#include "eigenIntegration.h"

#include "internal/StepperBase.h"

#include "internal/SteppersODEInt.h"
#include "internal/SteppersCT.h"

#include <ct/core/types/AutoDiff.h>

namespace ct {
namespace core {


enum IntegrationType
{
    EULER,
    RK4,
    MODIFIED_MIDPOINT,
    ODE45,
    RK5VARIABLE,
    RK78,
    BULIRSCHSTOER,
    EULERCT,
    RK4CT,
    EULER_SYM,
    RK_SYM
};



template <size_t STATE_DIM, typename SCALAR = double>
class Integrator
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef std::shared_ptr<EventHandler<STATE_DIM, SCALAR>> EventHandlerPtr;
    typedef std::vector<EventHandlerPtr, Eigen::aligned_allocator<EventHandlerPtr>> EventHandlerPtrVector;


    Integrator(const std::shared_ptr<System<STATE_DIM, SCALAR>>& system,
        const IntegrationType& intType = IntegrationType::EULERCT,
        const EventHandlerPtrVector& eventHandlers = EventHandlerPtrVector(0));

    Integrator(const std::shared_ptr<System<STATE_DIM, SCALAR>>& system,
        const IntegrationType& intType,
        const EventHandlerPtr& eventHandler);

    void changeIntegrationType(const IntegrationType& intType);


    void setApadativeErrorTolerances(const SCALAR absErrTol, const SCALAR& relErrTol);


    void integrate_n_steps(StateVector<STATE_DIM, SCALAR>& state,
        const SCALAR& startTime,
        size_t numSteps,
        SCALAR dt,
        StateVectorArray<STATE_DIM, SCALAR>& stateTrajectory,
        tpl::TimeArray<SCALAR>& timeTrajectory);


    void integrate_n_steps(StateVector<STATE_DIM, SCALAR>& state, const SCALAR& startTime, size_t numSteps, SCALAR dt);


    void integrate_const(StateVector<STATE_DIM, SCALAR>& state,
        const SCALAR& startTime,
        const SCALAR& finalTime,
        SCALAR dt,
        StateVectorArray<STATE_DIM, SCALAR>& stateTrajectory,
        tpl::TimeArray<SCALAR>& timeTrajectory);


    void integrate_const(StateVector<STATE_DIM, SCALAR>& state,
        const SCALAR& startTime,
        const SCALAR& finalTime,
        SCALAR dt);


    void integrate_adaptive(StateVector<STATE_DIM, SCALAR>& state,
        const SCALAR& startTime,
        const SCALAR& finalTime,
        StateVectorArray<STATE_DIM, SCALAR>& stateTrajectory,
        tpl::TimeArray<SCALAR>& timeTrajectory,
        const SCALAR dtInitial = SCALAR(0.01));


    void integrate_adaptive(StateVector<STATE_DIM, SCALAR>& state,
        const SCALAR& startTime,
        const SCALAR& finalTime,
        SCALAR dtInitial = SCALAR(0.01));


    void integrate_times(StateVector<STATE_DIM, SCALAR>& state,
        const tpl::TimeArray<SCALAR>& timeTrajectory,
        StateVectorArray<STATE_DIM, SCALAR>& stateTrajectory,
        SCALAR dtInitial = SCALAR(0.01));

private:
    void initializeCTSteppers(const IntegrationType& intType);
    template <typename S = SCALAR>
    typename std::enable_if<std::is_same<S, double>::value, void>::type initializeAdaptiveSteppers(
        const IntegrationType& intType)
    {
        switch (intType)
        {
            case ODE45:
            {
                integratorStepper_ =
                    std::shared_ptr<internal::StepperODEIntControlled<internal::runge_kutta_dopri5_t<STATE_DIM, SCALAR>,
                        Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>>(
                        new internal::StepperODEIntControlled<internal::runge_kutta_dopri5_t<STATE_DIM, SCALAR>,
                            Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>());
                break;
            }

            case RK5VARIABLE:
            {
                integratorStepper_ = std::shared_ptr<internal::StepperODEIntDenseOutput<
                    internal::runge_kutta_dopri5_t<STATE_DIM, SCALAR>, Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>>(
                    new internal::StepperODEIntDenseOutput<internal::runge_kutta_dopri5_t<STATE_DIM, SCALAR>,
                        Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>());
                break;
            }

            case BULIRSCHSTOER:
            {
                integratorStepper_ =
                    std::shared_ptr<internal::StepperODEInt<internal::bulirsch_stoer_t<STATE_DIM, SCALAR>,
                        Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>>(
                        new internal::StepperODEInt<internal::bulirsch_stoer_t<STATE_DIM, SCALAR>,
                            Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>());
                break;
            }
            default:
                break;
        }
    }

    template <typename S = SCALAR>
    typename std::enable_if<!std::is_same<S, double>::value, void>::type initializeAdaptiveSteppers(
        const IntegrationType& intType)
    {
    }

#ifdef CPPADCG
    template <typename S = SCALAR>
    typename std::enable_if<std::is_same<S, ADCGScalar>::value, void>::type initializeODEIntSteppers(
        const IntegrationType& intType)
    {
    }
#endif

    template <typename S = SCALAR>
    typename std::enable_if<std::is_same<S, double>::value, void>::type initializeODEIntSteppers(
        const IntegrationType& intType)
    {
        switch (intType)
        {
            case EULER:
            {
                integratorStepper_ = std::shared_ptr<internal::StepperODEInt<internal::euler_t<STATE_DIM, SCALAR>,
                    Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>>(
                    new internal::StepperODEInt<internal::euler_t<STATE_DIM, SCALAR>,
                        Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>());
                break;
            }

            case RK4:
            {
                integratorStepper_ =
                    std::shared_ptr<internal::StepperODEInt<internal::runge_kutta_4_t<STATE_DIM, SCALAR>,
                        Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>>(
                        new internal::StepperODEInt<internal::runge_kutta_4_t<STATE_DIM, SCALAR>,
                            Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>());
                break;
            }

            case MODIFIED_MIDPOINT:
            {
                integratorStepper_ =
                    std::shared_ptr<internal::StepperODEInt<internal::modified_midpoint_t<STATE_DIM, SCALAR>,
                        Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>>(
                        new internal::StepperODEInt<internal::modified_midpoint_t<STATE_DIM, SCALAR>,
                            Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>());
                break;
            }

            case RK78:
            {
                integratorStepper_ =
                    std::shared_ptr<internal::StepperODEInt<internal::runge_kutta_fehlberg78_t<STATE_DIM, SCALAR>,
                        Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>>(
                        new internal::StepperODEInt<internal::runge_kutta_fehlberg78_t<STATE_DIM, SCALAR>,
                            Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>());

                break;
            }
            default:
                break;
        }
    }


    void reset();

    void retrieveTrajectoriesFromObserver(StateVectorArray<STATE_DIM, SCALAR>& stateTrajectory,
        tpl::TimeArray<SCALAR>& timeTrajectory);
    void retrieveStateVectorArrayFromObserver(StateVectorArray<STATE_DIM, SCALAR>& stateTrajectory);

    void setupSystem();

    std::shared_ptr<System<STATE_DIM, SCALAR>> system_;  
    std::function<void(const Eigen::Matrix<SCALAR, STATE_DIM, 1>&, Eigen::Matrix<SCALAR, STATE_DIM, 1>&, SCALAR)>
        systemFunction_;  
    std::shared_ptr<internal::StepperBase<Eigen::Matrix<SCALAR, STATE_DIM, 1>, SCALAR>> integratorStepper_;
    Observer<STATE_DIM, SCALAR> observer_;  
};
}
}