Feedback Control Tutorial

Table of Contents

• PID Control
• Custom Controller

In this tutorial, we will take a look how to implement and simulate simple (feedback) controllers.

PID Control

First, we will implement a controller based on a simple PID Controller. There are at least three different ways to realize a PID Controller in CT. While there is a ct::core::PIDController, it is only SISO. However, CT assumes all systems and controllers are MIMO. Therefore, if you want to use the pre-implemented ct::core::PIDController, you will have to provide a mapping from states to PIDController input and from the output to the control input. Alternatively, you can implement your own Controller class which could utilize the ct::core::PIDController An example is shown in the Custom Controller Tutorial. The third and easiest option is to implement the PID controller as a linear state feedback controller of the form . Herefore, we can use ct::core::StateFeedbackController.

Custom Controller

In this example, we create our own implementation of a controller. We create a controller that has two states and one control input. Our control structure will be a PD controller with a feedforward control action. To implement a controller, we derive from ct::core::Controller. As we will see in a bit, this will ensure our feedback controller is interoperable with the rest of the Control Toolbox. Our CustomController.h looks like the following

/**********************************************************************************************************************
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

class CustomController : public ct::core::Controller<2, 1>
{
public:
    static const size_t state_dim = 2;    // two states
    static const size_t control_dim = 1;  // one control action

    CustomController(const ct::core::ControlVector<control_dim>& uff,  // feedforward control
        const double& kp,                                              // P gain
        const double& kd                                               // D gain
        )
        : uff_(uff), kp_(kp), kd_(kd)
    {
    }

    ~CustomController() {}
    CustomController(const CustomController& other) : uff_(other.uff_), kp_(other.kp_), kd_(other.kd_) {}
    CustomController* clone() const override
    {
        return new CustomController(*this);  // calls copy constructor
    }

    void computeControl(const ct::core::StateVector<state_dim>& state,
        const double& t,
        ct::core::ControlVector<control_dim>& controlAction) override
    {
        controlAction = uff_;                                 // apply feedforward control
        controlAction(0) -= kp_ * state(0) + kd_ * state(1);  // add feedback control
    }

private:
    ct::core::ControlVector<control_dim> uff_;  
    double kp_;                                 
    double kd_;                                 
};

Note

Any ct::core::Integrator will evaluate the controller at every integration step. Therefore, if you wish to implement a Controller with a fixed or lower sampling rate, you will have to implement this in your Controller class.

We can now use this controller to control and simulate a damped oscillator again

/**********************************************************************************************************************
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)
**********************************************************************************************************************/
#include <ct/core/core.h>
#include <ct/core/examples/CustomController.h>

int main(int argc, char** argv)
{
    // a damped oscillator has two states, position and velocity
    const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;      // = 2
    const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;  // = 1

    // create a state
    ct::core::StateVector<state_dim> x;

    // we initialize it at a point with unit deflection and zero velocity
    x(0) = 1.0;
    x(1) = 0.0;

    // create our oscillator
    double w_n = 50;
    std::shared_ptr<ct::core::SecondOrderSystem> oscillator(new ct::core::SecondOrderSystem(w_n));

    // create our controller
    double kp = 10;
    double kd = 1;
    ct::core::ControlVector<control_dim> uff;
    uff << 2.0;
    std::shared_ptr<CustomController> controller(new CustomController(uff, kp, kd));

    // assign our controller
    oscillator->setController(controller);

    // create an integrator
    ct::core::Integrator<state_dim> integrator(oscillator, ct::core::IntegrationType::RK4);

    // simulate 1000 steps
    double dt = 0.001;
    ct::core::Time t0 = 0.0;
    size_t nSteps = 1000;
    integrator.integrate_n_steps(x, t0, nSteps, dt);

    // print the new state
    std::cout << "state after integration: " << x.transpose() << std::endl;

    return 0;
}

You can run this example with the following command

rosrun ct_core ex_DampedOscillatorCustomController

Note

Make sure you have built the examples before trying to run it.