SecondOrderSystem.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 <cmath>
#include <memory>
#include <iostream>

#include "ControlledSystem.h"

namespace ct {
namespace core {

namespace tpl {


template <typename SCALAR>
class SecondOrderSystem : public ControlledSystem<2, 1, SCALAR>
{
public:
    static const size_t STATE_DIM = 2;    
    static const size_t CONTROL_DIM = 1;  

    typedef ControlledSystem<2, 1, SCALAR> Base;
    typedef typename Base::time_t time_t;

    SecondOrderSystem() = delete;


    SecondOrderSystem(SCALAR w_n,
        SCALAR zeta = SCALAR(1.0),
        SCALAR g_dc = SCALAR(1.0),
        std::shared_ptr<Controller<2, 1, SCALAR>> controller = nullptr)
        : ControlledSystem<2, 1, SCALAR>(controller, SYSTEM_TYPE::SECOND_ORDER),
          w_n_(w_n),
          w_n_square_(w_n_ * w_n_),
          zeta_(zeta),
          g_dc_(g_dc)
    {
    }

    SecondOrderSystem(const SecondOrderSystem& arg)
        : ControlledSystem<2, 1, SCALAR>(arg),
          w_n_(arg.w_n_),
          w_n_square_(arg.w_n_square_),
          zeta_(arg.zeta_),
          g_dc_(arg.g_dc_)
    {
    }


    SecondOrderSystem(SCALAR k, SCALAR m, SCALAR d, SCALAR g_dc, std::shared_ptr<Controller<2, 1>> controller = nullptr)
        : ControlledSystem<2, 1>(controller),
          w_n_(std::sqrt(k / m)),
          w_n_square_(w_n_ * w_n_),
          zeta_(d / (2.0 * m * k)),
          g_dc_(g_dc)
    {
    }

    SecondOrderSystem* clone() const override { return new SecondOrderSystem(*this); }
    virtual ~SecondOrderSystem() {}

    void setDynamics(SCALAR w_n, SCALAR zeta = SCALAR(1.0), SCALAR g_dc = SCALAR(1.0))
    {
        w_n_ = w_n;
        w_n_square_ = w_n_ * w_n_;
        zeta_ = zeta;
        g_dc_ = g_dc;
    }


    virtual void computeControlledDynamics(const StateVector<2, SCALAR>& state,
        const time_t& t,
        const ControlVector<1, SCALAR>& control,
        StateVector<2, SCALAR>& derivative) override
    {
        derivative(0) = state(1);
        derivative(1) = g_dc_ * control(0) - 2.0 * zeta_ * w_n_ * state(1) - w_n_square_ * state(0);
    }


    bool checkParameters()
    {
        if (zeta_ < 0)
        {
            std::cout << "Warning: Damping is negative!" << std::endl;
            return false;
        }
        if (w_n_ < 0)
        {
            std::cout << "Warning: Frequency w_n is negative!" << std::endl;
            return false;
        }
        if (g_dc_ < 0)
        {
            std::cout << "Warning: Steady state gain is negative!" << std::endl;
            return false;
        }
        if (g_dc_ == 0)
        {
            std::cout << "Warning: Steady state gain is zero!" << std::endl;
            return false;
        }

        return true;
    }

    void printSystemInfo()
    {
        std::cout << "Frequency: " << w_n_ << std::endl;
        std::cout << "Zeta: " << zeta_ << std::endl;
        std::cout << "DC gain: " << g_dc_ << std::endl;

        std::cout << "System is ";
        if (zeta_ == 0.0)
        {
            std::cout << "undamped" << std::endl;
        }
        if (zeta_ == 1.0)
        {
            std::cout << "critically damped" << std::endl;
        }
        if (zeta_ > 1.0)
        {
            std::cout << "overdamped" << std::endl;
        }
        if (zeta_ < 1.0)
        {
            std::cout << "underdamped" << std::endl;
        }
    }

private:
    SCALAR w_n_;         
    SCALAR w_n_square_;  
    SCALAR zeta_;        
    SCALAR g_dc_;        
};

}  // namespace tpl

typedef tpl::SecondOrderSystem<double> SecondOrderSystem;  

}  // namespace core
}  // namespace ct