Book Python For Control

Python for control purposes
Prof. Roberto Bucher

Scuola Universitaria Professionale della Svizzera Italiana
Dipartimento Tecnologie Innovative
6928 Manno
roberto.bucher@supsi.ch
May 1, 2016
2
Contents
1 Introduction 9
1.1 Install the packages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.2 The simplest way . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3 Linux . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.4 Windows . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.5 Mac OSX . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2 Python - Some hints for Matlab users 11

2.1 Basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2 The python shell . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3 Python vs. Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.4 List, array and matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.5 List . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.6 Arrays . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.7 Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.8 Indexing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.9 Lists . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.10 Arrays . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.11 Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.12 Multidimensional arrays and matrices . . . . . . . . . . . . . . . . . . . . . . . . 17
3 The Python Control System toolbox 19

3.1 Basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.2 Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.3 Continuous systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.4 State-space representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.5 Transfer function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.6 Zeros-Poles-Gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.7 Discrete time systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.8 State-space representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.9 Transfer function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.10 Conversions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.11 Casting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.12 Models interconnection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3
4 CONTENTS
4 System analysis 25
4.1 Time response . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.2 Frequency analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.3 Poles, zeros and root locus analysis . . . . . . . . . . . . . . . . . . . . . . . . . 32
5 Modeling 35
5.1 Model of a DC motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
5.1.1 Plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
5.1.2 Modules and constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
5.1.3 Reference frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
5.1.4 Body and inertia of the load . . . . . . . . . . . . . . . . . . . . . . . . . 36
5.1.5 Forces and torques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
5.1.6 Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.1.7 State-space matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.2 Model of the inverted pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.2.2 Frames - Car and pendulum . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.2.3 Points, bodies, masses and inertias . . . . . . . . . . . . . . . . . . . . . 40
5.2.4 Forces, frictions and gravity . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.2.5 Final model and linearized state-space matrices . . . . . . . . . . . . . . 41
5.3 Model of the Ball-on-Wheel plant . . . . . . . . . . . . . . . . . . . . . . . . . . 42
5.3.2 Reference frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.3.3 Centers of mass of the ball . . . . . . . . . . . . . . . . . . . . . . . . . . 43
5.3.4 Masses and inertias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.3.5 Forces and torques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.3.6 Kanes model and linearized state-space matrices . . . . . . . . . . . . . 44
6 Control design 47
6.1 PI+Lead design example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
6.1.1 Define the system and the project specifications . . . . . . . . . . . . . . 47
6.1.2 PI part . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
6.1.3 Lead part . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
6.1.4 Controller Gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
6.1.5 Simulation of the controlled system . . . . . . . . . . . . . . . . . . . . . 52
6.2 Discrete-state feedback controller design . . . . . . . . . . . . . . . . . . . . . . 54
6.2.1 Plant and project specifications . . . . . . . . . . . . . . . . . . . . . . . 54
6.2.2 Motor parameters identification . . . . . . . . . . . . . . . . . . . . . . . 54
6.2.3 Required modules . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
6.2.4 Function for least square identification . . . . . . . . . . . . . . . . . . . 54
6.2.5 Parameter identification . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
6.2.6 Check of the identified parameters . . . . . . . . . . . . . . . . . . . . . . 55
6.2.7 Continuous and discrete model . . . . . . . . . . . . . . . . . . . . . . . 56
6.2.8 Controller design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
6.2.9 Observer design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
6.2.10 Controller in compact form . . . . . . . . . . . . . . . . . . . . . . . . . 58
CONTENTS 5
6.2.11 Anti windup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

6.2.12 Simulation of the controlled plant . . . . . . . . . . . . . . . . . . . . . . 59
7 Hybrid simulation and code generation 61

7.1 Basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
7.2 pyEdit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
7.3 Basic operations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.4 Configuring some parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.4.1 Inserting a block . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.4.2 connecting blocks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.4.3 Inserting a node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.4.4 Deleting a block or a node . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.5 Remove a node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
8 Simulation and Code generation 65

8.1 Interface functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
8.2 The implementation functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
8.3 I/O connections . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
8.4 Translating the block list into C-code . . . . . . . . . . . . . . . . . . . . . . . . 68
8.4.1 Finding the right execution sequence . . . . . . . . . . . . . . . . . . . . 68
8.4.2 Generating the C-code . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
8.4.3 The init function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
8.4.4 The termination function . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
8.4.5 The ISR function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
8.5 The main file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
9 Example 73
9.1 The plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
9.2 The plant model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
9.3 Controller design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
9.4 Observer design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
9.5 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
9.6 Real-time controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
6 CONTENTS
List of Figures
4.1 Step response for continuous-time systems . . . . . . . . . . . . . . . . . . . . . 25

4.2 Step response for discrete-time systems . . . . . . . . . . . . . . . . . . . . . . . 26
4.3 Continuous time systems - Initial condition response . . . . . . . . . . . . . . . . 27
4.4 Continuous time systems - Impulse response . . . . . . . . . . . . . . . . . . . . 28
4.5 Continuous time systems - Generic input . . . . . . . . . . . . . . . . . . . . . . 29
4.6 Bode plot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.7 Nyquist plot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.8 Nichols plot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.9 Poles and zeros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
4.10 Root locus plot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
5.1 Inverted pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

5.2 Inverted pendulum - Real plant . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5.3 Ball-On-Wheel plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
6.1 Bode diagram of the plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

6.2 Bode diagram: G (dashed) and Gpi*G . . . . . . . . . . . . . . . . . . . . . . . 49
6.3 Bode diagram - G (dashed), Gpi*G (dotted) and Gpi*GLead*G . . . . . . . . . 51
6.4 Bode diagram - G (dashed), Gpi*G (dotted), Gpi*GLead*G (dot-dashed) and
K*Gpi*GLead*G . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
6.5 Step response of the controlled plant . . . . . . . . . . . . . . . . . . . . . . . . 53
6.6 Step response and collected data . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
6.7 Block diagram of the controlled system . . . . . . . . . . . . . . . . . . . . . . . 59
7.1 Some pyEdit blocks for control design . . . . . . . . . . . . . . . . . . . . . . . . 62

7.2 The pyEdit application . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
8.1 Window with the block libraries . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

8.2 Dialog for code generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
8.3 Simple block diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
9.1 The disks and spring plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

9.2 Anti windup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
9.3 Block diagram for the simulation . . . . . . . . . . . . . . . . . . . . . . . . . . 77
9.4 Simulation of the plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
9.5 Block diagram for the RT implementation . . . . . . . . . . . . . . . . . . . . . 78
9.6 RT execution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
7
8 LIST OF FIGURES
Chapter 1
Introduction
1.1 Install the packages

1.2 The simplest way
I prepared a VirtualBox disk image [1] with a Debian distribution and all the required packages.
VirtualBox is available for Windows, Linux, OS X and Solaris. All the features described in
this document are available.
Please contact me via email to receive the link to the zipped file.
1.3 Linux
The required modules can be simply installed using the usual package manager of the Linux
distribution. It is also possible to install the Anaconda distribution [2] for Linux to get the
basic Python modules.
It is important to check the versions of the Python modules, in particular numpy, scipy and
sympy. Old versions of these packages dont allows to perform all the tasks described in this
document. In case of old versions, it is possible to download the last versions of these packages
from the SciPy download page [3], and install them from a Linux shell.
Under Debian jessie we can use the apt manager to install the following packages:
python-numpy (Vers. 1.8.2)
python-scipy (Vers. 0.14)
python-matplotlib
python-sympy (Vers. 0.7.5)
python-setuptools
python-psutils
ipython
ipython-qtconsole
9
10 CHAPTER 1. INTRODUCTION
Under Debian and Ubuntu it is possible to check if all the required development packages are
correctly installed using the shell command
sudo apt-get build-dep python-scipy
The following packages are not available as distribution packages and should be installed sep-
arately.
The Python Control toolbox [4]
The Slycot libraries [5]
The SupsiCtrl package [6]
For the second part of the project (code generation etc.) the following packages are required
python-pyqt4
python-pyqt4-dev
python-qwt5-qt4
This features presented in the second part of this document are at present only interesting under
the Linux OS, because the real-time code is generated for a Linux PREEMPT-RT machine..
1.4 Windows
Under Windows it is sufficient to install the Anaconda package [2], to have all the python and
ipython modules installed. The Slycot libraries for Windows can be downloaded from here [7].
At present it is not possible to perform hybrid simulation and code generation under the
Windows OS.
1.5 Mac OSX

The Anaconda package [2] is available for Mac OSX. The Slycot libraries can be downloaded
from here [7].
Chapter 2
Python - Some hints for Matlab users
2.1 Basics
There are important differences between Matlab and Python. In particular, the Python ap-
proach to matrices and to indexed objects is quite different compared to Matlab.
More information about a comparison between Python and Matlab is available online at [8].
The web contains a lot of documentation about Python and its packages. In particular, the
book of David Pine [9] gives a good introduction about the features of Python for scientific
applications.
Other links present tutorials for numpy [10], scipy [11], matplotlib [12] and sympy [13].
2.2 The python shell

A Python script can run within a Python shell, but can also be launched as executable.
The basic python shell is similar to the Matlab shell without the java improvements (matlab
-nojvm).
A better shell is for example ipython. In this interactive form, when started with the extension
pylab, ipython already loads at startup a set of functions and modules.
Another interesting environment, more similar to the Matlab shell, is represented by the Spyder
application. In this application it is possible to debug scripts and functions like in the Matlab
environment.
In this document we are mostly working with ipython launched with the shell commands
ipython qtconsole --pylab inline
or
ipython --pylab
Sometimes not all the functions and modules are explicitly loaded at the beginning of the
examples. In addition, ipython implements some useful commands like for example whos and
run (for launching scripts).
In the ipython shell it is possible to start single commands, paste a set of commands or launch
a .py program using run.
11
12 CHAPTER 2. PYTHON - SOME HINTS FOR MATLAB USERS
In [ 1 ] : # s i n g l e command
In [ 2 ] : a = 5
In [ 3 ] : # p a s t e a s e t o f commands
In [ 4 ] : a=5
...: b=7
...: c=a b
...: print c
...:
35
In [ 5 ] : # run a . py f i l e
In [ 6 ] : run DCmotorKane . py
Matrix ( [ [ Dmw( t ) + k t I ( t ) ] ] )
Matrix ( [ [ J D e r i v a t i v e (w( t ) , t ) ] ] )
[ [ 0 1]
[ 0 Dm/ J ] ]
[[0]
[ kt /J ] ]
2.3 Python vs. Matlab

Differently from Matlab, Python implements more types of variables
In [ 1 ] : a=5
In [ 2 ] : b=2.7
In [ 3 ] : c = [ [ 1 , 2 , 3 ] , [ 4 , 5 , 6 ] ]
In [ 4 ] : d= Ciao
In [ 5 ] : whos
Variable Type Data / I n f o

a int 5
b float 2.7
c list n=2
d str Ciao
2.4 List, array and matrix

Python implements three kind of multidimensional objects: list, array and matrix. These
objects are handled differently than in Matlab.
2.5 List
A Python list implements the Matlab cell. It represents the simplest and default indexed
object.
2.6. ARRAYS 13
In [ 1 ] : a = [ [ [ 1 , 2 ] , [ 3 , 4 ] ] , abcd , 2 ]
In [ 2 ] : b = [ [ 1 , 2 , 3 ] , [ 4 , 5 , 6 ] , [ 7 , 8 , 9 ] ]
In [ 3 ] : whos

a list n=3
b list n=3
2.6 Arrays
In Python the array is a multidimensional variable that implements sets of values of the same
type. Usually the elements of an array are numbers, but can also be booleans, strings, or other
objects. An array is the basic instance for most scientific applications.
Operations like *, /, ** etc. implement the dot operations of the Matlab environment (.*,
./ and .). For example, the multiplication of two arrays a a represents the value-by-value
multiplication implemented in Matlab with the operation a. a.
In [ 1 ] : from numpy import mat , matrix , a r r a y
In [ 2 ] : a=a r r a y ( [ [ 1 , 2 , 3 ] , [ 4 , 5 , 6 ] ] )
In [ 3 ] : b=a r r a y ( [ [ 1 ] , [ 2 ] ] )
In [ 4 ] : print a a
[ [ 1 4 9]
[ 1 6 25 3 6 ] ]
In [ 5 ] : print a b
[ [ 1 2 3]
[ 8 10 1 2 ] ]
2.7 Matrices
The matrix object is useful in case of linear algebra operations. In this case the variables are
instanced using the mat or the matrix function.
In [ 1 ] : from numpy import mat , matrix , a r r a y
In [ 2 ] : a=mat ( a )
In [ 3 ] : b=a r r a y ( [ [ 1 ] , [ 2 ] , [ 3 ] ] )
In [ 4 ] : a b
Out [ 5 ] :
m a tr i x ( [ [ 1 4 ] ,
[32]])
In [ 6 ] : a=a r r a y ( a )
In [ 7 ] : a b

ValueError Traceback ( most r e c e n t
call last )
<i p y th o ninput 98201 c27d19b7 > in <module >()
> 1 a b
V a l u e E r r o r : o p e r a n d s c o u l d not be b r o a d c a s t t o g e t h e r with
shapes (2 , 3) (3 , 1)
In [ 8 ] : b=mat ( b )
In [ 9 ] : a b
Out [ 1 0 ] :
m a tr i x ( [ [ 1 4 ] ,
[32]])
2.8 Indexing
Indexing in Python is quite different compared with the syntax used in Matlab. Indices start
from 0 (and not 1 as in Matlab). In addition, the syntax is different for lists, arrays and
matrices.
2.9 Lists
1-dimension lists can be accessed using one index (ex. a[2]). Multidimensional lists require
multiple indices in the form [i][j]. . .
2.10. ARRAYS 15
In [ 1 ] : a = [ 1 , 2 , 3 , 4 , 5 ]
In [ 2 ] : %whos

a list n=5
In [ 3 ] : a [ 3 ]
Out [ 3 ] : 4
In [ 4 ] : b = [ [ 1 , 2 , 3 ] , [ 4 , 5 , 6 ] ]
In [ 5 ] : %whos

a list n=5
b list n=2
In [ 6 ] : b [ 1 ] [ 2 ]
Out [ 6 ] : 6
In [ 7 ] : b [ 0 ]
Out [ 7 ] : [ 1 , 2 , 3 ]
2.10 Arrays
Multidimensional arrays allow the use of indices in the forms [i, j] and [i][j].
In [ 1 ] : from numpy import a r r a y
In [ 2 ] : a=a r r a y ( [ 1 , 2 , 3 , 4 , 5 ] )
In [ 3 ] : b=a r r a y ( [ [ 1 , 2 , 3 ] , [ 4 , 5 , 6 ] ] )
In [ 4 ] : %whos

a ndarray 5 : 5 elems , type i n t 6 4 , 40 b y t e s
b ndarray 2 x3 : 6 elems , type i n t 6 4 , 48 b y t e s
In [ 5 ] : a . shape
Out [ 5 ] : ( 5 , )
In [ 6 ] : b . shape
Out [ 6 ] : ( 2 , 3 )
In [ 7 ] : a [ 3 ]
Out [ 7 ] : 4
In [ 8 ] : b [ 0 , 2 ]
Out [ 8 ] : 3
In [ 9 ] : b [ 0 ] [ 2 ]
Out [ 9 ] : 3
In [ 1 0 ] : b [ : , 0 ]
Out [ 1 0 ] : a r r a y ( [ 1 , 4 ] )
In [ 1 1 ] : b [ 0 , : ]
Out [ 1 1 ] : a r r a y ( [ 1 , 2 , 3 ] )
In [ 1 2 ] : b [ 0 ]
Out [ 1 2 ] : a r r a y ( [ 1 , 2 , 3 ] )
2.11 Matrices
Matrices can be only indexed using the [i, j] syntax. A matrix has always a minimum of 2
dimensions.
In [ 1 ] : from numpy import mat
In [ 2 ] : a=a r r a y ( [ 1 , 2 , 3 , 4 , 5 ] )
In [ 3 ] : b=a r r a y ( [ [ 1 , 2 , 3 ] , [ 4 , 5 , 6 ] ] )
In [ 4 ] : %whos

a m a tr i x [[1 2 3 4 5]]
b m a tr i x [ [ 1 2 3]\n [4 5 6 ] ]
Out [ 5 ] : ( 1 , 5 )
In [ 6 ] : b . shape
Out [ 6 ] : ( 2 , 3 )
2.12. MULTIDIMENSIONAL ARRAYS AND MATRICES 17
In [ 7 ] : a [ 0 , 2 ]
Out [ 7 ] : 3
In [ 8 ] : b [ 1 , 1 ]
Out [ 8 ] : 5
In [ 9 ] : b [ : , 0 ]
Out [ 9 ] :
m a tr i x ( [ [ 1 ] ,
[4]])
In [ 1 0 ] : b [ 0 , : ]
Out [ 1 0 ] : m a tr i x ( [ [ 1 , 2 , 3 ] ] )
2.12 Multidimensional arrays and matrices

Matrices and arrays can be defined with more than 2 dimensions.
In [ 1 ] : from numpy import a r r a y , mat
In [ 2 ] : a=z e r o s ( ( 3 , 3 , 3 ) , i n t 8 )
Out [ 3 ] : ( 3 , 3 , 3 )
In [ 4 ] : %whos

a ndarray 3 x3x3 : 27 elems , type i n t 8 , 27
bytes
In [ 5 ] : a [ 1 , 1 , 1 ]
Out [ 5 ] : 0
In [ 6 ] : a [ 1 , 1 , 1 ] = 5
In [ 7 ] : a
Out [ 7 ] :
array ( [ [ [ 0 , 0 , 0 ] ,
[0 , 0 , 0] ,
[0 , 0, 0]] ,
[[0 , 0 , 0] ,
[0 , 5 , 0] ,
[0 , 0, 0]] ,
[[0 , 0 , 0] ,
[0 , 0 , 0] ,
[ 0 , 0 , 0 ] ] ] , dtype=i n t 8 )
Chapter 3
The Python Control System toolbox
3.1 Basics
The Python Control Systems Library, is a package initially developed by Richard Murray at
Caltech. This toolbox contains a set of python classes and functions that implement common
operations for the analysis and design of feedback control systems. In addition, a MATLAB
compatibility package (control.matlab) has been integrated in order to provide functions equiv-
alent to the commands available in the MATLAB Control Systems Toolbox.
3.2 Models
LTI systems can be described in state-space form or as transfer functions.
19
20 CHAPTER 3. THE PYTHON CONTROL SYSTEM TOOLBOX
3.3 Continuous systems

3.4 State-space representation
In [ 1 ] : from c o n t r o l import
In [ 2 ] : a = [ [ 0 , 1 ] , [ 1 , 1 ] ]
In [ 3 ] : b = [ [ 0 ] , [ 1 ] ]
In [ 4 ] : c = [ 1 , 0 ]
In [ 5 ] : d=0
In [ 6 ] : s y s = s s ( a , b , c , d )
In [ 7 ] : print s y s
A = [ [ 0 1]
[ 1 1 ] ]
B = [[0]
[1]]
C = [[1 0]]
D = [[0]]
3.5 Transfer function
In [ 2 ] : g=t f ( 1 , [ 1 , 1 , 1 ] )
In [ 3 ] : print g
1

s 2 + s + 1
3.6 Zeros-Poles-Gain
This method is not implemented in control toolbox yet. It is available in the package scipy.signal
but it is not completely compatible with the class of LTI objects defined in the Python control
toolbox.
3.7 Discrete time systems

An additional fields (dt) in the StateSpace and TransferFunction classes is used to differ-
entiate continuous-time and discrete-time systems.
3.8. STATE-SPACE REPRESENTATION 21
3.8 State-space representation
In [ 4 ] : a = [ [ 0 , 1 ] , [ 1 , 1 ] ]
In [ 5 ] : b = [ [ 0 ] , [ 1 ] ]
In [ 6 ] : c =[1 , 1]
In [ 7 ] : d=0
In [ 8 ] : s y s d = s s ( a , b , c , d , 0 . 0 1 )
In [ 9 ] : print s y s d
A = [ [ 0 1]
[ 1 1]]
B = [[0]
[1]]
C = [ [ 1 1]]
D = [[0]]
dt = 0 . 0 1
3.9 Transfer function
In [ 2 ] : g=t f ( [ 1 , 1 ] , [ 1 , 1 , 1 ] , 0 . 0 1 )
In [ 3 ] : print g
z 1

z 2 z + 1
dt = 0 . 0 1
3.10 Conversions
The Python control system toolbox only implements conversion from continuous time systems
to discrete-time systems ( c2d ) with the methods zoh, tustin and matched. No conver-
sion from discrete to continuous has been implemented yet.
The supsictrl.yottalab package implements both functions c2d and d2c with the methods zoh,
foh, tustin and matched (matched is only implemented in c2d).
In [ 2 ] : from c o n t r o l . Matlab import
In [ 3 ] : g=t f ( 1 , [ 1 , 1 , 1 ] )
In [ 4 ] # Matlab c o m p a t i b i l i t y
In [ 5 ] : gd = c2d ( g , 0 . 0 1 )
In [ 6 ] # control toolbox
In [ 7 ] : gd2 = s a m p l e s y s te m ( g , 0 . 0 1 )
In [ 8 ] : print g
1

s 2 + s + 1
In [ 9 ] : print gd
4 . 9 8 3 e 05 z + 4 . 9 6 7 e 05

z 2 1 . 9 9 z + 0 . 9 9
dt = 0 . 0 1
In [ 2 ] : from c o n t r o l . Matlab import c2d
In [ 3 ] : from s u p s i c t r l . y o t t a l a b import d2c
In [ 4 ] : g=t f ( 1 , [ 1 , 1 , 1 ] )
In [ 5 ] : gd =c2d ( g , 0 . 0 1 )
In [ 6 ] : g2=d2c ( gd )
In [ 7 ] : print g
1

s 2 + s + 1
In [ 8 ] : print g2
1 . 7 2 9 e 14 s + 1

s 2 + s + 1
3.11 Casting
The control.matlab module implements the casting functions to transform LTI systems to a
transfer function (tf) or to a state-space form (ss).
3.12. MODELS INTERCONNECTION 23
In [ 8 ] : g = t f ( s y s )
In [ 9 ] : print g
1

s 2 + s + 1
and transfer functions into one of the state-space representation
In [ 1 0 ] : s y s = s s ( g )
In [ 1 1 ] : print s y s
A = [ [ 0 . 1.]
[ 1. 1.]]
B = [[ 1.]
[ 0.]]
C = [[ 0. 1.]]
D = [[ 0.]]
3.12 Models interconnection
Commands like parallel and series are available in order to interconnect systems. The op-
erators + and * have been overloaded for the LTI class to perform the same operations. In
addition the command feedback is implemented exactly as in Matlab.
In [ 2 ] : g1=t f ( 1 , [ 1 , 1 ] )
In [ 3 ] : g2=t f ( 1 , [ 1 , 2 ] )
In [ 4 ] : print p a r a l l e l ( g1 , g2 )
2 s + 3

s 2 + 3 s + 2
In [ 5 ] : print g1+g2
2 s + 3

s 2 + 3 s + 2
In [ 6 ] : print s e r i e s ( g1 , g2 )
1

s 2 + 3 s + 2
In [ 7 ] : print g1 g2
1

s 2 + 3 s + 2
In [ 8 ] : print f e e d b a c k ( g1 , g2 )
s + 2

s 2 + 3 s + 3
Chapter 4
System analysis
4.1 Time response

The Python Control toolbox offers own functions to simulate the time response of systems. For
Matlab users, the control.matlab module gives the possibility to work with the same syntax as
in Matlab. Please take care about the order of the return values!
Examples of time responses are shown in the figures 4.1, 4.2, 4.3, 4.4 and 4.5.
1.2
1.0
In [ 2 ] : import m a t p l o t l i b . p y p l o t a s p l t
0.8
In [ 3 ] : g = t f ( 1 , [ 1 , 1 , 1 ] )
0.6
y
In [ 4 ] : t , y = s t e p r e s p o n s e ( g )
0.4
In [ 5 ] : plt . plot (t , y)
...: plt . grid ()
0.2
...: plt . xlabel ( t )
...: plt . ylabel ( y )
0.0
0 2 4 6 8 10 12 14 16
t
or alternatively
In [ 2 ] : from c o n t r o l . matlab import
In [ 4 ] : g = t f ( 1 , [ 1 , 1 , 1 ] )
In [ 5 ] : y , t = s t e p ( g )
In [ 6 ] : plt . plot (t , y)
...: plt . grid ()
Figure 4.1: Step response for continuous-time systems
25
26 CHAPTER 4. SYSTEM ANALYSIS
1.2
1.0
In [ 2 ] : from c o n t r o l . matlab import c2d
0.8
In [ 4 ] : g = t f ( 1 , [ 1 , 1 , 1 ] ) 0.6
y
In [ 5 ] : gz=c2d ( g , 0 . 1 ) 0.4
In [ 6 ] : t=a r a n g e ( 0 , 1 6 , 0 . 1 ) 0.2
In [ 7 ] : t1 , y = s t e p r e s p o n s e ( gz , t ) 0.0
0 2 4 6 8 10 12 14 16
In [ 8 ] : p l t . s t e p ( t , y .T [ 0 ] ) # t r a n s p o s e t
col m at r i x y [ 0 ]
...: plt . grid ()
or alternatively
In [ 4 ] : g = t f ( 1 , [ 1 , 1 , 1 ] )
In [ 5 ] : gz=c2d ( g , 0 . 1 )
In [ 6 ] : t=a r a n g e ( 0 , 1 6 , 0 . 1 )
In [ 7 ] : y , t1 = s t e p ( gz , t )
In [ 8 ] : plt . step ( t , y [ 0 ] ) # get f i r s t

row from y m at r i x
...: plt . grid ()
Figure 4.2: Step response for discrete-time systems

4.1. TIME RESPONSE 27
1.4
1.2
In [ 2 ] : import m a t p l o t l i b . p y p l o t a s p l t 1.0
0.8
In [ 3 ] : a = [ [ 0 , 1 ] , [ 1 , 1 ] ]
0.6
In [ 4 ] : b = [ [ 0 ] , [ 1 ] ]
y
0.4
In [ 5 ] : c = [ 1 , 0 ] 0.2
0.0
In [ 6 ] : d = [ 0 ]
0.2
In [ 7 ] : s y s=s s ( a , b , c , d ) 0.4
0 2 4 6 8 10 12 14
In [ 8 ] : t , y=i n i t i a l r e s p o n s e ( s y s , t
X0 = [ 1 , 1 ] )
In [ 9 ] : plt . plot (t , y)
...: plt . grid ()
or alternatively
In [ 4 ] : a = [ [ 0 , 1 ] , [ 1 , 1 ] ]
In [ 5 ] : b = [ [ 0 ] , [ 1 ] ]
In [ 6 ] : c = [ 1 , 0 ]
In [ 7 ] : d = [ 0 ]
In [ 8 ] : s y s=s s ( a , b , c , d )
In [ 9 ] : y , t= i n i t i a l ( s y s , X0 = [ 1 , 1 ] )
In [ 1 0 ] : plt . plot (t , y)
...: plt . grid ()
Figure 4.3: Continuous time systems - Initial condition response

0.6
0.5
In [ 3 ] : g = t f ( 1 , [ 1 , 1 , 1 ] ) 0.4
In [ 4 ] : t , y = i m p u l s e r e s p o n s e ( g ) 0.3
y
In [ 5 ] : plt . plot (t , y) 0.2
...: plt . grid ()
...: plt . xlabel ( t ) 0.1
0.0
or alternatively 0.1
0 2 4 6 8 10 12 14 16
t
In [ 4 ] : g = t f ( 1 , [ 1 , 1 , 1 ] )
In [ 5 ] : y , t = i m p u l s e ( g )
In [ 6 ] : plt . plot (t , y)
...: plt . grid ()
Figure 4.4: Continuous time systems - Impulse response

4.1. TIME RESPONSE 29
1 .0
0.5
In [ 3 ] : g=t f ( [ 1 , 2 ] , [ 1 , 2 , 3 , 4 ] )
In [ 4 ] : t=l i n s p a c e ( 0 , 6 p i ) 0.0
y
In [ 5 ] : u=s i n ( t )
0.5
In [ 6 ] : t , y , x = f o r c e d r e s p o n s e ( g , t , u )
In [ 7 ] : plt . plot (t , y) 1 .0
...: plt . xlabel ( t ) 0 5 10 15 20
...: plt . ylabel ( y ) t
...: plt . grid ()
or alternatively
In [ 4 ] : g=t f ( [ 1 , 2 ] , [ 1 , 2 , 3 , 4 ] )
In [ 5 ] : t=l i n s p a c e ( 0 , 6 p i )
In [ 6 ] : u=s i n ( t )
In [ 7 ] : y , t , x = l s i m ( g , u , t )
In [ 8 ] : plt . plot (t , y)
...: plt . grid ()
Figure 4.5: Continuous time systems - Generic input

4.2 Frequency analysis
The frequency analysis includes some commands like bode response, nyquist response,
nichols response and the corresponding Matlab versions bode, nyquist and nichols. (See
figures 4.6, 4.7 and 4.8)
0
In [ 1 ] : from c o n t r o l import )
B 0
d(
e 0
In [ 2 ] : g=t f ( [ 1 ] , [ 1 , 0 . 5 , 1 ] ) d
u
i
t
n 20
g
In [ 3 ] : b o d e p l o t ( g , dB=True ) ; a 30
M
40
- 0
0 0 0
or alternatively )
g
e
d(
es
a
h
In [ 1 ] : from c o n t r o l import P
In [ 3 ] : g=t f ( [ 1 ] , [ 1 , 0 . 5 , 1 ] )
In [ 4 ] : bode ( g , dB=True ) ;
Figure 4.6: Bode plot
The command margins returns the gain margin, the phase margin and the corresponding
crossover frequencies.
In [ 2 ] : g=t f ( 2 , [ 1 , 2 , 3 , 1 ] )
In [ 3 ] : gm, pm, wg , wp = margin ( g )
In [ 4 ] : gm # Gain , n ot dB !
Out [ 4 ] : 2 . 5 0 0 0 0 0 0 0 0 0 0 0 0 0 1 3
In [ 5 ] : pm
Out [ 5 ] : 7 6 . 2 7 4 0 7 5 2 5 6 9 2 1 3 9 2 # deg
In [ 6 ] : wg
Out [ 6 ] : 0 . 8 5 8 6 4 8 7 7 6 1 0 1 6 7 2 0 1 # rad / s
In [ 7 ] : wp
Out [ 7 ] : 1 . 7 3 2 0 5 0 8 0 7 5 6 8 8 7 7 6 # rad / s
In addition, the command stability margins returns the stability margin and the correspond-
ing frequency. The stability margin values ws and sm , which correspond to the shortest distance
from the Nyquist curve to the critical point 1, are useful for the sensitivity analysis.
4.2. FREQUENCY ANALYSIS 31
0. 8
0.6
0.4
In [ 3 ] : g=t f ( [ 1 ] , [ 1 , 2 , 1 ] ) 0.2
In [ 3 ] : nyquist plot (g) , plt . grid () 0.0
0.2
0.4
or alternatively
0.6
0.8
In [ 1 ] : from c o n t r o l import .0 0.5 0.0 0.5 .0
In [ 4 ] : g=t f ( 1 , [ 1 , 2 , 1 ] )
In [ 5 ] : n y q u i s t ( g ) , p l t . g r i d ( )
Figure 4.7: Nyquist plot
Nc ol! "lo t

50
0.0 0.0
In [ 1 ] : from c o n t r o l import 0.25 0.25
0.5 0.5 0.5 0.5
.0 .0 .0 .0
3.0 3.0 3.0 3.0
In [ 2 ] : g=t f ( 1 , [ 1 , 2 , 3 , 4 , 0 ] ) 0
2.0
6.0 6.0 2.0
6.0
6.0
2.0 2.0
20.0 20.0
In [ 3 ] : nichols plot (g)
40.0 40.0

50
60.0 60.0

t

0.0 0.0

or alternatively
M
00 00.0 00.0
20.0 20.0
In [ 1 ] : from c o n t r o l import 40.0 40.0

50
60.0 60.0
In [ 2 ] : g=t f ( 1 , [ 1 , 2 , 3 , 4 , 0 ] ) 7 00 600 500 400 300 200 00 0

In [ 3 ] : nichols (g)
Figure 4.8: Nichols plot

In [ 2 ] : g=t f ( 2 , [ 1 , 2 , 3 , 1 ] )
In [ 3 ] : gm, pm, sm , wg , wp , ws = s t a b i l i t y m a r g i n s ( g )
In [ 4 ] : gm
Out [ 4 ] : 2 . 5 0 0 0 0 0 0 0 0 0 0 0 0 0 1 3 # Gain n ot dB
In [ 5 ] : pm
Out [ 5 ] : 7 6 . 2 7 4 0 7 5 2 5 6 9 2 1 3 9 2 # deg
In [ 6 ] : wg
Out [ 6 ] : 1 . 7 3 2 0 5 0 8 0 7 5 6 8 8 7 7 6 # rad / s
In [ 7 ] : wp
Out [ 7 ] : 0 . 8 5 8 6 4 8 7 7 6 1 0 1 6 7 2 0 1 # rad / s
In [ 8 ] : sm
Out [ 8 ] : 0 . 5 4 4 9 7 5 7 7 5 5 3 0 9 6 4 2 1 #
In [ 9 ] : ws
Out [ 9 ] : 1 . 3 6 6 9 3 7 1 2 0 6 5 3 8 0 9 7 # rad / s
4.3 Poles, zeros and root locus analysis

Poles and zeros of an open loop system can be calculated with the commands pole, zero or
plotted and calculated with pzmap.
In addition there are two functions that implement the root locus command: rlocus and
root locus. At present no algorithm to automatically choose the values of K has been imple-
mented: if not provided, the K vector is calculated in rlocus with log values between 103 and
103 . For the root locus function the K values should be provided.
If the ipython shell is not launched with the inline flag, the root locus is plotted on an external
window and it is possible to get the values of gain and damp by clicking with the mouse on the
curves.
Clicked at 0.5724 +1.293 j g a i n 1 . 7 2 2 damp

0.4048
0.9999
0.504
4.3. POLES, ZEROS AND ROOT LOCUS ANALYSIS 33
Pole Zero Map

2.0
1.5
In [ 2 ] : from c o n t r o l . pzmap import pzmap

1.0
In [ 3 ] : g=t f ( [ 1 , 1 ] , [ 1 , 2 , 3 , 4 , 0 ] ) 0.5
In [ 4 ] : g . p o l e ( )
Im
0.0
Out [ 4 ] :
a r r a y ([ 1 . 6 5 0 6 2 9 1 9 + 0 . j , 0.5
0.17468540+1.5468688 9 j ,
1.0
0.17468540 1.54686889 j ,
0.00000000+0. j ]) 1.5
In [ 5 ] : g . z e r o ( ) 2.0
2.0 1.5 1.0 0.5 0.0 0.5
Out [ 5 ] : a r r a y ( [ 1 . ] ) Re
In [ 6 ] : p o l e s , z e r o s = pzmap ( g ) , g r i d ( )
In [ 7 ] : poles
Out [ 7 ] :
a r r a y ([ 1 . 6 5 0 6 2 9 1 9 + 0 . j ,
0.17468540+1.5468688 9 j ,
0.17468540 1.54686889 j ,
0.00000000+0. j ])
In [ 8 ] : z e r o s
Out [ 8 ] : a r r a y ( [ 1 . ] )
Figure 4.9: Poles and zeros
$0
In [ 2 ] : g=t f ( 1 , [ 1 , 2 , 3 , 0 ] )
5
In [ 3 ] : rlocus (g) ; grid ()

r
y
+
./
, 0
+
mI
or alternatively
#5
# $0
In [ 2 ] : g=t f ( 1 , [ 1 , 2 , 3 , 0 ] ) # $2 # $0 #% #6 #4 #2 0 2 4 6
R&'*
In [ 3 ] : k=l o g s p a c e ( 3 , 3 , 1 0 0 )
In [ 4 ] : root locus (g , k) ; grid ()
Figure 4.10: Root locus plot

Chapter 5
Modeling
The sympy module (symbolic python) contains a full set of operations to manage physical
systems. In particular, it is possible to find the linearized model of a mechanical system using
the Lagranges method or the Kanes method. More details about the Kanes method are
available at [14], [15], [16], [17], [18] and [19].
In the next sections we present the modelling of 3 plants that we can find in our laboratories
and that are quite familiar to us.
5.1 Model of a DC motor
5.1.1 Plant
In this first example we model a DC servo motor with a current input in order to find its
state-space representation. The motor is characterized by a torque constant kt , an inertia
(motor+load) J and a friction constant Dm .
The input of the plant is the current I and the output is the position . The rotation center is
the point O, the main coordinates system is N and we add a local reference frame Nr which
rotates with the load (angle and speed ).
35
36 CHAPTER 5. MODELING
5.1.2 Modules and constants
n [ 1 ] : from sympy import symbols , Matrix , p i

. . . : from sympy . p h y s i c s . m e c h a n i c s import
. . . : import numpy a s np
...:
. . . : # Modeling t h e s y s t e m w i t h Kane method
...:
. . . : # Signals
. . . : ph = dynamicsymbols ( ph ) # motor a n g l e
. . . : w = dynamicsymbols ( w ) # motor r o t . s pe e d
. . . : I = dynamicsymbols ( I ) # input current
...:
. . . : # C on s t an t s
. . . : Dm = symbols ( Dm ) # friction
. . . : M, J = symbols ( M J ) # Mass and i n e r t i a
. . . : t = symbols ( t ) # time
. . . : k t = symbols ( k t ) # torque constant
...:
5.1.3 Reference frames
In [ 2 ] : # R e f e r e n c e frame f o r t h e motor and Load

...: N = ReferenceFrame ( N )
...:
...: O = P o i n t ( O ) # center of rotation
...: O. s e t v e l (N, 0 )
...:
...: # R e f e r e n c e frames f o r t h e r o t a t i n g d i s k
...: Nr = N. o r i e n t n e w ( Nr , Axis , [ ph , N. x ] ) #
r ot at i n g r e f e r e n c e ( load )
...:
. . . : Nr . s e t a n g v e l (N, wN. x )
...:
5.1.4 Body and inertia of the load
In [ 3 ] : # Mechanics
...: I o = J o u t e r ( Nr . x , Nr . x )
...:
...: InT = ( Io , O)
...:
...: B = RigidBody ( B , O, Nr , M, InT )
...:
5.1.5 Forces and torques

In order to find the dynamic model of the plant we need some other definitions, in particular
the relation between angle and angular velocity , the forces and torques applied to the
system and a vector that contains the rigid bodies of the system.
5.1. MODEL OF A DC MOTOR 37
In [ 4 ] : # For c e s and t o r q u e s
. . . : f o r c e s = [ ( Nr , ( k t IDmw) N. x ) ]
...:
. . . : k i n d i f f s = [ ( ph . d i f f ( t )w) ]
...:
. . . : b o d i e s =[B ]
...:
5.1.6 Model
Using the Kanes method is now possible to find the dynamic matrices related to the plant.
In [ 5 ] : # Model
. . . : KM = KanesMethod (N, q i n d =[ph ] , u i n d =[w ] , k d e q s=
kindiffs )
. . . : f r , f r s t a r = KM. k a n e s e q u a t i o n s ( f o r c e s , b o d i e s )
...:
. . . : print f r
. . . : print f r s t a r
...:
Matrix ( [ [ Dmw( t ) + k t I ( t ) ] ] )
Matrix ( [ [ J D e r i v a t i v e (w( t ) , t ) ] ] )
5.1.7 State-space matrices
From the results of the Kanes model identification, we can now extract the matrices A and B
of the state-space representation.
In [ 6 ] : # s y m b o l i c a l l y l i n e a r i z e ab ou t a r b i t r a r y
equilibrium
. . . : l i near s tate m atr i x , linear input matrix , inputs =
KM. l i n e a r i z e ( new method=True )
...:
. . . : # set the the equilibrium point
. . . : eq pt = [ 0 , 0]
. . . : e q d i c t = d i c t ( zip ( [ ph , w ] , e q p t ) )
...:
. . . : f A l i n = l i n e a r s t a t e m a t r i x . subs ( e q d i c t )
. . . : f B l i n = l i n e a r i n p u t m a t r i x . subs ( e q d i c t )
. . . : m mat = KM. m a s s m a t r i x f u l l . s u b s ( e q d i c t )
...:
. . . : # compute A and B m a t r i c e s
. . . : A = np . m a tr i x ( m mat . i n v ( ) f A l i n )
. . . : B = np . m a tr i x ( m mat . i n v ( ) f B l i n )
In [ 6 ] : print A
. . . : print B
...:
[ [ 0 1]
[ 0 Dm/ J ] ]
[[0]
[ kt /J ] ]
5.2 Model of the inverted pendulum
The second example is represented by the classical inverted pendulum as shown in figure 5.1.
P
th, w
x1
y
y1
F C
x, v
Figure 5.1: Inverted pendulum
The global reference frame is Nf (x, y) The point P is the center of mass of the pendulum. The
car is moving with speed v and position C. The pole is rotating with the angle th and angular
velocity w, In addition to the main coordinate frame Nf (x, y), we define a local body-fixed
frame to the pendulum Npend (x1 , y1 ).
5.2. MODEL OF THE INVERTED PENDULUM 39
Figure 5.2: Inverted pendulum - Real plant
In [ 1 ] : from sympy import symbols , Matrix , p i

...: from sympy . p h y s i c s . m e c h a n i c s import
...: import numpy a s np
...:
...: # Modeling t h e s y s t e m w i t h Kane method
...:
...: # Signals
...: x , th = dynamicsymbols ( x th )
...: v , w = dynamicsymbols ( v w )
...: F = dynamicsymbols ( F )
...:
...: # C on s t an t s
...: d = symbols ( d ) # f r i c t i o n
...: m, r = symbols ( m r )
...: M = symbols ( M )
...: g , t = symbols ( g t )
...: J = symbols ( J )
...:
5.2.2 Frames - Car and pendulum
In [ 2 ] : # Frames and Coord . s y s t e m

...:
...: # Car r e f e r e n c e x , y
...: Nf = ReferenceFram e ( Nf )
...: C = P o i n t ( C )
...: C . s e t v e l ( Nf , v Nf . x )
...: Car = P a r t i c l e ( Car ,C,M)
...:
...: # Pendulum r e f e r e n c e x1 , y1
...: Npend = Nf . o r i e n t n e w ( Npend , Axis , [ th , Nf . z ] )
...: Npend . s e t a n g v e l ( Nf , w Nf . z )
...:
...: P = C . l o c a t e n e w ( P , r Npend . x )
...: P . v 2 p t t h e o r y (C, Nf , Npend )
...: Pa = P a r t i c l e ( Pa , P , m)
...:
5.2.3 Points, bodies, masses and inertias
In [ 3 ] : I = o u t e r ( Nf . z , Nf . z )
. . . : I n e r t i a t u p l e = ( J I , P)
. . . : Bp = RigidBody ( Bp , P , Npend , m, I n e r t i a t u p l e )
...:
5.2.4 Forces, frictions and gravity
In [ 4 ] : # For c e s and t o r q u e s
. . . : f o r c e s = [ ( C, F Nf . xdv Nf . x ) , ( P,m g Nf . y ) ]
. . . : f r a m e s = [ Nf , Npend ]
. . . : p o i n t s = [ C, P ]
...:
. . . : k i n d i f f s = [ x . d i f f ( t )v , th . d i f f ( t ) w ]
. . . : p a r t i c l e s = [ Car , Bp ]
...:
5.2. MODEL OF THE INVERTED PENDULUM 41
5.2.5 Final model and linearized state-space matrices
n [ 5 ] : # Model
. . . : KM = KanesMethod ( Nf , q i n d =[x , th ] , u i n d =[v , w ] ,
k d e q s=k i n d i f f s )
. . . : f r , f r s t a r = KM. k a n e s e q u a t i o n s ( f o r c e s , p a r t i c l e s )
...:
. . . : # Equilibrium point
. . . : eq pt = [ 0 , pi /2 ,0 ,0]
. . . : e q d i c t = d i c t ( zip ( [ x , th , v , w ] , e q p t ) )
...:
. . . : # s y m b o l i c a l l y l i n e a r i z e ab ou t a r b i t r a r y
equilibrium
...:
. . . : # s u b i n t h e e q u i l i b r i u m p o i n t and t h e par am e t e r s
...:
. . . : # compute A and B
. . . : A = m mat . i n v ( ) f A l i n
. . . : B = m mat . i n v ( ) f B l i n
...:
In [ 6 ] : A
Out [ 6 ] :
Matrix ( [
[0 , 0 , 1 , 0] ,
[0 , 0 , 0 , 1] ,
[0 , g m2 r 2 / ( J M + J m + Mm r 2 ) , d (m2 r 2 / ( (
M + m) ( J M + J m
+ Mm r 2 ) ) + 1 / (M + m) ) , 0 ] ,
[ 0 , g m r (M + m) / ( J M + J m + Mm r 2 ) ,
dm r / ( J M + J m + Mm r 2 ) , 0 ] ] )
In [ 7 ] : B
Out [ 7 ] :
Matrix ( [
[
0] ,
[
0] ,
[m2 r 2 / ( (M + m) ( JM + Jm + Mm r 2 ) ) + 1 / (M + m) ] ,
[ m r / ( J M + J m + Mm r 2 ) ] ] )
And the results can be written in a better form as

0 0 1 0
0 0 0 1
A= d(Jc+mr 2 )

gm2 r 2
0 JcM +Jcm+M mr2 0

JcM +Jcm+M mr 2
gmr(M +m) dmr
0 JcM +Jcm+M mr 2
JcM +Jcm+M mr 2
0
and
0

0
B=
Jc+mr 2

JcM +Jcm+M mr 2
mr
JcM +Jcm+M mr 2
5.3 Model of the Ball-on-Wheel plant
A more complex plant is represented by the Ball-on-Wheel system of figure 5.3, where a ball
must be maintened in the unstable equilibrium point on the top of a bike wheel.
y2
y1
ph2, w2 ph0
y
x2
x1
ph1, w1, T
Figure 5.3: Ball-On-Wheel plant
In this system we have 4 reference frames. The frame N is the main reference frame, N0 rotates
with the line connecting the centers of mass of the wheel (O) and of the ball (CM2), N1 (x1 ,
y1 ) rotates with the wheel and N2 (x2 , y2 ) is body-fixed to the ball.
The radius of the wheel and of the ball are respectively R1 and R2 . The non sliding condition
is given by
(R1 + R2 ) ph0 = R1 ph1 + R2 ph2
The input of the system is represented by the torque T applied to the wheel.
5.3. MODEL OF THE BALL-ON-WHEEL PLANT 43
In [ 1 ] : from sympy import symbols , Matrix , p i

...: from sympy . p h y s i c s . m e c h a n i c s import
...: import numpy a s np
...:
...: ph0 , ph1 , ph2 = dynamicsymbols ( ph0 ph1 ph2 )
...: w1 , w2 = dynamicsymbols ( w1 w2 )
...:
...: T = dynamicsymbols ( T )
...:
...: J1 , J2 = symbols ( J1 J2 )
...: M1, M2 = symbols ( M1 M2 )
...: R1 , R2 = symbols ( R1 R2 )
...: d1 = symbols ( d1 )
...: g = symbols ( g )
...: t = symbols ( t )
...:
5.3.2 Reference frames
In [ 2 ] : N = ReferenceFrame ( N )
...:
...: O = P o i n t ( O )
...: O. s e t v e l (N, 0 )
...:
...: ph0 = (R1 ph1+R2 ph2 ) / (R1+R2 )
...:
...: N0 = N. o r i e n t n e w ( N0 , Axis , [ ph0 ,N. z ] )
...: N1 . s e t a n g v e l (N, w1N. z )
...: N2 . s e t a n g v e l (N, w2N. z )
...:
5.3.3 Centers of mass of the ball
In [ 3 ] : CM2 = O. l o c a t e n e w ( CM2 , ( R1+R2 ) N0 . y )

. . . : CM2. v 2 p t t h e o r y (O, N, N0)
...:
Out [ 3 ] : (R1 ph1 R2 ph2 ) N0 . x
5.3.4 Masses and inertias
In [ 4 ] : I z = o u t e r (N. z ,N. z )
...: In1T = ( J1 Iz , O)
...: In2T = ( J2 Iz , CM2)
...:
...: B1 = RigidBody ( B1 , O, N1 , M1, In1T )
...: B2 = RigidBody ( B2 , CM2, N2 , M2, In2T )
...:
5.3.5 Forces and torques
In [ 5 ] : #f o r c e s = [ ( N1 , (Td1 w1 ) N. z ) , (CM2,M2 g N. y ) ]
. . . : f o r c e s = [ ( N1 , TN. z ) , (CM2,M2 g N. y ) ]
...:
. . . : k i n d i f f s = [ ph1 . d i f f ( t )w1 , ph2 . d i f f ( t )w2 ]
...:
5.3.6 Kanes model and linearized state-space matrices
In [ 6 ] : KM = KanesMethod (N, q i n d =[ph1 , ph2 ] , u i n d =[w1 , w2

] , k d e q s=k i n d i f f s )
. . . : f r , f r s t a r = KM. k a n e s e q u a t i o n s ( f o r c e s , [ B1 , B2 ] )
...:
In [ 7 ] : # E q u i l i b r i u m p o i n t
. . . : eq pt = [ 0 , 0 , 0 , 0 , 0]
. . . : e q d i c t = d i c t ( zip ( [ ph1 , ph2 , w1 , w2 , T ] , e q p t ) )
...:
In [ 8 ] : # s y m b o l i c a l l y l i n e a r i z e ab ou t a r b i t r a r y
equilibrium
...:
. . . : # s u b i n t h e e q u i l i b r i u m p o i n t and t h e par am e t e r s
...:
. . . : # compute A and B
. . . : A = m mat . i n v ( ) f A l i n
. . . : B = m mat . i n v ( ) f B l i n
5.3. MODEL OF THE BALL-ON-WHEEL PLANT 45
In [ 9 ] : A
Out [ 9 ] :
Matrix ( [
[0 , 0 , 1 , 0] ,
[0 , 0 , 0 , 1] ,
[M22R12R22 g / ( ( R1 + R2 ) ( J1 J2 + J1 M2R22 + J2
M2R1 2 ) ) +
M2R12 g (M22R12R2 2 / ( ( J1 + M2R1 2 ) ( J1 J2 + J1
M2R22 +
J2 M2R1 2 ) ) + 1 / ( J1 + M2R1 2 ) ) / ( R1 + R2 ) , M22R1R2
3 g / ( ( R1 +
R2 ) ( J1 J2 + J1 M2R22 + J2 M2R1 2 ) ) + M2R1R2 g (M2
2R12R2 2 / ( ( J1 +
M2R1 2 ) ( J1 J2 + J1 M2R22 + J2 M2R1 2 ) ) + 1 / ( J1 +
M2R1 2 ) ) / ( R1 + R2 ) ,
0 , 0] ,
[ M22R13
R2 g / ( ( R1 + R2 ) ( J1 J2
+ J1 M2R22 + J2 M2R1 2 ) ) + M2R1R2 g ( J1 + M2R1 2 )
/ ( ( R1 + R2 ) ( J1 J2 +
J1 M2R22 + J2 M2R1 2 ) ) ,
M22R12R22 g / ( ( R1 + R2 ) ( J1 J2 + J1 M2R22 + J2
M2R1 2 ) ) +
M2R22 g ( J1 + M2R1 2 ) / ( ( R1 + R2 ) ( J1 J2 + J1 M2R22
+ J2 M2R1 2 ) ) , 0 ,
0]])
In [ 1 0 ] : B
Out [ 1 0 ] :
Matrix ( [
[
0] ,
[
0] ,
[M22R12R2 2 / ( ( J1 + M2R1 2 ) ( J1 J2 + J1 M2R22 +
J2 M2R1 2 ) ) +
1 / ( J1 + M2R1 2 ) ] ,
[ M2R1R2 / (
J1 J2 + J1 M2R22 +
J2 M2R1 2 ) ] ] )
or as formula
0 0 1 0

0 0 0 1
J2 M 2 R2
1g J2 M 2 R1 R2 g
0 0

A= J1 J2 R1 +J1 J2 R2 +J1 M2 R1 R2 3 3 2
2 +J1 M2 R2 +J2 M2 R1 +J2 M2 R1 R2 J1 J2 R1 +J1 J2 R2 +J1 M2 R1 R2 3 3 2
2 +J1 M2 R2 +J2 M2 R1 +J2 M2 R1 R2
J1 M 2 R2

J1 M 2 R1 R2 g 2g 0 0
(
(R1 +R2 ) J1 J2 +J1 M2 R2 +J2 M2 R2
2 1 ) (
(R1 +R2 ) J1 J2 +J1 M2 R2 +J2 M2 R2
2 )
1
and
0
0
M2 2 R2 R2
1 2 1
+
B = (J1 +M2 R21 )(

J1 +M2 R2
J1 J2 +J1 M2 R2 2
2 +J2 M2 R1 ) 1
M 2 R1 R2

J1 J2 +J1 M2 R2 +J2 M2 R2
2 1
Chapter 6
Control design
6.1 PI+Lead design example

6.1.1 Define the system and the project specifications
In this first example we design a controller for a plant with the transfer function
1
G(s) =
s2 + 6 s + 5
The requirements for the control are
e = 0
for a step input
P M 60o
and
gc = 10rad/s
The controller can be written in the form
1 + s Ti 1 + TD s
C(s) = K
s Ti 1 + s TD
with a PI and a lead part.

We have to design the controller and find the values of Ti , , TD and K. The full design is
performed using the bode diagram.
After installing the required modules, we can define the plant transfer function and the require-
ments of the project.
47
48 CHAPTER 6. CONTROL DESIGN
In [ 1 ] : # Modules
In [ 2 ] : from m a t p l o t l i b . p y p l o t import
In [ 4 ] : from numpy import pi , l i n s p a c e
In [ 5 ] : from s c i p y import s i n , s q r t
In [ 6 ] : from s u p s i c t r l . y o t t a l a b import
In [ 7 ] : g=t f ( [ 1 ] , [ 1 , 6 , 5 ] )
In [ 8 ] : bode ( g , dB=True ) ;
In [ 9 ] : s u b p l o t ( 2 1 1 ) , l e g e n d ( [ G( s ) ] , prop={ s i z e : 1 0 } )
Out [ 9 ] :
(< m a t p l o t l i b . a x e s . AxesSubplot a t 0 x 7 f 8 5 b 5 1 9 3 5 5 0 >,
<m a t p l o t l i b . l e g e n d . Legend a t 0 x 7 f 8 5 b 4 7 e 6 9 5 0 >)
In [ 1 0 ] : wgc = 10 # D e s i r e d Bandwidth
In [ 1 1 ] : desiredPM = 60 # D e s i r e d Phase margin
Figure 6.1 shows the bode diagram of the plant.
5 00
GFHJ
5 20
E 5 30
D
C@ 5 40
A
@ 5 50
?
>
t
= 5 60
<
;
5 :0
M
5 60
5 9 0 34 0 4 2
00 00 00 00
0
5 20
5 40
E
< 5 60
A
C@ 5 60
A\ 5 0 00
;
[ 5 0 20
Z
5 0 40
5 0 60
5 06 0 34 0 4 2
00 00 00 00
KLMqOMQS y TLUVWXMSY
Figure 6.1: Bode diagram of the plant
6.1.2 PI part
Now we choose the integration time for the PI part of the controller. In this example we set
Ti = 0.15s
6.1. PI+LEAD DESIGN EXAMPLE 49
In [ 1 2 ] : # PI p a r t
In [ 1 3 ] : Ti =0.15
In [ 1 4 ] : Gpi=t f ( [ Ti , 1 ] , [ Ti , 0 ] )
In [ 1 5 ] : print PI p a r t i s : , Gpi
PI p a r t i s :
0.15 s + 1

0.15 s
In [ 1 6 ] : f i g u r e ( )
Out [ 1 6 ] : <m a t p l o t l i b . f i g u r e . F i g u r e a t 0 x 7 f 8 5 b 4 7 e a a 1 0 >
In [ 1 7 ] : bode ( g , dB=True , l i n e s t y l e = dashed ) ;
In [ 1 8 ] : h o l d
Out [ 1 8 ] : <f u n c t i o n m a t p l o t l i b . p y p l o t . hold>
In [ 1 9 ] : bode ( Gpi g , dB=True ) ;
In [ 2 0 ] : s u b p l o t ( 2 1 1 ) , l e g e n d ( ( [ G( s ) , Gpi ( s ) G( s ) ] ) ,
prop={ s i z e : 1 0 } )
Out [ 2 0 ] :
(< m a t p l o t l i b . a x e s . AxesSubplot a t 0 x 7 f 8 5 b 4 8 0 6 2 5 0 >,
<m a t p l o t l i b . l e g e n d . Legend a t 0 x 7 f 8 5 b 4 3 0 3 8 5 0 >)
Figure 6.2 shows the bode plot of the plant with and without the PI controller part.
40
{|}~
20 {|}~ {|}~
z
y 0
xv
w ` 20
v
t
p ` 40
t
k
j
f ` 60
M
` b0
` ] 00 ^_
0 _ 2
]0 ]0 ]0 ]0
0
z ` 50
j
w
vx
w ` ] 00

f

` ] 50
` 200 ^_
0 _ 2
]0 ]0 ]0 ]0
y
Figure 6.2: Bode diagram: G (dashed) and Gpi*G

6.1.3 Lead part

Now we can get the P M at the frequency gc in order to calculate the additional phase contri-
bution of the lead part of the controller.
In [ 2 1 ] : mag , phase , omega = bode ( Gpi g , [ wgc ] , P l o t=F a l s e )
In [ 2 2 ] : ph = phase [ 0 ]
In [ 2 3 ] : i f ph>=0:
...: ph = phase [ 0 ] 3 6 0 ;
...:
In [ 2 4 ] : Phase = 180+ desiredPM
In [ 2 5 ] : dPM = Phaseph
In [ 2 6 ] : print A d d i t i o n a l phase from Lead p a r t : , dPM

A d d i t i o n a l phase from Lead p a r t : 61.4144232114
Now it is possible to calculate the lead controller by finding the values of and TD .
In [ 2 7 ] : # Lead p a r t
In [ 2 8 ] : dPMrad = dPM/180 p i
In [ 2 9 ] : a l f a = (1+ s i n ( dPMrad ) ) /(1 s i n ( dPMrad ) ) ;
In [ 3 0 ] : print Alpha i s : , a l f a
Alpha i s : 15.4073552425
In [ 3 1 ] : TD = 1 / ( s q r t ( a l f a ) wgc ) ;
In [ 3 2 ] : Glead = t f ( [ a l f a TD, 1 ] , [ TD, 1 ] )
In [ 3 3 ] : print Lead p a r t i s : , Glead

Lead p a r t i s :
0.3925 s + 1

0.02548 s + 1
In [ 3 4 ] : f i g u r e ( )
Out [ 3 4 ] : <m a t p l o t l i b . f i g u r e . F i g u r e a t 0 x7f85b43462d 0 >
In [ 3 6 ] : h o l d
In [ 3 7 ] : bode ( Gpi Glead g , dB=True ) ;
In [ 3 8 ] : s u b p l o t ( 2 1 1 ) ,
l e g e n d ( ( [ G( s ) , Gpi ( s ) G( s ) , Gpi ( s ) GLead ( s ) G( s ) ] ) ,
prop={ s i z e : 1 0 } )
Out [ 3 8 ] :
(< m a t p l o t l i b . a x e s . AxesSubplot a t 0 x7f85b43736 d0 >,
<m a t p l o t l i b . l e g e n d . Legend a t 0 x 7 f 8 5 b 3 b 1 f 4 5 0 >)
Figure 6.3 shows now the bode plot of the plant, the plant with the PI part and the plant with
PI and Lead part
40

20

0

20

40
t

60
M
0
00 0 2 3
0 0 0 0 0
0
50

00

50
200 0 2 3
0 0 0 0 0
y
Figure 6.3: Bode diagram - G (dashed), Gpi*G (dotted) and Gpi*GLead*G
6.1.4 Controller Gain

The last step is to find the amplification K of the controller which move up the bode gain plot
in order to obtain the required crossover frequency gc .
In [ 3 9 ] : mag , phase , omega = bode ( Gpi Glead g , [ wgc ] , P l o t=

False )
In [ 4 0 ] : print Phase a t wgc i s : , phase [ 0 ]

Phase a t wgc i s : 120.0
In [ 4 1 ] : K=1/mag [ 0 ]
In [ 4 2 ] : print Gain to have MAG a t gwc 0dB : , K

Gain to have MAG a t gwc 0dB : 23.8177769548
In [ 4 3 ] : f i g u r e ( )
Out [ 4 3 ] : <m a t p l o t l i b . f i g u r e . F i g u r e a t 0 x7f85b3a703d 0 >
In [ 4 5 ] : h o l d
In [ 4 6 ] : bode ( Gpi Glead g , dB=True , l i n e s t y l e = . ) ;
In [ 4 7 ] : bode (K Gpi Glead g , dB=True ) ;
In [ 4 8 ] :
s u b p l o t ( 2 1 1 ) , l e g e n d ( ( [ G( s ) , Gpi ( s ) G( s ) , Gpi ( s ) GLead ( s
) G( s ) ,
K Gpi ( s ) GLead ( s ) G( s ) ] ) , prop={ s i z e : 1 0 } )
Out [ 4 8 ] :
(< m a t p l o t l i b . a x e s . AxesSubplot a t 0 x 7 f 8 5 b 3 a 7 6 6 9 0 >,
<m a t p l o t l i b . l e g e n d . Legend a t 0 x 7 f 8 5 b 3 3 e 6 f 9 0 >)
In the figure 6.4 we see now that the gain plot has been translated up to get 0dB at the gain
crossover frequency gc .
60

40

20
0

20

t
40

60
M
0
00 0 2 3
0 0 0 0 0
0
50

00

50
200 0 2 3
0 0 0 0 0
y
Figure 6.4: Bode diagram - G (dashed), Gpi*G (dotted), Gpi*GLead*G (dot-dashed) and
K*Gpi*GLead*G
6.1.5 Simulation of the controlled system
Now it is possible to simulate the controlled system after closing the loop.
In [ 4 9 ] : Contr = K Gpi Glead
In [ 5 0 ] : print F u l l c o n t r o l l e r : , Contr
Full c o n t r o l le r :
1 . 4 0 2 s 2 + 1 2 . 9 2 s + 2 3 . 8 2

0 . 0 0 3 8 2 1 s 2 + 0 . 1 5 s
In [ 5 1 ] : mag , phase , omega = bode (K Gpi Glead g , [ wgc ] , P l o t=

False )
In [ 5 2 ] : print Data a t wgc wgc : , omega [ 0 ] , Magnitude

: ,mag [ 0 ] , Phase :
, phase [ 0 ]
Data a t wgc wgc : 10 Magnitude : 1 . 0 Phase : 120.0
In [ 5 3 ] : g t=f e e d b a c k (K Gpi Glead g , 1 )
In [ 5 4 ] : t=l i n s p a c e ( 0 , 1 . 5 , 3 0 0 )
In [ 5 5 ] : y , t = s t e p ( gt , t )
In [ 5 6 ] : f i g u r e ( )
Out [ 5 6 ] : <m a t p l o t l i b . f i g u r e . F i g u r e a t 0 x 7 f 8 5 b 3 5 1 4 2 9 0>
In [ 5 7 ] : p l o t ( t , y ) , x l a b e l ( t ) , y l a b e l ( y ) , t i t l e ( Step
r e s p o n s e o f th e
c o n t r o l l e d plant )
Out [ 5 7 ] :
([ < m a t p l o t l i b . l i n e s . Line2D a t 0 x7f85b34252 d0 > ] ,
In [ 5 8 ] : g r i d ( )
The simulation of the controlled plant with a step input is shown in figure 6.5.
t t t t
.2
.0
0.
0.6
y
0.4
0.2
0.0
0.0 0.2 0.4 0.6 0. .0 .2 .4 .6
t
Figure 6.5: Step response of the controlled plant

6.2 Discrete-state feedback controller design
6.2.1 Plant and project specifications

In this example we design a discrete-state feedback controller for a DC servo motor.
We want to have a controlled system with a maximum of 4% overshooting and an error e = 0
with a step input. In addition we desire a bandwidth of the controlled system of at least 6
rad/s.
The step response of the motor with the current input of Iin = 500mA) has been saved into
the file MOT.
6.2.2 Motor parameters identification

We try to find the parameters of the srvo motor using a least square identification from the
collected data.
The transfer function of the DC motor from input current I(s) to output angle (s) can be
represented as
(s) Kt /J
G(s) = = 2
Iin (s) s + s D/J
6.2.3 Required modules
In [ 1 ] : from s c i p y . o p t i m i z e import l e a s t s q
In [ 2 ] : from s c i p y . s i g n a l import s t e p 2
In [ 3 ] : import numpy a s np
In [ 4 ] : import s c i p y a s sp
In [ 6 ] : from c o n t r o l . Matlab import
In [ 7 ] : from s u p s i c t r l . y o t t a l a b import
6.2.4 Function for least square identification

We define now the function residuals which returns the error between the collected and the
simulated data. Using this function we can try to minimize the error using a least square
approach.
6.2. DISCRETE-STATE FEEDBACK CONTROLLER DESIGN 55
In [ 8 ] : # Motor r e s p o n s e f o r l e a s t s q u a r e i d e n t i f i c a t i o n
In [ 9 ] : def r e s i d u a l s ( p , y , t ) :
...: [ k , alpha ] = p
...: g = t f ( k , [ 1 , alpha , 0 ] )
...: Y, T = s t e p ( g , t )
...: e r r=yY
...: return e r r
...:
6.2.5 Parameter identification
We load the collected data to perform the parameter identification of the numerator K = Kt /J
and the denominator value = D/J.
In [ 1 0 ] : # I d e n t i f y motor
In [ 1 1 ] : x = np . l o a d t x t ( MOT ) ;
In [ 1 2 ] : t = x [ : , 0 ]
In [ 1 3 ] : y = x [ : , 2 ]
In [ 1 4 ] : I o = 1000
In [ 1 5 ] : y1 = y/ I o
In [ 1 6 ] : p0 = [ 1 , 4 ]
In [ 1 7 ] : p l s q = l e a s t s q ( r e s i d u a l s , p0 , a r g s =(y1 , t ) )
In [ 1 8 ] : k t = 0 . 0 0 0 0 3 8 2 # Motor t o r q u e c o n s t a n t
In [ 1 9 ] : Jm=k t / p l s q [ 0 ] [ 0 ] # Motor I n e r t i a
In [ 2 0 ] : Dm=p l s q [ 0 ] [ 1 ] Jm # Motor f r i c t i o n
In [ 2 1 ] : g=t f ( [ k t /Jm ] , [ 1 ,Dm/Jm , 0 ] ) # T r an s f e r f u n c t i o n
6.2.6 Check of the identified parameters
The next step is to check how good our parameters have been identified by comparing the
simulated function with the measured data (see figure 6.6)
In [ 2 2 ] : Y, T = s t e p ( g , t )
In [ 2 3 ] : p l o t (T, Y, t , y1 ) , l e g e n d ( ( I d e n t i f i e d t r a n s f e r
function , Collected
data ) , prop={ s i z e : 1 0 } , l o c =2) , x l a b e l ( t ) , y l a b e l ( y ) ,
t i t l e ( Step
response ) , grid ()
Out [ 2 3 ] :
([ < m a t p l o t l i b . l i n e s . Line2D a t 0 x7fb9a1b6b590 >,
<m a t p l o t l i b . l i n e s . Line2D a t 0 x7fb9a1b6b710 > ] ,
<m a t p l o t l i b . l e g e n d . Legend a t 0 x7fb9a1b6bb10 >,
<m a t p l o t l i b . t e x t . Text a t 0 x 7 f b 9 a 3 c e c 3 1 0 >,
<m a t p l o t l i b . t e x t . Text a t 0 x7fb9a1b8b910 >,
<m a t p l o t l i b . t e x t . Text a t 0 x7fb9a1b3cbd0 >,
None )
St ep response
0.16
Ident ified t ransfer funct ion
Collect ed dat a
0.14
0.12
0.10
0.08
y
0.06
0.04
0.02
0.00
0 1 2 3 4 5
t
Figure 6.6: Step response and collected data
6.2.7 Continuous and discrete model
For the state controller design we need to model our motor in the state-space form. We define
the continuous-state and the discrete-state space model
In [ 2 4 ] : # Design C o n t r o l l e r Motor 1
In [ 2 5 ] : a = [ [ 0 , 1 ] , [ 0 , Dm/Jm ] ]
In [ 2 6 ] : b = [ [ 0 ] , [ 1 ] ]
In [ 2 7 ] : c = [ [ k t /Jm , 0 ] ] ;
In [ 2 8 ] : d = [ 0 ] ;
In [ 2 9 ] : s y s c=s s ( a , b , c , d ) # Continuous
s t a t e s pac e form
In [ 3 0 ] : Ts =0.01 # Sampling t i m e
In [ 3 1 ] : s y s = c2d ( s y s c , Ts , zoh ) # Discrete ss

form
6.2.8 Controller design

For the controller we set a bandwidth to 6 rad/s with a damping factor of = 2/2.
In [ 3 2 ] : # C o n t r o l s y s t e m d e s i g n
In [ 3 3 ] : print rank ( c t r b ( s y s . A, s y s . B) )==2 #

C o n t r o l l a b i l i t y c he c k
True
In [ 3 4 ] : # S t a t e f e e d b a c k w i t h i n t e g r a l p a r t
In [ 3 5 ] : wn=6
In [ 3 6 ] : x i=np . s q r t ( 2 ) /2
In [ 3 7 ] : a n g l e = np . a r c c o s ( x i )
We add a discrete integral part to eliminate the steady state error and we obtain an additional
state for the error between reference and output signal. The two matrices and required by
the pole placement routine must be extended with the additional state.
In [ 3 8 ] : c l p o l e s = wn a r r a y ( [ 1 , exp (1 j a n g l e ) , exp (1 j
angle ) ] ) # three pole s
In [ 3 9 ] : c l p o l e s d=sp . exp ( c l p o l e s Ts ) # Desired

discrete poles
In [ 4 0 ] : s z 1=sp . shape ( s y s .A) ;
In [ 4 1 ] : s z 2=sp . shape ( s y s . B) ;
In [ 4 2 ] : # Add d i s c r e t e i n t e g r a t o r f o r s t e a d y s t a t e z e r o
error
In [ 4 3 ] : P h i f=np . v s t a c k ( ( s y s . A, s y s . CTs ) )
In [ 4 4 ] : P h i f=np . h s t a c k ( ( P h i f , [ [ 0 ] , [ 0 ] , [ 1 ] ] ) )
In [ 4 5 ] : G f=np . v s t a c k ( ( s y s . B , z e r o s ( ( 1 , 1 ) ) ) )
In [ 4 6 ] : k=p l a c e ( P h i f , G f , c l p o l e s d )
6.2.9 Observer design

Now we can implement the observer: in this example we choose a reduced-order observer and
we can use the function provided by the yottalab module to obtain it.
In [ 4 7 ] : #Reduced o r d e r o b s e r v e r
In [ 4 8 ] : print rank ( obsv ( s y s . A, s y s . C) )==2 #

O b s e r v a b i l i t y c he c k
True
In [ 4 9 ] : p o c =10max( abs ( c l p o l e s ) )
In [ 5 0 ] : p od=sp . exp ( p o c Ts ) ;
In [ 5 1 ] : T= [ 0 , 1 ]
In [ 5 2 ] : r o b s=r e d o b s ( s y s , T , [ p od ] )
6.2.10 Controller in compact form

The yottalab function comp form i allows to integrate the controller gains and the observer
into an unique block.
In [ 5 3 ] : # C o n t r o l l e r + i n t e g r a l + o b s e r v e r i n compact
form
In [ 5 4 ] : c o n t r I=c o m p f o r m i ( s y s , r o b s , k )
6.2.11 Anti windup

The last operation consists in dividing the controller into an input part and a feedback part in
order to realize the anti-windup mechanism and considering the saturation block.
In [ 5 5 ] : # Anti windup
In [ 5 6 ] : [ g s s i n , g s s o u t ]= s e t a w ( c o n t r I , [ 0 , 0 ] )
6.2.12 Simulation of the controlled plant

The block diagram of the final controlled system is represented in figure 6.7.
Figure 6.7: Block diagram of the controlled system
It is not possible to simulate the resulting system in Python because of:
The controller is discrete and the plant is continuous. At present it is not possible to
perform hybrid simulation. In some cases we can substitute the plant with the discrete-
time system and perform a discrete simulation.
The block CTRIN has two inputs. The step function can only find the output from a
single input.
The control toolbox can handle only linear system (and there is a saturation in the final
system).
Chapter 7
Hybrid simulation and code generation
7.1 Basics
CACSD environments usually offer a graphical editor to perform the hybrid simulation (MatlabSimulink,
ScioslabScicos, ScilabxCos etc.).
The pyEdit.py application should cover this task for the Python Control environment.
In the following well explain how it is possible, from the pyEdit schematics, to generate code
for the hybrid simulation. Code for the RT controller can be generated in the same way: users
should only replace the mathematical model of the plant with the blocks interfacing the sensors
and the actuators of the real system.
7.2 pyEdit
The application pyEdit is a block diagram editor to design schematics for simulation and
code generation.
Starting points for the pyEdit application were the PySimEd project ([20]) and the qtnodes-
develop project ([21]).
PyEdit offers the most used blocks in control design. A little set of these blocks is shown in
figure 7.1.
The application offers set of operations in the toolbar as shown in the figure 7.2 and other
operations are available as popup menu by clicking on a block.
By clicking with the right mouse button on a block, a popup menu is shown, offering the
following operations:
Block I/Os to modify (if possible) the number of input and output ports of the block
Flip block Flip left/right the block
Change name Each block in the diagram must have a unique name
Block parameters to modify the parameters: this operation is available with a double click
tool
Clone block to get a copy of the selected block
61
62 CHAPTER 7. HYBRID SIMULATION AND CODE GENERATION
Figure 7.1: Some pyEdit blocks for control design
rem
gen
de
con
File ops.
sta
let
rt p
era
ov
nec
ee
en
te
yth
lem
od
cod
on
en
e
e
Figure 7.2: The pyEdit application

7.3. BASIC OPERATIONS 63
7.3 Basic operations

7.4 Configuring some parameters
The file const.py in the pyeditor folder contains some definitions required ba he applications.
In particular it is possible to change the variables path and pycmd.
7.4.1 Inserting a block

Get a block from a library and drag it into the main window.
7.4.2 connecting blocks

Select the connect tool from the toolbar.
Move to the output port of a block or to a node.
Click and release the left button of the mouse.
Move the mouse to draw the connection.
Click again the left mouse on an input port of a block to finish the connection or click
the mouse to obtain a node and to continue to draw the connection.
7.4.3 Inserting a node

Select the connect tool from the toolbar.
Click with the right mouse button on a connection to create a new node.
If a new node is needed into a connection simply click on it with the right mouse button.
7.4.4 Deleting a block or a node

Select delete element from the toolbar.
Move to a block or node and click with the left mouse button.
7.5 Remove a node

Select remove node from the toolbar.
Move to the node.
Click with the left mouse button on the node. The connection is maintained but the node
is cleared.
64 CHAPTER 7. HYBRID SIMULATION AND CODE GENERATION
Chapter 8
Simulation and Code generation
Each element of a block diagram is defined with two functions:

The interface function that describes how the block must be drawn in the block diagram
The Implementation function that contains the code to be executed to perform the tasks
related with this block.
In addition we need to know all the nodes connected to the inputs and to the outputs of each
block.
8.1 Interface functions

Each block is defined into a library file with extension .blk, stored in the blocks folder. The
library file is defined using the XML syntax. The blocks are defined with the following fields:
<blockdata>
<blockname>STEP</blockname>
<inputs>0</inputs>
<outputs>1</outputs>
<settable>0</settable>
<icon>STEP</icon>
<params>stepBlk|Step Time: 1|Step Value: 1</params>
</blockdata>
blockname is the name of the block which can be changed in the block diagram
iputs give the number of the input ports of the block
outputs give the number of the output ports of the block
settable indicates if the number of input or output ports can be modified in the block diagram
icon is the name of the PNG file of the block icon
params is a list containing the blocks parameters
The block libraries are loaded after launching the pyEdit application as shown in figure 8.1
Each block must be renamed with a unique name (popup menu Change name), and its
parameters can be modified directly in the pyEdit application with a double click.
65
66 CHAPTER 8. SIMULATION AND CODE GENERATION
Figure 8.1: Window with the block libraries

8.2. THE IMPLEMENTATION FUNCTIONS 67
8.2 The implementation functions

In a schematic, each block can be described with the functions (8.1) for continuous-time systems
or (8.2) for discrete-time systems.
y = g(x, u, t)
(8.1)
x = f(x, u, t)
yk = g(xk , uk , k)
(8.2)
xk+1 = f(xk , uk , k)
The g(. . .) function represents the static part of the block. This function is used to read inputs,
read sensors, write actuators or update the outputs of the block.
The second function (f(. . .)) is only required if the block has internal states, and it is only used
by dynamic systems. In addition, each block implements two other functions, one for the block
initialization and one to cleanly terminate it.
All these functions are programmed as C-files, compiled and archived into a library.
8.3 I/O connections

After clicking on the code generation tool on the toolbar, the user is asked to introduce some
parameters in a dialog box (see figure 8.2).
Figure 8.2: Dialog for code generation
In this dialog it is possible to choose the template makefile for simulation or real-time execu-
tion, the sampling time of the system and some additional libraries, reuired for the simulation
with FMU packages.
After this first setup it is possible to translate the block diagram into a list of elements of
the class RCPblk provided by the suspictrl package. This class contains all the information
required for the code generation and can be expanded in the future to handle additional fields
(ex. the type of the I/O signals: int, double etc.).
This class contains the following fields:
fcn: the name of the C-Function to be used to handle this block
pin: an array containing the id of the input nodes

pout: an array containing the id of the output nodes
nx: the number of internal states (continuous or discrete)
uy: a flag which indicates a direct dependency between input and output signals (feed-through
flag).
realPar: an array containing the real parameters of the block
intPar: an array containing the integer parameters of the block
str: a string related to the block
For example, the diagram in figure 8.3 is translated into the following code
from s u p s i c t r l . RCPblk import
STEP = s t e p B l k ( [ 1 ] , 1 , 1)
PM = sumBlk ( [ 1 , 3 ] , [ 2 ] , [1 , 1])
CSS = c s s B l k ( [ 2 ] , [ 3 ] , sys , 0)
PRINT = p r i n t B l k ( [ 1 , 3 ] )
b l k s = [ STEP ,PM, CSS , PRINT , ]

fname = s t e p
genCode ( fname , 0 . 0 1 , b l k s )
genMake ( fname , sim . tmf , addObj = )
1 2
3
Figure 8.3: Simple block diagram
The block CSS has one input connected to node and one output connected to node , it is
a continuous transfer function (cssBlk, 1/(s + 1)) with zero initial conditions. The PM block
has 2 inputs connected to node and , one output connected to node and performs a
subtraction of the output from the input signals.
8.4 Translating the block list into C-code

8.4.1 Finding the right execution sequence
Before starting with the translation of the block diagram into C-code, we need to find the
correct sequence of execution of the blocks. This task can be performed by analizing the uy
8.4. TRANSLATING THE BLOCK LIST INTO C-CODE 69
flag of the block object. When in a block the uy flag is set to 1, we need the output of the
blocks connected at his input before starting to update his output. This means that we have
to generate a dependency tree of all the blocks and then we must rearrange the order of the
block list for code generation.
In linear blocks for examples, the uy flag is set if the D matrix is not null.
In the blockdiagram of figure 8.3, the PM and the PRINT blocks require to know their inputs
before update their outputs.
In [ 5 ] : NrOfNodes = 3
In [ 6 ] : o r d e r e d l i s t = d e tB l k S e q ( NrOfNodes , b l k s )
In [ 7 ] : f o r n in o r d e r e d l i s t :
...: print n
...:
F u n c ti o n : css
In p u t p o r t s : [2]
Outputs p o r t s : [3]
Nr . o f s t a t e s : [2 0]
R e l a t i o n u>y : 0
Real p a r a m e te r s : [ [ 0. 0 . 1. 1 . 1. 1.
0. 0 . 1. 0 . 0. 0.]]
I n t e g e r p a r a m e te r s : [ 2 1 1 1 5 7 9 1 0 ]
S t r i n g Parameter :
F u n c ti o n : step
In p u t p o r t s : []
Nr . o f s t a t e s : [0 0]
Real p a r a m e te r s : [1 1]
I n t e g e r p a r a m e te r s : []
F u n c ti o n : print
In p u t p o r t s : [1 3]
Outputs p o r t s : []
Nr . o f s t a t e s : [0 0]
Real p a r a m e te r s : []
F u n c ti o n : sum
In p u t p o r t s : [1 3]
Nr . o f s t a t e s : [0 0]
Real p a r a m e te r s : [ 1 1]
If the block diagram contains algebraic loops it is not possible to find a solution for the det-
BlkSeq function and an error is raised.
8.4.2 Generating the C-code

Starting from the ordered list of blocks, it is possible to generate C-code.
The code contains 3 functions:
The initialization function
The termination function
The periodic task
8.4.3 The init function
In this function each block is translated into a python block structure defined as follows:
typedef struct {
int nin ; / Number o f i n p u t s /
i n t nout ; / Number o f o u t p u t s /
i n t nx ; / Cont . and D i s c r s t a t e s /
v o i d u ; / i n p u t s /
v o i d y ; / o u t p u t s /
double r eal P ar ; / Real p a r a m e te r s /
int intPar ; / I n t p a r a m e te r s /
char str ; / S t r i n g /
v o i d p tr P a r ; / G e n e r i c p o i n t e r /
} python block ;
The nodes of the block diagram are defined as double variables and the inputs and outputs
of the blocks are defined as vectors of pointers to them.
...
/ Nodes /
s t a t i c d o u b l e Node 1 [ ] = { 0 . 0 } ;
/ In p u t and o u t p u t s /
s t a t i c void i nptr 0 [ ] = {0};
s t a t i c void outptr 0 [ ] = {0};
s t a t i c void i nptr 2 [ ] = {0 ,0};
s t a t i c void i nptr 3 [ ] = {0 ,0};
...
i n p t r 0 [ 0 ] = ( v o i d ) Node 2 ;
o u t p t r 0 [ 0 ] = ( v o i d ) Node 3 ;
..
b l o c k t e s t [ 0 ] . nin = 1 ;
b l o c k t e s t [ 0 ] . nout = 1 ;
b l o c k t e s t [ 0 ] . nx = nx 0 ;
block test [ 0 ] . u = inptr 0 ;
block test [ 0 ] . y = outptr 0 ;
...
After this initialization phase, the implementation functions of the blocks are called with the
flag INIT.
8.5. THE MAIN FILE 71
c s s ( INIT , &b l o c k t e s t [ 0 ] ) ;
s t e p ( INIT , &b l o c k t e s t [ 1 ] ) ;
print ( INIT , &b l o c k t e s t [ 2 ] ) ;
sum( INIT , &b l o c k t e s t [ 3 ] ) ;
8.4.4 The termination function
This procedure calls the implementation functions of the blocks with the flag END.
8.4.5 The ISR function
This procedure represents the periodic task of the RT execution. First of all, the implementation
functions are called with the flag OUT, in order to perform the output update of each blocks.
As a second step, the implementation functions of the block containing internal states (nx 6= 0)
are called with the flag STUPD (state update).
...
c s s (OUT, &b l o c k t e s t [ 0 ] ) ;
s t e p (OUT, &b l o c k t e s t [ 1 ] ) ;
print (OUT, &b l o c k t e s t [ 2 ] ) ;
sum(OUT, &b l o c k t e s t [ 3 ] ) ;
...
c s s (OUT, &b l o c k t e s t [ 0 ] ) ;
c s s (STUPD, &b l o c k t e s t [ 0 ] ) ;
...
8.5 The main file
The core of the RT execution is represented by the python main rt.c file. During the RT
execution, the main procedure starts a high priority thread for handling the RT behavior of
the system. The following main file, for example, is used to launch the executable in a Linux
preempt rt environment.
v o i d r t t a s k ( v o i d p )
{
...
param . s c h e d p r i o r i t y = p r i o ;
i f ( s c h e d s e t s c h e d u l e r ( 0 , SCHED FIFO , &param )==1){
perror ( sched setscheduler f a i l e d ) ;
e x i t ( 1) ;
}
...
d o u b l e Tsamp = NAME(MODEL, g e t t s a m p ) ( ) ;
...
NAME(MODEL, i n i t ) ( ) ;
while ( ! end ) {
/ w a i t u n t i l l next s h o t /
c l o c k n a n o s l e e p (CLOCK MONOTONIC,
TIMER ABSTIME, &t , NULL) ;
...
/ p e r i o d i c t a s k /
NAME(MODEL, i s r ) (T) ;
...
}
NAME(MODEL, e n d ) ( ) ;
}
Chapter 9
Example
9.1 The plant
One of the educational plants available at the SUPSI laboratory is the system shown in fig-
ure 9.1. This example is located in to the pycontrol/Tests/ControlDesign/DisksAndSpring
folder,
Figure 9.1: The disks and spring plant
Two disks are connected by a spring. The goal for the students is to control the angle of the
disk on the right by applying an appropriate torque to the disk on the left.
The physical model of this plant can be directly calculated in python using for example the
sympy toolbox. Sympy can deliver a symbolic description of the system and through a python
dictionary it is possible to easily obtain the numerical matrices of the state-space representation
of the plant.
73
74 CHAPTER 9. EXAMPLE
In [ 4 ] : A
Out [ 4 ] :
m a tr i x ( [ [ 0 , 0 , 1 , 0 ] ,
[0 , 0 , 0 , 1] ,
[ c / J1 , c / J1 , (d d1 ) / J1 , d/ J1 ] ,
[ c / J2 , c / J2 , d/ J2 , (d d2 ) / J2 ] ] )
In [ 5 ] : B1
Out [ 5 ] :
m a tr i x ( [ [ 0 , 0 ] ,
[0 , 0] ,
[ k t1 / J1 , 0 ] ,
[ 0 , k t2 / J2 ] ] )
In [ 6 ] : B = B1 [ : , 0 ]
In [ 7 ] : C
Out [ 7 ] : [ [ 1 , 0 , 0 , 0 ] , [ 0 , 1 , 0 , 0 ] ]
In [ 8 ] : C2
Out [ 8 ] : [ 0 , 1 , 0 , 0 ]
In [ 9 ] : D
Out [ 9 ] : [ [ 0 ] , [0]]
In [ 1 0 ] : D2
Out [ 1 0 ] : [ 0 ]
The control system toolbox and the additional yottalab.py package contain all the functions
required for the design of the controller. In this case we design a discrete-state feedback
controller with integral part for eliminating steady-state errors. The states are estimated with
a reduced-order observer. In addition, an anti-windup mechanism has been implemented. The
sampling time is set to 10 ms.
The yottalab module offers 3 functions that facilitate the controller design:
The function red obs(sys, T, poles) which implements the reduced-order observer for the
system sys, using the submatrix T (required to obtain the estimator C-matrix and the
desired state-estimator poles poles.
P = [C; T ] C = C P 1 = [Iq , O(nq) ]
The function comp form i(sys,obs,K,Cy) that transforms the observer obs with the
state-feedback gains K and the integrator part into a single dynamic block with the
reference signal and the two positions 1 and 2 as inputs and the control current I1 as
output. The vector Cy is used to select 2 as the output signal that is compared with the
reference signal for generating the steady-state error for the integral part of the controller.
The function set aw(sys,poles) that transforms the previous controller (Contr(s) =
N(s)/D(s)) in an input state-space system and a feedback state-space system, imple-
menting the anti-windup mechanism. The vector poles contains the desired poles of the
two new systems (Dnew (s)) (see figure 9.2).
9.2. THE PLANT MODEL 75
N(s)
sysin (s) =
Dnew (s)
D(s)
sysf bk (s) = 1
Dnew (s)
1 sys_in 1
In1 Out1
LTI System Saturation
sys_fbk
LTI System1
Figure 9.2: Anti windup
9.2 The plant model
# Sampling t i m e
t s = 10 e3
g s s 1 = s s (A, B, C,D)
g s s = s s (A, B, C2 , D2)
gz = c2d ( g s s , ts , zoh )
9.3 Controller design
# Control design
wn = 10
x i 1 = np . s q r t ( 2 ) /2
xi2 = 0.85
cl p 1 = [ 1 , 2 x i 1 wn , wn 2 ]
cl p 2 = [ 1 , 2 x i 2 wn , wn 2 ]
cl p 3 = [ 1 , wn ]
cl p o l y 1 = sp . polymul ( c l p 1 , c l p 2 )
cl p o l y = sp . polymul ( c l p o l y 1 , c l p 3 )
cl p o l e s = sp . r o o t s ( c l p o l y ) # Desired continuous
poles
c l p o l e s d = sp . exp ( c l p o l e s t s ) # D e s i r e d d i s c r e t e p o l e s
# Add d i s c r e t e i n t e g r a t o r for steady s t a t e zero error

Phi f = np . v s t a c k ( ( gz . A, gz . C t s ) )
Phi f = np . h s t a c k ( ( P h i f , [ [ 0 ] , [ 0 ] , [ 0 ] , [ 0 ] , [ 1 ] ] ) )
G f = np . v s t a c k ( ( gz . B, z e r o s ( ( 1 , 1 ) ) ) )
# P ol e pl ac e m e n t
k = placep ( Phi f , G f , c l p o l e s d )
9.4 Observer design
# O b s e r v e r d e s i g n r e du c e d o r d e r o b s e r v e r
p o l i o = 5 c l p o l e s [ 0 : 2 ]
p o l i o z = sp . exp ( p o l i o t s )
d i s k s = s s (A, B, C,D)
d i s k s z = S t a t e S p a c e ( gz . A, gz . B , C, D, t s )
T = [[0 ,0 ,1 ,0] ,[0 ,0 ,0 ,1]]
# Reduced o r d e r o b s e r v e r
r o b s = r e d o b s ( d i s k s z ,T, p o l i o z )
# C o n t r o l l e r and o b s e r v e r i n t h e same m at r i x Compact

form
c o n t r I = comp form i ( d i s k s z , r obs , k , [ 0 , 1 ] )
# Implement a n t i windup
[ gs s i n , gs s out ] = set aw ( contr I , [ 0 . 1 , 0 . 1 , 0 . 1 ] )
9.5 Simulation
We can perform the simulation of the discrete-time controller with the continuous-time math-
ematic plant model using the block diagram of figure 9.3
This diagram is stored as disks sim.dgm in the folder.
The plant is represented by a continuous-time state-space block with 1 input and 2 outputs.
The controller implements the state-feedback gains and the state observer and it has been split
into a CTRIN block and a CTRFBK block in order to implement the anti-windup mechanism.
9.6. REAL-TIME CONTROLLER 77
Figure 9.3: Block diagram for the simulation
We can now generate the code for the simulation and launch the generated executable. The
template makefile for this executable is sim.tmf.
After creating the script for code generation (button in the toolbar) it is possible to proceed
with the simulation,
Open a python terminal (for example by clicking on the python button in the toolbar), and
give the following commands:
run DisksAndSpringKane . py
run i d i s k s s i m . py
! make
! pySim d i s k s s i m 40
The plots resulting from the simulation are shown in figure 9.4.
9.6 Real-time controller

In order to generate the RT controller for the real plant, we first have to substitute the plant
with the interfaces for sensors and actuators using blocks that send and receive CAN message
using a USB dongle of Peak System. The template makefile for this system is now rt.tmf, that
allows to generate code with real-time behaviour.
The block diagram for the real-time controller is represented in figure 9.5.
The motor position can be plotted in python at the end of the execution (see figure 9.6).
Figure 9.4: Simulation of the plant
Figure 9.5: Block diagram for the RT implementation

9.6. REAL-TIME CONTROLLER 79
Figure 9.6: RT execution

Bibliography
[1] VirtualBox. [Online]. Available: https://www.virtualbox.org
[2] Download Anaconda. [Online]. Available: http://continuum.io/downloads
[3] Obtaining NumPy and SciPy libraries. [Online]. Available:

http://www.scipy.org/scipylib/download.html
[4] Python Control toolbox. [Online]. Available: https://github.com/python-control/python-

control
[5] Slycot. [Online]. Available: https://github.com/jgoppert/Slycot
[6] pyControlDistro.tgz. [Online]. Available: http://robertobucher.dti.supsi.ch/python/
[7] Slycot Master - 0.1.0. [Online]. Available: http://www.lfd.uci.edu/ gohlke/python-

libs/#slycot
[8] NumPy for Matlab Users. [Online]. Available:

http://wiki.scipy.org/NumPy for Matlab Users
[9] David J. Pine. Introduction to Python for Science. [Online]. Available:

https://github.com/djpine/pyman
[10] Tentative NumPy Tutorial. [Online]. Available:

http://wiki.scipy.org/Tentative NumPy Tutorial
[11] SciPy Tutorial. [Online]. Available: http://docs.scipy.org/doc/scipy-

0.14.0/reference/tutorial/index.html
[12] Matplotlib. [Online]. Available: http://matplotlib.org/
[13] SymPy Tutorial. [Online]. Available: http://docs.sympy.org/dev/tutorial/index.html
[14] Kanes Method in Physics/Mechanics. [Online]. Available:

http://docs.sympy.org/0.7.5/modules/physics/mechanics/kane.html
[15] Kanes Method and Lagranges Method (Docstrings). [Online]. Available:

http://docs.sympy.org/latest/modules/physics/mechanics/api/kane lagrange.html
[16] P. C. M. . T. R. Kane. Motion Variables Leading to Efficient Equations of Motions. [Online].

Available: http://www2.mae.ufl.edu/ fregly/PDFs/efficient generalized speeds.pdf
81
82 BIBLIOGRAPHY
[17] A Brief Synopsis of Kanes Method. [Online]. Available: www.cs.cmu.edu/ delucr/kane.doc
[18] L. A. Sandino1, M. Bejar2, and A. Ollero1. Tutorial for the applica-

tion of Kanes Method to model a small-size helicopter. [Online]. Available:
http://grvc.us.es/publica/congresosint/documentos/Sandino RED-UAS Sevilla2011.pdf
[19] A. Purushotham1 and M. J.Anjeneyulu. Kanes Method for Robotic Arm Dynamics: a
Novel Approach. [Online]. Available: http://www.iosrjournals.org/iosr-jmce/papers/vol6-
issue4/B0640713.pdf
[20] PySimEd. [Online]. Available: http://www.kiwiki.info/index.php/PySimEd
[21] A port of qnodeseditor to PySide. [Online]. Available: https://github.com/cb109/qtnodes

Book Python For Control

Uploaded by

Copyright:

Available Formats

Book Python For Control

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Book Python For Control

Uploaded by

Copyright:

Available Formats

Python for control purposes

Prof. Roberto Bucher

2 Python - Some hints for Matlab users 11

3 The Python Control System toolbox 19

6.2.11 Anti windup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

7 Hybrid simulation and code generation 61

8 Simulation and Code generation 65

4.1 Step response for continuous-time systems . . . . . . . . . . . . . . . . . . . . . 25

5.1 Inverted pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.1 Bode diagram of the plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

7.1 Some pyEdit blocks for control design . . . . . . . . . . . . . . . . . . . . . . . . 62

8.1 Window with the block libraries . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

9.1 The disks and spring plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

1.1 Install the packages

python-scipy (Vers. 0.14)

python-sympy (Vers. 0.7.5)

sudo apt-get build-dep python-scipy

The Python Control toolbox [4]

The Slycot libraries [5]

The SupsiCtrl package [6]

1.5 Mac OSX

Python - Some hints for Matlab users

2.2 The python shell

ipython qtconsole --pylab inline

2.3 Python vs. Matlab

2.4 List, array and matrix

In [ 1 ] : from numpy import mat , matrix , a r r a y

In [ 1 ] : from numpy import mat , matrix , a r r a y

In [ 1 ] : from numpy import a r r a y

In [ 1 ] : from numpy import mat

2.12 Multidimensional arrays and matrices

In [ 1 ] : from numpy import a r r a y , mat

The Python Control System toolbox

LTI systems can be described in state-space form or as transfer functions.

3.3 Continuous systems

3.5 Transfer function

3.7 Discrete time systems

3.8 State-space representation

3.9 Transfer function

In [ 2 ] : from c o n t r o l . Matlab import

In [ 2 ] : from c o n t r o l . Matlab import c2d

In [ 3 ] : from s u p s i c t r l . y o t t a l a b import d2c

and transfer functions into one of the state-space representation

3.12 Models interconnection

4.1 Time response

In [ 2 ] : from c o n t r o l . matlab import

Figure 4.1: Step response for continuous-time systems

In [ 2 ] : from c o n t r o l . matlab import

In [ 8 ] : plt . step ( t , y [ 0 ] ) # get f i r s t

Figure 4.2: Step response for discrete-time systems

In [ 2 ] : from c o n t r o l . matlab import

Figure 4.3: Continuous time systems - Initial condition response

In [ 2 ] : from c o n t r o l . matlab import

Figure 4.4: Continuous time systems - Impulse response

...: plt . grid ()

In [ 2 ] : from c o n t r o l . matlab import

Figure 4.5: Continuous time systems - Generic input

4.2 Frequency analysis

In [ 2 ] : from c o n t r o l . matlab import

Nc ol! "lo t

In [ 1 ] : from c o n t r o l import 40.0 40.0

Figure 6.3: Bode diagram - G (dashed), GpiG (dotted) and GpiGLead*G