Sensitivity.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 {

struct SensitivityApproximationSettings
{
    enum class APPROXIMATION
    {
        FORWARD_EULER = 0,
        BACKWARD_EULER,
        SYMPLECTIC_EULER,
        TUSTIN,
        MATRIX_EXPONENTIAL
    };

    SensitivityApproximationSettings(double dt, APPROXIMATION approx) : dt_(dt), approximation_(approx) {}
    double dt_;

    APPROXIMATION approximation_;
};

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

    typedef std::shared_ptr<StateVectorArray<STATE_DIM, SCALAR>> StateVectorArrayPtr;
    typedef std::shared_ptr<ControlVectorArray<CONTROL_DIM, SCALAR>> ControlVectorArrayPtr;


    Sensitivity() : xSubstep_(nullptr), uSubstep_(nullptr) {}
    virtual ~Sensitivity() {}
    virtual Sensitivity<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
    {
        throw std::runtime_error("clone not implemented for Sensitivity");
    }

    virtual void setLinearSystem(const std::shared_ptr<LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& linearSystem) = 0;

    virtual void setTimeDiscretization(const SCALAR& dt) = 0;

    virtual void setApproximation(const SensitivityApproximationSettings::APPROXIMATION& approx) {}
    void setSubstepTrajectoryReference(
        std::vector<StateVectorArrayPtr, Eigen::aligned_allocator<StateVectorArrayPtr>>* xSubstep,
        std::vector<ControlVectorArrayPtr, Eigen::aligned_allocator<ControlVectorArrayPtr>>* uSubstep)
    {
        xSubstep_ = xSubstep;
        uSubstep_ = uSubstep;
    }

    virtual void getAandB(const StateVector<STATE_DIM, SCALAR>& x,
        const ControlVector<CONTROL_DIM, SCALAR>& u,
        const StateVector<STATE_DIM, SCALAR>& x_next,
        const int n,
        size_t numSteps,
        StateMatrix<STATE_DIM, SCALAR>& A,
        StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR>& B) override = 0;

protected:
    std::vector<StateVectorArrayPtr, Eigen::aligned_allocator<StateVectorArrayPtr>>* xSubstep_;
    std::vector<ControlVectorArrayPtr, Eigen::aligned_allocator<ControlVectorArrayPtr>>* uSubstep_;
};
}
}