DiscreteSystemLinearizerAD.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

#ifdef CPPAD

namespace ct {
namespace core {


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

    typedef DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR> Base;

    typedef CppAD::AD<SCALAR> ADScalar;
    typedef DiscreteControlledSystem<STATE_DIM, CONTROL_DIM, ADScalar> system_t;  
    typedef DynamicsLinearizerAD<STATE_DIM, CONTROL_DIM, ADScalar, int>
        linearizer_t;  

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


    DiscreteSystemLinearizerAD(std::shared_ptr<system_t> nonlinearSystem)
        : Base(nonlinearSystem->getType()),
          dFdx_(state_matrix_t::Zero()),
          dFdu_(state_control_matrix_t::Zero()),
          nonlinearSystem_(nonlinearSystem),
          linearizer_(std::bind(&system_t::propagateControlledDynamics,
              nonlinearSystem_.get(),
              std::placeholders::_1,
              std::placeholders::_2,
              std::placeholders::_3,
              std::placeholders::_4))
    {
    }

    DiscreteSystemLinearizerAD(const DiscreteSystemLinearizerAD& arg)
        : Base(arg.nonlinearSystem_->getType()),
          dFdx_(arg.dFdx_),
          dFdu_(arg.dFdu_),
          nonlinearSystem_(arg.nonlinearSystem_->clone()),
          linearizer_(std::bind(&system_t::propagateControlledDynamics,
              nonlinearSystem_.get(),
              std::placeholders::_1,
              std::placeholders::_2,
              std::placeholders::_3,
              std::placeholders::_4))
    {
    }

    virtual ~DiscreteSystemLinearizerAD() {}
    DiscreteSystemLinearizerAD<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
    {
        return new DiscreteSystemLinearizerAD<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
    }


    const state_matrix_t& getDerivativeState(const state_vector_t& x, const control_vector_t& u, const int t = 0)
    {
        dFdx_ = linearizer_.getDerivativeState(x, u, t);
        return dFdx_;
    }



    const state_control_matrix_t& getDerivativeControl(const state_vector_t& x,
        const control_vector_t& u,
        const int t = 0)
    {
        dFdu_ = linearizer_.getDerivativeControl(x, u, t);
        return dFdu_;
    }


    void getAandB(const state_vector_t& x,
        const control_vector_t& u,
        const state_vector_t& x_next,
        const int n,
        size_t numSteps,
        state_matrix_t& A,
        state_control_matrix_t& B) override
    {
        dFdx_ = linearizer_.getDerivativeState(x, u, n);
        dFdu_ = linearizer_.getDerivativeControl(x, u, n);

        A = dFdx_;
        B = dFdu_;
    }

protected:
    state_matrix_t dFdx_;
    state_control_matrix_t dFdu_;

    std::shared_ptr<system_t> nonlinearSystem_;  

    linearizer_t linearizer_;  
};

}  // namespace core
}  // namespace ct

#endif