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
with
The linear control law is given by
with the control gain and the prefilter . 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 2 3 4 5 6 7 | # -*- coding: utf-8 -*-
import numpy as np
from collections import OrderedDict
import pymoskito as pm
|
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:
9 10 11 12 | class BasicController(pm.Controller):
public_settings = OrderedDict([("poles", [-2, -2, -2, -2])
])
|
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 system_state, system_output,
Observer and Sensor. In our case we will go for system_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 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 | def __init__(self, settings):
settings.update(input_order=0)
settings.update(input_type="system_state")
pm.Controller.__init__(self, settings)
# model parameters
g = 9.81 # gravity [m/s^2]
M = 4.2774 # cart mass [kg]
D = 10 # cart friction constant [Ns/m]
m = 0.3211 # pendulum mass [kg]
d = 0.023 # pendulum friction constant [Nms]
l = 0.3533 # pendulum length [m]
J = 0.072 # pendulum moment of intertia [kg*m^2]
# the system matrices after linearization in phi=PI
z = (M + m) * J - (m * l) ** 2
A = np.array([[0, 0, 1, 0],
[0, 0, 0, 1],
[0, (m * l) ** 2 * g / z, -J * D / z, m * l * d / z],
[0, -(M + m) * m * l * g / z, m * l * D / z,
-(M + m) * d / z]
])
B = np.array([[0],
[0],
[J / z],
[-l * m / z]
])
C = np.array([[1, 0, 0, 0]])
# the equilibrium state as a vector
self._eq_state = np.array([0, np.pi, 0, 0])
# pole placement of linearized state feedback
self._K = pm.controltools.place_siso(A, B, self._settings["poles"])
self._V = pm.controltools.calc_prefilter(A, B, C, self._K)
|
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 53 54 55 56 57 58 | def _control(self, time, trajectory_values=None, feedforward_values=None,
input_values=None, **kwargs):
x = input_values - self._eq_state
yd = trajectory_values - self._eq_state[0]
output = - np.dot(self._K, x) + np.dot(self._V, yd[0])
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 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 | # -*- coding: utf-8 -*-
import pymoskito as pm
# import custom modules
import model
import controller
if __name__ == '__main__':
# register model
pm.register_simulation_module(pm.Model, model.PendulumModel)
# register controller
pm.register_simulation_module(pm.Controller, controller.BasicController)
# start the program
pm.run()
|
Having put all pieces together, we are now ready to run our scenario.