Python for control purposes

Prof. Roberto Bucher

Scuola Universitaria Professionale della Svizzera Italiana
Dipartimento Tecnologie Innovative
6928 Manno
roberto.bucher@supsi.ch

July 27, 2018

2
Contents

1 Introduction 9
1.1 Install the packages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.2 The simplest way . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3 Linux . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.1 Required packages . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.3.2 Install the pycontrol.tgz package . . . . . . . . . . . . . . . . . . . . . . . 10
1.4 Windows . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.5 Mac OSX . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2 Python - Some hints for Matlab users 13

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

3 The Python Control System toolbox 21

3.1 Basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.2 Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.3 Continuous systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.4 State-space representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.5 Transfer function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.6 Zeros-Poles-Gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.7 Discrete time systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.8 State-space representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.9 Transfer function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.10 Conversions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
3.11 Casting . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.12 Models interconnection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3
4 CONTENTS

4 System analysis 27
4.1 Time response . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.2 Frequency analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.3 Poles, zeros and root locus analysis . . . . . . . . . . . . . . . . . . . . . . . . . 34

5 Modeling 37
5.1 Model of a DC motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.1.1 Plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.1.2 Modules and constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.1.3 Reference frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.1.4 Body and inertia of the load . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.1.5 Forces and torques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
5.1.6 Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5.1.7 State-space matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5.2 Model of the inverted pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.2.1 Modules and constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
5.2.2 Frames - Car and pendulum . . . . . . . . . . . . . . . . . . . . . . . . . 42
5.2.3 Points, bodies, masses and inertias . . . . . . . . . . . . . . . . . . . . . 42
5.2.4 Forces, frictions and gravity . . . . . . . . . . . . . . . . . . . . . . . . . 42
5.2.5 Final model and linearized state-space matrices . . . . . . . . . . . . . . 43
5.3 Model of the Ball-on-Wheel plant . . . . . . . . . . . . . . . . . . . . . . . . . . 44
5.3.1 Modules and constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.3.2 Reference frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.3.3 Centers of mass of the ball . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.3.4 Masses and inertias . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
5.3.5 Forces and torques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
5.3.6 Kane’s model and linearized state-space matrices . . . . . . . . . . . . . 46

6 Control design 49
6.1 PI+Lead design example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
6.1.1 Define the system and the project specifications . . . . . . . . . . . . . . 49
6.1.2 PI part . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
6.1.3 Lead part . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
6.1.4 Controller Gain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
6.1.5 Simulation of the controlled system . . . . . . . . . . . . . . . . . . . . . 54
6.2 Discrete-state feedback controller design . . . . . . . . . . . . . . . . . . . . . . 55
6.2.1 Plant and project specifications . . . . . . . . . . . . . . . . . . . . . . . 55
6.2.2 Motor parameters identification . . . . . . . . . . . . . . . . . . . . . . . 55
6.2.3 Required modules . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
6.2.4 Function for least square identification . . . . . . . . . . . . . . . . . . . 56
6.2.5 Parameter identification . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
6.2.6 Check of the identified parameters . . . . . . . . . . . . . . . . . . . . . . 57
6.2.7 Continuous and discrete model . . . . . . . . . . . . . . . . . . . . . . . 57
6.2.8 Controller design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
6.2.9 Observer design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
6.2.10 Controller in compact form . . . . . . . . . . . . . . . . . . . . . . . . . 60
CONTENTS 5

6.2.11 Anti windup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

6.2.12 Simulation of the controlled plant . . . . . . . . . . . . . . . . . . . . . . 60

7 Hybrid simulation and code generation 63

7.1 Basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.2 pyEdit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.2.1 The editor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.2.2 The first example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
7.2.3 Personalize the most used blocks . . . . . . . . . . . . . . . . . . . . . . 66
7.2.4 Defining new blocks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
7.2.5 Some remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
7.3 The editor window . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
7.3.1 The toolbar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
7.3.2 Operations with the right mouse button . . . . . . . . . . . . . . . . . . 68
7.3.3 Operations with the right mouse button on a block . . . . . . . . . . . . 68
7.3.4 Operations with the right mouse button on a connection . . . . . . . . . 69
7.3.5 Operations with the right mouse button on a node . . . . . . . . . . . . 70
7.3.6 Behaviour of the right mouse button by drawing a connection . . . . . . 70
7.4 Basic editor operations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.4.1 Inserting a block . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.4.2 Connecting blocks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.4.3 Inserting a node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.4.4 Deleting a block or a node . . . . . . . . . . . . . . . . . . . . . . . . . . 70
7.5 Remove a node . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

8 Simulation and Code generation 73

8.1 Interface functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
8.2 The implementation functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
8.3 Translating the block into the RCPblk class . . . . . . . . . . . . . . . . . . . . 75
8.4 Special dialog box for the block parameters . . . . . . . . . . . . . . . . . . . . . 75
8.5 Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
8.6 The parameters for the code generation . . . . . . . . . . . . . . . . . . . . . . . 77
8.7 Translating the diagram into elements of the RCPdlg class . . . . . . . . . . . . 77
8.8 Translating the block list into C-code . . . . . . . . . . . . . . . . . . . . . . . . 78
8.8.1 Finding the right execution sequence . . . . . . . . . . . . . . . . . . . . 78
8.8.2 Generating the C-code . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
8.8.3 The init function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
8.8.4 The termination function . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
8.8.5 The ISR function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
8.9 The main file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

9 Example 83
9.1 The plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
9.2 The plant model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
9.3 Controller design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
9.4 Observer design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
6 CONTENTS

9.5 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
9.6 Real-time controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
List of Figures

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

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

5.1 Inverted pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

5.2 Inverted pendulum - Real plant . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
5.3 Ball-On-Wheel plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

6.1 Bode diagram of the plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

6.2 Bode diagram: G (dashed) and Gpi*G . . . . . . . . . . . . . . . . . . . . . . . 51
6.3 Bode diagram - G (dashed), Gpi*G (dotted) and Gpi*GLead*G . . . . . . . . . 53
6.4 Bode diagram - G (dashed), Gpi*G (dotted), Gpi*GLead*G (dot-dashed) and
K*Gpi*GLead*G . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
6.5 Step response of the controlled plant . . . . . . . . . . . . . . . . . . . . . . . . 55
6.6 Step response and collected data . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
6.7 Block diagram of the controlled system . . . . . . . . . . . . . . . . . . . . . . . 61

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

7.2 The first example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
7.3 The pyEdit environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
7.4 Result from the drag and drop operations . . . . . . . . . . . . . . . . . . . . . 65
7.5 Result after parametrization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
7.6 Result (plot) of the simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
7.7 The pyEdit application . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

8.1 Window with the block libraries . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

8.2 Dialog box for the Pulse generator block . . . . . . . . . . . . . . . . . . . . . . 76
8.3 Dialog for code generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
8.4 Simple block diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78

7
8 LIST OF FIGURES

9.1 The disks and spring plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

9.2 Anti windup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
9.3 Block diagram for the simulation . . . . . . . . . . . . . . . . . . . . . . . . . . 87
9.4 Simulation of the plant . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
9.5 Block diagram for the RT implementation . . . . . . . . . . . . . . . . . . . . . 88
9.6 RT execution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
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
1.3.1 Required packages
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 don’t 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

• jupyter

9
10 CHAPTER 1. INTRODUCTION

• jupyter-qtconsole

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 pycontrol.tgz 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.3.2 Install the pycontrol.tgz package

After downloading the pycontrol.tgz package, unpack it for example under “/usr/local” and
you obtain a new folder “pycontrol”.
The installation is quite simple. Launch as superuser the command

make

or

make reduced

if you don’t want to have COMEDI installed.

The installation download the control-master package, the slycot-master package and install
the full software.
As last step it is important to update the “.bashrc” file as normal user with the command

make user

The system has been tested under “Debian stable”, “Debian testing” with python-2.7, python-
3.5 and python-3.6.
1.4. WINDOWS 11

1.4 Windows
Under Windows it is sufficient to install the “Anaconda” package [2], to have all the python and
jupyter 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].
12 CHAPTER 1. INTRODUCTION
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 jupyter. In this interactive form, when started as jupyter-
qtconsole, jupyter 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 jupyter launched with the shell commands

jupyter-qtconsole

Sometimes not all the functions and modules are explicitly loaded at the beginning of the
examples. In addition, jupyter implements some useful commands like for example whos and
run (for launching scripts).
In the jupyter shell it is possible to start single commands, paste a set of commands or launch
a “.py” program using run.

13
14 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 ( [ [ −Dm∗w( 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]
[ k t /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 15

In [ 1 ] : a = [ [ [ 1 , 2 ] , [ 3 , 4 ] ] , ’ abcd ’ , 2 ]

In [ 2 ] : b = [ [ 1 , 2 , 3 ] , [ 4 , 5 , 6 ] , [ 7 , 8 , 9 ] ]

In [ 3 ] : whos
Variable Type Data / I n f o
−−−−−−−−−−−−−−−−−−−−−−−−−−−−
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.
16 CHAPTER 2. PYTHON - SOME HINTS FOR MATLAB USERS

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 ] :
matrix ( [ [ 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 )
<ipython −input −9−8201c27d19b7> 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 ] :
matrix ( [ [ 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 17

In [ 1 ] : a = [ 1 , 2 , 3 , 4 , 5 ]

In [ 2 ] : %whos
Variable Type Data / I n f o
−−−−−−−−−−−−−−−−−−−−−−−−−−−−
a list n=5

In [ 3 ] : a [ 3 ]
Out [ 3 ] : 4

In [ 4 ] : b = [ [ 1 , 2 , 3 ] , [ 4 , 5 , 6 ] ]

In [ 5 ] : %whos
Variable Type Data / I n f o
−−−−−−−−−−−−−−−−−−−−−−−−−−−−
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
Variable Type Data / I n f o
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
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
18 CHAPTER 2. PYTHON - SOME HINTS FOR MATLAB USERS

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
Variable Type Data / I n f o
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
a matrix [[1 2 3 4 5]]
b matrix [ [ 1 2 3]\n [4 5 6 ] ]

In [ 5 ] : a . shape
Out [ 5 ] : ( 1 , 5 )

In [ 6 ] : b . shape
Out [ 6 ] : ( 2 , 3 )
2.12. MULTIDIMENSIONAL ARRAYS AND MATRICES 19

In [ 7 ] : a [ 0 , 2 ]
Out [ 7 ] : 3

In [ 8 ] : b [ 1 , 1 ]
Out [ 8 ] : 5

In [ 9 ] : b [ : , 0 ]
Out [ 9 ] :
matrix ( [ [ 1 ] ,
[4]])

In [ 1 0 ] : b [ 0 , : ]
Out [ 1 0 ] : m a t r 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 )

In [ 3 ] : a . shape
Out [ 3 ] : ( 3 , 3 , 3 )

In [ 4 ] : %whos
Variable Type Data / I n f o
−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
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 )
20 CHAPTER 2. PYTHON - SOME HINTS FOR MATLAB USERS
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.

21
22 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

I n [ 1 ] : from c o n t r o l import ∗

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 23

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 [ 1 ] : from c o n t r o l import ∗

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.ctr repl package implements both functions c2d and d2c with the methods “zoh’,
”foh“, ”tustin“ and ”matched“ (”matched“ is only implemented in c2d).
24 CHAPTER 3. THE PYTHON CONTROL SYSTEM TOOLBOX

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

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 t e 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 [ 1 ] : from c o n t r o l 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 . c t r l r e p l 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 25

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 [ 1 ] : from c o n t r o l import ∗

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
26 CHAPTER 3. THE PYTHON CONTROL SYSTEM TOOLBOX

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.

I n [ 1 ] : from c o n t r o l import ∗ 1.2

1.0
I n [ 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

I n [ 1 ] : from c o n t r o l import ∗

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

I n [ 3 ] : import m a t p l o t l i b . p y p l o t a s p l t

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 . xlabel ( ’ t ’ )
...: plt . ylabel ( ’y ’ )
...: plt . grid ()

Figure 4.1: Step response for continuous-time systems

27
28 CHAPTER 4. SYSTEM ANALYSIS

1.2
I n [ 1 ] : from c o n t r o l import ∗
1.0
I n [ 2 ] : from c o n t r o l . matlab import c2d
0.8
I n [ 3 ] : import m a t p l o t l i b . p y p l o t a s p l t

In [ 4 ] : g = t f ( 1 , [ 1 , 1 , 1 ] ) 0.6

y
I n [ 5 ] : gz=c2d ( g , 0 . 1 ) 0.4

I n [ 6 ] : t=a r a n g e ( 0 , 1 6 , 0 . 1 ) 0.2

I n [ 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 . step ( t , y .T [ 0 ] ) # transpose t

col matrix y [ 0 ]
...: plt . grid ()
...: plt . xlabel ( ’ t ’ )
...: plt . ylabel ( ’y ’ )

or alternatively

I n [ 1 ] : from c o n t r o l import ∗

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

I n [ 3 ] : import m a t p l o t l i b . p y p l o t a s p l t

In [ 4 ] : g = t f ( 1 , [ 1 , 1 , 1 ] )

I n [ 5 ] : gz=c2d ( g , 0 . 1 )

I n [ 6 ] : t=a r a n g e ( 0 , 1 6 , 0 . 1 )

I n [ 7 ] : y , t 1 = s t e p ( gz , t )

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

row from y m a t r i x
...: plt . grid ()
...: plt . xlabel ( ’ t ’ )
...: plt . ylabel ( ’y ’ )

Figure 4.2: Step response for discrete-time systems

4.1. TIME RESPONSE 29

1.4
I n [ 1 ] : from c o n t r o l import ∗
1.2

I n [ 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

In [ 3 ] : a = [ [ 0 , 1 ] , [ − 1 , − 1 ] ] 0.8

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
I n [ 7 ] : s y s=s s ( a , b , c , d )
0.4
0 2 4 6 8 10 12 14
I n [ 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 ()
...: plt . xlabel ( ’ t ’ )
...: plt . ylabel ( ’y ’ )

or alternatively

I n [ 1 ] : from c o n t r o l import ∗

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

I n [ 3 ] : import m a t p l o t l i b . p y p l o t a s p l t

In [ 4 ] : a = [ [ 0 , 1 ] , [ − 1 , − 1 ] ]

In [ 5 ] : b = [ [ 0 ] , [ 1 ] ]

In [ 6 ] : c = [ 1 , 0 ]

In [ 7 ] : d =[0]

I n [ 8 ] : s y s=s s ( a , b , c , d )

I n [ 9 ] : y , t= i n i t i a l ( s y s , X0 = [ 1 , 1 ] )

In [ 1 0 ] : plt . plot (t , y)
...: plt . xlabel ( ’ t ’ )
...: plt . ylabel ( ’y ’ )
...: plt . grid ()

Figure 4.3: Continuous time systems - Initial condition response

30 CHAPTER 4. SYSTEM ANALYSIS

0.6
I n [ 1 ] : from c o n t r o l import ∗
0.5
I n [ 2 ] : import m a t p l o t l i b . p y p l o t a s p l t

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
...: plt . ylabel ( ’y ’ )
0.0

or alternatively
0.1
0 2 4 6 8 10 12 14 16
t

I n [ 1 ] : from c o n t r o l import ∗

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

I n [ 3 ] : import m a t p l o t l i b . p y p l o t a s p l t

In [ 4 ] : g = t f ( 1 , [ 1 , 1 , 1 ] )

In [ 5 ] : y , t = impulse ( g )

In [ 6 ] : plt . plot (t , y)
...: plt . grid ()
...: plt . xlabel ( ’ t ’ )
...: plt . ylabel ( ’y ’ )

Figure 4.4: Continuous time systems - Impulse response

4.1. TIME RESPONSE 31

1.0
I n [ 1 ] : from c o n t r o l import ∗

I n [ 2 ] : import m a t p l o t l i b . p y p l o t a s p l t
0.5

I n [ 3 ] : g=t f ( [ 1 , 2 ] , [ 1 , 2 , 3 , 4 ] )

I n [ 4 ] : t=l i n s p a c e ( 0 , 6 ∗ p i ) 0.0

y
I n [ 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

I n [ 1 ] : from c o n t r o l import ∗

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

I n [ 3 ] : import m a t p l o t l i b . p y p l o t a s p l t

I n [ 4 ] : g=t f ( [ 1 , 2 ] , [ 1 , 2 , 3 , 4 ] )

I n [ 5 ] : t=l i n s p a c e ( 0 , 6 ∗ p i )

I n [ 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 . xlabel ( ’ t ’ )
...: plt . ylabel ( ’y ’ )
...: plt . grid ()

Figure 4.5: Continuous time systems - Generic input

32 CHAPTER 4. SYSTEM ANALYSIS

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)

10
I n [ 1 ] : from c o n t r o l import ∗

Magnit ude (dB)

0

10
I n [ 2 ] : g=t f ( [ 1 ] , [ 1 , 0 . 5 , 1 ] )
20
I n [ 3 ] : b o d e p l o t ( g , dB=True ) ; 30

40
-1 0 1
10 10 10

or alternatively

Phase (deg)
I n [ 1 ] : from c o n t r o l import ∗

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

I n [ 3 ] : g=t f ( [ 1 ] , [ 1 , 0 . 5 , 1 ] )

I n [ 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 [ 1 ] : from c o n t r o l import ∗

In [ 2 ] : g=t f ( 2 , [ 1 , 2 , 3 , 1 ] )

In [ 3 ] : gm , pm, wg , wp = margin ( g )

In [ 4 ] : gm # Gain , n o t 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 33

0.8
I n [ 1 ] : from c o n t r o l import ∗
0.6
I n [ 2 ] : import m a t p l o t l i b . p y p l o t a s p l t
0.4

I n [ 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
I n [ 1 ] : from c o n t r o l import ∗ − 1.0 − 0.5 0.0 0.5 1.0

I n [ 2 ] : import m a t p l o t l i b . p y p l o t a s p l t

I n [ 3 ] : from c o n t r o l . matlab import ∗

I n [ 4 ] : g=t f ( 1 , [ 1 , 2 , 1 ] )

In [ 5 ] : nyquist ( g ) , p l t . g r i d ( )

Figure 4.7: Nyquist plot

Nichols Plot
50 0.0 dB 0.0 dB
I n [ 1 ] : from c o n t r o l import ∗ 0.25 dB 0.25 dB
0.5 dB -0.5 dB 0.5 dB -0.5 dB
1.0 dB -1.0 dB 1.0 dB -1.0 dB
3.0 dB -3.0 dB 3.0 dB -3.0 dB
I n [ 2 ] : g=t f ( 1 , [ 1 , 2 , 3 , 4 , 0 ] ) 0
12.0dB
6.0 dB -6.0 dB 12.0dB
6.0 dB -6.0 dB
-12.0 dB -12.0 dB
-20.0 dB -20.0 dB
Magnit ude (dB)

In [ 3 ] : nichols plot (g)

-40.0 dB -40.0 dB
− 50
-60.0 dB -60.0 dB

-80.0 dB -80.0 dB
or alternatively -100.0 dB -100.0 dB
− 100
-120.0 dB -120.0 dB

I n [ 1 ] : from c o n t r o l import ∗ -140.0 dB -140.0 dB

− 150
-160.0 dB -160.0 dB
I n [ 2 ] : g=t f ( 1 , [ 1 , 2 , 3 , 4 , 0 ] ) − 700 − 600 − 500 − 400 − 300 − 200 − 100 0
Phase (deg)

In [ 3 ] : nichols (g)

Figure 4.8: Nichols plot

34 CHAPTER 4. SYSTEM ANALYSIS

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

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 o t 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 10−3 and
103 . For the root locus function the K values should be provided.
If in the jupyter shell you set the command %matplotlib qt, 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
Clicked at −1.119 +0.01874 j g a i n 2 . 2 5 2 damp
0.9999
Clicked at −0.7545 +1.293 j g a i n 1 . 1 1 4 damp
0.504
4.3. POLES, ZEROS AND ROOT LOCUS ANALYSIS 35

I n [ 1 ] : from c o n t r o l import ∗

I n [ 2 ] : from c o n t r o l . pzmap import pzmap

I n [ 3 ] : g=t f ( [ 1 , 1 ] , [ 1 , 2 , 3 , 4 , 0 ] )

In [ 4 ] : g . p o l e ( ) 2.0
Pole Zero Map

Out [ 4 ] :
array ([ −1.65062919+0. j , 1.5

−0.17468540+1.54686889 j ,
−0.17468540 −1.54686889 j ,
1.0

0.00000000+0. j ]) 0.5

In [ 5 ] : g . z e r o ( )

Im
0.0

Out [ 5 ] : a r r a y ( [ − 1 . ] )
−0.5

I n [ 6 ] : p o l e s , z e r o s = pzmap ( g ) , g r i d ( )
−1.0

In [ 7 ] : poles −1.5

Out [ 7 ] : −2.0
−2.0 −1.5 −1.0 −0.5 0.0 0.5

array ([ −1.65062919+0. j , Re

−0.17468540+1.54686889 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

10
I n [ 1 ] : from c o n t r o l import ∗

I n [ 2 ] : g=t f ( 1 , [ 1 , 2 , 3 , 0 ] )
5

In [ 3 ] : rlocus (g) ; grid ()

Im aginary

or alternatively
−5

I n [ 1 ] : from c o n t r o l import ∗
− 10
I n [ 2 ] : g=t f ( 1 , [ 1 , 2 , 3 , 0 ] ) − 12 − 10 −8 −6 −4 −2 0 2 4 6
Real

I n [ 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

36 CHAPTER 4. SYSTEM ANALYSIS
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 Lagrange’s method or the Kane’s method. More details about the Kane’s 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 ω).

37
38 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 sys te 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 p e e d
. . . : I = dynamicsymbols ( ’ I ’ ) # input current
...:
. . . : # Constants
. . . : 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 ] ) #
rotating reference ( load )
...:
. . . : Nr . s e t a n g v e l (N, w∗N. 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 39

In [ 4 ] : # Fo rc e s and t o r q u e s
. . . : f o r c e s = [ ( Nr , ( k t ∗ I−Dm∗w) ∗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 Kane’s 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 ( [ [ −Dm∗w( 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 Kane’s 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 a b o u t a r b i t r a r y
equilibrium
. . . : linear state matrix , 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 t r i x ( m mat . i n v ( ) ∗ f A l i n )
. . . : B = np . m a t r i x ( m mat . i n v ( ) ∗ f B l i n )
40 CHAPTER 5. MODELING

In [ 6 ] : print A
. . . : print B
...:
[ [ 0 1]
[ 0 −Dm/J ] ]
[[0]
[ k t /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 41

Figure 5.2: Inverted pendulum - Real plant

5.2.1 Modules and constants

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 sys te m w i t h Kane method
...:
...: # Signals
...: x , th = dynamicsymbols ( ’ x th ’ )
...: v , w = dynamicsymbols ( ’ v w ’ )
...: F = dynamicsymbols ( ’F ’ )
...:
...: # Constants
...: 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 ’ )
...:
42 CHAPTER 5. MODELING

5.2.2 Frames - Car and pendulum

In [ 2 ] : # Frames and Coord . s ys tem

...:
...: # Car − r e f e r e n c e x , y
...: Nf = ReferenceFrame ( ’ 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 ] : # Fo rc e s and t o r q u e s
. . . : f o r c e s = [ ( C, F∗ Nf . x−d∗v∗ 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 43

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 about a r b i t r a r y
equilibrium
. . . : linear state matrix , linear input matrix , inputs =
KM. l i n e a r i z e ( new method=True )
...:
. . . : # 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 p a r a m e t e r s
. . . : 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
. . . : 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 ∗m∗∗2∗ r ∗ ∗ 2 / ( J∗M + J∗m + M∗m∗ r ∗ ∗ 2 ) , −d ∗ (m∗∗2∗ r ∗ ∗ 2 / ( (
M + m) ∗ ( J∗M + J∗m
+ M∗m∗ r ∗ ∗ 2 ) ) + 1 / (M + m) ) , 0 ] ,
[ 0 , g ∗m∗ r ∗ (M + m) / ( J ∗M + J∗m + M∗m∗ r ∗ ∗ 2 ) ,
−d∗m∗ r / ( J∗M + J∗m + M∗m∗ r ∗ ∗ 2 ) , 0 ] ] )

In [ 7 ] : B
Out [ 7 ] :
Matrix ( [
[
0] ,
[
0] ,
[m∗∗2∗ r ∗ ∗ 2 / ( (M + m) ∗ ( J∗M + J∗m + M∗m∗ r ∗ ∗ 2 ) ) + 1 / (M + m) ] ,
[ m∗ r / ( J∗M + J∗m + M∗m∗ r ∗ ∗ 2 ) ] ] )

And the results can be written in a better form as

 
0 0 1 0
0 0 0 1
 
A= gm2 r2 d(Jc+mr2 ) 
0 JcM +Jcm+M mr 2
− JcM +Jcm+M mr2 0
0 gmr(M +m)
JcM +Jcm+M mr 2
− JcM +Jcm+M
dmr
mr 2
0
44 CHAPTER 5. MODELING

and

 
0
 0 
B=
 Jc+mr2 

JcM +Jcm+M mr2
mr
JcM +Jcm+M mr2

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 45

5.3.1 Modules and constants

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
...:
...: N0 = N. o r i e n t n e w ( ’N0 ’ , ’ Axis ’ , [ ph0 ,N. z ] )
...: N1 = N. o r i e n t n e w ( ’N1 ’ , ’ Axis ’ , [ ph1 ,N. z ] )
...: N2 = N. o r i e n t n e w ( ’N2 ’ , ’ Axis ’ , [ ph2 ,N. z ] )
...: N1 . s e t a n g v e l (N, w1∗N. z )
...: N2 . s e t a n g v e l (N, w2∗N. 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
46 CHAPTER 5. MODELING

5.3.4 Masses and inertias

In [ 4 ] : I z = o u t e r (N. z ,N. z )
...: In1T = ( J1 ∗ I z , O)
...: In2T = ( J2 ∗ I z , 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 , (T−d1 ∗w1 ) ∗N. z ) , (CM2,−M2∗ g ∗N. y ) ]

. . . : f o r c e s = [ ( N1 , T∗N. 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 Kane’s 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 a b o u t a r b i t r a r y
equilibrium
. . . : linear state matrix , linear input matrix , inputs =
KM. l i n e a r i z e ( new method=True )
...:
. . . : # 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 p a r a m e t e r s
. . . : 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
. . . : 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 47

In [ 9 ] : A
Out [ 9 ] :
Matrix ( [
[0 , 0 , 1 , 0] ,
[0 , 0 , 0 , 1] ,
[−M2∗∗2∗R1∗∗2∗R2∗∗2∗ g / ( ( R1 + R2) ∗ ( J1 ∗ J2 + J1 ∗M2∗R2∗∗2 + J2
∗M2∗R1∗ ∗ 2 ) ) +
M2∗R1∗∗2∗ g ∗ (M2∗∗2∗R1∗∗2∗R2 ∗ ∗ 2 / ( ( J1 + M2∗R1∗ ∗ 2 ) ∗ ( J1 ∗ J2 + J1
∗M2∗R2∗∗2 +
J2 ∗M2∗R1∗ ∗ 2 ) ) + 1 / ( J1 + M2∗R1∗ ∗ 2 ) ) / (R1 + R2) , −M2∗∗2∗R1∗R2
∗∗3∗ g / ( ( R1 +
R2) ∗ ( J1 ∗ J2 + J1 ∗M2∗R2∗∗2 + J2 ∗M2∗R1∗ ∗ 2 ) ) + M2∗R1∗R2∗ g ∗ (M2
∗∗2∗R1∗∗2∗R2 ∗ ∗ 2 / ( ( J1 +
M2∗R1∗ ∗ 2 ) ∗ ( J1 ∗ J2 + J1 ∗M2∗R2∗∗2 + J2 ∗M2∗R1∗ ∗ 2 ) ) + 1 / ( J1 +
M2∗R1∗ ∗ 2 ) ) / (R1 + R2) ,
0 , 0] ,
[ −M2∗∗2∗R1∗∗3∗
R2∗ g / ( ( R1 + R2) ∗ ( J1 ∗ J2
+ J1 ∗M2∗R2∗∗2 + J2 ∗M2∗R1∗ ∗ 2 ) ) + M2∗R1∗R2∗ g ∗ ( J1 + M2∗R1∗ ∗ 2 )
/ ( ( R1 + R2) ∗ ( J1 ∗ J2 +
J1 ∗M2∗R2∗∗2 + J2 ∗M2∗R1∗ ∗ 2 ) ) ,
−M2∗∗2∗R1∗∗2∗R2∗∗2∗ g / ( ( R1 + R2) ∗ ( J1 ∗ J2 + J1 ∗M2∗R2∗∗2 + J2 ∗
M2∗R1∗ ∗ 2 ) ) +
M2∗R2∗∗2∗ g ∗ ( J1 + M2∗R1∗ ∗ 2 ) / ( ( R1 + R2) ∗ ( J1 ∗ J2 + J1 ∗M2∗R2∗∗2
+ J2 ∗M2∗R1∗ ∗ 2 ) ) , 0 ,
0]])
In [ 1 0 ] : B
Out [ 1 0 ] :
Matrix ( [
[
0] ,
[
0] ,
[M2∗∗2∗R1∗∗2∗R2 ∗ ∗ 2 / ( ( J1 + M2∗R1∗ ∗ 2 ) ∗ ( J1 ∗ J2 + J1 ∗M2∗R2∗∗2 +
J2 ∗M2∗R1∗ ∗ 2 ) ) +
1 / ( J1 + M2∗R1∗ ∗ 2 ) ] ,
[ −M2∗R1∗R2/ (
J1 ∗ J2 + J1 ∗M2∗R2∗∗2 +
J2 ∗M2∗R1∗ ∗ 2 ) ] ] )

or as formula

 0 0 1 0

0 0 0 1
 2g
J2 M2 R1 J2 M2 R1 R2 g 
A=

2 +J M R3 +J M R3 +J M R2 R
J1 J2 R1 +J1 J2 R2 +J1 M2 R1 R2 1 2 2 2 2 1 2 2 1 2
2 +J M R3 +J M R3 +J M R2 R
J1 J2 R1 +J1 J2 R2 +J1 M2 R1 R2 1 2 2 2 2 1 2 2 1 2
0 0

J1 M2 R22g
J1 M2 R1 R2 g
0 0
(
(R1 +R2 ) J1 J2 +J1 M2 R22 +J M R2
2 2 1 ) (
(R1 +R2 ) J1 J2 +J1 M2 R2 )
2 +J M R2
2 2 1

and
 0 
0
 M2 2 R2 R2
1 2 1 
B =  (J1 +M2 R12 )( 2 +J M R2
J1 J2 +J1 M2 R2 2 2 1 )
+ 2
J1 +M2 R1 
M2 R1 R2
− 2 +J M R2
J1 J2 +J1 M2 R2 2 2 1
48 CHAPTER 5. MODELING
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.

49
50 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 [ 3 ] : from c o n t r o l 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 s i m . RCPblk import ∗

In [ 7 ] : from s u p s i c t r l . c t r l u t i l s import ∗

In [ 8 ] : from s u p s i c t r l . c t r l r e p l import ∗

In [ 9 ] : g=t f ( [ 1 ] , [ 1 , 6 , 5 ] )

In [ 1 0 ] : bode ( g , dB=True ) ;

In [ 1 1 ] : l e g e n d ( [ ’G( s ) ’ ] , prop={ ’ s i z e ’ : 1 0 } )
Out [ 1 1 ] :
(< 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 2 ] : wgc = 10 # D e s i r e d Bandwidth

In [ 1 3 ] : desiredPM = 60 # D e s i r e d Phase margin

Figure 6.1 shows the bode diagram of the plant.

 10
G(s)
 20
 30
Magnit ude (dB)

 40
 50
 60
 70
80

90 -1 0 1 2
10 10 10 10
0
 20
 40
 60
Phase (deg)

 80
100
 120
 140

160
180 -1 0 1 2
10 10 10 10
Frequency (rad/sec)

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
6.1. PI+LEAD DESIGN EXAMPLE 51

Ti = 0.15s

In [ 1 4 ] : # PI p a r t

In [ 1 5 ] : Ti =0.15

In [ 1 6 ] : Gpi=t f ( [ Ti , 1 ] , [ Ti , 0 ] )

In [ 1 7 : 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 8 : f i g u r e ( )
Out [ 1 8 : <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 9 : bode ( g , dB=True , l i n e s t y l e= ’ dashed ’ ) ;

In [ 2 0 h o l d
Out [ 2 0 <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 [ 2 1 bode ( Gpi ∗g , dB=True ) ;

In [ 2 2 : l e g e n d ( ( [ ’G( s ) ’ , ’ Gpi ( s ) ∗G( s ) ’ ] ) , prop={ ’ s i z e ’ : 1 0 } )

Out [ 2 2 :
(< 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
G(s)
20 Gpi(s)* G(s)
Magnit ude (dB)

0
 20
 40
 60
80

100 -1 0 1 2
10 10 10 10
0

 50
Phase (deg)

 100

 150

 200 -1 0 1 2
10 10 10 10
Frequency (rad/sec)

Figure 6.2: Bode diagram: G (dashed) and Gpi*G

52 CHAPTER 6. CONTROL DESIGN

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 3 : mag , phase , omega = bode ( Gpi ∗g , [ wgc ] , P l o t=F a l s e )

In [ 2 4 : ph = phase [ 0 ]

In [ 2 5 : i f ph>=0:
...: ph = phase [ 0 ] − 3 6 0 ;
...:

In [ 2 6 : Phase = −180+desiredPM

In [ 2 7 : dPM = Phase−ph

In [ 2 8 : 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 9 ] : # Lead p a r t

In [ 3 0 ] : dPMrad = dPM/180∗ p i

In [ 3 1 ] : a l f a = (1+ s i n ( dPMrad ) ) /(1− s i n ( dPMrad ) ) ;

In [ 3 2 ] : print ” Alpha i s : ” , a l f a
Alpha i s : 15.4073552425

In [ 3 3 ] : TD = 1 / ( s q r t ( a l f a ) ∗wgc ) ;

In [ 3 4 ] : Glead = t f ( [ a l f a ∗TD, 1 ] , [ TD, 1 ] )

In [ 3 5 ] : 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 6 ] : f i g u r e ( )
Out [ 3 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 x7f85b43462d0>

In [ 3 7 ] : bode ( g , dB=True , l i n e s t y l e= ’ dashed ’ ) ;

In [ 3 8 ] : h o l d
Out [ 3 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 [ 3 9 ] : bode ( Gpi ∗ Glead ∗g , dB=True ) ;

In [ 4 0 ] :
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 [ 4 0 ] :
(< m a t p l o t l i b . a x e s . AxesSubplot a t 0 x7f85b43736d0 >,
<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
6.1. PI+LEAD DESIGN EXAMPLE 53

40
G(s)
20 Gpi(s)* G(s)

Magnit ude (dB)

0 Gpi(s)* GLead(s)* G(s)

 20
 40
 60
80

100 -1 0 1 2 3
10 10 10 10 10
0

 50
Phase (deg)

 100

 150

 200 -1 0 1 2 3
10 10 10 10 10
Frequency (rad/sec)

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 [ 4 1 ] : mag , phase , omega = bode ( Gpi ∗ Glead ∗g , [ wgc ] , P l o t=

False )

In [ 4 2 ] : print ” Phase a t wgc i s : ” , phase [ 0 ]

Phase a t wgc i s : −120.0

In [ 4 3 ] : K=1/mag [ 0 ]

In [ 4 4 ] : print ” Gain t o have MAG a t gwc 0dB : ” , K

Gain t o have MAG a t gwc 0dB : 23.8177769548

In [ 4 5 ] : f i g u r e ( )
Out [ 4 5 ] : <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 x7f85b3a703d0>

In [ 4 6 ] : bode ( g , dB=True , l i n e s t y l e= ’ dashed ’ ) ;

In [ 4 7 ] : h o l d
Out [ 4 7 ] : <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 [ 4 8 ] : bode ( Gpi ∗ Glead ∗g , dB=True , l i n e s t y l e= ’ −. ’ ) ;

In [ 4 9 ] : bode (K∗ Gpi ∗ Glead ∗g , dB=True ) ;

In [ 5 0 ] :
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 [ 5 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 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 .
54 CHAPTER 6. CONTROL DESIGN

60
G(s)
40
Gpi(s)* G(s)

Magnit ude (dB)

20 Gpi(s)* GLead(s)* G(s)
0 K* Gpi(s)* GLead(s)* G(s)

 20
 40
 60
80

100 -1 0 1 2 3
10 10 10 10 10
0

 50
Phase (deg)

 100

 150

 200 -1 0 1 2 3
10 10 10 10 10
Frequency (rad/sec)

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 [ 5 1 ] : Contr = K∗ Gpi ∗ Glead

In [ 5 2 ] : print ” F u l l c o n t r o l l e r : ” , Contr
Full c o n t r o l l e 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 3 ] : mag , phase , omega = bode (K∗ Gpi ∗ Glead ∗g , [ wgc ] , P l o t=

False )

In [ 5 4 ] : 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 5 ] : g t=f e e d b a c k (K∗ Gpi ∗ Glead ∗g , 1 )

In [ 5 6 ] : t=l i n s p a c e ( 0 , 1 . 5 , 3 0 0 )

In [ 5 7 ] : y , t = s t e p ( gt , t )

In [ 5 8 ] : f i g u r e ( )
Out [ 5 8 ] : <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 51 4 2 9 0 >

In [ 5 9 ] : 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
response of the
controlled plant ’ )
Out [ 5 9 ] :
([ < m a t p l o t l i b . l i n e s . Line2D a t 0 x7f85b34252d0 >] ,

In [ 6 0 ] : g r i d ( )
6.2. DISCRETE-STATE FEEDBACK CONTROLLER DESIGN 55

The simulation of the controlled plant with a step input is shown in figure 6.5.

St ep response of t he cont rolled plant

1.2

1.0

0.8

0.6
y

0.4

0.2

0.0
0.0 0.2 0.4 0.6 0.8 1.0 1.2 1.4 1.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
56 CHAPTER 6. CONTROL DESIGN

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 [ 5 ] : from c o n t r o l import ∗

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

In [ 7 ] : from s u p s i s i m . RCPblk import ∗

. . . : from s u p s i c t r l . c t r l u t i l s import ∗
. . . : from s u p s i c t r l . c t r l r e p l 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.

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=y−Y
...: 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.
6.2. DISCRETE-STATE FEEDBACK CONTROLLER DESIGN 57

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 ] ) # Transfer function

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 )

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
58 CHAPTER 6. CONTROL DESIGN

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

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 p a c e form

In [ 3 0 ] : Ts =0.01 # Sampling time

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.
6.2. DISCRETE-STATE FEEDBACK CONTROLLER DESIGN 59

In [ 3 2 ] : # C o n t r o l s y s te 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 check
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 poles

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 . C∗Ts ) )

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.
60 CHAPTER 6. CONTROL DESIGN

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 check
True

In [ 4 9 ] : p o c =−10∗max( 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.
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 usin the control package. In some cases we can substitute the
plant with the discrete-time system and perform a discrete simulation. Hybrid simulation
is possible using the pyEdit application described in the next chapter.

• The block “CTRIN” has two inputs. The step function can only find the output from a
single input.
6.2. DISCRETE-STATE FEEDBACK CONTROLLER DESIGN 61

Figure 6.7: Block diagram of the controlled system

• The control toolbox can handle only linear system (and there is a saturation in the final
system).
62 CHAPTER 6. CONTROL DESIGN
Chapter 7

Hybrid simulation and code generation

7.1 Basics
CACSD environments usually offer a graphical editor to perform the hybrid simulation (Matlab↔Simulin
Scioslab↔Scicos, Scilab↔xCos etc.).
The “pyEdit.py” application should cover this task for the Python Control environment.
In the following we’ll 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
7.2.1 The editor
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.

7.2.2 The first example

Using the editor we wont create the block diagram of figure 7.2.
We open a shell and we give the command
pyEdit
The application opens 2 windows as shown in figure 7.3
The window on the left shows the library with the available blocks and on the right we have
the diagram window. Now we can start to draw our block diagram.
From the library window we can choos the tab ”input“ and using ”drag and drop“ we can get
the block ”Step“ and move it into the editor window. We can do the same operation with the
”LTI continous“ (from tab ”linear“) and the ”Plot“ (from tab ”output“) blocks.

63
64 CHAPTER 7. HYBRID SIMULATION AND CODE GENERATION

Figure 7.1: Some pyEdit blocks for control design

Figure 7.2: The first example

7.2. PYEDIT 65

Figure 7.3: The pyEdit environment

Now we should obtain the diagram shown in figure 7.4

Figure 7.4: Result from the drag and drop operations

Before starting with the connection, we set some parameters to the blocks.

• Souble click with the mouse on the block ”LTI continous“. In the dialog windows set the
System to tf(1,[1,1])

• Click the right mouse on the LTI continous block“. In the new menu choose ”Change
Name“ and rename it as Plant.

• Click the right mouse on the Plot block. In the new menu choose “Block I/Os” and set
the number of inputs to 2.
66 CHAPTER 7. HYBRID SIMULATION AND CODE GENERATION

Figure 7.5: Result after parametrization

Figure 7.5 shows the new diagram.

Now we can proceed with the connections.

• Move the mouse on the output of the block “Step”: the mouse pointer should become a
“cross”. Click and release the left mouse button.

• Now we can move the mouse to the input of the block “Plant”: the mouse pointer should
become a “cross”. Click and release the left mouse button.

• Do the same operation from the output of the block “Plant” to the second input of the
block “Plot”

• Now move to the node (the little circle) between the “Step” and the block “Plant”: the
mouse pointer should become a “cross”. Click and release the left mouse button.

• move the mouse up, click, and continue to move left the mouse. Left of the position of the
block “Plot”, click and release again the left mouse button and then finish the connection
on the first input of the block “Plot” (click and release the left mouse button)

You should obtain the diagram of figure 7.2

Now we are able to simulate the diagram.

• From the menu “Simulation” choose “Simulate” or click on the button “Simulate” on the
toolbar (the button with the triangle).

• Double click with the mouse on the block “Plot” to get the graphical output of the
simulation (see figure 7.6).

7.2.3 Personalize the most used blocks

A special file “common.blks” is defined in the “resources/block/block” folder. This file contains
a list of clocks, that the user can modify to have its own most used blocks. The blocks are
shown in the library both in the “common” tab and in the specific tab.
7.2. PYEDIT 67

Figure 7.6: Result (plot) of the simulation

7.2.4 Defining new blocks

The user can define new blocks and integrate them into the pyEdit application.

First of all he have to define the “.xblk” file, using for example the application “defBlock” (which
is part of pycontrol.tgz). This block must be moved in the folder “ /Documents/PYTHON/blocks”.

A new function must be inserted in the file “ /Document/PYTHON/addBlocks.py” (the file

must be created the first time that the user insert a new block!). The first line of this file
must be:

from s u p s i s i m . RCPblk import RCPblk

Then, the user must define a function for each new block. In the following there are some
example with 3 blocks, the first with only input ports, the second with only output ports, and
the last with both input and output ports.
68 CHAPTER 7. HYBRID SIMULATION AND CODE GENERATION

from s u p s i s i m . RCPblk import RCPblk

def d i g i l e n t A O B l k ( pin , ch ) :
i f np . s i z e ( p i n ) != 1 :
r a i s e V a l u e E r r o r ( ” Block s h o u l d have 1 i n p u t p o r t ;
r e c e i v e d %i ! ” %
np . s i z e ( p i n ) )

b l k = RCPblk ( ’ d i g i l e n t a n a l o g o u t p u t ’ , pin
, [ ] , [ 0 , 0 ] , 0 , [ ] , [ ch ] )
return b l k

def d i g i l e n t A I B l k ( pout , ch , rng , o f f s e t ) :

i f np . s i z e ( pout ) != 1 :
r a i s e V a l u e E r r o r ( ” Block s h o u l d have 1 i n p u t p o r t ;
r e c e i v e d %i ! ” %
np . s i z e ( pout ) )

b l k = RCPblk ( ’ d i g i l e n t a n a l o g i n p u t ’ , [ ] , pout , [ 0 , 0 ] , 0 , [
rng , o f f s e t ] , [ ch ] )
return b l k

def M e a n F i l t e r B l k ( pin , pout , N) :

b l k = RCPblk ( ’ m e a n f i l t e r ’ , pin , pout , [ 0 , 0 ] , 1 , [ ] , [ N, 0 ] )
return b l k

The implementation of the RCPblk class is defined in the file “toolbox/supsisim/src/RCP-

blk.py”.

7.2.5 Some remarks

• the simulation result (Plot) is available only after the simulation. Please be sure to
restart the simulation before opening the plot result. The simualtion creates a file with
the name of the block in “/tmp” folder: this file is overwritten by every new simulation.
• For the simulation, the application creates and compile a C-executable. The sources are
written in the folder “xxxxxx gen”, where “xxxxx” is the name of the diagram.

7.3 The editor window

7.3.1 The toolbar
The application offers set of operations in the toolbar as shown in the figure 7.7.

7.3.2 Operations with the right mouse button

Depending on the position of the mouse, clicking and releasing the right mouse button leads to
different behaviours.

7.3.3 Operations with the right mouse button on a block

Clicking with the right mouse button on a block opens a popup menu with the following
commands:
7.3. THE EDITOR WINDOW 69

Sim

Ge

Py
Copy/Paste

Pr
File ops

oje

th
ne
ula

o
ct
r

ns
at
te

se
ec

he
tt i
od

ll
ng
e

Figure 7.7: The pyEdit application

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

7.3.4 Operations with the right mouse button on a connection

Moving the mouse on a connection, change the pointer to a pointing hand and by clicking with
the right mouse button a popup menu is opened with the following commands:

Delete conncetion deletes the pointed connection

Insert node inserts a new node on the connection. This is needed for examples if we have to
draw a new connection.
70 CHAPTER 7. HYBRID SIMULATION AND CODE GENERATION

7.3.5 Operations with the right mouse button on a node

Moving the mouse on a node, change the pointer to a cross and by clicking with the right mouse
button a popup menu is opened with the following commands:

Delete node deletes the node and the connections associated with this node.

Bind node eliminate the node without eliminate the connection.

7.3.6 Behaviour of the right mouse button by drawing a connection

Clicking the right mouse button by drawing a connection, put a new node in the mouse position
and exit the drawing mode.

7.4 Basic editor operations

7.4.1 Inserting a block
Get a block from a library and drag it into the main window.

7.4.2 Connecting blocks

• 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

• Move to a connection and click the right mouse button

• Select the ”insert node“ menu.

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

• Move to a block or node and click with the right mouse button.

• Choose the submenu ”delete“

7.5. REMOVE A NODE 71

7.5 Remove a node

• Move to the node.

• Click with the right mouse button on the node.

• Choose the submenu ”Bind node“ The connection is maintained but the node is cleared.
72 CHAPTER 7. HYBRID SIMULATION AND CODE GENERATION
Chapter 8

Simulation and Code generation

Each element of a block diagram is defined with three or four 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.

The translation of the block into the RCPblk class described in the RCPblk.py module

A dlg function to implement a special dialog box for the block parameters (only if required)

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 file with extension “.xblk”, stored in the “resources/blocks/blocks”
folder. The file is defined as a Python dictionary:
{"lib": "math", "name": "Sum", "ip": 2, "op": 1, "st": 1, "icon": "SUM", "params": "sumBlk|Gains: [1,-1]"}

using the following fields:

“lib” the name of the tab for the block library (example “tab”:“linear”)

“name” the default name of the block

“ip” number of inputs

“op” number of outputs

“st” flag which indicates if the number of I/O can be modified

“icon” the name of the “.svg” file with the icon of the block

“param” the parameters of the block

73
74 CHAPTER 8. SIMULATION AND CODE GENERATION

Figure 8.1: Window with the block libraries

8.2. THE IMPLEMENTATION FUNCTIONS 75

The first string in the param field is used as name of the Python function used to prepare the
block to be translated into C-Code.
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.

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)
ẋ = 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 Translating the block into the RCPblk class

Before generating the C-Code, each block in the diagram must be translated into an element of
the RCPblk class (see section 8.7 for more details). For each block, the corresponding function
(the name is given by the 1. string in the parameters line) must exists and should be declared
with the required parameters. This function is responsible to fill all the RCPblk fields.

8.4 Special dialog box for the block parameters

Usually, the graphic editor build a simple dialog box to enter the block parameters.
In special cases, it is possible to write a special function to enter the parameters.. In this case,
the user should provide this function in the RCPDlg.py file. The name of this function is built
using the first string of the parameter line, by subsistuting the las 3 letters “Blk” with “Dlg”.
This new function must receive as input:

• Numper of inputs
• Number of outputs
• The parameters line

This function returns a modified parameters line. Anexample is the “PlotDlg” function in the
file “toolbox/supsisim/src/RCPGDlg.py”.
76 CHAPTER 8. SIMULATION AND CODE GENERATION

8.5 Example
We can show with an example what happens with a block in the different phases from block to
RCPblk class.
The “Pulse generator” input block is stored in the “PulseGenerator.xblk” file with the following
infos
{"lib": "input", "name": "PulseGenerator", "ip": 0, "op": 1, "st": 0, "icon":
"SQUARE", "params": "squareBlk|Amplitude: 1|Period: 4|Width: 2|Bias: 0|Delay:
0"}

The block has no inputs, 1 ouput, the I/O are not modifiable (settable=0).
After a double click on the block, the “params” field is parsed and translated into the the dialog
box shown in figure 8.2.

Figure 8.2: Dialog box for the Pulse generator block

By generating the element of the class RCPblk, the function “squareBlk” is called with the
following parameters:

SQUARE = squareBlk(pout, Amp, Period, Width, Bias, Delay)

where

pout is the matrix with the id of the inputs (connections)

Amp is the signal amplitude

Period is the period of the signal

width is the duration where the signal has value “Amp-bias”

bias is an offset for the signal

delay represent the time wenn the signal start

8.6. THE PARAMETERS FOR THE CODE GENERATION 77

The function translate the block into the following object of the RCPblk class

Function : square
Input ports : []
Output ports : [2]
Nr. of states : [0 0]
Relation u->y : 0
Real parameters : [ 4 8 3 0 12]
Integer parameters : []

8.6 The parameters for the code generation

Before clicking on the “code generation” tool on the toolbar, the user should fill some parameters
in a dialog box (see figure 8.3).

Figure 8.3: Dialog for code generation

In this dialog it is possible to choose the “template makefile” for simulation or real-time ex-
ecution, the sampling time of the system and some additional libraries, required by special
blocks.

8.7 Translating the diagram into elements of the RCPdlg

class
After this first setup it is possible to translate the block diagram into a list of elements of
the class RCPblk provided by the suspisim package. This class contains all the information
required for the code generation.
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)

78 CHAPTER 8. SIMULATION AND CODE GENERATION

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.4 is translated into the following code

from s u p s i s i m . 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.4: 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.8 Translating the block list into C-code

8.8.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
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
8.8. TRANSLATING THE BLOCK LIST INTO C-CODE 79

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.4, 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 t B 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
...:
Function : css
Input 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 t e r s : [ [ 0. 0 . −1. 1 . −1. −1.
0. 0 . −1. 0 . 0. 0.]]
I n t e g e r parameters : [ 2 1 1 1 5 7 9 10]
S t r i n g Parameter :

Function : step
Input p o r t s : []
Outputs p o r t s : [1]
Nr . o f s t a t e s : [0 0]
R e l a t i o n u−>y : 0
Real p a r a m e t e r s : [1 1]
I n t e g e r parameters : []
S t r i n g Parameter :

Function : print
Input p o r t s : [1 3]
Outputs p o r t s : []
Nr . o f s t a t e s : [0 0]
R e l a t i o n u−>y : 1
Real p a r a m e t e r s : []
I n t e g e r parameters : []
S t r i n g Parameter :

Function : sum
Input p o r t s : [1 3]
Outputs p o r t s : [2]
Nr . o f s t a t e s : [0 0]
R e l a t i o n u−>y : 1
Real p a r a m e t e r s : [ 1 −1]
I n t e g e r parameters : []
S t r i n g Parameter :

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.8.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

80 CHAPTER 8. SIMULATION AND CODE GENERATION

• The termination function

• The periodic task

8.8.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 ∗ realPar ; /∗ Real p a r a m e t e r s ∗/
int ∗ intPar ; /∗ I n t p a r a m e t e r s ∗/
char ∗ str ; /∗ S t r i n g ∗/
void ∗ ptrPar ; /∗ 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 } ;
s t a t i c d o u b l e Node 2 [ ] = { 0 . 0 } ;
s t a t i c d o u b l e Node 3 [ ] = { 0 . 0 } ;

/∗ I n p u t and o u t p u t s ∗/
s t a t i c void ∗ i n p t r 0 [ ] = {0};
s t a t i c void ∗ outptr 0 [ ] = {0};
s t a t i c void ∗ outptr 1 [ ] = {0};
s t a t i c void ∗ i n p t r 2 [ ] = {0 ,0};
s t a t i c void ∗ i n p t r 3 [ ] = {0 ,0};
s t a t i c void ∗ outptr 3 [ ] = {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.

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.9. THE MAIN FILE 81

8.8.4 The termination function

This procedure calls the implementation functions of the blocks with the flag END.

8.8.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 ̸= 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.9 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.
82 CHAPTER 8. SIMULATION AND CODE GENERATION

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.

83
84 CHAPTER 9. EXAMPLE

In [ 4 ] : A
Out [ 4 ] :
matrix ( [ [ 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 ] :
matrix ( [ [ 0 , 0 ] ,
[0 , 0] ,
[ k t 1 / J1 , 0 ] ,
[ 0 , k t 2 / 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(n−q) ]

• 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 85

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 time
t s = 10 e−3

g s s 1 = s s (A, B, C,D)
g s s = s s (A, B, C2 , D2)
gz = c2d ( g s s , t s , ’ zoh ’ )
86 CHAPTER 9. EXAMPLE

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 integrator 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 ) ) ) )

# Pole placement
k = placep ( Phi f , G f , c l p o l e s d )

9.4 Observer design

# Observer design − reduced order observer

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 a t r i x − Compact

form
c o n t r I = comp form i ( disksz , r obs , k , [ 0 , 1 ] )

# Implement a n t i windup
[ gss in , gss 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 87

Figure 9.3: Block diagram for the simulation

Now we can launch the simulation with the command “Simulate” from the toolbar or from the
menu.
A double click on the ‘block “Plot” show the result of the simulation (see 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).
88 CHAPTER 9. EXAMPLE

Figure 9.4: Simulation of the plant

Figure 9.5: Block diagram for the RT implementation

9.6. REAL-TIME CONTROLLER 89

Figure 9.6: RT execution

90 CHAPTER 9. EXAMPLE
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] pycontrol.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] Kane’s Method in Physics/Mechanics. [Online]. Available:

http://docs.sympy.org/0.7.5/modules/physics/mechanics/kane.html

[15] Kane’s Method and Lagrange’s 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

91
92 BIBLIOGRAPHY

[17] A Brief Synopsis of Kane’s 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 Kane’s 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. Kane’s 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