TestDiscreteNonlinearSystemLinearized.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 <ct/core/core.h>

namespace ct {
namespace core {
namespace generated {

class TestDiscreteNonlinearSystemLinearized : public ct::core::DiscreteLinearSystem<2, 1, double>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef ct::core::DiscreteLinearSystem<2, 1, double> Base;

    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;

    TestDiscreteNonlinearSystemLinearized(const ct::core::SYSTEM_TYPE& type = ct::core::SYSTEM_TYPE::GENERAL) : Base(type)
    {
        initialize();
    }

    TestDiscreteNonlinearSystemLinearized(const TestDiscreteNonlinearSystemLinearized& other) { initialize(); }
    virtual ~TestDiscreteNonlinearSystemLinearized(){};

    virtual TestDiscreteNonlinearSystemLinearized* clone() const override { return new TestDiscreteNonlinearSystemLinearized; }
    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
    {
        getDerivativeState(x, u, n);
        getDerivativeControl(x, u, n);

        A = dFdx_;
        B = dFdu_;
    }

    virtual const state_matrix_t& getDerivativeState(const state_vector_t& x,
        const control_vector_t& u,
        const int t = 0);

    virtual const state_control_matrix_t& getDerivativeControl(const state_vector_t& x,
        const control_vector_t& u,
        const int t = 0);

private:
    void initialize()
    {
        dFdx_.setZero();
        dFdu_.setZero();
        vX_.fill(0.0);
        vU_.fill(0.0);
    }

    state_matrix_t dFdx_;
    state_control_matrix_t dFdu_;
    std::array<double, 0> vX_;
    std::array<double, 0> vU_;
};

}  // namespace generated
}  // namespace core
}  // namespace ct