OptconContinuousSystemInterface.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 optcon {


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 OptconContinuousSystemInterface : public OptconSystemInterface<STATE_DIM,
                                            CONTROL_DIM,
                                            ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>,
                                            SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef OptconSystemInterface<STATE_DIM,
        CONTROL_DIM,
        ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>,
        SCALAR>
        Base;

    typedef typename Base::control_vector_t control_vector_t;
    typedef typename Base::state_vector_t state_vector_t;
    typedef typename Base::state_matrix_t state_matrix_t;
    typedef typename Base::state_control_matrix_t state_control_matrix_t;

    typedef ct::core::SystemDiscretizer<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> discretizer_t;
    typedef std::shared_ptr<discretizer_t> system_discretizer_ptr_t;

    typedef ct::core::Sensitivity<STATE_DIM, CONTROL_DIM, SCALAR> Sensitivity_t;
    typedef std::shared_ptr<Sensitivity_t> SensitivityPtr;

    typedef typename Base::StateVectorArrayPtr StateVectorArrayPtr;
    typedef typename Base::StateSubstepsPtr StateSubstepsPtr;
    typedef typename Base::ControlVectorArrayPtr ControlVectorArrayPtr;
    typedef typename Base::ControlSubstepsPtr ControlSubstepsPtr;

    typedef typename Base::optConProblem_t optConProblem_t;
    typedef typename Base::settings_t settings_t;

    OptconContinuousSystemInterface(const optConProblem_t& problem, const settings_t& settings);

    virtual ~OptconContinuousSystemInterface() {}
    virtual void initialize() override;
    virtual void configure(const settings_t& settings) override;


    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,
        const size_t threadId) override;


    virtual void propagateControlledDynamics(const state_vector_t& state,
        const time_t n,
        const control_vector_t& control,
        state_vector_t& stateNext,
        const size_t threadId) override;

    virtual void changeNonlinearSystem(const typename optConProblem_t::DynamicsPtr_t& dyn) override;
    virtual void changeLinearSystem(const typename optConProblem_t::LinearPtr_t& lin) override;

    virtual void changeNumStages(const int numStages) override;

    virtual void getSubstates(StateVectorArrayPtr& subStepsX, const size_t threadId) override;
    virtual void getSubcontrols(ControlVectorArrayPtr& subStepsU, const size_t threadId) override;

    virtual void setSubstepTrajectoryReference(const StateSubstepsPtr& xSubsteps,
        const ControlSubstepsPtr& uSubsteps,
        const size_t threadId) override;

private:
    std::vector<system_discretizer_ptr_t, Eigen::aligned_allocator<system_discretizer_ptr_t>>
        discretizers_;  

    std::vector<SensitivityPtr, Eigen::aligned_allocator<SensitivityPtr>>
        sensitivity_;  
};

}  // namespace optcon
}  // namespace ct