Implementing a Controller

To close the loop a controller has to be added. This can easily be done by deriving from the Controller class. Its task is to stabilize the pendulum by calculating a suitable input for the model. To keep things simple, the linear state-feedback controller in this scenario and it is based on the linearized system which is given by

\boldsymbol{A} = \begin{pmatrix} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & \frac{m^{2}l^{2}g}{z} & -\frac{JD}{z} & \frac{mld}{z}\\ 0 & -\frac{(M+m)mlg}{z} & \frac{mlD}{z} & -\frac{(M+m)d}{z}\\ \end{pmatrix} \ \ \boldsymbol{B} = \begin{pmatrix} 0\\ 0\\ \frac{J}{z}\\ -\frac{ml}{z}\\ \end{pmatrix} \ \ \boldsymbol{C} = \begin{pmatrix} 1 & 0 & 0 & 0\\ \end{pmatrix}

with

z = J (M+m) - m^{2}l^{2}.

The linear control law is given by

u = -\boldsymbol{K} \boldsymbol{x} + \boldsymbol{V} \boldsymbol{y_d}

with the control gain \boldsymbol{K} and the prefilter \boldsymbol{V}. One possibility to calculate the control gain is by using the Ackermann formula.

With all necessary equations, the implementation of the controller class can begin. Start by creating a file called:

controller.py

Import the same classes as in the model class:

1# -*- coding: utf-8 -*-
2import numpy as np
3from collections import OrderedDict
4
5import pymoskito as pm
6
7

Derive your controller from Controller Next, create public_settings as before in the model. Its entries will be accessable in the graphical interface of PyMoskito during runtime. This time, the only parameters will be the desired poles of the closed loop, which the controller shall establish:

 9class BasicController(pm.Controller):
10    public_settings = OrderedDict([("poles", [-2, -2, -2, -2])
11                                   ])
12

Within the constructor, it is obligatory to set the input order and an input type. The input order determines how many derivatives of the trajectory will be required, sinc eour controller is very simple a 0 will do here. Valid entries for input type are Model_State, Model_Output, Observer and Sensor. In our case we will go for Model_State. After all necessary updates, call the constructor of the base class as seen in line 20. Store the linearized system matrices and the equilibrium state. To make matrix operations possible, use the array type provided by NumPy. PyMoskito’s Controltools provide functions to calculate the values of a linear state feedback and a prefilter, which can be used as seen in lines 49-50. The method place_siso() is an implementation of the Ackermann formula:

14    def __init__(self, settings):
15        settings.update(input_order=0)
16        settings.update(input_type="Model_State")
17
18        pm.Controller.__init__(self, settings)
19
20        # model parameters
21        g = 9.81  # gravity [m/s^2]
22        M = 4.2774  # cart mass [kg]
23        D = 10  # cart friction constant [Ns/m]
24        m = 0.3211  # pendulum mass [kg]
25        d = 0.023  # pendulum friction constant [Nms]
26        l = 0.3533  # pendulum length [m]
27        J = 0.072  # pendulum moment of intertia [kg*m^2]
28
29        # the system matrices after linearization in phi=PI
30        z = (M + m) * J - (m * l) ** 2
31        A = np.array([[0, 0, 1, 0],
32                      [0, 0, 0, 1],
33                      [0, (m * l) ** 2 * g / z, -J * D / z, m * l * d / z],
34                      [0, -(M + m) * m * l * g / z, m * l * D / z,
35                       -(M + m) * d / z]
36                      ])
37        B = np.array([[0],
38                      [0],
39                      [J / z],
40                      [-l * m / z]
41                      ])
42        C = np.array([[1, 0, 0, 0]])
43
44        # the equilibrium state as a vector
45        self._eq_state = np.array([0, np.pi, 0, 0])
46
47        # pole placement of linearized state feedback
48        self._K = pm.controltools.place_siso(A, B, self._settings["poles"])
49        self._V = pm.controltools.calc_prefilter(A, B, C, self._K)
50

That would be all for the constructor. The only other method left to implement contains the actual control law and will be called by the solver during runtime. Its parameters are the current time, the current values of trajectory, feedforward and controller input. The parameter **kwargs holds further information, which is explained in pymoskito.simulation_modules.Controller. For our example, we will just ignore it. Since this controller will be stabilizing the system in the steady state [0,0,0,0], it has to be subtracted to work on the small signal scale.

52    def _control(self, time, trajectory_values=None, feedforward_values=None,
53                 input_values=None, **kwargs):
54        x = input_values - self._eq_state
55        yd = trajectory_values - self._eq_state[0]
56        output = - np.dot(self._K, x) + np.dot(self._V, yd[0])
57
58        return output

Finally, import the controller file and register the controller class to PyMoskito by adding two lines to the main.py file as done before with the model class. Your main.py should now look like this, with the changed lines highlighted:

 1# -*- coding: utf-8 -*-
 2import pymoskito as pm
 3
 4# import custom modules
 5import model
 6import controller
 7
 8
 9if __name__ == '__main__':
10    # register model
11    pm.register_simulation_module(pm.Model, model.PendulumModel)
12
13    # register controller
14    pm.register_simulation_module(pm.Controller, controller.BasicController)
15
16    # start the program
17    pm.run()

Having put all pieces together, we are now ready to run our scenario.