IntegratorSymplectic.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 <boost/numeric/odeint.hpp>

#include "eigenIntegration.h"

#include <ct/core/systems/continuous_time/SymplecticSystem.h>

#include "internal/SteppersODEIntDefinitions.h"


namespace ct {
namespace core {

template <size_t POS_DIM, size_t VEL_DIM, size_t CONTROL_DIM, class Stepper, typename SCALAR = double>
class IntegratorSymplectic
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef typename std::pair<Eigen::Matrix<SCALAR, POS_DIM, 1>, Eigen::Matrix<SCALAR, VEL_DIM, 1>> pair_t;

    typedef std::shared_ptr<EventHandler<POS_DIM + VEL_DIM, SCALAR>> EventHandlerPtr;
    typedef std::vector<EventHandlerPtr, Eigen::aligned_allocator<EventHandlerPtr>> EventHandlerPtrVector;


    IntegratorSymplectic(const std::shared_ptr<SymplecticSystem<POS_DIM, VEL_DIM, CONTROL_DIM, SCALAR>> system,
        const EventHandlerPtrVector& eventHandlers = EventHandlerPtrVector(0));

    IntegratorSymplectic(const std::shared_ptr<SymplecticSystem<POS_DIM, VEL_DIM, CONTROL_DIM, SCALAR>> system,
        const EventHandlerPtr& eventHandler);


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

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

    void reset();

private:
    void setupSystem();
    StateVector<POS_DIM + VEL_DIM, SCALAR> xCached_;  

    std::function<void(const Eigen::Matrix<SCALAR, POS_DIM, 1>&, Eigen::Matrix<SCALAR, POS_DIM, 1>&)>
        systemFunctionPosition_;  
    std::function<void(const Eigen::Matrix<SCALAR, VEL_DIM, 1>&, Eigen::Matrix<SCALAR, VEL_DIM, 1>&)>
        systemFunctionVelocity_;  

    std::shared_ptr<SymplecticSystem<POS_DIM, VEL_DIM, CONTROL_DIM, SCALAR>> systemSymplectic_;

    Stepper stepper_;

    Observer<POS_DIM + VEL_DIM, SCALAR> observer_;  
};


/*******************************************************************
 * Defining the integrators
 *******************************************************************/

template <size_t POS_DIM, size_t VEL_DIM, size_t CONTROL_DIM, typename SCALAR = double>
using IntegratorSymplecticEuler =
    IntegratorSymplectic<POS_DIM, VEL_DIM, CONTROL_DIM, internal::symplectic_euler_t<POS_DIM, VEL_DIM, SCALAR>, SCALAR>;

template <size_t POS_DIM, size_t VEL_DIM, size_t CONTROL_DIM, typename SCALAR = double>
using IntegratorSymplecticRk =
    IntegratorSymplectic<POS_DIM, VEL_DIM, CONTROL_DIM, internal::symplectic_rk_t<POS_DIM, VEL_DIM, SCALAR>, SCALAR>;
}
}