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


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

    typedef LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR> Base;  
    
    typedef typename Base::time_t time_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;  

    typedef ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR> system_t;  



    SystemLinearizer(std::shared_ptr<system_t> nonlinearSystem, bool doubleSidedDerivative = true)
        : Base(nonlinearSystem->getType()),
          nonlinearSystem_(nonlinearSystem),
          linearizer_(std::bind(&system_t::computeControlledDynamics,
                          nonlinearSystem_.get(),
                          std::placeholders::_1,
                          std::placeholders::_2,
                          std::placeholders::_3,
                          std::placeholders::_4),
              doubleSidedDerivative)
    {
        if (nonlinearSystem == nullptr)
            throw std::runtime_error("SystemLinearizer: Nonlinear system is nullptr!");

        if (nonlinearSystem->getType() == SECOND_ORDER)
            isSecondOrderSystem_ = true;
        else
            isSecondOrderSystem_ = false;

        dFdx_.setZero();
        dFdu_.setZero();
    }

    SystemLinearizer(const SystemLinearizer& arg)
        : Base(arg),
          nonlinearSystem_(arg.nonlinearSystem_->clone()),
          linearizer_(std::bind(&system_t::computeControlledDynamics,
                          nonlinearSystem_.get(),
                          std::placeholders::_1,
                          std::placeholders::_2,
                          std::placeholders::_3,
                          std::placeholders::_4),
              arg.linearizer_.getDoubleSidedDerivativeFlag()),
          dFdx_(arg.dFdx_),
          dFdu_(arg.dFdu_),
          isSecondOrderSystem_(arg.getType() == SECOND_ORDER)
    {
    }

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


    virtual const state_matrix_t& getDerivativeState(const state_vector_t& x,
        const control_vector_t& u,
        const time_t t = 0.0) override
    {
        dFdx_ = linearizer_.getDerivativeState(x, u, t);

        if (isSecondOrderSystem_)
        {
            dFdx_.template topLeftCorner<STATE_DIM / 2, STATE_DIM / 2>().setZero();
            dFdx_.template topRightCorner<STATE_DIM / 2, STATE_DIM / 2>().setIdentity();
        }

        return dFdx_;
    }



    virtual const state_control_matrix_t& getDerivativeControl(const state_vector_t& x,
        const control_vector_t& u,
        const time_t t = 0.0) override
    {
        dFdu_ = linearizer_.getDerivativeControl(x, u, t);

        if (isSecondOrderSystem_)
        {
            dFdu_.template topRows<STATE_DIM / 2>().setZero();
        }

        return dFdu_;
    }


protected:
    std::shared_ptr<system_t> nonlinearSystem_;  

    DynamicsLinearizerNumDiff<STATE_DIM, CONTROL_DIM, SCALAR, SCALAR>
        linearizer_;  

    state_matrix_t dFdx_;          
    state_control_matrix_t dFdu_;  

    bool isSecondOrderSystem_;  
};


}  // namespace core
}  // namespace ct