├── 04-LQR-Design-And-Simulation ├── sim │ ├── ludyk_ltv.p │ ├── ludyk_lti_0.p │ ├── ludyk_lti_5.p │ ├── pendulum_lti.p │ ├── pendulum_ltv.p │ ├── ludyk_pseudoltv.p │ ├── pendulum_pseudoltv.p │ ├── generate_pendulum_swingup.py │ ├── plots.py │ ├── 01_lqr_pendulum.py │ ├── 01_lqr.py │ └── Planner.py ├── doc │ ├── img │ │ ├── pendulum.pdf │ │ ├── ludyk_lti.pdf │ │ ├── ludyk_ltv.pdf │ │ ├── pendulum_lti.pdf │ │ ├── pendulum_ltv.pdf │ │ ├── ludyk_pseudoltv.pdf │ │ ├── pendulum_pseudoltv.pdf │ │ └── pendulum.svg │ ├── literature.bib │ ├── luainputlisting.lua │ ├── tutorial4.md │ └── tutorial4.tex └── README.md ├── 01-System-Simulation-ODE ├── doc │ ├── img │ │ ├── animation.png │ │ ├── state_trajectory.pdf │ │ ├── car-like_mobile_robot.pdf │ │ ├── car-like_mobile_robot.pdf_tex │ │ └── car-like_mobile_robot.svg │ ├── literature.bib │ └── luainputlisting.lua ├── README.md └── sim │ ├── 03_events.py │ ├── 01_car_example_plotting.py │ └── 02_car_example_animation.py ├── 02-Trajectories-And-Control ├── doc │ ├── img │ │ ├── gevrey0.pdf │ │ ├── gevrey1.pdf │ │ ├── failedcontrol.pdf │ │ ├── linear_system.pdf │ │ ├── successcontrol.pdf │ │ ├── plane_trajectory.pdf │ │ ├── state_transition.pdf │ │ ├── control_trajectory.pdf │ │ ├── planned_trajectory.pdf │ │ └── linear_system_feedback.pdf │ ├── literature.bib │ ├── gevrey-package.tex │ └── luainputlisting.lua ├── README.md └── sim │ ├── 01_trajectory_planning.py │ ├── TrajGen.py │ ├── 02_car_feedforward_control.py │ └── 03_car_feedback_control.py ├── 03-Jupyter-Notebook-And-SymPy ├── sim │ └── img │ │ └── cart-pole.png ├── doc │ ├── img │ │ └── jupyter_screenshot.png │ └── tutorial3.tex └── README.md ├── README.md └── .gitignore /04-LQR-Design-And-Simulation/sim/ludyk_ltv.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/sim/ludyk_ltv.p -------------------------------------------------------------------------------- /01-System-Simulation-ODE/doc/img/animation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/01-System-Simulation-ODE/doc/img/animation.png -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/ludyk_lti_0.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/sim/ludyk_lti_0.p -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/ludyk_lti_5.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/sim/ludyk_lti_5.p -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/gevrey0.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/gevrey0.pdf -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/gevrey1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/gevrey1.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/img/pendulum.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/doc/img/pendulum.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/pendulum_lti.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/sim/pendulum_lti.p -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/pendulum_ltv.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/sim/pendulum_ltv.p -------------------------------------------------------------------------------- /03-Jupyter-Notebook-And-SymPy/sim/img/cart-pole.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/03-Jupyter-Notebook-And-SymPy/sim/img/cart-pole.png -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/img/ludyk_lti.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/doc/img/ludyk_lti.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/img/ludyk_ltv.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/doc/img/ludyk_ltv.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/ludyk_pseudoltv.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/sim/ludyk_pseudoltv.p -------------------------------------------------------------------------------- /01-System-Simulation-ODE/doc/img/state_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/01-System-Simulation-ODE/doc/img/state_trajectory.pdf -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/failedcontrol.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/failedcontrol.pdf -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/linear_system.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/linear_system.pdf -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/successcontrol.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/successcontrol.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/img/pendulum_lti.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/doc/img/pendulum_lti.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/img/pendulum_ltv.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/doc/img/pendulum_ltv.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/pendulum_pseudoltv.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/sim/pendulum_pseudoltv.p -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/plane_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/plane_trajectory.pdf -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/state_transition.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/state_transition.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/img/ludyk_pseudoltv.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/doc/img/ludyk_pseudoltv.pdf -------------------------------------------------------------------------------- /01-System-Simulation-ODE/doc/img/car-like_mobile_robot.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/01-System-Simulation-ODE/doc/img/car-like_mobile_robot.pdf -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/control_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/control_trajectory.pdf -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/planned_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/planned_trajectory.pdf -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/img/pendulum_pseudoltv.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/04-LQR-Design-And-Simulation/doc/img/pendulum_pseudoltv.pdf -------------------------------------------------------------------------------- /03-Jupyter-Notebook-And-SymPy/doc/img/jupyter_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/03-Jupyter-Notebook-And-SymPy/doc/img/jupyter_screenshot.png -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/img/linear_system_feedback.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUD-RST/pytutorials/HEAD/02-Trajectories-And-Control/doc/img/linear_system_feedback.pdf -------------------------------------------------------------------------------- /01-System-Simulation-ODE/doc/literature.bib: -------------------------------------------------------------------------------- 1 | @book {KnollPython21, 2 | author = {Knoll, Carsten AND Heedt, Robert}, 3 | title = {Python für Ingenieure für Dummies: Mit vielen Programmbeispielen zu Numpy, Matplotlib und mehr}, 4 | publisher = {Wiley-VCH}, 5 | year = {2021}, 6 | address = {Weinheim}, 7 | } -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/literature.bib: -------------------------------------------------------------------------------- 1 | @book {ludyk, 2 | author = { Ludyk, Günter }, 3 | title = { Theoretische Regelungstechnik 2 Zustandsrekonstruktion, optimale und nichtlineare Regelungssysteme }, 4 | publisher = {Springer}, 5 | year = {1995}, 6 | address = { Berlin, Heidelberg }, 7 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pytutorials 2 | A collection of tutorials for the Python programming language, especially for control theoretic purposes. 3 | 4 | ## Available tutorials 5 | 6 | 1. Simulation of a dynamical system represented by a set of nonlinear ordinary differential equations: [01-SystemSimulation-ODE](https://github.com/TUD-RST/pytutorials/tree/master/01-System-Simulation-ODE) 7 | 8 | 2. Trajectory planning, feedforward and feedback control of a kinematic model of a car: [02-Trajectories-And-Control](https://github.com/TUD-RST/pytutorials/tree/master/02-Trajectories-And-Control) 9 | -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/README.md: -------------------------------------------------------------------------------- 1 | # LQR-Design-And-Simulation 2 | This tutorial shows how to design controllers that minimize a quadratic cost function for linear time-invariant and time-variant systems. The process is explained using an example and the controllers are implemented in simulation. 3 | 4 | ## The following topics are covered by the tutorial 5 | - Something useful 6 | 7 | ## Prerequisites (software) 8 | - Python 3.6 incl. NumPy, Scipy, and MatplotLib 9 | 10 | ## Prerequisites (knowledge) 11 | - Basic knowledge in the Python programming language, esp. lists, tupels and lambda-functions 12 | 13 | ## How to build the tutorial 14 | - Run `pdflatex tutorial4.tex` in the doc directory of this repository. 15 | -------------------------------------------------------------------------------- /03-Jupyter-Notebook-And-SymPy/README.md: -------------------------------------------------------------------------------- 1 | # Jupyter Notebook and SymPy 2 | This tutorial shows how to model and analyze the cart-pole system using a Jupyter Notebook and the symbolic mathematics library SymPy 3 | 4 | ## The following topics are covered by the tutorial 5 | - Derivation of the equations of motion through scientific computing 6 | - Linearization of the resulting nonlinear system equations to obtain a linear system in state space form 7 | - Investigation of the control theoretic properties of the system 8 | - Design of a linear-quadratic regulator (LQR) 9 | 10 | ## Prerequisites (software) 11 | - Python 3.6 incl. NumPy, Scipy, and MatplotLib 12 | 13 | ## Prerequisites (knowledge) 14 | - Basic knowledge in the Python programming language, esp. lists, tupels and lambda-functions 15 | - Tutorial 01-System-Simulation-ODE 16 | 17 | ## How to build the tutorial 18 | - Run `pdflatex tutorial3.tex` in the doc directory of this repository. 19 | 20 |    -------------------------------------------------------------------------------- /02-Trajectories-And-Control/README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | # Trajectory planning and control 4 | 5 | This tutorial shows how to implement a trajectory generator and how to establish a flatness 6 | based feedforward and feedback control 7 | 8 | ## The following topics are covered by the tutorial 9 | 10 | - Implementation of different trajectory generators in a class hierarchy 11 | - Flatness based feedforward control 12 | - Flatness based feedback control 13 | - Some topics of object-oriented programming in Python (as needed) 14 | 15 | ## Prerequisites (software) 16 | 17 | - Python >= 3.7 incl. NumPy, Scipy, and MatplotLib 18 | 19 | ## Prerequisites (knowledge) 20 | 21 | - Basic knowledge in the Python programming language, esp. lists, tupels and lambda-functions 22 | - [Tutorial 01-System-Simulation-ODE](../01-System-Simulation-ODE/README.md) 23 | 24 | ## How to build the tutorial 25 | 26 | In the `doc` directory of this repository run 27 | 28 | ``` bash 29 | lualatex tutorial2.tex 30 | biber tutorial2 31 | lualatex tutorial2.tex 32 | ``` 33 | -------------------------------------------------------------------------------- /01-System-Simulation-ODE/README.md: -------------------------------------------------------------------------------- 1 | 2 | # SystemSimulation-ODE 3 | 4 | This tutorial shows how to simulate a dynamic system described by a set of ordinary differential equations and how to illustrate the results by plots and animations. 5 | 6 | ## The following topics are covered by the tutorial 7 | 8 | - Simulation of a dynamical system using odeint or solve_ivp of the SciPy integration package 9 | - Presentation of the results using MatplotLib 10 | - Creation of animations based on the simulation results in order impress decision makers 11 | - Implementation of a flatness based feedforward and feedback control 12 | 13 | ## Prerequisites (software) 14 | 15 | - Python >= 3.7 incl. NumPy, Scipy, and MatplotLib 16 | 17 | ## Prerequisites (knowledge) 18 | 19 | - Basic knowledge in the Python programming language, esp. lists, tupels and lambda-functions 20 | 21 | ## How to build the tutorial 22 | 23 | In the `doc` directory of this repository run 24 | 25 | ``` bash 26 | lualatex tutorial1.tex 27 | biber tutorial1 28 | makeglossaries tutorial1 29 | lualatex tutorial1.tex 30 | ``` 31 | -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/literature.bib: -------------------------------------------------------------------------------- 1 | @book {Rud03, 2 | author = { Rudolph, Joachim AND Winkler, Jan AND Woittennek, Frank }, 3 | title = { Flatness based control of distributed parameter systems : examples and computer exercises from various technological domains}, 4 | publisher = {Shaker,}, 5 | keywords = { Technisches System , Komplexes System , System mit verteilten Parametern , Folgeregelung , Bahnplanung , Reglerentwurf }, 6 | year = {2003}, 7 | booktitle = {Berichte aus der Steuerungs- und Regelungstechnik}, 8 | address = { Aachen : }, 9 | } 10 | 11 | @book {KnollPython21, 12 | author = {Knoll, Carsten AND Heedt, Robert}, 13 | title = {Python für Ingenieure für Dummies: Mit vielen Programmbeispielen zu Numpy, Matplotlib und mehr}, 14 | publisher = {Wiley-VCH}, 15 | year = {2021}, 16 | address = {Weinheim}, 17 | } 18 | 19 | @techreport{Tut1, 20 | author = {Winkler, Jan AND Pritzkoleit, Max}, 21 | institution = {Institute of Control Theory, Faculty of Electrical and Computer Engineering, Technische Universität Dresden, Germany}, 22 | title = {Python for simulation, animation and control Part 1: Introductory tutorial for the simulation of dynamic systems -- Demonstration using the model of a kinematic vehicle}, 23 | year = {2021}, 24 | url = {https://github.com/TUD-RST/pytutorials/tree/master/01-System-Simulation-ODE} 25 | } -------------------------------------------------------------------------------- /01-System-Simulation-ODE/sim/03_events.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | import scipy.integrate as sci 4 | import matplotlib.pyplot as plt 5 | from dataclasses import dataclass 6 | 7 | 8 | # ODE parameter 9 | @dataclass 10 | class Para: 11 | a: float = 1 # dxdt = a*x 12 | 13 | 14 | # Simulation parameter 15 | @dataclass 16 | class SimPara: 17 | t0: float = 0 # start time 18 | tf: float = 10 # final time 19 | dt: float = 0.04 # step-size 20 | 21 | 22 | def ode(t, x, p): 23 | """Function of the robots kinematics 24 | 25 | Args: 26 | x : state 27 | t : time 28 | p(object): parameter container class 29 | 30 | Returns: 31 | dxdt: state derivative 32 | """ 33 | x1, = x # state vector 34 | 35 | # dxdt = f(x, u): 36 | dxdt = np.array([p.a*x1]) 37 | 38 | # return state derivative 39 | return dxdt 40 | 41 | 42 | def event(t, x): 43 | x_max = 5 44 | return np.abs(x)-x_max 45 | 46 | 47 | event.terminal = True 48 | 49 | # time vector 50 | tt = np.arange(SimPara.t0, SimPara.tf + SimPara.dt, SimPara.dt) 51 | 52 | # initial state 53 | x0 = [1] 54 | 55 | # simulation 56 | sol = sci.solve_ivp(lambda t, x: ode(t, x, Para), (SimPara.t0, SimPara.tf), x0, t_eval=tt, events=event) 57 | x_traj = sol.y.T 58 | 59 | plt.plot(sol.t, x_traj) 60 | plt.xlabel(r't in s') 61 | plt.ylabel(r'$x(t)$') 62 | plt.show() 63 | -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/gevrey-package.tex: -------------------------------------------------------------------------------- 1 | \usepackage{amsfonts,amsthm} 2 | \usepackage{framed} 3 | \usepackage{makeidx} 4 | \usepackage{floatflt} 5 | \usepackage{subfigure} 6 | \usepackage{lscape} 7 | \hyphenation{con-di-tion} 8 | \numberwithin{equation}{section} 9 | 10 | % Partielle Ableitung #1: Zaehler, #2: Nenner 11 | \newcommand{\pdiff}[2] 12 | { 13 | \frac{\partial #1}{\partial #2} 14 | } 15 | 16 | \newtheorem{lmm}{Lemma}[section] 17 | \newtheorem{thrm}[lmm]{Theorem} 18 | \newtheorem{note}[lmm]{Note} 19 | 20 | \newcommand{\pAbl}[1]{\frac{\partial}{\partial #1}} % partielle Ableitung 21 | % Einige Wertebereiche 22 | \newcommand{\domC}{\mathbb{C}} %Natuerliche Zahlen 23 | \newcommand{\domN}{\mathbb{N}} %Natuerliche Zahlen 24 | \newcommand{\domR}{\mathbb{R}} %Reelle Zahle 25 | \def\RR{\mathbb R} 26 | \newcommand{\domRp}{\mathbb{R}^+} %Positive Reele Zahlen 27 | \newcommand{\Gras}[1]{{#1}}%\boldmath 28 | \newcommand{\bolpi}{{\Gras \pi}} 29 | \newcommand{\bols}{{\Gras s}} 30 | \newcommand{\Deltas}{{\Gras \delta}} 31 | \newcommand{\boltheta}{{\Gras \theta}} 32 | \newcommand{\bolsigma}{{\Gras \sigma}} 33 | \newcommand{\bolbeta}{{\Gras \beta}} 34 | \newcommand{\bolepsilon}{{\Gras \epsilon}} 35 | \newcommand{\ie}{{i.e.},\ } 36 | \newcommand{\cf}{{cf.}\ } 37 | \newcommand{\quoted}[1]{{\glqq #1\grqq}} 38 | \newcommand{\myparagraph}[1]{\underline{\textbf{#1}}\\[0ex]} 39 | \newcounter{exercise}%[chapter] 40 | \numberwithin{exercise}{section} 41 | \newenvironment{mycompexer}{% 42 | \refstepcounter{exercise}% 43 | \pagebreak[3] 44 | \begin{shadedwithicon}{13.5}{\protect{\parbox{1cm}{\center\includegraphics[width=0.75cm]{ico_compexer}}}\nopagebreak}% 45 | {\noindent\underline{\textbf{Exercise \theexercise:}}\\[0.5ex]}% 46 | }{\end{shadedwithicon}} 47 | \newcommand{\df}{\mathrm{d}} 48 | 49 | %%% Local Variables: 50 | %%% mode: latex 51 | %%% TeX-master: t 52 | %%% End: 53 | -------------------------------------------------------------------------------- /02-Trajectories-And-Control/sim/01_trajectory_planning.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from TrajGen import PolynomialTrajGen, PrototypeTrajGen, GevreyTrajGen 5 | 6 | # example 1 - polynomial TrajGen 7 | # LISTING_START ArrayDef 8 | YA = np.array([0, 0, 0]) # t = t0 9 | YB = np.array([1, 0, 0]) # t = tf 10 | # LISTING_END ArrayDef 11 | 12 | # LISTING_START TimeDef 13 | t0 = 0 # start time of transition 14 | tf = 1 # final time of transition 15 | tt = np.linspace(t0, tf, 100) # -1 to 4 in 500 steps 16 | # LISTING_END TimeDef 17 | 18 | # LISTING_START PolyTrajGenInstanciation 19 | d = 2 # smooth derivative up to order d 20 | yd = PolynomialTrajGen(YA, YB, t0, tf, d) 21 | # LISTING_END PolyTrajGenInstanciation 22 | 23 | # display the parameter vector 24 | print("c = ", yd.c) 25 | 26 | # sample the generated trajectory 27 | # LISTING_START CalcAndPlot 28 | Y = yd.eval_vec(tt) 29 | 30 | # plot the trajectory 31 | plt.figure(1) 32 | plt.plot(tt, Y) 33 | plt.title('Planned trajectory') 34 | plt.legend([r'$y_d(t)$', r'$\dot{y}_d(t)$', r'$\ddot{y}_d(t)$']) 35 | plt.xlabel(r't in s') 36 | plt.grid(True) 37 | # LISTING_END CalcAndPlot 38 | 39 | 40 | # example 2 - prototype TrajGen 41 | yd2 = PrototypeTrajGen(YA, YB, t0, tf, d) 42 | 43 | # sample the generated trajectory 44 | Y2 = yd2.eval_vec(tt) 45 | 46 | # plot the trajectory 47 | plt.figure(2) 48 | plt.plot(tt, Y2) 49 | plt.title('Planned trajectory') 50 | plt.legend([r'$y_d(t)$', r'$\dot{y}_d(t)$', r'$\ddot{y}_d(t)$']) 51 | plt.xlabel(r't in s') 52 | plt.grid(True) 53 | 54 | # example 3 - Gevrey TrajGen 55 | s1 = 1.1 56 | s2 = 1.9 57 | yd3 = GevreyTrajGen(YA, YB, t0, tf, d, s1) 58 | yd4 = GevreyTrajGen(YA, YB, t0, tf, d, s2) 59 | 60 | # sample the generated trajectory 61 | Y3 = yd3.eval_vec(tt) 62 | Y4 = yd4.eval_vec(tt) 63 | 64 | # plot the trajectory 65 | plt.figure(3) 66 | plt.plot(tt, Y3) 67 | plt.title('Planned trajectory') 68 | plt.legend([r'$y_d(t)$', r'$\dot{y}_d(t)$', r'$\ddot{y}_d(t)$']) 69 | plt.xlabel(r't in s') 70 | plt.grid(True) 71 | 72 | plt.figure(4) 73 | plt.plot(tt, Y3[:, 0], tt, Y4[:, 0]) 74 | plt.title(r'$y = \varphi^{(0)}_{\sigma,T}(t)$') 75 | plt.legend([r'$\sigma = 1.1$', r'$\sigma = 1.9$']) 76 | plt.xlabel(r't in s') 77 | plt.grid(True) 78 | 79 | plt.figure(5) 80 | plt.plot(tt, Y3[:, 1], tt, Y4[:, 1]) 81 | plt.title(r'$y = \varphi^{(1)}_{\sigma,T}(t)$') 82 | plt.legend([r'$\sigma = 1.1$', r'$\sigma = 1.9$']) 83 | plt.xlabel(r't in s') 84 | plt.grid(True) 85 | 86 | plt.show() 87 | -------------------------------------------------------------------------------- /01-System-Simulation-ODE/doc/img/car-like_mobile_robot.pdf_tex: -------------------------------------------------------------------------------- 1 | %% Creator: Inkscape inkscape 0.91, www.inkscape.org 2 | %% PDF/EPS/PS + LaTeX output extension by Johan Engelen, 2010 3 | %% Accompanies image file 'car-like_mobile_robot.pdf' (pdf, eps, ps) 4 | %% 5 | %% To include the image in your LaTeX document, write 6 | %% \input{.pdf_tex} 7 | %% instead of 8 | %% \includegraphics{.pdf} 9 | %% To scale the image, write 10 | %% \def\svgwidth{} 11 | %% \input{.pdf_tex} 12 | %% instead of 13 | %% \includegraphics[width=]{.pdf} 14 | %% 15 | %% Images with a different path to the parent latex file can 16 | %% be accessed with the `import' package (which may need to be 17 | %% installed) using 18 | %% \usepackage{import} 19 | %% in the preamble, and then including the image with 20 | %% \import{}{.pdf_tex} 21 | %% Alternatively, one can specify 22 | %% \graphicspath{{/}} 23 | %% 24 | %% For more information, please see info/svg-inkscape on CTAN: 25 | %% http://tug.ctan.org/tex-archive/info/svg-inkscape 26 | %% 27 | \begingroup% 28 | \makeatletter% 29 | \providecommand\color[2][]{% 30 | \errmessage{(Inkscape) Color is used for the text in Inkscape, but the package 'color.sty' is not loaded}% 31 | \renewcommand\color[2][]{}% 32 | }% 33 | \providecommand\transparent[1]{% 34 | \errmessage{(Inkscape) Transparency is used (non-zero) for the text in Inkscape, but the package 'transparent.sty' is not loaded}% 35 | \renewcommand\transparent[1]{}% 36 | }% 37 | \providecommand\rotatebox[2]{#2}% 38 | \ifx\svgwidth\undefined% 39 | \setlength{\unitlength}{283.46456693bp}% 40 | \ifx\svgscale\undefined% 41 | \relax% 42 | \else% 43 | \setlength{\unitlength}{\unitlength * \real{\svgscale}}% 44 | \fi% 45 | \else% 46 | \setlength{\unitlength}{\svgwidth}% 47 | \fi% 48 | \global\let\svgwidth\undefined% 49 | \global\let\svgscale\undefined% 50 | \makeatother% 51 | \begin{picture}(1,0.6)% 52 | \put(0,0){\includegraphics[width=\unitlength,page=1]{img/car-like_mobile_robot.pdf}}% 53 | \put(0.27053241,0.03506941){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$Y_1$}}}% 54 | \put(0.03748157,0.29838656){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$Y_2$}}}% 55 | \put(0.49375143,0.30024091){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$\theta$}}}% 56 | \put(0.71301268,0.21633586){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$\phi$}}}% 57 | \put(0.26023251,0.252724){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$(y_1,y_2)$}}}% 58 | \put(0.55597377,0.17913271){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$l$}}}% 59 | \put(0.39638997,0.32873697){\color[rgb]{0,0,0}\makebox(0,0)[lb]{\smash{$\vec{v}$}}}% 60 | \end{picture}% 61 | \endgroup% 62 | -------------------------------------------------------------------------------- /01-System-Simulation-ODE/doc/luainputlisting.lua: -------------------------------------------------------------------------------- 1 | --[[ 2 | Reference certain part of the code via a tag and print it using the listings package. 3 | 4 | In order to use this function the listings package must be loaded in the LaTeX document. 5 | The advantage of this approach is that you can modify the source code without redefine 6 | the referencing line numbers in the LaTeX document. 7 | 8 | Usage: print_code_part{filename}{tagname} 9 | 10 | filename: The name of the file from which parts of the code are going to be listed 11 | tagname : The tag in the file which marks the part to be listed. See expl. below. 12 | 13 | In the source code file mark the part which is going to be shown as follows: 14 | 15 | At begin source code part going to be listed add the following line with a unique tagname 16 | # LISTING_START tagname 17 | 18 | At the end of the source code part going to be listed add the following line with a unique tagname: 19 | # LISTING_END tagname 20 | 21 | Written by Robert Heedt, Institut für Regelungs- und Steuerungstheorie, TU Dresden, 2019 22 | --]] 23 | function print_code_part(file, listing_tag) 24 | local current_line = 1 25 | local line_start = 1 26 | local line_end = 1 27 | io.input(file) 28 | 29 | while true do 30 | line_str = io.read("*line") 31 | 32 | if line_str == nil then 33 | break 34 | -- the thing we're looking for should be at the end of the line or followed by whitespace 35 | -- this prevents 'tag10' erroneously matching 'tag1' 36 | elseif string.match(line_str, "LISTING_START " .. listing_tag .. "$") or string.match(line_str, "LISTING_START " .. listing_tag .. "%s") then 37 | line_start = current_line + 1 38 | elseif string.match(line_str, "LISTING_END " .. listing_tag .. "$") or string.match(line_str, "LISTING_END " .. listing_tag .. "%s") then 39 | line_end = current_line - 1 40 | break 41 | end 42 | 43 | current_line = current_line + 1 44 | end 45 | 46 | local latex_command = "" 47 | if (line_start == 1) then 48 | latex_command = string.format("\\newline \\textbf{No code-block section found starting with \\emph{%s} (or LISTING\\_END placed before LISTING\\_START)} \\newline", listing_tag) 49 | elseif (line_end == 1) then 50 | latex_command = string.format("\\newline \\textbf{No code-block section found ending with \\emph{%s}} \\newline", listing_tag) 51 | else 52 | latex_command = string.format("\\lstinputlisting[numbers=left,firstnumber=%s,firstline=%s,lastline=%s]{%s}", line_start, line_start, line_end, file) 53 | end 54 | tex.print(latex_command) 55 | end 56 | 57 | 58 | --[[ 59 | Reference a single line of code via its beginning and print it using the listings package. 60 | 61 | In order to use this function the listings package must be loaded in the LaTeX document. 62 | The advantage of this approach is that you can modify the source code without redefine 63 | the referencing line numbers in the LaTeX document. 64 | 65 | Usage: print_code_line{filename}{line_starts_with} 66 | 67 | filename: The name of the file from which the code line is going to be listed 68 | tagname : The string the line begins with (arbitrary number of leading spaces 69 | and tabstopss are allowed) 70 | 71 | Written by Jan Winkler, Institut für Regelungs- und Steuerungstheorie, TU Dresden, 2020 72 | --]] 73 | function print_code_line(file, line_starts_with) 74 | local current_line = 0 75 | local MatchFound = false 76 | io.input(file) 77 | 78 | while (MatchFound == false) do 79 | line_str = io.read("*line") 80 | 81 | if line_str == nil then 82 | break 83 | else 84 | current_line = current_line + 1 85 | end 86 | 87 | -- Escape the magics which may reside in the string ( e.g. print("%s") -> print%("%%s"%) )... 88 | esacped_line = line_starts_with:gsub("[%(%)%.%%%+%-%*%?%[%]%^%$]", function(c) return "%" .. c end) 89 | 90 | -- Find line (incl. possible white spacesand tabstops) 91 | if string.match(line_str, "^([%s\t]*)" .. esacped_line) then 92 | MatchFound = true 93 | end 94 | end 95 | 96 | local latex_command = "" 97 | if MatchFound then 98 | latex_command = string.format("\\lstinputlisting[numbers=left,firstnumber=%s,firstline=%s,lastline=%s]{%s}", current_line, current_line, current_line, file) 99 | else 100 | latex_command = string.format("\\newline \\textbf{No line found starting with \\emph{%s}} \\newline", line_starts_with) 101 | end 102 | tex.print(latex_command) 103 | end -------------------------------------------------------------------------------- /02-Trajectories-And-Control/doc/luainputlisting.lua: -------------------------------------------------------------------------------- 1 | --[[ 2 | Reference certain part of the code via a tag and print it using the listings package. 3 | 4 | In order to use this function the listings package must be loaded in the LaTeX document. 5 | The advantage of this approach is that you can modify the source code without redefine 6 | the referencing line numbers in the LaTeX document. 7 | 8 | Usage: print_code_part{filename}{tagname} 9 | 10 | filename: The name of the file from which parts of the code are going to be listed 11 | tagname : The tag in the file which marks the part to be listed. See expl. below. 12 | 13 | In the source code file mark the part which is going to be shown as follows: 14 | 15 | At begin source code part going to be listed add the following line with a unique tagname 16 | # LISTING_START tagname 17 | 18 | At the end of the source code part going to be listed add the following line with a unique tagname: 19 | # LISTING_END tagname 20 | 21 | Written by Robert Heedt, Institut für Regelungs- und Steuerungstheorie, TU Dresden, 2019 22 | --]] 23 | function print_code_part(file, listing_tag) 24 | local current_line = 1 25 | local line_start = 1 26 | local line_end = 1 27 | io.input(file) 28 | 29 | while true do 30 | line_str = io.read("*line") 31 | 32 | if line_str == nil then 33 | break 34 | -- the thing we're looking for should be at the end of the line or followed by whitespace 35 | -- this prevents 'tag10' erroneously matching 'tag1' 36 | elseif string.match(line_str, "LISTING_START " .. listing_tag .. "$") or string.match(line_str, "LISTING_START " .. listing_tag .. "%s") then 37 | line_start = current_line + 1 38 | elseif string.match(line_str, "LISTING_END " .. listing_tag .. "$") or string.match(line_str, "LISTING_END " .. listing_tag .. "%s") then 39 | line_end = current_line - 1 40 | break 41 | end 42 | 43 | current_line = current_line + 1 44 | end 45 | 46 | local latex_command = "" 47 | if (line_start == 1) then 48 | latex_command = string.format("\\newline \\textbf{No code-block section found starting with \\emph{%s} (or LISTING\\_END placed before LISTING\\_START)} \\newline", listing_tag) 49 | elseif (line_end == 1) then 50 | latex_command = string.format("\\newline \\textbf{No code-block section found ending with \\emph{%s}} \\newline", listing_tag) 51 | else 52 | latex_command = string.format("\\lstinputlisting[numbers=left,firstnumber=%s,firstline=%s,lastline=%s]{%s}", line_start, line_start, line_end, file) 53 | end 54 | tex.print(latex_command) 55 | end 56 | 57 | 58 | --[[ 59 | Reference a single line of code via its beginning and print it using the listings package. 60 | 61 | In order to use this function the listings package must be loaded in the LaTeX document. 62 | The advantage of this approach is that you can modify the source code without redefine 63 | the referencing line numbers in the LaTeX document. 64 | 65 | Usage: print_code_line{filename}{line_starts_with} 66 | 67 | filename: The name of the file from which the code line is going to be listed 68 | tagname : The string the line begins with (arbitrary number of leading spaces 69 | and tabstopss are allowed) 70 | 71 | Written by Jan Winkler, Institut für Regelungs- und Steuerungstheorie, TU Dresden, 2020 72 | --]] 73 | function print_code_line(file, line_starts_with) 74 | local current_line = 0 75 | local MatchFound = false 76 | io.input(file) 77 | 78 | while (MatchFound == false) do 79 | line_str = io.read("*line") 80 | 81 | if line_str == nil then 82 | break 83 | else 84 | current_line = current_line + 1 85 | end 86 | 87 | -- Escape the magics which may reside in the string ( e.g. print("%s") -> print%("%%s"%) )... 88 | esacped_line = line_starts_with:gsub("[%(%)%.%%%+%-%*%?%[%]%^%$]", function(c) return "%" .. c end) 89 | 90 | -- Find line (incl. possible white spacesand tabstops) 91 | if string.match(line_str, "^([%s\t]*)" .. esacped_line) then 92 | MatchFound = true 93 | end 94 | end 95 | 96 | local latex_command = "" 97 | if MatchFound then 98 | latex_command = string.format("\\lstinputlisting[numbers=left,firstnumber=%s,firstline=%s,lastline=%s]{%s}", current_line, current_line, current_line, file) 99 | else 100 | latex_command = string.format("\\newline \\textbf{No line found starting with \\emph{%s}} \\newline", line_starts_with) 101 | end 102 | tex.print(latex_command) 103 | end -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/luainputlisting.lua: -------------------------------------------------------------------------------- 1 | --[[ 2 | Reference certain part of the code via a tag and print it using the listings package. 3 | 4 | In order to use this function the listings package must be loaded in the LaTeX document. 5 | The advantage of this approach is that you can modify the source code without redefine 6 | the referencing line numbers in the LaTeX document. 7 | 8 | Usage: print_code_part{filename}{tagname} 9 | 10 | filename: The name of the file from which parts of the code are going to be listed 11 | tagname : The tag in the file which marks the part to be listed. See expl. below. 12 | 13 | In the source code file mark the part which is going to be shown as follows: 14 | 15 | At begin source code part going to be listed add the following line with a unique tagname 16 | # LISTING_START tagname 17 | 18 | At the end of the source code part going to be listed add the following line with a unique tagname: 19 | # LISTING_END tagname 20 | 21 | Written by Robert Heedt, Institut für Regelungs- und Steuerungstheorie, TU Dresden, 2019 22 | --]] 23 | function print_code_part(file, listing_tag) 24 | local current_line = 1 25 | local line_start = 1 26 | local line_end = 1 27 | io.input(file) 28 | 29 | while true do 30 | line_str = io.read("*line") 31 | 32 | if line_str == nil then 33 | break 34 | -- the thing we're looking for should be at the end of the line or followed by whitespace 35 | -- this prevents 'tag10' erroneously matching 'tag1' 36 | elseif string.match(line_str, "LISTING_START " .. listing_tag .. "$") or string.match(line_str, "LISTING_START " .. listing_tag .. "%s") then 37 | line_start = current_line + 1 38 | elseif string.match(line_str, "LISTING_END " .. listing_tag .. "$") or string.match(line_str, "LISTING_END " .. listing_tag .. "%s") then 39 | line_end = current_line - 1 40 | break 41 | end 42 | 43 | current_line = current_line + 1 44 | end 45 | 46 | local latex_command = "" 47 | if (line_start == 1) then 48 | latex_command = string.format("\\newline \\textbf{No code-block section found starting with \\emph{%s} (or LISTING\\_END placed before LISTING\\_START)} \\newline", listing_tag) 49 | elseif (line_end == 1) then 50 | latex_command = string.format("\\newline \\textbf{No code-block section found ending with \\emph{%s}} \\newline", listing_tag) 51 | else 52 | latex_command = string.format("\\lstinputlisting[numbers=left,firstnumber=%s,firstline=%s,lastline=%s]{%s}", line_start, line_start, line_end, file) 53 | end 54 | tex.print(latex_command) 55 | end 56 | 57 | 58 | --[[ 59 | Reference a single line of code via its beginning and print it using the listings package. 60 | 61 | In order to use this function the listings package must be loaded in the LaTeX document. 62 | The advantage of this approach is that you can modify the source code without redefine 63 | the referencing line numbers in the LaTeX document. 64 | 65 | Usage: print_code_line{filename}{line_starts_with} 66 | 67 | filename: The name of the file from which the code line is going to be listed 68 | tagname : The string the line begins with (arbitrary number of leading spaces 69 | and tabstopss are allowed) 70 | 71 | Written by Jan Winkler, Institut für Regelungs- und Steuerungstheorie, TU Dresden, 2020 72 | --]] 73 | function print_code_line(file, line_starts_with) 74 | local current_line = 0 75 | local MatchFound = false 76 | io.input(file) 77 | 78 | while (MatchFound == false) do 79 | line_str = io.read("*line") 80 | 81 | if line_str == nil then 82 | break 83 | else 84 | current_line = current_line + 1 85 | end 86 | 87 | -- Escape the magics which may reside in the string ( e.g. print("%s") -> print%("%%s"%) )... 88 | esacped_line = line_starts_with:gsub("[%(%)%.%%%+%-%*%?%[%]%^%$]", function(c) return "%" .. c end) 89 | 90 | -- Find line (incl. possible white spacesand tabstops) 91 | if string.match(line_str, "^([%s\t]*)" .. esacped_line) then 92 | MatchFound = true 93 | end 94 | end 95 | 96 | local latex_command = "" 97 | if MatchFound then 98 | latex_command = string.format("\\lstinputlisting[numbers=left,firstnumber=%s,firstline=%s,lastline=%s]{%s}", current_line, current_line, current_line, file) 99 | else 100 | latex_command = string.format("\\newline \\textbf{No line found starting with \\emph{%s}} \\newline", line_starts_with) 101 | end 102 | tex.print(latex_command) 103 | end -------------------------------------------------------------------------------- /01-System-Simulation-ODE/sim/01_car_example_plotting.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | from numpy import cos, sin, tan 4 | import scipy.integrate as sci 5 | import matplotlib.pyplot as plt 6 | from typing import Type 7 | from dataclasses import dataclass 8 | 9 | 10 | # LISTING_START ParaClass 11 | # Physical parameter 12 | @dataclass 13 | class Para: 14 | l: float = 0.3 # define car length 15 | w: float = l*0.3 # define car width 16 | # LISTING_END ParaClass 17 | 18 | 19 | # LISTING_START SimuPara 20 | # Simulation parameter 21 | @dataclass 22 | class SimPara: 23 | t0: float = 0 # start time 24 | tf: float = 10 # final time 25 | dt: float = 0.04 # step-size 26 | # LISTING_END SimuPara 27 | 28 | 29 | # LISTING_START OdeFunDef 30 | def ode(t, x, p: Type[Para]): 31 | """Function of the robots kinematics 32 | 33 | Args: 34 | x : state 35 | t : time 36 | p(object): parameter container class 37 | 38 | Returns: 39 | dxdt: state derivative 40 | """ 41 | x1, x2, x3 = x # state vector 42 | u1, u2 = control(t) # control vector 43 | 44 | # dxdt = f(x, u): 45 | dxdt = np.array([u1 * cos(x3), 46 | u1 * sin(x3), 47 | 1 / p.l * u1 * tan(u2)]) 48 | 49 | # return state derivative 50 | return dxdt 51 | # LISTING_END OdeFunDef 52 | 53 | 54 | # LISTING_START ControlFunDef 55 | def control(t): 56 | """Function of the control law 57 | 58 | Args: 59 | t: time 60 | 61 | Returns: 62 | u: control vector 63 | 64 | """ 65 | u1 = np.maximum(0, 1.0 - 0.1 * t) 66 | u2 = np.full(u1.shape, 0.25) 67 | return np.array([u1, u2]).T 68 | # LISTING_END ControlFunDef 69 | 70 | 71 | # LISTING_START PlotFunDef 72 | def plot_data(x, u, t, fig_width, fig_height, save=False): 73 | """Plotting function of simulated state and actions 74 | 75 | Args: 76 | x(ndarray) : state-vector trajectory 77 | u(ndarray) : control vector trajectory 78 | t(ndarray) : time vector 79 | fig_width : figure width in cm 80 | fig_height : figure height in cm 81 | save (bool): save figure (default: False) 82 | Returns: None 83 | 84 | """ 85 | # creating a figure with 3 subplots, that share the x-axis 86 | fig1, (ax1, ax2, ax3) = plt.subplots(3) 87 | 88 | # set figure size to desired values 89 | fig1.set_size_inches(fig_width / 2.54, fig_height / 2.54) 90 | 91 | # plot y_1 in subplot 1 92 | ax1.plot(t, x[:, 0], label='$y_1(t)$', lw=1, color='r') 93 | 94 | # plot y_2 in subplot 1 95 | ax1.plot(t, x[:, 1], label='$y_2(t)$', lw=1, color='b') 96 | 97 | # plot theta in subplot 2 98 | ax2.plot(t, np.rad2deg(x[:, 2]), label=r'$\theta(t)$', lw=1, color='g') 99 | 100 | # plot control in subplot 3, left axis red, right blue 101 | ax3.plot(t, np.rad2deg(u[:, 0]), label=r'$v(t)$', lw=1, color='r') 102 | ax3.tick_params(axis='y', colors='r') 103 | ax33 = ax3.twinx() 104 | ax33.plot(t, np.rad2deg(u[:, 1]), label=r'$\phi(t)$', lw=1, color='b') 105 | ax33.spines["left"].set_color('r') 106 | ax33.spines["right"].set_color('b') 107 | ax33.tick_params(axis='y', colors='b') 108 | 109 | # Grids 110 | ax1.grid(True) 111 | ax2.grid(True) 112 | ax3.grid(True) 113 | 114 | # set the labels on the x and y axis and the titles 115 | ax1.set_title('Position coordinates') 116 | ax1.set_ylabel(r'm') 117 | ax1.set_xlabel(r't in s') 118 | ax2.set_title('Orientation') 119 | ax2.set_ylabel(r'deg') 120 | ax2.set_xlabel(r't in s') 121 | ax3.set_title('Velocity / steering angle') 122 | ax3.set_ylabel(r'm/s') 123 | ax33.set_ylabel(r'deg') 124 | ax3.set_xlabel(r't in s') 125 | 126 | # put a legend in the plot 127 | ax1.legend() 128 | ax2.legend() 129 | ax3.legend() 130 | li3, lab3 = ax3.get_legend_handles_labels() 131 | li33, lab33 = ax33.get_legend_handles_labels() 132 | ax3.legend(li3 + li33, lab3 + lab33, loc=0) 133 | 134 | # automatically adjusts subplot to fit in figure window 135 | plt.tight_layout() 136 | 137 | # save the figure in the working directory 138 | if save: 139 | plt.savefig('state_trajectory.pdf') # save output as pdf 140 | # plt.savefig('state_trajectory.pgf') # for easy export to LaTeX, needs a lot of extra packages 141 | return None 142 | # LISTING_END PlotFunDef 143 | 144 | 145 | # LISTING_START Simulation 146 | # time vector 147 | tt = np.arange(SimPara.t0, SimPara.tf + SimPara.dt, SimPara.dt) 148 | 149 | # initial state 150 | x0 = [0, 0, 0] 151 | 152 | # simulation 153 | sol = sci.solve_ivp(lambda t, x: ode(t, x, Para), (SimPara.t0, SimPara.tf), x0, t_eval=tt) 154 | x_traj = sol.y.T 155 | u_traj = control(tt) 156 | # LISTING_END Simulation 157 | 158 | # LISTING_START PlotResult 159 | # plot 160 | plot_data(x_traj, u_traj, tt, 12, 16, save=True) 161 | plt.show() 162 | # LISTING_END PlotResult 163 | -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/generate_pendulum_swingup.py: -------------------------------------------------------------------------------- 1 | """ 2 | Adapted from PyTrajectory's ex0_InvertedPendulumSwingUp 3 | """ 4 | from pytrajectory import TransitionProblem 5 | import numpy as np 6 | from sympy import cos, sin 7 | from numpy import pi 8 | 9 | import sys 10 | import matplotlib as mpl 11 | from pytrajectory.visualisation import Animation 12 | 13 | 14 | def f(xx, uu, uuref, t, pp): 15 | """ Right hand side of the vectorfield defining the system dynamics 16 | 17 | :param xx: state 18 | :param uu: input 19 | :param uuref: reference input (not used) 20 | :param t: time (not used) 21 | :param pp: additionial free parameters (not used) 22 | 23 | :return: xdot 24 | """ 25 | x1, x2, x3, x4 = xx # system variables 26 | u1, = uu # input variable 27 | 28 | l = 0.5 # length of the pendulum 29 | g = 9.81 # gravitational acceleration 30 | 31 | # this is the vectorfield 32 | ff = [x2, 33 | u1, 34 | x4, 35 | (1/l)*(g*sin(x3)+u1*cos(x3))] 36 | 37 | return ff 38 | 39 | 40 | # then we specify all boundary conditions 41 | a = 0.0 42 | xa = [0.0, 0.0, pi, 0.0] 43 | 44 | b = 2.0 45 | xb = [0.0, 0.0, 0.0, 0.0] 46 | 47 | ua = [0.0] 48 | ub = [0.0] 49 | 50 | from pytrajectory import log 51 | log.console_handler.setLevel(20) 52 | 53 | # now we create our Trajectory object and alter some method parameters via the keyword arguments 54 | 55 | first_guess = {'seed': 20} 56 | S = TransitionProblem(f, a, b, xa, xb, ua, ub, first_guess=first_guess, kx=2, eps=5e-2, use_chains=False, sol_steps=1300) 57 | 58 | # time to run the iteration 59 | S.solve() 60 | 61 | 62 | # now that we (hopefully) have found a solution, 63 | # we can visualise our systems dynamic 64 | 65 | # therefore we define a function that draws an image of the system 66 | # according to the given simulation data 67 | def draw(xt, image): 68 | # to draw the image we just need the translation `x` of the 69 | # cart and the deflection angle `phi` of the pendulum. 70 | x = xt[0] 71 | phi = xt[2] 72 | 73 | # next we set some parameters 74 | car_width = 0.05 75 | car_heigth = 0.02 76 | 77 | rod_length = 0.5 78 | pendulum_size = 0.015 79 | 80 | # then we determine the current state of the system 81 | # according to the given simulation data 82 | x_car = x 83 | y_car = 0 84 | 85 | x_pendulum = -rod_length * sin(phi) + x_car 86 | y_pendulum = rod_length * cos(phi) 87 | 88 | # now we can build the image 89 | 90 | # the pendulum will be represented by a black circle with 91 | # center: (x_pendulum, y_pendulum) and radius `pendulum_size 92 | pendulum = mpl.patches.Circle(xy=(x_pendulum, y_pendulum), radius=pendulum_size, color='black') 93 | 94 | # the cart will be represented by a grey rectangle with 95 | # lower left: (x_car - 0.5 * car_width, y_car - car_heigth) 96 | # width: car_width 97 | # height: car_height 98 | car = mpl.patches.Rectangle((x_car-0.5*car_width, y_car-car_heigth), car_width, car_heigth, 99 | fill=True, facecolor='grey', linewidth=2.0) 100 | 101 | # the joint will also be a black circle with 102 | # center: (x_car, 0) 103 | # radius: 0.005 104 | joint = mpl.patches.Circle((x_car,0), 0.005, color='black') 105 | 106 | # and the pendulum rod will just by a line connecting the cart and the pendulum 107 | rod = mpl.lines.Line2D([x_car,x_pendulum], [y_car,y_pendulum], 108 | color='black', zorder=1, linewidth=2.0) 109 | 110 | # finally we add the patches and line to the image 111 | image.patches.append(pendulum) 112 | image.patches.append(car) 113 | image.patches.append(joint) 114 | image.lines.append(rod) 115 | 116 | # and return the image 117 | return image 118 | 119 | 120 | # now we can create an instance of the `Animation` class 121 | # with our draw function and the simulation results 122 | 123 | # save the simulation data (solution of IVP) to csv 124 | tt, xx, uu = S.sim_data 125 | export_array = np.hstack((tt.reshape(-1, 1), xx, uu)) 126 | np.savetxt("pendulum.csv", export_array, delimiter=",") 127 | 128 | 129 | # first column: time 130 | # next n columns: state (here n = 4) 131 | # last m columns: input (here m = 1) 132 | 133 | # to plot the curves of some trajectories along with the picture 134 | # we also pass the appropriate lists as arguments (see documentation) 135 | A = Animation(drawfnc=draw, simdata=S.sim_data, 136 | plotsys=[(0,'x'), (2,'phi')], plotinputs=[(0,'u')]) 137 | 138 | # as for now we have to explicitly set the limits of the figure 139 | # (may involves some trial and error) 140 | xmin = np.min(S.sim_data[1][:,0]); xmax = np.max(S.sim_data[1][:,0]) 141 | A.set_limits(xlim=(xmin - 0.5, xmax + 0.5), ylim=(-0.6,0.6)) 142 | 143 | A.animate() 144 | 145 | # then we can save the animation as a `mp4` video file or as an animated `gif` file 146 | A.save('pendulum.mp4') 147 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # ========== 2 | # LaTeX suff 3 | # ========== 4 | 5 | ## Core latex/pdflatex auxiliary files: 6 | *.aux 7 | *.lof 8 | *.log 9 | *.lot 10 | *.fls 11 | *.out 12 | *.toc 13 | *.fmt 14 | *.fot 15 | *.cb 16 | *.cb2 17 | .*.lb 18 | 19 | ## Intermediate documents: 20 | *.dvi 21 | *.xdv 22 | *-converted-to.* 23 | # these rules might exclude image files for figures etc. 24 | # *.ps 25 | # *.eps 26 | # *.pdf 27 | 28 | ## Generated if empty string is given at "Please type another file name for output:" 29 | .pdf 30 | 31 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 32 | *.bbl 33 | *.bcf 34 | *.blg 35 | *-blx.aux 36 | *-blx.bib 37 | *.run.xml 38 | 39 | ## Build tool auxiliary files: 40 | *.fdb_latexmk 41 | *.synctex 42 | *.synctex(busy) 43 | *.synctex.gz 44 | *.synctex.gz(busy) 45 | *.pdfsync 46 | 47 | ## Auxiliary and intermediate files from other packages: 48 | # algorithms 49 | *.alg 50 | *.loa 51 | 52 | # achemso 53 | acs-*.bib 54 | 55 | # amsthm 56 | *.thm 57 | 58 | # beamer 59 | *.nav 60 | *.pre 61 | *.snm 62 | *.vrb 63 | 64 | # changes 65 | *.soc 66 | 67 | # cprotect 68 | *.cpt 69 | 70 | # elsarticle (documentclass of Elsevier journals) 71 | *.spl 72 | 73 | # endnotes 74 | *.ent 75 | 76 | # fixme 77 | *.lox 78 | 79 | # feynmf/feynmp 80 | *.mf 81 | *.mp 82 | *.t[1-9] 83 | *.t[1-9][0-9] 84 | *.tfm 85 | 86 | #(r)(e)ledmac/(r)(e)ledpar 87 | *.end 88 | *.?end 89 | *.[1-9] 90 | *.[1-9][0-9] 91 | *.[1-9][0-9][0-9] 92 | *.[1-9]R 93 | *.[1-9][0-9]R 94 | *.[1-9][0-9][0-9]R 95 | *.eledsec[1-9] 96 | *.eledsec[1-9]R 97 | *.eledsec[1-9][0-9] 98 | *.eledsec[1-9][0-9]R 99 | *.eledsec[1-9][0-9][0-9] 100 | *.eledsec[1-9][0-9][0-9]R 101 | 102 | # glossaries 103 | *.acn 104 | *.acr 105 | *.glg 106 | *.glo 107 | *.gls 108 | *.glsdefs 109 | 110 | # gnuplottex 111 | *-gnuplottex-* 112 | 113 | # gregoriotex 114 | *.gaux 115 | *.gtex 116 | 117 | # htlatex 118 | *.4ct 119 | *.4tc 120 | *.idv 121 | *.lg 122 | *.trc 123 | *.xref 124 | 125 | # hyperref 126 | *.brf 127 | 128 | # knitr 129 | *-concordance.tex 130 | # TODO Comment the next line if you want to keep your tikz graphics files 131 | *.tikz 132 | *-tikzDictionary 133 | 134 | # listings 135 | *.lol 136 | 137 | # makeidx 138 | *.idx 139 | *.ilg 140 | *.ind 141 | *.ist 142 | 143 | # minitoc 144 | *.maf 145 | *.mlf 146 | *.mlt 147 | *.mtc[0-9]* 148 | *.slf[0-9]* 149 | *.slt[0-9]* 150 | *.stc[0-9]* 151 | 152 | # minted 153 | _minted* 154 | *.pyg 155 | 156 | # morewrites 157 | *.mw 158 | 159 | # nomencl 160 | *.nlg 161 | *.nlo 162 | *.nls 163 | 164 | # pax 165 | *.pax 166 | 167 | # pdfpcnotes 168 | *.pdfpc 169 | 170 | # sagetex 171 | *.sagetex.sage 172 | *.sagetex.py 173 | *.sagetex.scmd 174 | 175 | # scrwfile 176 | *.wrt 177 | 178 | # sympy 179 | *.sout 180 | *.sympy 181 | sympy-plots-for-*.tex/ 182 | 183 | # pdfcomment 184 | *.upa 185 | *.upb 186 | 187 | # pythontex 188 | *.pytxcode 189 | pythontex-files-*/ 190 | 191 | # thmtools 192 | *.loe 193 | 194 | # TikZ & PGF 195 | *.dpth 196 | *.md5 197 | *.auxlock 198 | 199 | # todonotes 200 | *.tdo 201 | 202 | # easy-todo 203 | *.lod 204 | 205 | # xmpincl 206 | *.xmpi 207 | 208 | # xindy 209 | *.xdy 210 | 211 | # xypic precompiled matrices 212 | *.xyc 213 | 214 | # endfloat 215 | *.ttt 216 | *.fff 217 | 218 | # Latexian 219 | TSWLatexianTemp* 220 | 221 | ## Editors: 222 | # WinEdt 223 | *.bak 224 | *.sav 225 | 226 | # Texpad 227 | .texpadtmp 228 | 229 | # Kile 230 | *.backup 231 | 232 | # KBibTeX 233 | *~[0-9]* 234 | 235 | # auto folder when using emacs and auctex 236 | ./auto/* 237 | *.el 238 | 239 | # expex forward references with \gathertags 240 | *-tags.tex 241 | 242 | # standalone packages 243 | *.sta 244 | 245 | 246 | # ============ 247 | # Python stuff 248 | # ============ 249 | 250 | # Byte-compiled / optimized / DLL files 251 | __pycache__/ 252 | *.py[cod] 253 | *$py.class 254 | 255 | # C extensions 256 | *.so 257 | 258 | # Distribution / packaging 259 | .Python 260 | env/ 261 | build/ 262 | develop-eggs/ 263 | dist/ 264 | downloads/ 265 | eggs/ 266 | .eggs/ 267 | lib/ 268 | lib64/ 269 | parts/ 270 | sdist/ 271 | var/ 272 | wheels/ 273 | *.egg-info/ 274 | .installed.cfg 275 | *.egg 276 | 277 | # PyInstaller 278 | # Usually these files are written by a python script from a template 279 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 280 | *.manifest 281 | *.spec 282 | 283 | # Installer logs 284 | pip-log.txt 285 | pip-delete-this-directory.txt 286 | 287 | # Unit test / coverage reports 288 | htmlcov/ 289 | .tox/ 290 | .coverage 291 | .coverage.* 292 | .cache 293 | nosetests.xml 294 | coverage.xml 295 | *.cover 296 | .hypothesis/ 297 | 298 | # Translations 299 | *.mo 300 | *.pot 301 | 302 | # Django stuff: 303 | *.log 304 | local_settings.py 305 | 306 | # Flask stuff: 307 | instance/ 308 | .webassets-cache 309 | 310 | # Scrapy stuff: 311 | .scrapy 312 | 313 | # Sphinx documentation 314 | docs/_build/ 315 | 316 | # PyBuilder 317 | target/ 318 | 319 | # Jupyter Notebook 320 | .ipynb_checkpoints 321 | 322 | # pyenv 323 | .python-version 324 | 325 | # celery beat schedule file 326 | celerybeat-schedule 327 | 328 | # SageMath parsed files 329 | *.sage.py 330 | 331 | # dotenv 332 | .env 333 | 334 | # virtualenv 335 | .venv 336 | venv/ 337 | ENV/ 338 | 339 | # Spyder project settings 340 | .spyderproject 341 | .spyproject 342 | 343 | # Rope project settings 344 | .ropeproject 345 | 346 | # mkdocs documentation 347 | /site 348 | 349 | # mypy 350 | .mypy_cache/ 351 | 352 | # additions 353 | .idea 354 | *.code-workspace 355 | .vscode 356 | 357 | 99-Jupyter-Cheat-Sheets/array.npy 358 | 99-Jupyter-Cheat-Sheets/array.txt 359 | 360 | 01-System-Simulation-ODE/sim/animation.mp4 361 | 01-System-Simulation-ODE/sim/state_trajectory.pdf 362 | 01-System-Simulation-ODE/sim/state_trajectory.pgf 363 | 03-Jupyter-Notebook-And-SymPy/doc/tutorial3.pdf 364 | 04-LQR-Design-And-Simulation/doc/tutorial4.pdf 365 | -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/plots.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import pickle 3 | 4 | fig = plt.figure(figsize=(6, 6)) 5 | lti0 = pickle.load(open("ludyk_lti_0.p", "rb")) 6 | lti5 = pickle.load(open("ludyk_lti_5.p", "rb")) 7 | 8 | ax1 = plt.subplot(221) 9 | plt.plot(lti0["t"], lti0["x"][:, 0]) 10 | plt.plot(lti0["t"], lti0["x"][:, 1]) 11 | plt.plot(lti0["t"], lti0["xd"][:, 0], ls='--') 12 | plt.plot(lti0["t"], lti0["xd"][:, 1], ls='--') 13 | plt.ylabel("state") 14 | plt.title("Linearized at $t^*=0$") 15 | 16 | ax3 = plt.subplot(223, sharex=ax1) 17 | plt.plot(lti0["t"], lti0["u"][:]) 18 | plt.plot(lti0["t"], lti0["ud"][:], ls='--') 19 | plt.xlabel("$t$") 20 | plt.ylabel("input") 21 | 22 | ax2 = plt.subplot(222) 23 | plt.plot(lti5["t"], lti5["x"][:, 0], label="$x_1$") 24 | plt.plot(lti5["t"], lti5["x"][:, 1], label="$x_2$") 25 | plt.plot(lti5["t"], lti5["xd"][:, 0], ls='--', label="$x^*_1$") 26 | plt.plot(lti5["t"], lti5["xd"][:, 1], ls='--', label="$x^*_2$") 27 | plt.legend() 28 | plt.title("Linearized at $t^*=5$") 29 | 30 | ax4 = plt.subplot(224, sharex=ax2) 31 | plt.plot(lti5["t"], lti5["u"][:], label="$u$") 32 | plt.plot(lti5["t"], lti5["ud"][:], ls='--', label="$u^*$") 33 | plt.xlabel("$t$") 34 | plt.legend() 35 | 36 | plt.tight_layout() 37 | 38 | plt.savefig("../doc/img/ludyk_lti.pdf") 39 | 40 | fig = plt.figure(figsize=(6, 8)) 41 | pseudoltv = pickle.load(open("ludyk_pseudoltv.p", "rb")) 42 | 43 | plt.subplot(311) 44 | plt.plot(pseudoltv["t"], pseudoltv["x"][:, 0], label="$x_1$") 45 | plt.plot(pseudoltv["t"], pseudoltv["x"][:, 1], label="$x_2$") 46 | plt.plot(pseudoltv["t"], pseudoltv["xd"][:, 0], ls='--', label="$x^*_1$") 47 | plt.plot(pseudoltv["t"], pseudoltv["xd"][:, 1], ls='--', label="$x^*_2$") 48 | plt.ylabel("state") 49 | plt.legend() 50 | 51 | plt.subplot(312) 52 | plt.plot(pseudoltv["t"], pseudoltv["u"][:], label="$u$") 53 | plt.plot(pseudoltv["t"], pseudoltv["ud"][:], ls='--', label="$u^*$") 54 | plt.ylabel("input") 55 | plt.legend() 56 | 57 | plt.subplot(313) 58 | plt.plot(pseudoltv["t"], pseudoltv["K"][:, 0, 0], label="$k_1$") 59 | plt.plot(pseudoltv["t"], pseudoltv["K"][:, 0, 1], label="$k_2$") 60 | plt.xlabel("$t$") 61 | plt.ylabel("feedback gain") 62 | plt.legend() 63 | 64 | plt.savefig("../doc/img/ludyk_pseudoltv.pdf") 65 | 66 | fig = plt.figure(figsize=(6, 8)) 67 | ltv = pickle.load(open("ludyk_ltv.p", "rb")) 68 | 69 | plt.subplot(311) 70 | plt.plot(ltv["t"], ltv["x"][:, 0], label="$x_1$") 71 | plt.plot(ltv["t"], ltv["x"][:, 1], label="$x_2$") 72 | plt.plot(ltv["t"], ltv["xd"][:, 0], ls='--', label="$x^*_1$") 73 | plt.plot(ltv["t"], ltv["xd"][:, 1], ls='--', label="$x^*_2$") 74 | plt.ylabel("state") 75 | plt.legend() 76 | 77 | plt.subplot(312) 78 | plt.plot(ltv["t"], ltv["u"][:], label="$u$") 79 | plt.plot(ltv["t"], ltv["ud"][:], ls='--', label="$u^*$") 80 | plt.ylabel("input") 81 | plt.legend() 82 | 83 | plt.subplot(313) 84 | plt.plot(ltv["t"], ltv["K"][:, 0, 0], label="$k_1$") 85 | plt.plot(ltv["t"], ltv["K"][:, 0, 1], label="$k_2$") 86 | plt.xlabel("$t$") 87 | plt.ylabel("feedback gain") 88 | plt.legend() 89 | 90 | plt.savefig("../doc/img/ludyk_ltv.pdf") 91 | 92 | fig = plt.figure(figsize=(6, 8)) 93 | pendulum_lti = pickle.load(open("pendulum_lti.p", "rb")) 94 | 95 | plt.subplot(311) 96 | plt.plot(pendulum_lti["t"], pendulum_lti["x"][:, 0], label="$s$") 97 | plt.plot(pendulum_lti["t"], pendulum_lti["x"][:, 2], label="$\\varphi$") 98 | plt.plot(pendulum_lti["t"], pendulum_lti["xd"][:, 0], ls='--', label="$s^*$") 99 | plt.plot(pendulum_lti["t"], pendulum_lti["xd"][:, 2], ls='--', label="$\\varphi^*$") 100 | plt.ylim(-2, 5) 101 | plt.ylabel("state") 102 | plt.legend() 103 | 104 | plt.subplot(312) 105 | plt.plot(pendulum_lti["t"], pendulum_lti["u"][:], label="$u$") 106 | plt.plot(pendulum_lti["t"], pendulum_lti["ud"][:], ls='--', label="$u^*$") 107 | plt.ylabel("input") 108 | plt.ylim(-15, 10) 109 | plt.legend() 110 | 111 | plt.subplot(313) 112 | plt.plot(pendulum_lti["t"], pendulum_lti["K"][:, 0, 0]) 113 | plt.plot(pendulum_lti["t"], pendulum_lti["K"][:, 0, 1]) 114 | plt.plot(pendulum_lti["t"], pendulum_lti["K"][:, 0, 2]) 115 | plt.plot(pendulum_lti["t"], pendulum_lti["K"][:, 0, 3]) 116 | plt.xlabel("$t$") 117 | plt.ylabel("feedback gain") 118 | 119 | plt.savefig("../doc/img/pendulum_lti.pdf") 120 | 121 | fig = plt.figure(figsize=(6, 8)) 122 | pendulum_pseudoltv = pickle.load(open("pendulum_pseudoltv.p", "rb")) 123 | 124 | plt.subplot(311) 125 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["x"][:, 0], label="$s$") 126 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["x"][:, 2], label="$\\varphi$") 127 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["xd"][:, 0], ls='--', label="$s^*$") 128 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["xd"][:, 2], ls='--', label="$\\varphi^*$") 129 | plt.ylim(-2, 5) 130 | plt.ylabel("state") 131 | plt.legend() 132 | 133 | plt.subplot(312) 134 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["u"][:], label="$u$") 135 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["ud"][:], ls='--', label="$u^*$") 136 | plt.ylabel("input") 137 | plt.ylim(-15, 10) 138 | plt.legend() 139 | 140 | plt.subplot(313) 141 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["K"][:, 0, 0]) 142 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["K"][:, 0, 1]) 143 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["K"][:, 0, 2]) 144 | plt.plot(pendulum_pseudoltv["t"], pendulum_pseudoltv["K"][:, 0, 3]) 145 | plt.xlabel("$t$") 146 | plt.ylabel("feedback gain") 147 | 148 | plt.savefig("../doc/img/pendulum_pseudoltv.pdf") 149 | 150 | fig = plt.figure(figsize=(6, 8)) 151 | pendulum_ltv = pickle.load(open("pendulum_ltv.p", "rb")) 152 | 153 | plt.subplot(311) 154 | plt.plot(pendulum_ltv["t"], pendulum_ltv["x"][:, 0], label="$s$") 155 | plt.plot(pendulum_ltv["t"], pendulum_ltv["x"][:, 2], label="$\\varphi$") 156 | plt.plot(pendulum_ltv["t"], pendulum_ltv["xd"][:, 0], ls='--', label="$s^*$") 157 | plt.plot(pendulum_ltv["t"], pendulum_ltv["xd"][:, 2], ls='--', label="$\\varphi^*$") 158 | plt.ylim(-2, 5) 159 | plt.ylabel("state") 160 | plt.legend() 161 | 162 | plt.subplot(312) 163 | plt.plot(pendulum_ltv["t"], pendulum_ltv["u"][:], label="$u$") 164 | plt.plot(pendulum_ltv["t"], pendulum_ltv["ud"][:], ls='--', label="$u^*$") 165 | plt.ylabel("input") 166 | plt.ylim(-15, 10) 167 | plt.legend() 168 | 169 | plt.subplot(313) 170 | plt.plot(pendulum_ltv["t"], pendulum_ltv["K"][:, 0, 0]) 171 | plt.plot(pendulum_ltv["t"], pendulum_ltv["K"][:, 0, 1]) 172 | plt.plot(pendulum_ltv["t"], pendulum_ltv["K"][:, 0, 2]) 173 | plt.plot(pendulum_ltv["t"], pendulum_ltv["K"][:, 0, 3]) 174 | plt.xlabel("$t$") 175 | plt.ylabel("feedback gain") 176 | 177 | plt.savefig("../doc/img/pendulum_ltv.pdf") 178 | 179 | plt.show() -------------------------------------------------------------------------------- /03-Jupyter-Notebook-And-SymPy/doc/tutorial3.tex: -------------------------------------------------------------------------------- 1 | \documentclass[a4paper,11pt,headings=standardclasses,parskip=half]{scrartcl} 2 | 3 | % font, style, etc. 4 | \usepackage[utf8]{inputenc} % defines 5 | \usepackage{csquotes} 6 | \usepackage{xspace} % proper space after macros with 0 args 7 | 8 | % mathematics 9 | \usepackage{amsmath} 10 | \usepackage{amssymb} 11 | 12 | % figures, tables, etc. 13 | \usepackage{hyperref} % 14 | \usepackage{graphicx} 15 | \usepackage{tikz} 16 | \usepackage{pgf} 17 | \usepackage{xcolor} 18 | \usepackage{placeins} % -> floatbarrier 19 | \usepackage{siunitx} % -> handling of units 20 | \usepackage[printwatermark]{xwatermark} 21 | \newwatermark[allpages, color=red!50,angle=45,scale=1.8,xpos=0,ypos=0]{\textsf{DRAFT ONLY, NOT APPROVED}} 22 | 23 | % code 24 | \usepackage{listings} 25 | \lstset{ 26 | language=Python, 27 | backgroundcolor = \color{light-gray}, 28 | basicstyle=\scriptsize\sffamily, 29 | stringstyle=\color{orange}, 30 | breaklines=true, 31 | numberstyle=\tiny\color{gray}, 32 | keywordstyle=\bfseries\color{dark-blue}\textit, % print keywords dark-blue 33 | commentstyle=\color{dark-green}, % print comments dark-green 34 | showstringspaces=false} % spacing between strings not showed 35 | 36 | %\newcommand{\listcode}[3]{\lstinputlisting[numbers=left,firstnumber=#1,firstline=#1,lastline=#2]{#3}} 37 | %\newcommand{\listcodeplot}[2]{\listcode{#1}{#2}{../sim/01_car_example_plotting.py}} 38 | %\newcommand{\listcodeanim}[2]{\listcode{#1}{#2}{../sim/02_car_example_animation.py}} 39 | 40 | % others 41 | \usepackage{acronym} 42 | 43 | % theorems 44 | \newtheorem{defi}{Definition}[section] 45 | 46 | % setup the appearance of links 47 | \hypersetup{ 48 | colorlinks = true, % false -> red box arround links (not very nice) 49 | linkcolor={blue!100!black}, 50 | citecolor={blue!100!black}, 51 | urlcolor={blue!100!black}, 52 | } 53 | 54 | % manage glossaries 55 | %\usepackage{glossaries} 56 | %\makeglossaries 57 | %\newacronym{ivp}{IVP}{initial value problem} 58 | 59 | % define shortcuts 60 | \newcommand{\ad}{\mathrm{ad}} 61 | \renewcommand{\d}{\mathrm{d}} % d vor differential forms 62 | \newcommand{\NV}{{\cal N}\,} 63 | \newcommand{\rang}{\mathrm{rang}} 64 | \newcommand{\im}{\mathrm{im}} 65 | \newcommand{\spann}{\mathrm{span}} 66 | \newcommand{\R}{\mathbb{R}} % set of real numbers 67 | \newcommand{\py}{\emph{Python}\xspace} 68 | \newcommand{\scipy}{\emph{SciPy}\xspace} 69 | \newcommand{\jp}{\emph{Jupyter Notebook}\xspace} 70 | \newcommand{\sympy}{\emph{SymPy}\xspace} 71 | 72 | \newcommand{\mpl}{\emph{Matplotlib}\xspace} 73 | \newcommand{\uu}{\mathbf{u}} 74 | \newcommand{\x}{\mathbf{x}} 75 | \newcommand{\y}{\mathbf{y}} 76 | \newcommand{\z}{\mathbf{z}} 77 | \newcommand{\xZero}{\mathbf{x}_0} 78 | 79 | % color definitions 80 | \definecolor{light-gray}{gray}{0.95} 81 | \definecolor{dark-blue}{rgb}{0, 0, 0.5} 82 | \definecolor{dark-red}{rgb}{0.5, 0, 0} 83 | \definecolor{dark-green}{rgb}{0, 0.5, 0} 84 | \definecolor{gray}{rgb}{0.5, 0.5, 0.5} 85 | 86 | % ---------------------------------------------------------------------------- 87 | \subject{Control Theory Tutorial}% optional 88 | \title{\jp and \sympy} 89 | \subtitle{\py for simulation, animation and control}% optional 90 | \author{} 91 | \date{} 92 | % ---------------------------------------------------------------------------- 93 | 94 | 95 | \begin{document} 96 | 97 | \maketitle% create title 98 | 99 | \tableofcontents 100 | 101 | \newpage 102 | 103 | \section{Introduction} 104 | The goal of this tutorial is to teach the usage of the programming language \py as a tool for developing and simulating control systems. This tutorial shows how to model and analyze the cart-pole system using a \jp and the \py symbolic mathematics library \sympy. The following topics are covered: 105 | \begin{itemize} 106 | \item Derivation of the equations of motion through scientific computing 107 | \item Linearization of the resulting nonlinear system equations to obtain a linear time-invariant system in state space form 108 | \item Investigation of the control theoretic properties of the system 109 | \item Design of a linear-quadratic regulator (LQR) 110 | \end{itemize} 111 | \textbf{\emph{Jupyter Notebook} file: \texttt{01\_notebook.ipynb}} 112 | 113 | Please refer to the \href{http://cs231n.github.io/python-numpy-tutorial/#python-containers}{\py List-Dictionary-Tuple tutorial} and the \href{http://cs231n.github.io/python-numpy-tutorial/#numpy}{NumPy Array tutorial} if you are not familiar with the handling of containers and arrays in \py. If you are completely new to \py consult the very basic introduction on \href{https://www.tutorialspoint.com/python/index.htm}{tutorialspoint}. 114 | 115 | For information on \jp visit \href{https://jupyter.org}{\jp}. 116 | \section{\jp} 117 | The \jp app is a browser based interactive environment to display and execute code. The created notebook documents are based on cells of text and/or code (preferebly \py). They are widely used in the scientific community and can be used to display results and make them easily reproducable. 118 | \subsection{Installation} 119 | For information on how to install \jp, please refer to \href{https://jupyter.readthedocs.io/en/latest/install.html}{jupyter installation}. 120 | 121 | It is assumed \jp and all necessary dependencies are successfully installed on your computer. 122 | \subsection{Running notebooks} 123 | To run \jp: 124 | \begin{enumerate} 125 | \item Open the terminal (Linux and macOS) or the command prompt (Windows) 126 | \item Go to the folder, where the notebook file is located (optional) 127 | \subitem \texttt{cd ../03-Jupyter-Notebook-And-SymPy/sim} 128 | \item Run \jp: 129 | \subitem \texttt{jupyter notebook} 130 | \subitem The following window should open in your default browser: 131 | \begin{figure}[htb] 132 | \centering 133 | \includegraphics[scale=0.4]{img/jupyter_screenshot} 134 | \end{figure} 135 | \item Click on \texttt{01\_notebook.ipynb} to run the notebook or click on $\texttt{New}$ to create a new one. 136 | \end{enumerate} 137 | \newpage 138 | \section{SymPy} 139 | \sympy is a \py library for symbolic mathematics. It enables the user to solve equations, integrals and other mathematical problems and aims to become a full computer algebra system. 140 | 141 | A useful \href{http://daabzlatex.s3.amazonaws.com/9065616cce623384fe5394eddfea4c52.pdf}{cheat sheet} contains basic operations on one page. Remember, when importing \sympy with \texttt{import sympy as sp}, all function calls have to be made with the prefix \texttt{sp.function\_name}. 142 | 143 | For further information on \sympy, please consult its latest documentation: 144 | 145 | \href{http://docs.sympy.org/latest/index.html}{\sympy documentation} 146 | \end{document} 147 | 148 | %%% Local Variables: 149 | %%% mode: latex 150 | %%% TeX-master: t 151 | %%% End: -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/01_lqr_pendulum.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | from numpy import cos, sin, tan, pi 4 | import scipy.linalg as scilin 5 | import matplotlib.pyplot as plt 6 | from Planner import PolynomialPlanner 7 | from typing import Literal 8 | import pickle 9 | 10 | 11 | class Parameters(object): 12 | pass 13 | 14 | 15 | # define simulation parameters 16 | sim_para = Parameters() # instance of class Parameters 17 | sim_para.t0 = 0 # start time 18 | sim_para.tf = 2.5 # final time 19 | sim_para.dt = 0.01 # step-size 20 | sim_para.x0 = [0, 0, np.pi, 0] # initial value 21 | 22 | # already prepare the time vector because we'll need it very soon 23 | n_samples = int((sim_para.tf - sim_para.t0) / sim_para.dt) + 1 24 | t_sim = sim_para.t0 + np.arange(n_samples) * sim_para.dt 25 | 26 | # -- START SYSTEM SPECIFIC PART -- 27 | # LISTING_START defsystem 28 | # define system functions 29 | sys_para = Parameters() # instance of class Parameters 30 | sys_para.n = 4 # number of states 31 | sys_para.m = 1 # number of inputs 32 | sys_para.g = 9.81 # model parameter 33 | sys_para.l = 0.5 34 | 35 | 36 | def system_rhs(t, x, u, para): 37 | x1, x2, x3, x4 = x # state vector 38 | 39 | # dxdt = f(x, u): 40 | dxdt = np.array([x2, 41 | u, 42 | x4, 43 | (1/para.l)*(para.g*sin(x3)+u*cos(x3))]) 44 | 45 | # return state derivative 46 | return dxdt 47 | 48 | 49 | def system_matrices(t, x, u, para): 50 | x1, x2, x3, x4 = x 51 | A = np.array([[0, 1, 0, 0], 52 | [0, 0, 0, 0], 53 | [0, 0, 0, 1], 54 | [0, 0, 1/para.l*(para.g*cos(x3)-u*sin(x3)), 0]]) 55 | 56 | B = np.array([[0], 57 | [1], 58 | [0], 59 | [cos(x3)/para.l]]) 60 | 61 | return A, B 62 | # LISTING_END defsystem 63 | 64 | 65 | # define controller parameters 66 | Q = np.diag([1, 1, 1, 1]) 67 | R = np.array([[1]]) 68 | 69 | # LISTING_START loadcsv 70 | pendulum_csv = np.loadtxt("pendulum.csv", delimiter=",") 71 | 72 | t_traj = pendulum_csv[:, 0].flatten() 73 | n_traj_samples = len(t_traj) 74 | xd_traj = pendulum_csv[:, 1:5] 75 | ud_traj = pendulum_csv[:, 5] 76 | # LISTING_END loadcsv 77 | # -- END SYSTEM SPECIFIC PART -- 78 | 79 | R_inv = scilin.inv(R) 80 | 81 | 82 | # solve matrix riccati ODE 83 | # LISTING_START triuconvert 84 | def triu_to_full(triu): 85 | n = int(round((np.sqrt(1+8*len(triu))-1)/2)) 86 | mask = np.triu(np.ones((n, n), dtype=bool)) 87 | 88 | full = np.empty((n, n)) 89 | full[mask] = triu 90 | full = full.T 91 | full[mask] = triu 92 | 93 | return full 94 | 95 | 96 | def full_to_triu(full): 97 | mask = np.triu(np.ones(full.shape, dtype=bool)) 98 | return full[mask] 99 | # LISTING_END triuconvert 100 | 101 | 102 | # LISTING_START riccatiinit 103 | # Get initial value S for matrix Riccati ODE by solving algebraic Riccati equation 104 | A_f, B_f = system_matrices(t_traj[-1], xd_traj[-1], ud_traj[-1], sys_para) 105 | S = scilin.solve_continuous_are(A_f, B_f, Q, R) 106 | # LISTING_END riccatiinit 107 | # LISTING_START riccatiint 108 | Pbar_triu_traj = np.empty((n_traj_samples, int(sys_para.n*(sys_para.n + 1)/2))) # allocate array for P 109 | Pbar_triu_traj[0, :] = full_to_triu(S) # initialize with Pbar(0) = S 110 | 111 | K_traj = np.empty((n_traj_samples, sys_para.m, sys_para.n)) # allocate array for K 112 | 113 | # get trajectories for P and K via numerical integration 114 | for i_tau in range(n_traj_samples): # iterate forward in tau direction 115 | i_t = n_traj_samples - 1 - i_tau # index in t vector (counting down from last element) 116 | t_i = t_traj[i_t] 117 | tau_i = t_traj[i_tau] 118 | xd_i = xd_traj[i_t] 119 | ud_i = ud_traj[i_t] 120 | A_i, B_i = system_matrices(t_i, xd_i, ud_i, sys_para) 121 | Pbar_triu_i = Pbar_triu_traj[i_tau] # indices for Pbar run forward in tau direction 122 | Pbar_i = triu_to_full(Pbar_triu_i) 123 | 124 | K_traj[i_t] = R_inv @ B_i.T @ Pbar_i 125 | 126 | if i_tau < n_traj_samples - 1: # are we at the end yet? if not, compute next Pbar via numerical integration 127 | dPbar_dtau = - Pbar_i @ B_i @ R_inv @ B_i.T @ Pbar_i + Pbar_i @ A_i + A_i.T @ Pbar_i + Q 128 | dPbar_dtau_triu = full_to_triu(dPbar_dtau) 129 | 130 | Pbar_triu_traj[i_tau + 1] = Pbar_triu_i + sim_para.dt * dPbar_dtau_triu # one Euler step 131 | # LISTING_END riccatiint 132 | # LISTING_START linsys 133 | # compute static LQR feedback 134 | t_static = 2 135 | i_static = 0 136 | while i_static < len(t_traj) - 1 and t_traj[i_static] < t_static: # find index of that time in time vector 137 | i_static += 1 138 | x_static = xd_traj[i_static] 139 | ud_static = ud_traj[i_static] 140 | A_static, B_static = system_matrices(t_static, x_static, ud_static, sys_para) 141 | # LISTING_END linsys 142 | # LISTING_START solveare 143 | P_static = scilin.solve_continuous_are(A_static, B_static, Q, R) 144 | K_static = R_inv * B_static.T @ P_static 145 | # LISTING_END solveare 146 | 147 | # LISTING_START sim 148 | # main simulation loop 149 | x_sim = np.empty((n_samples, sys_para.n)) # allocate array for state over time 150 | x_sim[0] = sim_para.x0 # set initial state 151 | u_sim = np.empty((n_samples, sys_para.m)) # allocate array for input over time 152 | xd_log = np.empty((n_samples, sys_para.n)) 153 | ud_log = np.empty((n_samples, sys_para.m)) 154 | K_log = np.empty((n_samples, sys_para.m, sys_para.n)) # allocate array for feedback over time 155 | 156 | FeedbackMode = Literal["LTV", "LTI", "pseudoLTV"] 157 | feedback_mode: FeedbackMode = "LTV" 158 | 159 | feedback_only_after_swingup = False 160 | 161 | for i in range(n_samples): 162 | t_i = t_sim[i] 163 | x_i = x_sim[i] 164 | 165 | i_traj = min(i, n_traj_samples - 1) 166 | xd_i = xd_traj[i_traj] 167 | ud_i = ud_traj[i_traj] 168 | 169 | # switch between controller types 170 | if feedback_mode == "LTV": 171 | K_i = K_traj[i_traj] # read feedback matrix from pre-computed Riccati solution 172 | elif feedback_mode == "LTI": 173 | K_i = K_static 174 | elif feedback_mode == "pseudoLTV": 175 | A_i, B_i = system_matrices(t_i, xd_i, ud_i, sys_para) 176 | P_i = scilin.solve_continuous_are(A_i, B_i, Q, R) # retune feedback for current state from reference trajectory 177 | K_i = R_inv * B_i.T @ P_i 178 | 179 | if feedback_only_after_swingup and i < n_traj_samples: 180 | u_i = ud_i 181 | else: 182 | u_i = ud_i - K_i @ (x_i - xd_i) # the actual control law u_tilde=-K*x_tilde 183 | 184 | u_sim[i] = u_i 185 | xd_log[i] = xd_i 186 | ud_log[i] = ud_i 187 | K_log[i] = K_i 188 | 189 | if i < n_samples - 1: # have we reached the end yet? if not, integrate one step 190 | dxdt_i = system_rhs(t_sim[i], x_i, u_i, sys_para) 191 | x_sim[i + 1] = x_i + sim_para.dt * dxdt_i 192 | # LISTING_END sim 193 | 194 | # storing the results 195 | store_dict = dict(t=t_sim, x=x_sim, xd=xd_log, u=u_sim, ud=ud_log, K=K_log, P_triu=Pbar_triu_traj[::-1, :]) 196 | pickle.dump(store_dict, open("log.p", "wb")) 197 | 198 | # plotting 199 | plt.figure() 200 | 201 | plt.subplot(211) 202 | plt.plot(t_sim, x_sim) 203 | plt.plot(t_sim, xd_log, '--') 204 | 205 | plt.subplot(212) 206 | plt.plot(t_sim, u_sim) 207 | plt.plot(t_sim, ud_log, '--') 208 | 209 | plt.figure() 210 | 211 | plt.plot(t_sim, K_log.reshape((n_samples, sys_para.n))) 212 | 213 | plt.show() -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/01_lqr.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | from numpy import cos, sin, tan, pi 4 | import scipy.linalg as scilin 5 | import matplotlib.pyplot as plt 6 | from Planner import PolynomialPlanner 7 | from typing import Literal 8 | import pickle 9 | 10 | 11 | class Parameters(object): 12 | pass 13 | 14 | 15 | # define simulation parameters 16 | sim_para = Parameters() # instance of class Parameters 17 | sim_para.t0 = 0 # start time 18 | sim_para.tf = 10 # final time 19 | sim_para.dt = 0.1 # step-size 20 | sim_para.x0 = [-2.1, 0.1] # initial value 21 | 22 | # already prepare the time vector because we'll need it very soon 23 | n_samples = int((sim_para.tf - sim_para.t0) / sim_para.dt) + 1 24 | t_traj = sim_para.t0 + np.arange(n_samples) * sim_para.dt 25 | 26 | # -- START SYSTEM SPECIFIC PART -- 27 | # LISTING_START defsystem 28 | # define system functions 29 | sys_para = Parameters() # instance of class Parameters 30 | sys_para.n = 2 # number of states 31 | sys_para.m = 1 # number of inputs 32 | sys_para.a = 1 # model parameter 33 | 34 | 35 | def system_rhs(t, x, u, para): 36 | x1, x2 = x # state vector 37 | 38 | # dxdt = f(x, u): 39 | dxdt = np.array([para.a * sin(x2), 40 | -x1**2 + u]) 41 | 42 | # return state derivative 43 | return dxdt 44 | 45 | 46 | def system_matrices(t, x, u, para): 47 | x1, x2 = x 48 | A = np.array([[0, para.a * cos(x2)], 49 | [-2 * x1, 0]]) 50 | 51 | B = np.array([[0], 52 | [1]]) 53 | 54 | return A, B 55 | # LISTING_END defsystem 56 | 57 | # define controller parameters 58 | Q = np.diag([1, 1]) 59 | R = np.array([[1]]) 60 | # S = np.array([[4.5, 0.1], [0.1, 1.2]]) 61 | 62 | # LISTING_START plantraj 63 | # trajectory parameters 64 | traj_para = Parameters() 65 | traj_para.y0 = -2 66 | traj_para.yf = 2 67 | traj_para.t0 = 0 68 | traj_para.tf = 10 69 | 70 | # calculate trajectory 71 | planner = PolynomialPlanner([traj_para.y0, 0, 0], [traj_para.yf, 0, 0], traj_para.t0, traj_para.tf, 2) 72 | x1d_and_derivatives = planner.eval_vec(t_traj) 73 | # LISTING_END plantraj 74 | 75 | # LISTING_START flatness 76 | x1d = x1d_and_derivatives[:, 0] 77 | x1d_dot = x1d_and_derivatives[:, 1] 78 | x1d_ddot = x1d_and_derivatives[:, 2] 79 | x2d = np.arcsin(x1d_dot / sys_para.a) 80 | x2d_dot = x1d_ddot / np.sqrt(sys_para.a ** 2 - np.square(x1d_dot)) 81 | 82 | xd_traj = np.stack((x1d, x2d), axis=1) 83 | ud_traj = x2d_dot + np.square(x1d) 84 | # LISTING_END flatness 85 | 86 | # -- END SYSTEM SPECIFIC PART -- 87 | 88 | R_inv = scilin.inv(R) 89 | 90 | 91 | # solve matrix riccati ODE 92 | # LISTING_START triuconvert 93 | def triu_to_full(triu): 94 | n = int(round((np.sqrt(1+8*len(triu))-1)/2)) 95 | mask = np.triu(np.ones((n, n), dtype=bool)) 96 | 97 | full = np.empty((n, n)) 98 | full[mask] = triu 99 | full = full.T 100 | full[mask] = triu 101 | 102 | return full 103 | 104 | 105 | def full_to_triu(full): 106 | mask = np.triu(np.ones(full.shape, dtype=bool)) 107 | return full[mask] 108 | # LISTING_END triuconvert 109 | 110 | 111 | # LISTING_START riccatiinit 112 | # Get initial value S for matrix Riccati ODE by solving algebraic Riccati equation 113 | A_f, B_f = system_matrices(t_traj[-1], xd_traj[-1], ud_traj[-1], sys_para) 114 | S = scilin.solve_continuous_are(A_f, B_f, Q, R) 115 | # LISTING_END riccatiinit 116 | # LISTING_START riccatiint 117 | Pbar_triu_traj = np.empty((n_samples, int(sys_para.n*(sys_para.n + 1)/2))) # allocate array for P 118 | Pbar_triu_traj[0, :] = full_to_triu(S) # initialize with Pbar(0) = S 119 | 120 | K_traj = np.empty((n_samples, sys_para.m, sys_para.n)) # allocate array for K 121 | 122 | # get trajectories for P and K via numerical integration 123 | for i_tau in range(n_samples): # iterate forward in tau direction 124 | i_t = n_samples - 1 - i_tau # index in t vector (counting down from last element) 125 | t_i = t_traj[i_t] 126 | tau_i = t_traj[i_tau] 127 | xd_i = xd_traj[i_t] 128 | ud_i = ud_traj[i_t] 129 | A_i, B_i = system_matrices(t_i, xd_i, ud_i, sys_para) 130 | Pbar_triu_i = Pbar_triu_traj[i_tau] # indices for Pbar run forward in tau direction 131 | Pbar_i = triu_to_full(Pbar_triu_i) 132 | 133 | K_traj[i_t] = R_inv @ B_i.T @ Pbar_i 134 | 135 | if i_tau < n_samples - 1: # are we at the end yet? if not, compute next Pbar via numerical integration 136 | dPbar_dtau = - Pbar_i @ B_i @ R_inv @ B_i.T @ Pbar_i + Pbar_i @ A_i + A_i.T @ Pbar_i + Q 137 | dPbar_dtau_triu = full_to_triu(dPbar_dtau) 138 | 139 | Pbar_triu_traj[i_tau + 1] = Pbar_triu_i + sim_para.dt * dPbar_dtau_triu # one Euler step 140 | # LISTING_END riccatiint 141 | # LISTING_START linsys 142 | # compute static LQR feedback 143 | t_static = 5 144 | i_static = 0 145 | while i_static < len(t_traj) - 1 and t_traj[i_static] < t_static: # find index of that time in time vector 146 | i_static += 1 147 | x_static = xd_traj[i_static] 148 | ud_static = ud_traj[i_static] 149 | A_static, B_static = system_matrices(t_static, x_static, ud_static, sys_para) 150 | # LISTING_END linsys 151 | # LISTING_START solveare 152 | P_static = scilin.solve_continuous_are(A_static, B_static, Q, R) 153 | K_static = R_inv * B_static.T @ P_static 154 | # LISTING_END solveare 155 | 156 | # LISTING_START sim 157 | # main simulation loop 158 | x_traj = np.empty((n_samples, sys_para.n)) # allocate array for state over time 159 | x_traj[0] = sim_para.x0 # set initial state 160 | u_traj = np.empty((n_samples, sys_para.m)) # allocate array for input over time 161 | K_log = np.empty((n_samples, sys_para.m, sys_para.n)) # allocate array for feedback over time 162 | 163 | FeedbackMode = Literal["LTV", "LTI", "pseudoLTV"] 164 | feedback_mode: FeedbackMode = "LTV" 165 | 166 | for i in range(n_samples): 167 | t_i = t_traj[i] 168 | x_i = x_traj[i] 169 | 170 | xd_i = xd_traj[i] 171 | ud_i = ud_traj[i] 172 | 173 | # switch between controller types 174 | if feedback_mode == "LTV": 175 | K_i = K_traj[i] # read feedback matrix from pre-computed Riccati solution 176 | elif feedback_mode == "LTI": 177 | K_i = K_static 178 | elif feedback_mode == "pseudoLTV": 179 | A_i, B_i = system_matrices(t_i, xd_i, ud_i, sys_para) 180 | P_i = scilin.solve_continuous_are(A_i, B_i, Q, R) # retune feedback for current state from reference trajectory 181 | K_i = R_inv * B_i.T @ P_i 182 | 183 | u_i = ud_i - K_i @ (x_i - xd_i) # the actual control law u_tilde=-K*x_tilde 184 | 185 | u_traj[i] = u_i 186 | K_log[i] = K_i 187 | 188 | if i < n_samples - 1: # have we reached the end yet? if not, integrate one step 189 | dxdt_i = system_rhs(t_traj[i], x_i, u_i, sys_para) 190 | x_traj[i + 1] = x_i + sim_para.dt * dxdt_i 191 | # LISTING_END sim 192 | 193 | # storing the results 194 | store_dict = dict(t=t_traj, x=x_traj, xd=xd_traj, u=u_traj, ud=ud_traj, K=K_log, P_triu=Pbar_triu_traj[::-1, :]) 195 | pickle.dump(store_dict, open("log.p", "wb")) 196 | 197 | # plotting 198 | plt.figure() 199 | 200 | plt.subplot(211) 201 | plt.plot(t_traj, x_traj) 202 | plt.plot(t_traj, xd_traj, '--') 203 | plt.legend(["x1", "x2", "x1d", "x2d"]) 204 | 205 | plt.subplot(212) 206 | plt.plot(t_traj, u_traj) 207 | plt.plot(t_traj, ud_traj, '--') 208 | plt.legend(["u", "ud"]) 209 | 210 | plt.figure() 211 | 212 | plt.subplot(211) 213 | plt.plot(t_traj, Pbar_triu_traj[::-1, :]) 214 | plt.legend(["p11", "p12", "p22"]) 215 | 216 | plt.subplot(212) 217 | plt.plot(t_traj, K_log.reshape((n_samples, sys_para.n))) 218 | plt.legend(["k1", "k2"]) 219 | 220 | plt.show() -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/sim/Planner.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import abc # abstract base class 3 | import math 4 | import scipy as sp 5 | from scipy import special 6 | 7 | class Planner(object): 8 | """ Base class for a trajectory planner. 9 | 10 | Attributes: 11 | YA (int, float, ndarray): start value (size = d+1) 12 | YB (int, float, ndarray): final value (size = d+1) 13 | t0 (int, float): start time 14 | tf (int, float): final time 15 | d (int): trajectory is smooth up at least to the d-th derivative 16 | """ 17 | 18 | def __init__(self, YA, YB, t0, tf, d): 19 | self.YA = YA 20 | self.YB = YB 21 | self.t0 = t0 22 | self.tf = tf 23 | self.d = d 24 | 25 | @abc.abstractmethod 26 | def eval(self): 27 | return 28 | 29 | 30 | class PolynomialPlanner(Planner): 31 | """Planner subclass that uses a polynomial approach for trajectory generation 32 | 33 | Attributes: 34 | c (ndarray): parameter vector of polynomial 35 | 36 | """ 37 | 38 | def __init__(self, YA, YB, t0, tf, d): 39 | super(PolynomialPlanner, self).__init__(YA, YB, t0, tf, d) 40 | self.c = self.coefficients() 41 | 42 | def eval(self, t): 43 | """Evaluates the planned trajectory at time t. 44 | 45 | Args: 46 | t (int, float): time 47 | 48 | Returns: 49 | Y (ndarray): y and its derivatives at t 50 | """ 51 | if t < self.t0: 52 | Y = self.YA 53 | elif t > self.tf: 54 | Y = self.YB 55 | else: 56 | Y = np.dot(self.TMatrix(t), self.c) 57 | return Y 58 | 59 | 60 | def eval_vec(self, tt): 61 | """Samples the planned trajectory 62 | 63 | Args: 64 | tt (ndarray): time vector 65 | 66 | Returns: 67 | Y (ndarray): y and its derivatives at the sample points 68 | 69 | """ 70 | Y = np.zeros([len(tt), len(self.YA)]) 71 | for i in range(0, len(tt)): 72 | Y[i] = self.eval(tt[i]) 73 | return Y 74 | 75 | 76 | def TMatrix(self, t): 77 | """Computes the T matrix at time t 78 | 79 | Args: 80 | t (int, float): time 81 | 82 | Returns: 83 | T (ndarray): T matrix 84 | 85 | """ 86 | 87 | d = self.d 88 | n = d+1 # first dimension of T 89 | m = 2*d+2 # second dimension of T 90 | 91 | T = np.zeros([n, m]) 92 | 93 | for i in range(0, m): 94 | T[0, i] = t ** i / math.factorial(i) 95 | for j in range(1, n): 96 | T[j, j:m] = T[0, 0:m-j] 97 | return T 98 | 99 | 100 | def coefficients(self): 101 | """Calculation of the polynomial parameter vector 102 | 103 | Returns: 104 | c (ndarray): parameter vector of the polynomial 105 | 106 | """ 107 | t0 = self.t0 108 | tf = self.tf 109 | 110 | Y = np.append(self.YA, self.YB) 111 | 112 | T0 = self.TMatrix(t0) 113 | Tf = self.TMatrix(tf) 114 | 115 | T = np.append(T0, Tf, axis=0) 116 | 117 | # solve the linear equation system for c 118 | c = np.linalg.solve(T, Y) 119 | return c 120 | 121 | 122 | class PrototypePlanner(Planner): 123 | """Planner subclass that uses a polynomial approach for trajectory generation""" 124 | 125 | def __init__(self, YA, YB, t0, tf, d): 126 | super(PrototypePlanner, self).__init__(YA, YB, t0, tf, d) 127 | # check if values are 0 128 | if any(self.YA[1:]!=0) or any(self.YB[1:]!=0): 129 | print('Boundary conditions of the derivatives set to 0. All given values are ignored.') 130 | 131 | def eval(self, t): 132 | """Evaluates the planned trajectory at time t. 133 | 134 | Args: 135 | t (int, float): time 136 | 137 | Returns: 138 | Y (ndarray): y and its derivatives at t 139 | """ 140 | 141 | phi = self.prototype_fct((t - self.t0) / (self.tf - self.t0)) 142 | Y = np.zeros([(self.d + 1)]) 143 | if t < self.t0: 144 | Y[0] = self.YA[0] 145 | elif t > self.tf: 146 | Y[0] = self.YB[0] 147 | else: 148 | Y[0] = self.YA[0] + (self.YB[0]-self.YA[0])*phi[0] 149 | for i in range(1,self.d+1): 150 | Y[i] = (1/(self.tf-self.t0))**i * (self.YB[0]-self.YA[0])*phi[i] 151 | return Y 152 | 153 | 154 | def eval_vec(self,tt): 155 | """Samples the planned trajectory 156 | 157 | Args: 158 | tt (ndarray): time vector 159 | 160 | Returns: 161 | Y (ndarray): y and its derivatives at the sample points 162 | 163 | """ 164 | Y = np.zeros([len(tt),len(self.YA)]) 165 | for i in range(0,len(tt)): 166 | Y[i] = self.eval(tt[i]) 167 | return Y 168 | 169 | 170 | def prototype_fct(self, t): 171 | """Prototype function, that is used in the path planner and returns a polynomial and its derivatives up to order gamma. 172 | 173 | Args: 174 | t: time 175 | Returns: phi (vector of phi and its successive derivatives) 176 | 177 | """ 178 | phi = np.zeros([self.d + 1]) 179 | 180 | summation = sum([sp.special.binom(self.d, k) * (-1) ** k * t ** (k + self.d + 1) / (self.d + k + 1) 181 | for k in range(0, self.d + 1)]) 182 | phi[0] = self.faculty(2 * self.d + 1) / (self.faculty(self.d) ** 2) * summation 183 | 184 | # calculate it's derivatives up to order (d-1) 185 | 186 | for p in range(1, self.d + 1): 187 | summation = sum( 188 | [sp.special.binom(self.d, k) * (-1) ** k * t ** (k + self.d + 1 - p) / (self.d + k + 1) * self.prod_iter(k, p) 189 | for k in range(0, self.d + 1)]) 190 | phi[p] = self.faculty(2 * self.d + 1) / (self.faculty(self.d) ** 2) * summation 191 | 192 | return phi 193 | 194 | 195 | def faculty(self, x): 196 | """Calcualtes the faculty of x""" 197 | result = 1 198 | for i in range(2, x + 1): 199 | result *= i 200 | return result 201 | 202 | 203 | def prod_iter(self, k, p): 204 | """Calculates the iterative product""" 205 | result = 1 206 | for i in range(1, p + 1): 207 | result *= (self.d + k + 2 - i) 208 | return result 209 | 210 | 211 | class GevreyPlanner(Planner): 212 | """Planner that uses a Gevrey function approach and plans trajectories that are infinitely differentiable. 213 | / 0 t < t0 214 | phi(t) = | 1/2(1 + tanh( (2T-1) / (4T(1-T))^s )) t in [t0, tf] 215 | \ 1 t > tf 216 | 217 | T = (t-t0)/(tf-t0) 218 | 219 | / yA t < t0 220 | y_d(t) = | yA + (yB - yA)*phi(t) t in [t0, tf] 221 | \ yB t > tf 222 | 223 | based on: "J. Rudolph, J. Winkler, F. Woittenek: Flatness Based Control of Distributed Parameter Systems: 224 | Examples and Computer Exercises from Various Technological Domains" Pages 88ff. 225 | """ 226 | 227 | 228 | def __init__(self, YA, YB, t0, tf, d, s): 229 | super(GevreyPlanner, self).__init__(YA, YB, t0, tf, d) 230 | self.s = s 231 | if any(self.YA[1:]!=0) or any(self.YB[1:]!=0): 232 | print('Boundary conditions of the derivatives set to 0. All given values are ignored.') 233 | 234 | def eval(self, t): 235 | """Evaluates the planned trajectory at time t. 236 | 237 | Args: 238 | t (int, float): time 239 | 240 | Returns: 241 | Y (ndarray): y and its derivatives at t 242 | """ 243 | Y = np.zeros([(self.d + 1)]) 244 | if t < self.t0: 245 | Y[0] = self.YA[0] 246 | elif t > self.tf: 247 | Y[0] = self.YB[0] 248 | else: 249 | T = min(max((t-self.t0)/(self.tf-self.t0),0.001),0.999) 250 | phi = self.phi(T) 251 | Y = np.zeros_like(phi) 252 | Y[0] = self.YA[0] + (self.YB[0] - self.YA[0]) * phi[0] 253 | for i in range(1, self.d + 1): 254 | Y[i] = (1 / (self.tf - self.t0)) ** i * (self.YB[0] - self.YA[0]) * phi[i] 255 | return Y 256 | 257 | 258 | def eval_vec(self, tt): 259 | """Samples the planned trajectory 260 | 261 | Args: 262 | tt (ndarray): time vector 263 | 264 | Returns: 265 | Y (ndarray): y and its derivatives at the sample points 266 | 267 | """ 268 | Y = np.zeros([len(tt), len(self.YA)]) 269 | for i in range(0, len(tt)): 270 | Y[i] = self.eval(tt[i]) 271 | return Y 272 | 273 | def phi(self, t): 274 | """Calculates phi = 1/2*(1 + tanh( 2(2t-1) / (4t(1-t))^s )) ) and it's derivatives up to order d""" 275 | phi = np.zeros([self.d + 1]) 276 | phi[0] = 1/2*(1 + self.y(t, 0)) 277 | for i in range(1, self.d + 1): 278 | phi[i] = 1/2*self.y(t, i) 279 | return phi 280 | 281 | 282 | def y(self, t, n): 283 | """Calculates y = tanh( 2(2t-1) / (4t(1-t))^s )) and it's derivatives up to order n""" 284 | s = self.s 285 | if n == 0: 286 | # eq. A.3 287 | y = np.tanh(2*(2*t - 1) / ((4*t*(1 - t))**s)) 288 | elif n == 1: 289 | # eq. A.5 290 | y = self.a(t, 2)*(1 - self.y(t, 0)**2) 291 | else: 292 | # eq. A.7 293 | y = sum(sp.special.binom(n - 1, k)*self.a(t, k + 2)*self.z(t, n - 1 - k) for k in range(0, n)) 294 | return y 295 | 296 | 297 | def a(self, t, n): 298 | s = self.s 299 | if n == 0: 300 | # eq. A.4 301 | a = ((4*t*(1 - t))**(1 - s))/(2*(s - 1)) 302 | elif n == 1: 303 | # eq. for da/dt 304 | a = 2*(2*t - 1) / ((4*t*(1 - t))**s) 305 | else: 306 | # eq. for the n-th derivative of a 307 | a = 1/(t*(1 - t))*((s - 2+n)*(2*t - 1)*self.a(t, n - 1) + (n - 1)*(2*s - 4 + n)*self.a(t, n - 2)) 308 | return a 309 | 310 | 311 | def z(self, t, n): 312 | if n == 0: 313 | # eq. A.6 314 | z = (1-self.y(t, 0)**2) 315 | else: 316 | # eq. for n-th derivative of z 317 | z = - sum(sp.special.binom(n, k)*self.y(t, k)*self.y(t, n - k) for k in range(0, n + 1)) 318 | return z 319 | -------------------------------------------------------------------------------- /01-System-Simulation-ODE/sim/02_car_example_animation.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | from numpy import cos, sin, tan 4 | import scipy.integrate as sci 5 | import matplotlib.pyplot as plt 6 | import matplotlib.animation as mpla 7 | from dataclasses import dataclass 8 | from typing import Type 9 | 10 | # Set up of ffmpeg for creating the animation 11 | # Linux: 12 | # Make sure ffmpeg is installed (e.g.: sudo apt install ffmpeg) 13 | # 14 | # Windows: 15 | # 1. Download & install ffmepg from http://www.ffmpeg.org/download.html 16 | # 2. Uncomment the following line and set path appropriately 17 | #plt.rcParams['animation.ffmpeg_path'] = 'C:\\Progs\\ffmpeg\\bin\\ffmpeg.exe' 18 | 19 | 20 | # Physical parameter 21 | @dataclass 22 | class Para: 23 | l: float = 0.3 # define car length 24 | w: float = l*0.3 # define car width 25 | 26 | 27 | # Simulation parameter 28 | @dataclass 29 | class SimPara: 30 | t0: float = 0 # start time 31 | tf: float = 10 # final time 32 | dt: float = 0.04 # step-size 33 | 34 | 35 | def ode(t, x, p: Type[Para]): 36 | """Function of the robots kinematics 37 | 38 | Args: 39 | x: state 40 | t: time 41 | p(object): parameter container class 42 | 43 | Returns: 44 | dxdt: state derivative 45 | """ 46 | x1, x2, x3 = x # state vector 47 | u1, u2 = control(t) # control vector 48 | 49 | # dxdt = f(x, u): 50 | dxdt = np.array([u1 * cos(x3), 51 | u1 * sin(x3), 52 | 1 / p.l * u1 * tan(u2)]) 53 | 54 | # return state derivative 55 | return dxdt 56 | 57 | 58 | def control(t): 59 | """Function of the control law 60 | 61 | Args: 62 | t: time 63 | 64 | Returns: 65 | u: control vector 66 | 67 | """ 68 | u1 = np.maximum(0, 1.0 - 0.1*t) 69 | u2 = np.full(u1.shape, 0.25) 70 | return np.array([u1, u2]).T 71 | 72 | 73 | def plot_data(x, u, t, fig_width, fig_height, save=False): 74 | """Plotting function of simulated state and actions 75 | 76 | Args: 77 | x(ndarray): state-vector trajectory 78 | u(ndarray): control vector trajectory 79 | t(ndarray): time vector 80 | fig_width: figure width in cm 81 | fig_height: figure height in cm 82 | save (bool) : save figure (default: False) 83 | Returns: None 84 | 85 | """ 86 | # creating a figure with 3 subplots, that share the x-axis 87 | fig1, (ax1, ax2, ax3) = plt.subplots(3) 88 | 89 | # set figure size to desired values 90 | fig1.set_size_inches(fig_width / 2.54, fig_height / 2.54) 91 | 92 | # plot y_1 in subplot 1 93 | ax1.plot(t, x[:, 0], label='$y_1(t)$', lw=1, color='r') 94 | 95 | # plot y_2 in subplot 1 96 | ax1.plot(t, x[:, 1], label='$y_2(t)$', lw=1, color='b') 97 | 98 | # plot theta in subplot 2 99 | ax2.plot(t, np.rad2deg(x[:, 2]), label=r'$\theta(t)$', lw=1, color='g') 100 | 101 | # plot control in subplot 3, left axis red, right blue 102 | ax3.plot(t, np.rad2deg(u[:, 0]), label=r'$v(t)$', lw=1, color='r') 103 | ax3.tick_params(axis='y', colors='r') 104 | ax33 = ax3.twinx() 105 | ax33.plot(t, np.rad2deg(u[:, 1]), label=r'$\phi(t)$', lw=1, color='b') 106 | ax33.spines["left"].set_color('r') 107 | ax33.spines["right"].set_color('b') 108 | ax33.tick_params(axis='y', colors='b') 109 | 110 | # Grids 111 | ax1.grid(True) 112 | ax2.grid(True) 113 | ax3.grid(True) 114 | 115 | # set the labels on the x and y axis and the titles 116 | ax1.set_title('Position coordinates') 117 | ax1.set_ylabel(r'm') 118 | ax1.set_xlabel(r't in s') 119 | ax2.set_title('Orientation') 120 | ax2.set_ylabel(r'deg') 121 | ax2.set_xlabel(r't in s') 122 | ax3.set_title('Velocity / steering angle') 123 | ax3.set_ylabel(r'm/s') 124 | ax33.set_ylabel(r'deg') 125 | ax3.set_xlabel(r't in s') 126 | 127 | # put a legend in the plot 128 | ax1.legend() 129 | ax2.legend() 130 | ax3.legend() 131 | li3, lab3 = ax3.get_legend_handles_labels() 132 | li33, lab33 = ax33.get_legend_handles_labels() 133 | ax3.legend(li3 + li33, lab3 + lab33, loc=0) 134 | 135 | # automatically adjusts subplot to fit in figure window 136 | plt.tight_layout() 137 | 138 | # save the figure in the working directory 139 | if save: 140 | plt.savefig('state_trajectory.pdf') # save output as pdf 141 | #plt.savefig('state_trajectory.pgf') # for easy export to LaTeX, needs a lot of extra packages 142 | return None 143 | 144 | 145 | # LISTING_START CarAnimFunDef 146 | def car_animation(x, t, p: Type[Para]): 147 | """Animation function of the car-like mobile robot 148 | 149 | Args: 150 | x(ndarray): state-vector trajectory 151 | t(ndarray): time vector 152 | p(object): parameters 153 | 154 | Returns: None 155 | 156 | """ 157 | # LISTING_END CarAnimFunDef 158 | 159 | # LISTING_START CarPlotFunDef 160 | def draw_the_car(cur_x, cur_y): 161 | """Mapping from state x and action cur_y to the position of the car elements 162 | 163 | Args: 164 | cur_x: The current state vector 165 | cur_y: The current action vector 166 | 167 | Returns: 168 | 169 | """ 170 | wheel_length = 0.1 * p.l 171 | y1, y2, theta = cur_x 172 | v, phi = cur_y 173 | 174 | # define chassis lines 175 | chassis_y1 = [y1, y1 + p.l * cos(theta)] 176 | chassis_y2 = [y2, y2 + p.l * sin(theta)] 177 | 178 | # define lines for the front and rear axle 179 | rear_ax_y1 = [y1 + p.w * sin(theta), y1 - p.w * sin(theta)] 180 | rear_ax_y2 = [y2 - p.w * cos(theta), y2 + p.w * cos(theta)] 181 | front_ax_y1 = [chassis_y1[1] + p.w * sin(theta + phi), 182 | chassis_y1[1] - p.w * sin(theta + phi)] 183 | front_ax_y2 = [chassis_y2[1] - p.w * cos(theta + phi), 184 | chassis_y2[1] + p.w * cos(theta + phi)] 185 | 186 | # define wheel lines 187 | rear_l_wl_y1 = [rear_ax_y1[1] + wheel_length * cos(theta), 188 | rear_ax_y1[1] - wheel_length * cos(theta)] 189 | rear_l_wl_y2 = [rear_ax_y2[1] + wheel_length * sin(theta), 190 | rear_ax_y2[1] - wheel_length * sin(theta)] 191 | rear_r_wl_y1 = [rear_ax_y1[0] + wheel_length * cos(theta), 192 | rear_ax_y1[0] - wheel_length * cos(theta)] 193 | rear_r_wl_y2 = [rear_ax_y2[0] + wheel_length * sin(theta), 194 | rear_ax_y2[0] - wheel_length * sin(theta)] 195 | front_l_wl_y1 = [front_ax_y1[1] + wheel_length * cos(theta + phi), 196 | front_ax_y1[1] - wheel_length * cos(theta + phi)] 197 | front_l_wl_y2 = [front_ax_y2[1] + wheel_length * sin(theta + phi), 198 | front_ax_y2[1] - wheel_length * sin(theta + phi)] 199 | front_r_wl_y1 = [front_ax_y1[0] + wheel_length * cos(theta + phi), 200 | front_ax_y1[0] - wheel_length * cos(theta + phi)] 201 | front_r_wl_y2 = [front_ax_y2[0] + wheel_length * sin(theta + phi), 202 | front_ax_y2[0] - wheel_length * sin(theta + phi)] 203 | 204 | # empty value (to disconnect points, define where no line should be plotted) 205 | empty = [np.nan, np.nan] 206 | 207 | # concatenate set of coordinates 208 | data_y1 = [rear_ax_y1, empty, front_ax_y1, empty, chassis_y1, 209 | empty, rear_l_wl_y1, empty, rear_r_wl_y1, 210 | empty, front_l_wl_y1, empty, front_r_wl_y1] 211 | data_y2 = [rear_ax_y2, empty, front_ax_y2, empty, chassis_y2, 212 | empty, rear_l_wl_y2, empty, rear_r_wl_y2, 213 | empty, front_l_wl_y2, empty, front_r_wl_y2] 214 | 215 | # set data 216 | h_car.set_data(data_y1, data_y2) 217 | # LISTING_END CarPlotFunDef 218 | 219 | # LISTING_START InitFunDef 220 | def init(): 221 | """Initialize plot objects that change during animation. 222 | Only required for blitting to give a clean slate. 223 | 224 | Returns: 225 | 226 | """ 227 | h_x_traj_plot.set_data([], []) 228 | h_car.set_data([], []) 229 | return h_x_traj_plot, h_car 230 | # LISTING_END InitFunDef 231 | 232 | # LISTING_START AnimateFunDef 233 | def animate(i): 234 | """Defines what should be animated 235 | 236 | Args: 237 | i: frame number 238 | 239 | Returns: 240 | 241 | """ 242 | k = i % len(t) 243 | ax.set_title('Time (s): ' + '%.2f' % t[k], loc='left') 244 | h_x_traj_plot.set_xdata(x[0:k, 0]) 245 | h_x_traj_plot.set_ydata(x[0:k, 1]) 246 | draw_the_car(x[k, :], control(t[k])) 247 | return h_x_traj_plot, h_car 248 | # LISTING_END AnimateFunDef 249 | 250 | # LISTING_START CarAnimFunInit 251 | # Setup two empty axes with enough space around the trajectory so the car 252 | # can always be completely plotted. One plot holds the sketch of the car, 253 | # the other the curve 254 | dx = 1.5 * p.l 255 | dy = 1.5 * p.l 256 | fig2, ax = plt.subplots() 257 | ax.set_xlim([min(min(x_traj[:, 0] - dx), -dx), 258 | max(max(x_traj[:, 0] + dx), dx)]) 259 | ax.set_ylim([min(min(x_traj[:, 1] - dy), -dy), 260 | max(max(x_traj[:, 1] + dy), dy)]) 261 | ax.set_aspect('equal') 262 | ax.set_xlabel(r'$y_1$') 263 | ax.set_ylabel(r'$y_2$') 264 | 265 | # Axis handles 266 | h_x_traj_plot, = ax.plot([], [], 'b') # state trajectory in the y1-y2-plane 267 | h_car, = ax.plot([], [], 'k', lw=2) # car 268 | # LISTING_END CarAnimFunInit 269 | 270 | # LISTING_START DoAnimate 271 | ani = mpla.FuncAnimation(fig2, animate, init_func=init, frames=len(t) + 1, 272 | interval=(t[1] - t[0]) * 1000, 273 | blit=False) 274 | 275 | ani.save('animation.mp4', writer='ffmpeg', fps=1 / (t[1] - t[0])) 276 | # LISTING_END DoAnimate 277 | 278 | plt.show() 279 | return None 280 | 281 | 282 | # LISTING_START DoSimulate 283 | # time vector 284 | tt = np.arange(SimPara.t0, SimPara.tf + SimPara.dt, SimPara.dt) 285 | 286 | # initial state 287 | x0 = [0, 0, 0] 288 | 289 | # simulation 290 | sol = sci.solve_ivp(lambda t, x: ode(t, x, Para), (SimPara.t0, SimPara.tf), x0, t_eval=tt) 291 | x_traj = sol.y.T 292 | u_traj = control(tt) 293 | 294 | # plot 295 | plot_data(x_traj, u_traj, tt, 12, 16, save=True) 296 | 297 | # animation 298 | car_animation(x_traj, tt, Para) 299 | 300 | plt.show() 301 | # LISTING_END DoSimulate 302 | -------------------------------------------------------------------------------- /02-Trajectories-And-Control/sim/TrajGen.py: -------------------------------------------------------------------------------- 1 | # LISTING_START TrajGen 2 | import numpy as np 3 | import scipy.special as sps 4 | import abc # abstract base class 5 | 6 | 7 | class TrajGen(abc.ABC): 8 | """ Base class for a trajectory generator. 9 | 10 | Attributes: 11 | y_a (int, float, ndarray): start value (size = d+1) 12 | y_b (int, float, ndarray): final value (size = d+1) 13 | t_0 (int, float): start time 14 | t_f (int, float): final time 15 | d (int): trajectory is smooth up at least to the d-th derivative 16 | """ 17 | 18 | def __init__(self, y_a, y_b, t_0, t_f, d): 19 | self.YA = y_a 20 | self.YB = y_b 21 | self.t0 = t_0 22 | self.tf = t_f 23 | self.d = d 24 | 25 | @abc.abstractmethod 26 | def eval(self, t): 27 | pass 28 | 29 | @abc.abstractmethod 30 | def eval_vec(self, t): 31 | pass 32 | # LISTING_END TrajGen 33 | 34 | # LISTING_START FactorialFunc 35 | @classmethod 36 | def factorial(cls, x): 37 | """Calcualtes the faculty of x""" 38 | result = 1 39 | for i in range(2, x + 1): 40 | result *= i 41 | return result 42 | # LISTING_END FactorialFunc 43 | 44 | 45 | # LISTING_START PolynomialTrajGen 46 | class PolynomialTrajGen(TrajGen): 47 | """TrajGen subclass that uses a polynomial approach for trajectory generation 48 | 49 | Attributes: 50 | c (ndarray): parameter vector of polynomial 51 | 52 | """ 53 | def __init__(self, y_a, y_b, t_0, t_f, d): 54 | super().__init__(y_a, y_b, t_0, t_f, d) 55 | self.c = self.coefficients() 56 | # LISTING_END PolynomialTrajGen 57 | 58 | # LISTING_START PolynomialTrajGenEvalFunDef 59 | def eval(self, t): 60 | """Evaluates the planned trajectory at time t. 61 | 62 | Args: 63 | t (int, float): time 64 | 65 | Returns: 66 | y (ndarray): y and its derivatives at t 67 | """ 68 | if t < self.t0: 69 | y = self.YA 70 | elif t > self.tf: 71 | y = self.YB 72 | else: 73 | y = np.dot(self.t_matrix(t), self.c) 74 | return y 75 | # LISTING_END PolynomialTrajGenEvalFunDef 76 | 77 | # LISTING_START PolynomialTrajGenEvalVecFunDef 78 | def eval_vec(self, tt): 79 | """Samples the planned trajectory 80 | 81 | Args: 82 | tt (ndarray): time vector 83 | 84 | Returns: 85 | y (ndarray): y and its derivatives at the sample points 86 | 87 | """ 88 | y = np.zeros([len(tt), len(self.YA)]) 89 | for i in range(0, len(tt)): 90 | y[i] = self.eval(tt[i]) 91 | return y 92 | # LISTING_END PolynomialTrajGenEvalVecFunDef 93 | 94 | # LISTING_START TMatrixFunDef 95 | def t_matrix(self, t): 96 | """Computes the T matrix at time t 97 | 98 | Args: 99 | t (int, float): time 100 | 101 | Returns: 102 | t_mat (ndarray): T matrix 103 | 104 | """ 105 | 106 | d = self.d 107 | n = d+1 # first dimension of T 108 | m = 2*d+2 # second dimension of T 109 | 110 | t_mat = np.zeros([n, m]) 111 | 112 | for i in range(0, m): 113 | t_mat[0, i] = t ** i / self.factorial(i) 114 | for j in range(1, n): 115 | t_mat[j, j:m] = t_mat[0, 0:m-j] 116 | return t_mat 117 | # LISTING_END TMatrixFunDef 118 | 119 | # LISTING_START CoeffFunDef 120 | def coefficients(self): 121 | """Calculation of the polynomial parameter vector 122 | 123 | Returns: 124 | c (ndarray): parameter vector of the polynomial 125 | 126 | """ 127 | t0 = self.t0 128 | tf = self.tf 129 | 130 | y = np.append(self.YA, self.YB) 131 | 132 | t0_mat = self.t_matrix(t0) 133 | tf_mat = self.t_matrix(tf) 134 | 135 | t_mat = np.append(t0_mat, tf_mat, axis=0) 136 | 137 | # solve the linear equation system for c 138 | c = np.linalg.solve(t_mat, y) 139 | 140 | return c 141 | # LISTING_END CoeffFunDef 142 | 143 | 144 | class PrototypeTrajGen(TrajGen): 145 | """TrajGen subclass that uses a polynomial approach for trajectory generation""" 146 | 147 | def __init__(self, y_a, y_b, t_0, t_f, d): 148 | super().__init__(y_a, y_b, t_0, t_f, d) 149 | # check if values are 0 150 | if any(self.YA[1:] != 0) or any(self.YB[1:] != 0): 151 | print('Boundary conditions of the derivatives set to 0. All given values are ignored.') 152 | 153 | def eval(self, t): 154 | """Evaluates the planned trajectory at time t. 155 | 156 | Args: 157 | t (int, float): time 158 | 159 | Returns: 160 | y (ndarray): y and its derivatives at t 161 | """ 162 | 163 | phi = self.prototype_fct((t - self.t0) / (self.tf - self.t0)) 164 | y = np.zeros([(self.d + 1)]) 165 | if t < self.t0: 166 | y[0] = self.YA[0] 167 | elif t > self.tf: 168 | y[0] = self.YB[0] 169 | else: 170 | y[0] = self.YA[0] + (self.YB[0]-self.YA[0])*phi[0] 171 | for i in range(1, self.d+1): 172 | y[i] = (1/(self.tf-self.t0))**i * (self.YB[0]-self.YA[0])*phi[i] 173 | return y 174 | 175 | def eval_vec(self, tt): 176 | """Samples the planned trajectory 177 | 178 | Args: 179 | tt (ndarray): time vector 180 | 181 | Returns: 182 | y (ndarray): y and its derivatives at the sample points 183 | 184 | """ 185 | y = np.zeros([len(tt), len(self.YA)]) 186 | for i in range(0, len(tt)): 187 | y[i] = self.eval(tt[i]) 188 | return y 189 | 190 | def prototype_fct(self, t): 191 | """ Implementation of the prototype function 192 | 193 | Args: 194 | t: time 195 | Returns: phi (vector of phi and its successive derivatives) 196 | 197 | """ 198 | phi = np.zeros([self.d + 1]) 199 | 200 | summation = sum([sps.binom(self.d, k) * (-1) ** k * t ** (k + self.d + 1) / (self.d + k + 1) 201 | for k in range(0, self.d + 1)]) 202 | phi[0] = self.factorial(2 * self.d + 1) / (self.factorial(self.d) ** 2) * summation 203 | 204 | # calculate it's derivatives up to order (d-1) 205 | 206 | for p in range(1, self.d + 1): 207 | summation = sum( 208 | [sps.binom(self.d, k) * (-1) ** k * t ** (k + self.d + 1 - p) / (self.d + k + 1) * self.prod_iter(k, p) 209 | for k in range(0, self.d + 1)]) 210 | phi[p] = self.factorial(2 * self.d + 1) / (self.factorial(self.d) ** 2) * summation 211 | 212 | return phi 213 | 214 | def prod_iter(self, k, p): 215 | """Calculates the iterative product""" 216 | result = 1 217 | for i in range(1, p + 1): 218 | result *= (self.d + k + 2 - i) 219 | return result 220 | 221 | 222 | class GevreyTrajGen(TrajGen): 223 | """TrajGen that uses a Gevrey function approach and plans trajectories that are infinitely differentiable. 224 | | 0 t < t0 225 | phi(t) = | 1/2(1 + tanh( (2T-1) / (4T(1-T))^s )) t in [t0, tf] 226 | | 1 t > tf 227 | 228 | T = (t-t0)/(tf-t0) 229 | 230 | | yA t < t0 231 | y_d(t) = | yA + (yB - yA)*phi(t) t in [t0, tf] 232 | | yB t > tf 233 | 234 | based on: "J. Rudolph, J. Winkler, F. Woittenek: Flatness Based Control of Distributed Parameter Systems: 235 | Examples and Computer Exercises from Various Technological Domains" Pages 88ff. 236 | """ 237 | 238 | def __init__(self, y_a, y_b, t_0, t_f, d, s): 239 | super().__init__(y_a, y_b, t_0, t_f, d) 240 | self.s = s 241 | if any(self.YA[1:] != 0) or any(self.YB[1:] != 0): 242 | print('Boundary conditions of the derivatives set to 0. All given values are ignored.') 243 | 244 | def eval(self, t): 245 | """Evaluates the planned trajectory at time t. 246 | 247 | Args: 248 | t (int, float): time 249 | 250 | Returns: 251 | y (ndarray): y and its derivatives at t 252 | """ 253 | y = np.zeros([(self.d + 1)]) 254 | if t < self.t0: 255 | y[0] = self.YA[0] 256 | elif t > self.tf: 257 | y[0] = self.YB[0] 258 | else: 259 | t_val = min(max((t - self.t0) / (self.tf - self.t0), 0.001), 0.999) 260 | phi = self.phi(t_val) 261 | y = np.zeros_like(phi) 262 | y[0] = self.YA[0] + (self.YB[0] - self.YA[0]) * phi[0] 263 | for i in range(1, self.d + 1): 264 | y[i] = (1 / (self.tf - self.t0)) ** i * (self.YB[0] - self.YA[0]) * phi[i] 265 | return y 266 | 267 | def eval_vec(self, tt): 268 | """Samples the planned trajectory 269 | 270 | Args: 271 | tt (ndarray): time vector 272 | 273 | Returns: 274 | y (ndarray): y and its derivatives at the sample points 275 | 276 | """ 277 | y = np.zeros([len(tt), len(self.YA)]) 278 | for i in range(0, len(tt)): 279 | y[i] = self.eval(tt[i]) 280 | return y 281 | 282 | def phi(self, t): 283 | """Calculates phi = 1/2*(1 + tanh( 2(2t-1) / (4t(1-t))^s )) ) + its deriv. up to order d""" 284 | phi = np.zeros([self.d + 1]) 285 | phi[0] = 1/2*(1 + self.y(t, 0)) 286 | for i in range(1, self.d + 1): 287 | phi[i] = 1/2*self.y(t, i) 288 | return phi 289 | 290 | def y(self, t, n): 291 | """Calculates y = tanh( 2(2t-1) / (4t(1-t))^s )) and its derivatives up to order n""" 292 | s = self.s 293 | if n == 0: 294 | # eq. A.3 295 | y = np.tanh(2*(2*t - 1) / ((4*t*(1 - t))**s)) 296 | elif n == 1: 297 | # eq. A.5 298 | y = self.a(t, 2)*(1 - self.y(t, 0)**2) 299 | else: 300 | # eq. A.7 301 | y = sum(sps.binom(n - 1, k)*self.a(t, k + 2) * 302 | self.z(t, n - 1 - k) for k in range(0, n)) 303 | return y 304 | 305 | def a(self, t, n): 306 | s = self.s 307 | if n == 0: 308 | # eq. A.4 309 | a = ((4*t*(1 - t))**(1 - s))/(2*(s - 1)) 310 | elif n == 1: 311 | # eq. for da/dt 312 | a = 2*(2*t - 1) / ((4*t*(1 - t))**s) 313 | else: 314 | # eq. for the n-th derivative of a 315 | a = 1/(t*(1 - t))*((s - 2+n)*(2*t - 1)*self.a(t, n - 1) + (n - 1) * 316 | (2*s - 4 + n)*self.a(t, n - 2)) 317 | return a 318 | 319 | def z(self, t, n): 320 | if n == 0: 321 | # eq. A.6 322 | z = (1-self.y(t, 0)**2) 323 | else: 324 | # eq. for n-th derivative of z 325 | z = - sum(sps.binom(n, k)*self.y(t, k)*self.y(t, n - k) for k in range(0, n + 1)) 326 | return z 327 | -------------------------------------------------------------------------------- /02-Trajectories-And-Control/sim/02_car_feedforward_control.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | from numpy import cos, sin, tan, arctan, arctan2 4 | import scipy.integrate as sci 5 | import matplotlib.pyplot as plt 6 | import matplotlib.animation as mpla 7 | from TrajGen import PolynomialTrajGen 8 | from dataclasses import dataclass 9 | from typing import Type, List 10 | 11 | # Set up of ffmpeg for creating the animation 12 | # Linux: 13 | # Make sure ffmpeg is installed (e.g.: sudo apt install ffmpeg) 14 | # 15 | # Windows: 16 | # 1. Download & install ffmepg from http://www.ffmpeg.org/download.html 17 | # 2. Uncomment the following line and set path appropriately 18 | #plt.rcParams['animation.ffmpeg_path'] = 'C:\\Progs\\ffmpeg\\bin\\ffmpeg.exe' 19 | 20 | 21 | # Physical parameters 22 | @dataclass 23 | class Para: 24 | l: float = 0.3 # define car length 25 | w: float = l * 0.3 # define car width 26 | 27 | 28 | # Simulation parameters 29 | # LISTING_START DefineSimPara 30 | @dataclass 31 | class SimPara: 32 | t0: float = 0 # start time 33 | tf: float = 10 # final time 34 | dt: float = 0.04 # step-size 35 | tt = np.arange(0, tf + dt, dt) # time vector 36 | x0 = [0, 0, 0] # inital state at t0 37 | xf = [5, 5, 0] # final state at tf 38 | # LISTING_END DefineSimPara 39 | 40 | 41 | # LISTING_START DefineTrajGens 42 | def setup_trajectories(sp: Type[SimPara]) -> List[PolynomialTrajGen]: 43 | """Setup the trajectory objects 44 | 45 | Args: 46 | sp: Object of the SimPara 47 | 48 | Returns: 49 | A list holding two objects of type PolynomialTrajGen. The first 50 | one is f, the second g. 51 | 52 | """ 53 | 54 | # Start and final time of transistion 55 | t0: float = sp.t0 + 1 56 | tf: float = sp.tf - 1 57 | 58 | # boundary conditions for y1 59 | y1_a = np.array([sp.x0[0], 0]) 60 | y1_b = np.array([sp.xf[0], 0]) 61 | 62 | # boundary conditions for y2 63 | y2_a = np.array([sp.x0[1], tan(sp.x0[2]), 0]) 64 | y2_b = np.array([sp.xf[1], tan(sp.xf[2]), 0]) 65 | 66 | # From Y2A to Y2B on the interval [Y1A[0], Y1B[0]] 67 | f_traj = PolynomialTrajGen(y2_a, y2_b, y1_a[0], y1_b[0], 2) 68 | 69 | # from Y1A to Y1B on the interval [t0, tf] 70 | g_traj = PolynomialTrajGen(y1_a, y1_b, t0, tf, 1) 71 | 72 | return [f_traj, g_traj] 73 | # LISTING_END DefineTrajGens 74 | 75 | 76 | def ode(x, t, p: Type[Para]): 77 | """Function of the robots kinematics 78 | 79 | Args: 80 | x: state 81 | t: time 82 | p(object): parameter container class 83 | 84 | Returns: 85 | dxdt: state derivative 86 | """ 87 | x1, x2, x3 = x # state vector 88 | u1, u2 = control(t, p) # control vector 89 | 90 | # dxdt = f(x, u): 91 | dxdt = np.array([u1 * cos(x3), 92 | u1 * sin(x3), 93 | 1 / p.l * u1 * tan(u2)]) 94 | 95 | # return state derivative 96 | return dxdt 97 | 98 | 99 | # LISTING_START ControlLaw 100 | def control(t, p: Type[Para]): 101 | """Function of the control law 102 | 103 | Args: 104 | t (int): time 105 | p (object): parameter container class 106 | 107 | Returns: 108 | u (ndarry): control vector 109 | 110 | """ 111 | 112 | # evaluate the planned trajectories at time t 113 | g_t = g_traj_gen.eval(t) # y1 = g(t) 114 | f_y1 = f_traj_gen.eval(g_t[0]) # y2 = f(y1) = f(g(t)) 115 | 116 | # setting control laws 117 | u1 = g_t[1]*np.sqrt(1 + f_y1[1]**2) 118 | u2 = arctan2(p.l*f_y1[2], (1 + f_y1[1]**2)**(3/2)) 119 | 120 | return np.array([u1, u2]).T 121 | # LISTING_END ControlLaw 122 | 123 | 124 | def plot_data(x, xref, u, t, fig_width, fig_height, save=False): 125 | """Plotting function of simulated state and actions 126 | 127 | Args: 128 | x(ndarray): state-vector trajectory 129 | xref(ndarray): reference state-vector trajectory 130 | u(ndarray): control vector trajectory 131 | t(ndarray): time vector 132 | fig_width: figure width in cm 133 | fig_height: figure height in cm 134 | save (bool) : save figure (default: False) 135 | Returns: None 136 | 137 | """ 138 | # creating a figure with 3 subplots, that share the x-axis 139 | fig1, (ax1, ax2, ax3) = plt.subplots(3, sharex=True) 140 | 141 | # set figure size to desired values 142 | fig1.set_size_inches(fig_width / 2.54, fig_height / 2.54) 143 | 144 | # plot y_1 in subplot 1 145 | ax1.plot(t, x[:, 0], label='$y_1(t)$', lw=1, color='r') 146 | ax1.plot(t, xref[:, 0], label='$y_{1,d}(t)$', lw=1, color=(0.5, 0, 0)) 147 | 148 | # plot y_2 in subplot 1 149 | ax1.plot(t, x[:, 1], label='$y_2(t)$', lw=1, color='b') 150 | ax1.plot(t, xref[:, 1], label='$y_{2,d}(t)$', lw=1, color=(0, 0, 0.5)) 151 | 152 | # plot theta in subplot 2 153 | ax2.plot(t, np.rad2deg(x[:, 2]), label=r'$\theta(t)$', lw=1, color=(0, 0.7, 0)) 154 | ax2.plot(t, np.rad2deg(xref[:, 2]), label=r'$\theta_d(t)$', lw=1, color='g') 155 | 156 | # plot control in subplot 3, left axis red, right blue 157 | ax3.plot(t, np.rad2deg(u[:, 0]), label=r'$v(t)$', lw=1, color='r') 158 | ax3.tick_params(axis='y', colors='r') 159 | ax33 = ax3.twinx() 160 | ax33.plot(t, np.rad2deg(u[:, 1]), label=r'$\phi(t)$', lw=1, color='b') 161 | ax33.spines["left"].set_color('r') 162 | ax33.spines["right"].set_color('b') 163 | ax33.tick_params(axis='y', colors='b') 164 | 165 | # Grids 166 | ax1.grid(True) 167 | ax2.grid(True) 168 | ax3.grid(True) 169 | 170 | # set the labels on the x and y axis and the titles 171 | ax1.set_title('Position coordinates') 172 | ax1.set_ylabel(r'm') 173 | ax1.set_xlabel(r't in s') 174 | ax2.set_title('Orientation') 175 | ax2.set_ylabel(r'deg') 176 | ax2.set_xlabel(r't in s') 177 | ax3.set_title('Velocity / steering angle') 178 | ax3.set_ylabel(r'm/s') 179 | ax33.set_ylabel(r'deg') 180 | ax33.set_xlabel(r't in s') 181 | 182 | # put a legend in the plot 183 | ax1.legend() 184 | ax2.legend() 185 | ax3.legend() 186 | li3, lab3 = ax3.get_legend_handles_labels() 187 | li33, lab33 = ax33.get_legend_handles_labels() 188 | ax3.legend(li3 + li33, lab3 + lab33, loc=0) 189 | 190 | # automatically adjusts subplot to fit in figure window 191 | plt.tight_layout() 192 | 193 | # save the figure in the working directory 194 | if save: 195 | plt.savefig('state_trajectory.pdf') # save output as pdf 196 | plt.savefig('state_trajectory.pgf') # for easy export to LaTex 197 | return None 198 | 199 | 200 | def car_animation(x, u, t, p: Type[Para]): 201 | """Animation function of the car-like mobile robot 202 | 203 | Args: 204 | x(ndarray): state-vector trajectory 205 | u(ndarray): control vector trajectory 206 | t(ndarray): time vector 207 | p(object): parameters 208 | 209 | Returns: None 210 | 211 | """ 212 | # Setup two empty axes with enough space around the trajectory so the car 213 | # can always be completely plotted. One plot holds the sketch of the car, 214 | # the other the curve 215 | dx = 1.5 * p.l 216 | dy = 1.5 * p.l 217 | fig2, ax = plt.subplots() 218 | ax.set_xlim([min(min(x_traj[:, 0] - dx), -dx), 219 | max(max(x_traj[:, 0] + dx), dx)]) 220 | ax.set_ylim([min(min(x_traj[:, 1] - dy), -dy), 221 | max(max(x_traj[:, 1] + dy), dy)]) 222 | ax.set_aspect('equal') 223 | ax.set_xlabel(r'$y_1$') 224 | ax.set_ylabel(r'$y_2$') 225 | 226 | # Axis handles 227 | h_x_traj_plot, = ax.plot([], [], 'b') # state trajectory in the y1-y2-plane 228 | h_car, = ax.plot([], [], 'k', lw=2) # car 229 | 230 | def draw_the_car(cur_x, cur_u): 231 | """Mapping from state x and action cur_u to the position of the car elements 232 | 233 | Args: 234 | cur_x: The current state vector 235 | cur_u: The current action vector 236 | 237 | Returns: 238 | 239 | """ 240 | wheel_length = 0.1 * p.l 241 | y1, y2, theta = cur_x 242 | v, phi = cur_u 243 | 244 | # define chassis lines 245 | chassis_y1 = [y1, y1 + p.l * cos(theta)] 246 | chassis_y2 = [y2, y2 + p.l * sin(theta)] 247 | 248 | # define lines for the front and rear axle 249 | rear_ax_y1 = [y1 + p.w * sin(theta), y1 - p.w * sin(theta)] 250 | rear_ax_y2 = [y2 - p.w * cos(theta), y2 + p.w * cos(theta)] 251 | front_ax_y1 = [chassis_y1[1] + p.w * sin(theta + phi), 252 | chassis_y1[1] - p.w * sin(theta + phi)] 253 | front_ax_y2 = [chassis_y2[1] - p.w * cos(theta + phi), 254 | chassis_y2[1] + p.w * cos(theta + phi)] 255 | 256 | # define wheel lines 257 | rear_l_wl_y1 = [rear_ax_y1[1] + wheel_length * cos(theta), 258 | rear_ax_y1[1] - wheel_length * cos(theta)] 259 | rear_l_wl_y2 = [rear_ax_y2[1] + wheel_length * sin(theta), 260 | rear_ax_y2[1] - wheel_length * sin(theta)] 261 | rear_r_wl_y1 = [rear_ax_y1[0] + wheel_length * cos(theta), 262 | rear_ax_y1[0] - wheel_length * cos(theta)] 263 | rear_r_wl_y2 = [rear_ax_y2[0] + wheel_length * sin(theta), 264 | rear_ax_y2[0] - wheel_length * sin(theta)] 265 | front_l_wl_y1 = [front_ax_y1[1] + wheel_length * cos(theta + phi), 266 | front_ax_y1[1] - wheel_length * cos(theta + phi)] 267 | front_l_wl_y2 = [front_ax_y2[1] + wheel_length * sin(theta + phi), 268 | front_ax_y2[1] - wheel_length * sin(theta + phi)] 269 | front_r_wl_y1 = [front_ax_y1[0] + wheel_length * cos(theta + phi), 270 | front_ax_y1[0] - wheel_length * cos(theta + phi)] 271 | front_r_wl_y2 = [front_ax_y2[0] + wheel_length * sin(theta + phi), 272 | front_ax_y2[0] - wheel_length * sin(theta + phi)] 273 | 274 | # empty value (to disconnect points, define where no line should be plotted) 275 | empty = [np.nan, np.nan] 276 | 277 | # concatenate set of coordinates 278 | data_y1 = [rear_ax_y1, empty, front_ax_y1, empty, chassis_y1, 279 | empty, rear_l_wl_y1, empty, rear_r_wl_y1, 280 | empty, front_l_wl_y1, empty, front_r_wl_y1] 281 | data_y2 = [rear_ax_y2, empty, front_ax_y2, empty, chassis_y2, 282 | empty, rear_l_wl_y2, empty, rear_r_wl_y2, 283 | empty, front_l_wl_y2, empty, front_r_wl_y2] 284 | 285 | # set data 286 | h_car.set_data(data_y1, data_y2) 287 | 288 | def init(): 289 | """Initialize plot objects that change during animation. 290 | Only required for blitting to give a clean slate. 291 | 292 | Returns: 293 | 294 | """ 295 | h_x_traj_plot.set_data([], []) 296 | h_car.set_data([], []) 297 | return h_x_traj_plot, h_car 298 | 299 | def animate(i): 300 | """Defines what should be animated 301 | 302 | Args: 303 | i: frame number 304 | 305 | Returns: 306 | 307 | """ 308 | k = i % len(t) 309 | ax.set_title('Time (s): {:.2f}'.format(t[k]), loc='left') 310 | h_x_traj_plot.set_xdata(x[0:k, 0]) 311 | h_x_traj_plot.set_ydata(x[0:k, 1]) 312 | draw_the_car(x[k, :], u[k, :]) 313 | return h_x_traj_plot, h_car 314 | 315 | ani = mpla.FuncAnimation(fig2, animate, init_func=init, frames=len(t) + 1, 316 | interval=(t[1] - t[0]) * 1000, 317 | blit=False) 318 | 319 | ani.save('animation.mp4', writer='ffmpeg', fps=1 / (t[1] - t[0])) 320 | 321 | plt.show() 322 | return None 323 | 324 | 325 | # LISTING_START TrajSetupAndSim 326 | # Setup the trajectories, we hold it as global objects here 327 | [f_traj_gen, g_traj_gen] = setup_trajectories(SimPara) 328 | 329 | # simulation 330 | sol = sci.solve_ivp(lambda t, x: ode(x, t, Para), (SimPara.t0, SimPara.tf), SimPara.x0, 331 | method='RK45', t_eval=SimPara.tt) 332 | # LISTING_END TrajSetupAndSim 333 | x_traj = sol.y.T # size(sol.y) = len(x)*len(tt) (.T -> transpose) 334 | 335 | # LISTING_START ComputeControllerOutput 336 | u_traj = np.zeros([len(SimPara.tt), 2]) 337 | for i in range(0, len(SimPara.tt)): 338 | u_traj[i] = control(SimPara.tt[i], Para) 339 | # LISTING_END ComputeControllerOutput 340 | 341 | # get reference trajectories 342 | # LISTING_START PlotResults 343 | y1D = g_traj_gen.eval_vec(SimPara.tt) 344 | y2D = f_traj_gen.eval_vec(y1D[:, 0]) 345 | 346 | x_ref = np.zeros_like(x_traj) 347 | x_ref[:, 0] = y1D[:, 0] 348 | x_ref[:, 1] = y2D[:, 0] 349 | x_ref[:, 2] = arctan(y2D[:, 1]) 350 | 351 | # Plot results 352 | plot_data(x_traj, x_ref, u_traj, SimPara.tt, 12, 16, save=True) 353 | 354 | # animation 355 | car_animation(x_traj, u_traj, SimPara.tt, Para) 356 | 357 | plt.show() 358 | # LISTING_END PlotResults 359 | -------------------------------------------------------------------------------- /02-Trajectories-And-Control/sim/03_car_feedback_control.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import numpy as np 3 | from numpy import cos, sin, tan, arctan, arctan2 4 | import scipy.integrate as sci 5 | import matplotlib.pyplot as plt 6 | import matplotlib.animation as mpla 7 | from TrajGen import PolynomialTrajGen 8 | from dataclasses import dataclass 9 | from typing import Type, List 10 | 11 | # Set up of ffmpeg for creating the animation 12 | # Linux: 13 | # Make sure ffmpeg is installed (e.g.: sudo apt install ffmpeg) 14 | # 15 | # Windows: 16 | # 1. Download & install ffmepg from http://www.ffmpeg.org/download.html 17 | # 2. Uncomment the following line and set path appropriately 18 | #plt.rcParams['animation.ffmpeg_path'] = 'C:\\Progs\\ffmpeg\\bin\\ffmpeg.exe' 19 | 20 | 21 | # Real Physical Parameters 22 | @dataclass 23 | class PhysPara: 24 | l: float = 0.3 # define car length 25 | w: float = l * 0.3 # define car width 26 | 27 | 28 | # LISTING_START DefineControllerPara 29 | @dataclass 30 | class ControllerPara: 31 | k01: float = 3 32 | k02: float = 2 33 | k12: float = 10 34 | l: float = 0.5 * PhysPara.l 35 | # LISTING_END DefineControllerPara 36 | 37 | 38 | # Simulation Parameters 39 | # LISTING_START DefineSimPara 40 | @dataclass 41 | class SimPara: 42 | t0: float = 0 # start time 43 | tf: float = 10 # final time 44 | dt: float = 0.04 # step-size 45 | tt = np.arange(0, tf + dt, dt) # time vector 46 | x0d = [0, 0, 0] # reference inital state at t0 47 | xfd = [5, 5, 0] # reference final state at tf 48 | x0real = [0.25, 0.5, 0.25] # real inital state at t0 49 | # LISTING_END DefineSimPara 50 | 51 | 52 | def setup_trajectories(sp: Type[SimPara]) -> List[PolynomialTrajGen]: 53 | """Setup the trajectory objects 54 | 55 | Args: 56 | sp(SimPara): The simulation parameter 57 | 58 | Returns: 59 | A list holding two objects of type PolynomialTrajGen. The first 60 | one is f, the second g. 61 | 62 | """ 63 | 64 | # Start and final time of transistion 65 | t0: float = sp.t0 + 1 66 | tf: float = sp.tf - 1 67 | 68 | # boundary conditions for y1 69 | y1_a = np.array([sp.x0d[0], 0]) 70 | y1_b = np.array([sp.xfd[0], 0]) 71 | 72 | # boundary conditions for y2 73 | y2_a = np.array([sp.x0d[1], tan(sp.x0d[2]), 0]) 74 | y2_b = np.array([sp.xfd[1], tan(sp.xfd[2]), 0]) 75 | 76 | # From Y2A to Y2B on the interval [Y1A[0], Y1B[0]] 77 | f_traj = PolynomialTrajGen(y2_a, y2_b, y1_a[0], y1_b[0], 2) 78 | 79 | # from Y1A to Y1B on the interval [t0, tf] 80 | g_traj = PolynomialTrajGen(y1_a, y1_b, t0, tf, 1) 81 | 82 | return [f_traj, g_traj] 83 | 84 | 85 | def ode(x, t, p: Type[PhysPara], cp: Type[ControllerPara]): 86 | """Function of the robots kinematics 87 | 88 | Args: 89 | x: state 90 | t: time 91 | p(PhysPara): Real physical parameter 92 | cp(ControllerPara): Parameter used for simulation and control 93 | 94 | Returns: 95 | dxdt: state derivative 96 | """ 97 | x1, x2, x3 = x # state vector 98 | u1, u2 = control(x, t, cp) # control vector 99 | 100 | # dxdt = f(x, u): 101 | dxdt = np.array([u1 * cos(x3), 102 | u1 * sin(x3), 103 | 1 / p.l * u1 * tan(u2)]) 104 | 105 | # return state derivative 106 | return dxdt 107 | 108 | 109 | # LISTING_START ControlFunc 110 | def control(x, t, p: Type[ControllerPara]): 111 | """Function of the control law 112 | 113 | Args: 114 | x (ndarray, int): state vector 115 | t (int): time 116 | p (ControllerPara): Parameter 117 | 118 | Returns: 119 | u (ndarry): control vector 120 | 121 | """ 122 | # LISTING_END ControlFunc 123 | 124 | # state vector 125 | y1 = x[0] 126 | y2 = x[1] 127 | theta = x[2] 128 | 129 | dy2 = sin(theta) 130 | 131 | # LISTING_START DefineRefTraj 132 | # reference trajectories yd, yd', yd'' 133 | # evaluate the planned trajectories at time t 134 | g_t = g_traj_gen.eval(t) # y1 = g(t) 135 | f_y1 = f_traj_gen.eval(g_t[0]) # y2 = f(y1) = f(g(t)) 136 | 137 | y1d = g_t[0] 138 | dy1d = 1/(np.sqrt(1 + f_y1[1] ** 2)) 139 | 140 | y2d = f_y1[0] 141 | dy2d = f_y1[1]/(np.sqrt(1 + f_y1[1] ** 2)) 142 | ddy2d = f_y1[2]/(1 + f_y1[1] ** 2) 143 | # LISTING_END DefineRefTraj 144 | 145 | # LISTING_START CalcStabInputs 146 | # stabilizing inputs 147 | w1 = dy1d - p.k01 * (y1 - y1d) 148 | w2 = ddy2d - p.k12 * (dy2 - dy2d) - p.k02 * (y2 - y2d) 149 | # LISTING_END CalcStabInputs 150 | 151 | # LISTING_START ControlLaw 152 | # control laws 153 | ds = g_t[1] * np.sqrt(1 + (f_y1[1]) ** 2) # desired velocity 154 | u1 = ds*np.sqrt(w1**2+dy2**2) 155 | u2 = arctan2(p.l * (w2 * w1), 1) 156 | 157 | return np.array([u1, u2]).T 158 | # LISTING_END ControlLaw 159 | 160 | 161 | def plot_data(x, xref, u, t, fig_width, fig_height, save=False): 162 | """Plotting function of simulated state and actions 163 | 164 | Args: 165 | x(ndarray): state-vector trajectory 166 | xref(ndarray): reference state-vector trajectory 167 | u(ndarray): control vector trajectory 168 | t(ndarray): time vector 169 | fig_width: figure width in cm 170 | fig_height: figure height in cm 171 | save (bool) : save figure (default: False) 172 | Returns: None 173 | 174 | """ 175 | # creating a figure with 3 subplots, that share the x-axis 176 | fig1, (ax1, ax2, ax3) = plt.subplots(3, sharex=True) 177 | 178 | # set figure size to desired values 179 | fig1.set_size_inches(fig_width / 2.54, fig_height / 2.54) 180 | 181 | # plot y_1 in subplot 1 182 | ax1.plot(t, x[:, 0], label='$y_1(t)$', lw=1, color='r') 183 | ax1.plot(t, xref[:, 0], label='$y_{1,d}(t)$', lw=1, color=(0.5, 0, 0)) 184 | 185 | # plot y_2 in subplot 1 186 | ax1.plot(t, x[:, 1], label='$y_2(t)$', lw=1, color='b') 187 | ax1.plot(t, xref[:, 1], label='$y_{2,d}(t)$', lw=1, color=(0, 0, 0.5)) 188 | 189 | # plot theta in subplot 2 190 | ax2.plot(t, np.rad2deg(x[:, 2]), label=r'$\theta(t)$', lw=1, color=(0, 0.7, 0)) 191 | ax2.plot(t, np.rad2deg(xref[:, 2]), label=r'$\theta_d(t)$', lw=1, color='g') 192 | 193 | # plot control in subplot 3, left axis red, right blue 194 | ax3.plot(t, u[:, 0], label=r'$v(t)$', lw=1, color='r') 195 | ax3.tick_params(axis='y', colors='r') 196 | ax33 = ax3.twinx() 197 | ax33.plot(t, np.rad2deg(u[:, 1]), label=r'$\phi(t)$', lw=1, color='b') 198 | ax33.spines["left"].set_color('r') 199 | ax33.spines["right"].set_color('b') 200 | ax33.tick_params(axis='y', colors='b') 201 | 202 | # Grids 203 | ax1.grid(True) 204 | ax2.grid(True) 205 | ax3.grid(True) 206 | 207 | # set the labels on the x and y axis and the titles 208 | ax1.set_title('Position coordinates') 209 | ax1.set_ylabel(r'm') 210 | ax1.set_xlabel(r't in s') 211 | ax2.set_title('Orientation') 212 | ax2.set_ylabel(r'deg') 213 | ax2.set_xlabel(r't in s') 214 | ax3.set_title('Velocity / steering angle') 215 | ax3.set_ylabel(r'm/s') 216 | ax33.set_ylabel(r'deg') 217 | ax3.set_xlabel(r't in s') 218 | 219 | # put a legend in the plot 220 | ax1.legend() 221 | ax2.legend() 222 | ax3.legend() 223 | li3, lab3 = ax3.get_legend_handles_labels() 224 | li33, lab33 = ax33.get_legend_handles_labels() 225 | ax3.legend(li3 + li33, lab3 + lab33, loc=0) 226 | 227 | # automatically adjusts subplot to fit in figure window 228 | plt.tight_layout() 229 | 230 | # save the figure in the working directory 231 | if save: 232 | plt.savefig('state_trajectory.pdf') # save output as pdf 233 | plt.savefig('state_trajectory.pgf') # for easy export to LaTex 234 | return None 235 | 236 | 237 | def car_animation(x, u, t, p: Type[PhysPara]): 238 | """Animation function of the car-like mobile robot 239 | 240 | Args: 241 | x(ndarray): state-vector trajectory 242 | u(ndarray): control vector trajectory 243 | t(ndarray): time vector 244 | p(object): Parameters 245 | 246 | Returns: None 247 | 248 | """ 249 | # Setup two empty axes with enough space around the trajectory so the car 250 | # can always be completely plotted. One plot holds the sketch of the car, 251 | # the other the curve 252 | dx = 1.5 * p.l 253 | dy = 1.5 * p.l 254 | fig2, ax = plt.subplots() 255 | ax.set_xlim([min(min(x_traj[:, 0] - dx), -dx), 256 | max(max(x_traj[:, 0] + dx), dx)]) 257 | ax.set_ylim([min(min(x_traj[:, 1] - dy), -dy), 258 | max(max(x_traj[:, 1] + dy), dy)]) 259 | ax.set_aspect('equal') 260 | ax.set_xlabel(r'$y_1$') 261 | ax.set_ylabel(r'$y_2$') 262 | 263 | # Axis handles 264 | h_x_traj_plot, = ax.plot([], [], 'b') # state trajectory in the y1-y2-plane 265 | h_car, = ax.plot([], [], 'k', lw=2) # car 266 | 267 | def draw_the_car(cur_x, cur_u): 268 | """Mapping from state x and action cur_u to the position of the car elements 269 | 270 | Args: 271 | cur_x: The current state vector 272 | cur_u: The current action vector 273 | 274 | Returns: 275 | 276 | """ 277 | wheel_length = 0.1 * p.l 278 | y1, y2, theta = cur_x 279 | v, phi = cur_u 280 | 281 | # define chassis lines 282 | chassis_y1 = [y1, y1 + p.l * cos(theta)] 283 | chassis_y2 = [y2, y2 + p.l * sin(theta)] 284 | 285 | # define lines for the front and rear axle 286 | rear_ax_y1 = [y1 + p.w * sin(theta), y1 - p.w * sin(theta)] 287 | rear_ax_y2 = [y2 - p.w * cos(theta), y2 + p.w * cos(theta)] 288 | front_ax_y1 = [chassis_y1[1] + p.w * sin(theta + phi), 289 | chassis_y1[1] - p.w * sin(theta + phi)] 290 | front_ax_y2 = [chassis_y2[1] - p.w * cos(theta + phi), 291 | chassis_y2[1] + p.w * cos(theta + phi)] 292 | 293 | # define wheel lines 294 | rear_l_wl_y1 = [rear_ax_y1[1] + wheel_length * cos(theta), 295 | rear_ax_y1[1] - wheel_length * cos(theta)] 296 | rear_l_wl_y2 = [rear_ax_y2[1] + wheel_length * sin(theta), 297 | rear_ax_y2[1] - wheel_length * sin(theta)] 298 | rear_r_wl_y1 = [rear_ax_y1[0] + wheel_length * cos(theta), 299 | rear_ax_y1[0] - wheel_length * cos(theta)] 300 | rear_r_wl_y2 = [rear_ax_y2[0] + wheel_length * sin(theta), 301 | rear_ax_y2[0] - wheel_length * sin(theta)] 302 | front_l_wl_y1 = [front_ax_y1[1] + wheel_length * cos(theta + phi), 303 | front_ax_y1[1] - wheel_length * cos(theta + phi)] 304 | front_l_wl_y2 = [front_ax_y2[1] + wheel_length * sin(theta + phi), 305 | front_ax_y2[1] - wheel_length * sin(theta + phi)] 306 | front_r_wl_y1 = [front_ax_y1[0] + wheel_length * cos(theta + phi), 307 | front_ax_y1[0] - wheel_length * cos(theta + phi)] 308 | front_r_wl_y2 = [front_ax_y2[0] + wheel_length * sin(theta + phi), 309 | front_ax_y2[0] - wheel_length * sin(theta + phi)] 310 | 311 | # empty value (to disconnect points, define where no line should be plotted) 312 | empty = [np.nan, np.nan] 313 | 314 | # concatenate set of coordinates 315 | data_y1 = [rear_ax_y1, empty, front_ax_y1, empty, chassis_y1, 316 | empty, rear_l_wl_y1, empty, rear_r_wl_y1, 317 | empty, front_l_wl_y1, empty, front_r_wl_y1] 318 | data_y2 = [rear_ax_y2, empty, front_ax_y2, empty, chassis_y2, 319 | empty, rear_l_wl_y2, empty, rear_r_wl_y2, 320 | empty, front_l_wl_y2, empty, front_r_wl_y2] 321 | 322 | # set data 323 | h_car.set_data(data_y1, data_y2) 324 | 325 | def init(): 326 | """Initialize plot objects that change during animation. 327 | Only required for blitting to give a clean slate. 328 | 329 | Returns: 330 | 331 | """ 332 | h_x_traj_plot.set_data([], []) 333 | h_car.set_data([], []) 334 | return h_x_traj_plot, h_car 335 | 336 | def animate(i): 337 | """Defines what should be animated 338 | 339 | Args: 340 | i: frame number 341 | 342 | Returns: 343 | 344 | """ 345 | k = i % len(t) 346 | ax.set_title('Time (s): {:.2f}'.format(t[k]), loc='left') 347 | h_x_traj_plot.set_xdata(x[0:k, 0]) 348 | h_x_traj_plot.set_ydata(x[0:k, 1]) 349 | draw_the_car(x[k, :], u[k, :]) 350 | return h_x_traj_plot, h_car 351 | 352 | ani = mpla.FuncAnimation(fig2, animate, init_func=init, frames=len(t) + 1, 353 | interval=(t[1] - t[0]) * 1000, 354 | blit=False) 355 | 356 | ani.save('animation.mp4', writer='ffmpeg', fps=1 / (t[1] - t[0])) 357 | plt.show() 358 | return None 359 | 360 | 361 | # Setup the trajectories, we hold it as global objects here 362 | [f_traj_gen, g_traj_gen] = setup_trajectories(SimPara) 363 | 364 | # simulation 365 | sol = sci.solve_ivp(lambda t, x: ode(x, t, PhysPara, ControllerPara), (0.0, SimPara.tf), 366 | SimPara.x0real, method='RK45', t_eval=SimPara.tt) 367 | x_traj = sol.y.T # size(sol.y) = len(x)*len(tt) (.T -> transpose) 368 | u_traj = np.zeros([len(SimPara.tt), 2]) 369 | for i in range(0, len(SimPara.tt)): 370 | u_traj[i] = control(x_traj[i], SimPara.tt[i], ControllerPara) 371 | 372 | # get reference trajectories 373 | y1D = g_traj_gen.eval_vec(SimPara.tt) 374 | y2D = f_traj_gen.eval_vec(y1D[:, 0]) 375 | 376 | x_ref = np.zeros_like(x_traj) 377 | x_ref[:, 0] = y1D[:, 0] 378 | x_ref[:, 1] = y2D[:, 0] 379 | x_ref[:, 2] = arctan(y2D[:, 1]) 380 | 381 | # plot result 382 | plot_data(x_traj, x_ref, u_traj, SimPara.tt, 12, 16, save=True) 383 | 384 | # animation 385 | car_animation(x_traj, u_traj, SimPara.tt, PhysPara) 386 | 387 | plt.show() 388 | -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/tutorial4.md: -------------------------------------------------------------------------------- 1 | # Pytutorial 4 - LQR Design and Simulation 2 | 3 | ## Introduction 4 | 5 | The goal of this tutorial is to teach the usage of the programming language _Python_ as a tool for developing and simulating control systems. The following topics are covered: 6 | 7 | - Flatness based feedforward control using existing trajectory generators 8 | - Feedback control using LQR for linear time-invariant (LTI) system 9 | - Demonstration of problematic situations 10 | - Feedback control using LQR linear time-variant (LTV) system 11 | 12 | Later in this tutorial this process is applied to design control strategies for the cart-pole system from a previous tutorial. 13 | 14 | ## Implementing the system 15 | 16 | In order to demonstrate the design methods discussed in the following, a simple academic example from ==??== will be used. The system state $x=(x_1, x_2)$ has two components and the scalar input is called $u$. Written in state-space-form, the system equation then is: 17 | $$ 18 | \dot x = F(x, u) = 19 | \begin{pmatrix} 20 | a \sin(x_2)\\ 21 | -x_1^2+u\\ 22 | \end{pmatrix}\,. 23 | $$ 24 | Later, the jacobians 25 | $$ 26 | A(x^*, u^*) := \left.\frac{\partial F}{\partial x}\right\vert_{(x^*, u^*)} 27 | = \begin{pmatrix} 28 | 0 & a \cos(x^*_2)\\ 29 | -2 x^*_1 & 0 30 | \end{pmatrix} 31 | $$ 32 | and 33 | $$ 34 | B(x^*, u^*) := \left.\frac{\partial F}{\partial u}\right\vert_{(x^*, u^*)} 35 | = \begin{pmatrix} 36 | 0 \\ 1 37 | \end{pmatrix} 38 | $$ 39 | will also be needed. 40 | 41 | Implementing this system in Python then simply means expressing these terms as functions containing these computations. 42 | 43 | ==`defsystem`== 44 | 45 | Notable are mainly the usage of Numpy-Arrays for all vectors and the `Parameters` class, please refer to a previous tutorial if unsure about these aspects. 46 | 47 | ## Trajectory planning and feed-forward 48 | 49 | Similar to previous tutorials, feedforward control design can be simplified significantly by exploiting the flatness property of this system. Specifically, the flat output of this system is $y=x_1$. Recall, this means a desired trajectory $t \mapsto x_1^*(t)$ can be freely chosen (as long as it is sufficiently often differentiable). Then, the trajectories for all other system variables (states and inputs) is calculated analytically without integration. 50 | 51 | To this effect, the first system equation in ==??== is solved for $x_2$, yielding 52 | $$ 53 | x_2 = \arcsin\left(\frac{\dot x_1}{a}\right) 54 | $$ 55 | which also introduces the constraint $\forall t: |\dot x_1(t)| \leq |a|$ to the trajectory planning. 56 | 57 | After differentiating ==??== w.r.t. time, resulting in 58 | $$ 59 | \dot x_2 = \frac{\ddot x_1}{\sqrt{a^2-\dot x_1^2}}\,, 60 | $$ 61 | a term for $u$ is obtained by solving the second component of ==??==: 62 | $$ 63 | u = \dot x_2 + x_1^2 = \frac{\ddot x_1}{\sqrt{a^2-\dot x_1^2}} + x_1^2\, . 64 | $$ 65 | For the Python implementation, the trajectory planner from a previous tutorial is reused to obtain a polynomial that transitions from $y(t_0) = y_0$ to $y(t_f) = y_f$. This function is then evaluated at the time values stored in vector `t_traj`. 66 | 67 | ==`plantraj`== 68 | 69 | The previously derived formulas are then translated into NumPy operations to obtain the values for $x_2$ and $u$ at every time step. 70 | 71 | ==`flatness`== 72 | 73 | ## Reminder: LQR for LTI systems 74 | 75 | Before an LQR controller can be designed, the system in question must be linear. As a rough approximation, Taylor linearization is used to obtain a linear time-invariant (LTI) system which is valid in the proximity to the operating point $(x^*, u^*)$. In new coordinates $\tilde x=x - x^*$ and $\tilde u = u - u^*$ as well as by using the Jacobians ==??==, the new state-space-form is then 76 | $$ 77 | \dot{\tilde x}(t) = A(x^*, u^*) \tilde x(t) + B(x^*,u^*) \tilde u(t)\, . 78 | $$ 79 | In \py, the operating point is defined by a time $t^*$ along the planned trajectory. The corresponding array index is first determined and then used to get the values $(x^*, u^*)$, with which the system matrices are computed. 80 | 81 | ==`linsys`== 82 | 83 | Now an LQR controller will be designed by the book. This means defining a cost-function 84 | $$ 85 | J = \int_0^\infty \tilde x^T(t) Q x(t) + \tilde u^T(t) R u(t) \, \mathrm d t 86 | $$ 87 | with the tuning matrices $Q$ and $R$ usually chosen as diagonal. Minimizing this function leads to the algebraic Riccati equation 88 | $$ 89 | P A + A^T P - P B R^{-1} B^T P + Q = 0 90 | $$ 91 | having to be solved for $P$. In \py the SciPy package fortunately contains the routine `solve_continuous_are` which does exactly that. 92 | 93 | The cost function then is minimal if a constant state feedback $\tilde u = - K \tilde x$ is applied, with 94 | $$ 95 | K = \begin{pmatrix} k_1 & k_2 \end{pmatrix} = R^{-1} B^T P\, . 96 | $$ 97 | These computations -- save for the applying the actual control law, which happens in the main simulation loop -- can now be compactly written as 98 | 99 | ==`solveare`== 100 | 101 | Now it is time to look at some simulation results. Shown in Figure ==??== on the left side is the combination of feedforward control and feedback control, designed for a system linearized around the planned trajectory values at $t^*=0$. The initial state $x(0)$ is slightly offset from the planned initial position $x^*(0)$, but the controller manages to track the trajectory anyway. 102 | 103 | However, this choice of linearization point was arbitrary and lucky. When e. g. $t^*=5$ is picked instead, the closed loop system immediately starts moving away from the reference value and tracking capability is lost. This is demonstrated on the right side of Figure ==??==. 104 | 105 | ==DIAGRAM== 106 | 107 | In fact, simply sticking to $t^*=0$ also does not work. For other reference trajectories, e. g. for $y(0)=2$ and $y(10)=-2$ (the reverse trajectory if you will), choosing the same linearization time fails. In that case $t^*=10$ must instead be used. 108 | 109 | It appears that tracking arbitrary trajectories with this approach is not feasible, since the linearized system can never sufficiently represent the more involved non-linear dynamics and a "one size fits all" constant state feedback is therefore bound to fail. 110 | 111 | One might be tempted to improve the controller by simply re-linearizing the system and re-computing the feedback matrix at every time step. For this specific case this approach actually works and is very easy to implement when a normal LQR controller already exists. Simulation results are shown in Figure ==??==, along with a plot of the changing feedback matrix entries over time. 112 | 113 | ==DIAGRAM== 114 | 115 | Remember this is dangerous territory though. No stability guarantees can be made here, because this ad-hoc solution corresponds to a linear system approximation with time-variant system matrices 116 | $$ 117 | \dot{\tilde x}(t) = A(t)\tilde x(t) + B(t) u(t)\, . 118 | $$ 119 | For a linear time-variant system, the closed loop system matrix $A(t) - B(t)K(t)$ must only contain eigenvalues $s$ with $\mathrm{Re}\,(s) < 0$, but also be __constant__ in order for the closed loop to be stable! This property is not ensured in this controller design, using it is therefore not recommended. 120 | 121 | ## LQR for LTV systems 122 | 123 | Fortunately a more formal way to solve the problem demonstrated in the previous section exists. A time-variant linear system description provides a good compromise between reflecting the changing system properties and still being manageable using some of the tools known for linear systems. 124 | 125 | For ease of notation, first define the time-variant system matrices 126 | $$ 127 | A(t):=A^*(x^*(t), u^*(t)) \quad \text{and} \quad B(t):=B^*(x^*(t),u^*(t)) 128 | $$ 129 | and analogous to the previous section the corresponding state-space system 130 | $$ 131 | \dot{\tilde x}(t) = A(t)\tilde x(t) + B(t) u(t)\, . 132 | $$ 133 | Intuitively, this linear approximation stems not from linearization at one single operating point, but from linearization along the reference trajectory. Therefore this approach is valid as long as it can be assumed that in the closed loop the system closely tracks the desired states. 134 | 135 | The first step in the LQR design process again is to define a cost function to be minimized. Similar to the previous section this function is now 136 | $$ 137 | J=\tilde x^T(t_f) S \tilde x(t_f) +\int_0^{t_f} \tilde x^T(t) Q x(t) + \tilde u^T(t) R u(t) \, \mathrm d t\, . 138 | $$ 139 | Note these two differences: for one the integration interval has changed from an infinite horizon to the finite length of the planned trajectory, also an additional end cost term with the weighting matrix $S$ was added. 140 | 141 | To obtain the optimal state feedback, the initial value problem (IVP) defined by the matrix Riccati differential equation 142 | $$ 143 | \frac{\mathrm d P(t)}{\mathrm d t}= -P(t)A(t) - A(t)^T P(t) + P(t) B(t) R^{-1} B(t)^T P(t) - Q(t) 144 | $$ 145 | and 146 | $$ 147 | P(t_f) = S 148 | $$ 149 | must be solved. 150 | 151 | After performing all the matrix operations this would mean simulating $n^2$ scalar ODEs. It can be shown though that $P(t)$ is symmetric, which reduces the number of distinct entries to $\frac{n(n+1)}{2}$ in the upper half. To extract these entries into a one-dimensional vector for simulation purposes, as well as to reconstruct the full matrix, the following \py code is used. 152 | 153 | ==`triuconvert`== 154 | 155 | The initial value $S$ is technically a tuning parameter and can be freely chosen. However, for trajectories that transfer between two setpoints, such as in the example shown here, one should consider the following suggestion. If the final setpoint is to be stabilized after the transfer phase ends, it is desirable for the final state feedback of the time-variant section to be identical to the feedback designed for the LTI system resulting from linearization around the final point. This ensures a smooth transition between the two phases. 156 | 157 | In practice this means solving the algebraic Riccati equation ==??== for the reference state at $t_f$ and using the solution as the initial value $S$. The implementation is as follows: 158 | 159 | ==`riccatiinit`== 160 | 161 | The last quirk of the IVP ==??== is the "initial value" not being defined at time zero. While this mathematically does not make much of a difference, both our intuition and most existing software tools expect that the system "runs" in forward flowing time. The time reversal 162 | $$ 163 | P(t) =P(t_f - \tau)=:\bar P(\tau) 164 | $$ 165 | remedies this issue. This new function $\bar P(\tau)$ is almost the same as $P(t)$, only with a redefined argument, so that an increasing $\tau$ corresponds to $t$ decreasing from $t_f$ in the original function. Utilizing the chain rule $\frac{\mathrm d P(t)}{\mathrm d t} = -\frac{\mathrm d \bar P(\tau)}{\mathrm d \tau}$ yields a new IVP 166 | $$ 167 | \frac{\mathrm d \bar P(\tau)}{\mathrm d \tau}= -\bar P(\tau) B(T-\tau) R^{-1} B(T-\tau)^T \bar P(\tau) + \bar P(\tau)A(T-\tau)+A(T-\tau)^T \bar P(\tau) + Q 168 | $$ 169 | where the initial value is now the more familiar 170 | $$ 171 | \bar P(0)=S=P(t_f)\, . 172 | $$ 173 | As only system and reference trajectory information are required, the IVP can be solved offline. Values for the feedback matrix 174 | $$ 175 | K(t) = R^{-1} B(t)^T P(t) 176 | $$ 177 | are stored for the fixed time steps and then used later in the closed loop. Simulation of ==??== can be performed with a numerical integration algorithm of choice, a simple fixed step width Euler algorithm was used here. 178 | 179 | ==`riccatiint`== 180 | 181 | The main difficulty is keeping the array indices corresponding to $t$ and $\tau$ straight, otherwise the code is structurally identical to the simulation of any dynamic system. 182 | 183 | Finally, the actual simulation of the closed loop happens in the main simulation loop. The method of numerical integration is identical here. 184 | 185 | ==`sim`== 186 | 187 | Simulation results are shown in Figure ==??==. 188 | 189 | ==DIAGRAM== 190 | 191 | Unfortunately the final state trajectory is visually indistinguishable from the previous result. Still, for less benevolent systems than this academic example this method is the safer option, as it is applicable whenever the feedforward control is good enough to not deviate from the reference too far. 192 | 193 | ## Practical example: cart-pole system 194 | 195 | To demonstrate the broader applicability of this controller design, it will now be implemented for a second example. The chosen system is the cart-pole inverse pendulum seen in Figure ==??==. 196 | 197 | ==SKETCH== 198 | 199 | The state vector $x$ consists of the cart position $s$ and the angle $\varphi$ as well as their first time derivatives, so 200 | $$ 201 | x = \begin{pmatrix}x_1\\x_2\\x_3\\x_4\end{pmatrix} := \begin{pmatrix}s\\\dot s\\\varphi\\\dot\varphi\end{pmatrix}\, , 202 | $$ 203 | while the input is the cart acceleration $u = a$. The system equation already in state-space form then is: 204 | $$ 205 | \dot x = F(x, u) = \begin{pmatrix} 206 | x_2\\ 207 | u\\ 208 | x_4\\ 209 | \frac{1}{l}(g \sin x_3+u\cos x_3) 210 | \end{pmatrix}\, . 211 | $$ 212 | From differentiation follows for the linearized system matrices 213 | $$ 214 | A(x^*, u^*) := \left.\frac{\partial F}{\partial x}\right\vert_{(x^*, u^*)} 215 | = \begin{pmatrix} 216 | 0 & 1 & 0 & 0\\ 217 | 0 & 0 & 0 & 0\\ 218 | 0 & 0 & 0 & 1\\ 219 | 0 & 0 & \frac{1}{l}(g \cos x_3 - u \sin x_3) & 0 220 | \end{pmatrix} 221 | $$ 222 | and 223 | $$ 224 | B(x^*, u^*) := \left.\frac{\partial F}{\partial u}\right\vert_{(x^*, u^*)} 225 | = \begin{pmatrix} 226 | 0\\ 227 | 1\\ 228 | 0\\ 229 | \frac{\cos x_3}{l} 230 | \end{pmatrix}\, . 231 | $$ 232 | Converted to \py this system description becomes: 233 | 234 | ==`defsystem`== 235 | 236 | The task will be a swingup of the pendulum, meaning that it should move from the lower equilibrium $x(0)=(0, 0, \pi, 0)$ to the upper equilibrium $x(t_f) = (0, 0, 0, 0)$. How to plan trajectories like this is a separate topic that will not be discussed here. For now, assume that the necessary trajectory data is somehow known. 237 | 238 | The code trajectory contains a file `pendulum.csv` with this trajectory. The first column is a time vector with fixed step width of 10 ms, the following four columns describe the state at each time step and the last column is the input signal. In \py this data can be loaded like this: 239 | 240 | ==`loadcsv`== 241 | 242 | All the other code doing the controller design and the simulation can be reused. 243 | 244 | Because the dynamics at the upper and lower position are obviously very different, it is to be expected that an LQR designed for time-invariant system approximation will struggle. If it assumes the system to be at the lower position it will not be able to stabilize it when it's actually at the upper one. Reversely, designing it for the upper position makes tracking during the swingup difficult. This last case was simulated in Figure ==??==. Even though the feedforward control was very precise, tracking does not work at all. 245 | 246 | ==DIAGRAM== 247 | 248 | In the previous example, this issues could be alleviated with the workaround of retuning the controller at every time step. Figure ==??== shows how this can also fail when the system is slightly more involved. Even though the pendulum arm reaches the upper position and stays there, the cart position starts drifting off halfway through the trajectory. 249 | 250 | ==DIAGRAM== 251 | 252 | Finally, an LQR properly designed for a linear time-variant system approximation was used in Figure ==??==. At least in simulation tracking works with issues. Both during swingup and while remaining at the upper equilibrium the state follows the reference well. Furthermore the difference of the feedback matrix $K(t)$ between this version and the previous ad-hoc solution now becomes obvious. 253 | 254 | ==DIAGRAM== -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/img/pendulum.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 20 | 22 | 30 | 36 | 37 | 45 | 51 | 52 | 60 | 66 | 67 | 75 | 81 | 82 | 90 | 96 | 97 | 105 | 111 | 112 | 120 | 126 | 127 | 135 | 141 | 142 | 150 | 156 | 157 | 165 | 171 | 172 | 173 | 196 | 201 | 202 | 204 | 205 | 207 | image/svg+xml 208 | 210 | 211 | 212 | 213 | 214 | 219 | 231 | 233 | 236 | 242 | 243 | 244 | 245 | 257 | 259 | 262 | 268 | 269 | 270 | 271 | 283 | 289 | 294 | 300 | 305 | 312 | 317 | 329 | 331 | 334 | 340 | 341 | 342 | 343 | 349 | 361 | 363 | 366 | 372 | 373 | 374 | 375 | 376 | 377 | -------------------------------------------------------------------------------- /01-System-Simulation-ODE/doc/img/car-like_mobile_robot.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 19 | 41 | 48 | 49 | 51 | 59 | 64 | 65 | 73 | 78 | 79 | 88 | 93 | 94 | 102 | 107 | 108 | 117 | 122 | 123 | 132 | 137 | 138 | 147 | 152 | 153 | 162 | 167 | 168 | 177 | 182 | 183 | 192 | 197 | 198 | 202 | 207 | 208 | 209 | 211 | 212 | 214 | image/svg+xml 215 | 217 | 218 | 219 | 220 | 221 | 226 | 231 | 236 | 242 | 247 | 252 | 264 | 276 | 281 | 286 | 291 | 296 | 305 | 314 | 323 | 328 | 333 | 342 | 343 | 346 | 351 | $Y_1$ 362 | $Y_2$ 373 | $\theta$ 384 | $\phi$ 395 | $(y_1,y_2)$ 406 | $l$ 417 | $\vec{v}$ 428 | 429 | 430 | -------------------------------------------------------------------------------- /04-LQR-Design-And-Simulation/doc/tutorial4.tex: -------------------------------------------------------------------------------- 1 | \documentclass[a4paper,11pt,headinclude=true,headsepline,parskip=half,DIV=13]{scrartcl} 2 | 3 | % font, style, etc. 4 | \usepackage[utf8]{inputenc} % defines 5 | \usepackage[automark]{scrlayer-scrpage} 6 | \usepackage{csquotes} 7 | \usepackage{xspace} % proper space after macros with 0 args 8 | 9 | % mathematics 10 | \usepackage{amsmath} 11 | \usepackage{amssymb} 12 | 13 | % figures, tables, etc. 14 | \usepackage{graphicx} 15 | \usepackage{tikz} 16 | \usepackage{pgf} 17 | \usepackage{xcolor} 18 | \usepackage{placeins} % -> floatbarrier 19 | 20 | % Navigation 21 | \usepackage{hyperref} % 22 | \hypersetup{ 23 | colorlinks = true, % false -> red box arround links (not very nice) 24 | linkcolor={blue!100!black}, 25 | citecolor={blue!100!black}, 26 | urlcolor={blue!100!black}, 27 | } 28 | 29 | % others 30 | \usepackage{siunitx} % -> handling of units 31 | \usepackage{acronym} 32 | 33 | % Code listings 34 | \usepackage{luacode} 35 | \usepackage{listings} 36 | \lstset{ 37 | language=Python, 38 | backgroundcolor = \color{light-gray}, 39 | basicstyle=\scriptsize\sffamily, 40 | stringstyle=\color{orange}, 41 | breaklines=true, 42 | numberstyle=\tiny\color{gray}, 43 | keywordstyle=\bfseries\color{dark-blue}\textit, % print keywords dark-blue 44 | commentstyle=\color{dark-green}, % print comments dark-green 45 | showstringspaces=false % spacing between strings not showed 46 | } 47 | 48 | \luadirect{dofile("luainputlisting.lua")} 49 | \newcommand*\luainputlisting[2]{ 50 | \luadirect{print_code_part(\luastring{#1}, \luastring{#2})} 51 | } 52 | 53 | % bibliography 54 | \usepackage[style=verbose-ibid,backend=bibtex]{biblatex} 55 | \bibliography{literature} 56 | 57 | % manage glossaries 58 | % Call makeglossaries tutorial4 on a command prompt after LaTeX compiling, 59 | % the re-run LaTeX 60 | \usepackage{glossaries} 61 | \setacronymstyle{long-short} 62 | \makeglossaries 63 | \newacronym{ivp}{IVP}{initial value problem} 64 | \newacronym{ode}{ODE}{ordinary differential equation} 65 | 66 | % theorems 67 | \newtheorem{defi}{Definition}[section] 68 | 69 | % define shortcuts 70 | \newcommand{\ad}{\mathrm{ad}} 71 | \renewcommand{\d}{\mathrm{d}} % d vor differential forms 72 | \newcommand{\NV}{{\cal N}\,} 73 | \newcommand{\rang}{\mathrm{rang}} 74 | \newcommand{\im}{\mathrm{im}} 75 | \newcommand{\spann}{\mathrm{span}} 76 | \newcommand{\R}{\mathbb{R}} % set of real numbers 77 | \newcommand{\py}{\emph{Python}\xspace} 78 | \newcommand{\scipy}{\emph{SciPy}\xspace} 79 | \newcommand{\numpy}{\emph{NumPy}\xspace} 80 | \newcommand{\mpl}{\emph{Matplotlib}\xspace} 81 | \newcommand{\uu}{\mathbf{u}} 82 | \newcommand{\f}{\mathbf{f}} 83 | \newcommand{\x}{\mathbf{x}} 84 | \newcommand{\y}{\mathbf{y}} 85 | \newcommand{\z}{\mathbf{z}} 86 | \newcommand{\xZero}{\mathbf{x}_0} 87 | 88 | % color definitions 89 | \definecolor{light-gray}{gray}{0.95} 90 | \definecolor{dark-blue}{rgb}{0, 0, 0.5} 91 | \definecolor{dark-red}{rgb}{0.5, 0, 0} 92 | \definecolor{dark-green}{rgb}{0, 0.5, 0} 93 | \definecolor{gray}{rgb}{0.5, 0.5, 0.5} 94 | 95 | % Avoid ugly indentations in footnotes. 96 | \deffootnote[1em]{1em}{0em}{% 97 | \textsuperscript{\thefootnotemark}% 98 | } 99 | 100 | 101 | % ---------------------------------------------------------------------------- 102 | \subject{\py for simulation, animation and control} 103 | \title{Design and Simulation of LQR Control} 104 | \subtitle{An introductory tutorial for the design and implementation of LQR controllers for time-invariant and time-variant linear systems} 105 | \author{Robert Heedt\thanks{Institute for Control Theory, Faculty of Electrical and Computer Engineering, Technische Universität Dresden, Germany} \and Jan Winkler\footnotemark[1]} 106 | \publishers{} 107 | \date{\today} 108 | % ---------------------------------------------------------------------------- 109 | 110 | % Headings 111 | \pagestyle{scrheadings} 112 | \ihead{\leftmark} 113 | \chead{} 114 | \ohead{Page \pagemark} 115 | \ifoot{} 116 | \cfoot{Python Control Tutorial 4} 117 | \ofoot{} 118 | 119 | \begin{document} 120 | 121 | \maketitle 122 | 123 | \tableofcontents 124 | 125 | \newpage 126 | 127 | \section{Introduction} 128 | The goal of this tutorial is to teach the usage of the programming language \py as a tool for developing and simulating control systems. 129 | The following topics are covered: 130 | \begin{itemize} 131 | \item Flatness based feedforward control using existing trajectory generators 132 | \item Feedback control using LQR for linear time-invariant (LTI) system 133 | \item Demonstration of problematic situations 134 | \item Feedback control using LQR linear time-variant (LTV) system 135 | \end{itemize} 136 | Later in this tutorial this process is applied to design control strategies for the swingup of a cart-pole system. 137 | 138 | \section{Implementing the System} 139 | In order to demonstrate the design methods discussed in the following, a simple academic example\autocite{ludyk} will be used. 140 | The system state~$x=(x_1, x_2)$ has two components and the scalar input is called~$u$. 141 | Written in state-space-form, the system equation then is: 142 | \begin{equation} 143 | \dot x = F(x, u) = 144 | \begin{pmatrix} 145 | a \sin(x_2)\\ 146 | -x_1^2+u\\ 147 | \end{pmatrix}\,. 148 | \label{eq:academic_example_ss} 149 | \end{equation} 150 | Later, the jacobians 151 | \begin{equation} 152 | A^*(x^*, u^*) := \left.\frac{\partial F}{\partial x}\right\vert_{(x^*, u^*)}= \begin{pmatrix}0 & a \cos(x^*_2)\\-2 x^*_1 & 0\end{pmatrix} 153 | \label{eq:jac_A} 154 | \end{equation} 155 | and 156 | \begin{equation} 157 | B^*(x^*, u^*) := \left.\frac{\partial F}{\partial u}\right\vert_{(x^*, u^*)}= \begin{pmatrix}0 \\ 1\end{pmatrix} 158 | \label{eq:jac_B} 159 | \end{equation} 160 | will also be needed. 161 | 162 | Implementing this system in Python then simply means expressing these terms as functions containing these computations. 163 | \luainputlisting{../sim/01_lqr.py}{defsystem} 164 | Notable are mainly the usage of NumPy arrays for all vectors and the \lstinline{Parameters} class, please refer to a previous tutorial if unsure about these aspects. 165 | 166 | \section{Trajectory Planning and Feedforward} 167 | 168 | Similar to previous tutorials, feedforward control design can be simplified significantly by exploiting the flatness property of this system. 169 | Specifically, the flat output is~$y=x_1$. 170 | Recall, this means a desired trajectory $t \mapsto x_1^*(t)$ can be freely chosen (as long as it is sufficiently often differentiable). 171 | Then, the trajectories for all other system variables (states and inputs) is calculated analytically without integration. 172 | 173 | To this effect, the first system equation in~\eqref{eq:academic_example_ss} is solved for~$x_2$, yielding 174 | \begin{equation} 175 | x_2 = \arcsin\left(\frac{\dot x_1}{a}\right) 176 | \label{eq:flatness_x2} 177 | \end{equation} 178 | which also introduces the constraint~$\forall t: |\dot x_1(t)| \leq |a|$ to trajectory planning. 179 | 180 | After differentiating~\eqref{eq:flatness_x2} w.\,r.\,t.\ time, resulting in 181 | $$ 182 | \dot x_2 = \frac{\ddot x_1}{\sqrt{a^2-\dot x_1^2}}\,, 183 | $$ 184 | a term for $u$ is obtained by solving the second component of~\eqref{eq:academic_example_ss}: 185 | $$ 186 | u = \dot x_2 + x_1^2 = \frac{\ddot x_1}{\sqrt{a^2-\dot x_1^2}} + x_1^2\, . 187 | $$ 188 | For the \py implementation, the trajectory planner from a previous tutorial is reused to obtain a polynomial that transitions from~$y(t_0) = y_0$ to~$y(t_f) = y_f$. 189 | This function is then evaluated at the time values stored in vector \lstinline{t_traj}. 190 | \luainputlisting{../sim/01_lqr.py}{plantraj} 191 | The previously derived formulas are then translated into NumPy operations to obtain the values for~$x_2$ and~$u$ at every time step. 192 | \luainputlisting{../sim/01_lqr.py}{flatness} 193 | 194 | \section{Linear Time Invariant Systems} 195 | Before an LQR controller can be designed, the system in question must be linear. 196 | As a rough approximation, Taylor linearization is used to obtain a linear time-invariant (LTI) system which is valid in the proximity to the operating point $(x^*, u^*)$. 197 | In new coordinates $\tilde x=x - x^*$ and $\tilde u = u - u^*$ as well as by defining~$A:=A^*(x^*,u^*)$ and~$B:=B^*(x^*,u^*)$, the new state-space-form is then 198 | $$ 199 | \dot{\tilde x}(t) = A \tilde x(t) + B \tilde u(t)\, . 200 | $$ 201 | In \py, the operating point is defined by a time $t^*$ along the planned trajectory. 202 | The corresponding array index is first determined and then used to get the values~$(x^*, u^*)$, with which the system matrices are computed. 203 | \luainputlisting{../sim/01_lqr.py}{linsys} 204 | 205 | Now an LQR controller will be designed by the book. 206 | This means defining a cost-function 207 | $$ 208 | J = \int_0^\infty \tilde x^T(t) Q x(t) + \tilde u^T(t) R u(t) \, \mathrm d t 209 | $$ 210 | with the tuning matrices $Q$ and $R$ usually chosen as diagonal. 211 | Minimizing this function leads to the algebraic Riccati equation 212 | \begin{equation} 213 | P A + A^T P - P B R^{-1} B^T P + Q = 0 214 | \label{eq:riccati_are} 215 | \end{equation} 216 | having to be solved for $P$. 217 | In \py the SciPy package fortunately contains the routine \lstinline{solve_continuous_are} which does exactly that. 218 | 219 | The cost function then is minimal if a constant state feedback~$\tilde u = - K \tilde x$ is applied, with 220 | $$ 221 | K = \begin{pmatrix} k_1 & k_2 \end{pmatrix} = R^{-1} B^T P\, . 222 | $$ 223 | These computations -- save for the applying the actual control law, which happens in the main simulation loop -- can now be compactly written as 224 | \luainputlisting{../sim/01_lqr.py}{solveare} 225 | 226 | Now it is time to look at some simulation results. 227 | Shown in Figure~\ref{fig:ludyk_lti} on the left side is the combination of feedforward control and feedback control, designed for a system linearized around the planned trajectory values at $t^*=0$. 228 | The initial state~$x(0)$ is slightly offset from the planned initial position~$x^*(0)$, but the controller manages to track the trajectory anyway. 229 | 230 | However, this choice of linearization point was arbitrary and lucky. 231 | When e.\,g.\ $t^*=5$ is picked instead, the closed loop system immediately starts moving away from the reference value and tracking capability is lost. 232 | This is demonstrated on the right side of Figure~\ref{fig:ludyk_lti}. 233 | 234 | \begin{figure}[ht] 235 | \centering 236 | \includegraphics[scale=1]{img/ludyk_lti.pdf} 237 | \caption{Closed loop simulation of trajectory tracking, controller designed for different linearization points} 238 | \label{fig:ludyk_lti} 239 | \end{figure} 240 | 241 | In fact, simply sticking to $t^*=0$ also does not work. 242 | For other reference trajectories, e.\,g.\ for~$y(0)=2$ and~$y(10)=-2$ (the reverse trajectory if you will), choosing the same linearization time fails. 243 | In that case~$t^*=10$ must instead be used. 244 | 245 | It appears that tracking arbitrary trajectories with this approach is not feasible, since the linearized system can never sufficiently represent the more involved non-linear dynamics and a "one size fits all" constant state feedback is therefore bound to fail. 246 | 247 | One might be tempted to improve the controller by simply re-linearizing the system and re-computing the feedback matrix at every time step. 248 | For this specific case this approach actually works and is very easy to implement when a normal LQR controller already exists. 249 | Simulation results are shown in Figure~\ref{fig:ludyk_pseudoltv}, along with a plot of the changing feedback matrix entries over time. 250 | 251 | \begin{figure}[ht] 252 | \centering 253 | \includegraphics[scale=1]{img/ludyk_pseudoltv.pdf} 254 | \caption{Closed loop simulation of trajectory tracking, controller retuned for linearized system at every time step} 255 | \label{fig:ludyk_pseudoltv} 256 | \end{figure} 257 | 258 | Remember this is dangerous territory though. 259 | No stability guarantees can be made here, because this ad-hoc solution corresponds to a linear system approximation with time-variant system matrices 260 | $$ 261 | \dot{\tilde x}(t) = A(t)\tilde x(t) + B(t) u(t)\, . 262 | $$ 263 | For a linear time-variant system, the closed loop system matrix $A(t) - B(t)K(t)$ must only contain eigenvalues $s$ with $\mathrm{Re}\,(s) < 0$, but also be \textbf{constant} in order for the closed loop to be stable! 264 | This property is not ensured in this controller design, using it is therefore not recommended. 265 | 266 | \FloatBarrier 267 | \section{Linear Time Variant Systems} 268 | 269 | Fortunately a more formal way to solve the problem demonstrated in the previous section exists. 270 | A time-variant linear system description provides a good compromise between reflecting the changing system properties and still being manageable using some of the tools known for linear systems. 271 | 272 | For ease of notation, first define the time-variant system matrices 273 | $$ 274 | A(t):=A^*(x^*(t), u^*(t)) \quad \text{and} \quad B(t):=B^*(x^*(t),u^*(t)) 275 | $$ 276 | and analogous to the previous section the corresponding state-space system 277 | $$ 278 | \dot{\tilde x}(t) = A(t)\tilde x(t) + B(t) u(t)\, . 279 | $$ 280 | Intuitively, this linear approximation stems not from linearization at one single operating point, but from linearization along the reference trajectory. 281 | Therefore this approach is valid as long as it can be assumed that in the closed loop the system closely tracks the desired states. 282 | 283 | The first step in the LQR design process again is to define a cost function to be minimized. 284 | Similar to the previous section this function is now 285 | $$ 286 | J=\tilde x^T(t_f) S \tilde x(t_f) +\int_0^{t_f} \tilde x^T(t) Q x(t) + \tilde u^T(t) R u(t) \, \mathrm d t\, . 287 | $$ 288 | Note these two differences: for one the integration interval has changed from an infinite horizon to the finite length of the planned trajectory, also an additional end cost term with the weighting matrix~$S$ was added. 289 | 290 | To obtain the optimal state feedback, the initial value problem (IVP) defined by the matrix Riccati differential equation 291 | \begin{equation} 292 | \frac{\mathrm d P(t)}{\mathrm d t}= -P(t)A(t) - A(t)^T P(t) + P(t) B(t) R^{-1} B(t)^T P(t) - Q(t) 293 | \label{eq:riccati_ivp} 294 | \end{equation} 295 | and the initial value 296 | $$ 297 | P(t_f) = S 298 | $$ 299 | must be solved. 300 | 301 | After performing all the matrix operations this would mean simulating~$n^2$ scalar ODEs. 302 | It can be shown though that $P(t)$ is symmetric, which reduces the number of distinct entries to~$n(n+1)/2$ in the upper half. 303 | To extract these entries into a one-dimensional vector for simulation purposes, as well as to reconstruct the full matrix, the following \py code is used. 304 | \luainputlisting{../sim/01_lqr.py}{triuconvert} 305 | 306 | The initial value~$S$ is technically a tuning parameter and can be freely chosen. 307 | However, for trajectories that transfer between two setpoints, such as in the example shown here, one should consider the following suggestion. 308 | If the final setpoint is to be stabilized after the transfer phase ends, it is desirable for the final state feedback of the time-variant section to be identical to the feedback designed for the LTI system resulting from linearization around the final point. 309 | This ensures a smooth transition between the two phases. 310 | 311 | In practice this means solving the algebraic Riccati equation~\eqref{eq:riccati_are} for the reference state at~$t_f$ and using the solution as the initial value~$S$. 312 | The implementation is as follows: 313 | \luainputlisting{../sim/01_lqr.py}{riccatiinit} 314 | 315 | The last quirk of the IVP~\eqref{eq:riccati_ivp} is the `initial value' not being defined at time zero. 316 | While this mathematically does not make much of a difference, both our intuition and most existing software tools expect that the system `runs' in forward flowing time. 317 | The time reversal 318 | $$ 319 | P(t) =P(t_f - \tau)=:\bar P(\tau) 320 | $$ 321 | remedies this issue. 322 | This new function~$\bar P(\tau)$ is almost the same as~$P(t)$, only with a redefined argument, so that an increasing~$\tau$ corresponds to~$t$ decreasing from~$t_f$ in the original function. 323 | Utilizing the chain rule~$\frac{\mathrm d P(t)}{\mathrm d t} = -\frac{\mathrm d \bar P(\tau)}{\mathrm d \tau}$ yields a new IVP 324 | \begin{equation} 325 | \frac{\mathrm d \bar P(\tau)}{\mathrm d \tau}= -\bar P(\tau) B(T-\tau) R^{-1} B(T-\tau)^T \bar P(\tau) + \bar P(\tau)A(T-\tau)+A(T-\tau)^T \bar P(\tau) + Q 326 | \label{eq:riccati_new_ivp} 327 | \end{equation} 328 | where the initial value is now the more familiar 329 | $$ 330 | \bar P(0)=S=P(t_f)\, . 331 | $$ 332 | As only system and reference trajectory information are required, the IVP can be solved offline. 333 | Values for the feedback matrix 334 | $$ 335 | K(t) = R^{-1} B(t)^T P(t) 336 | $$ 337 | are stored for the fixed time steps and then used later in the closed loop. 338 | Simulation of~\eqref{eq:riccati_new_ivp} can be performed with a numerical integration algorithm of choice, a simple fixed step width Euler algorithm was used here. 339 | \luainputlisting{../sim/01_lqr.py}{riccatiint} 340 | The main difficulty is keeping the array indices corresponding to $t$ and $\tau$ straight, otherwise the code is structurally identical to the simulation of any dynamic system. 341 | 342 | Finally, the actual simulation of the closed loop happens in the main simulation loop. 343 | The method of numerical integration is identical here. 344 | \luainputlisting{../sim/01_lqr.py}{sim} 345 | Simulation results are shown in Figure~\ref{fig:ludyk_ltv}. 346 | 347 | \begin{figure}[ht] 348 | \centering 349 | \includegraphics[scale=1]{img/ludyk_ltv.pdf} 350 | \caption{Closed loop simulation of trajectory tracking, controller designed for linear time-variant system obtained from linearizing along reference trajectory} 351 | \label{fig:ludyk_ltv} 352 | \end{figure} 353 | 354 | Unfortunately the final state trajectory is visually indistinguishable from the previous result. 355 | Still, for less benevolent systems than this academic example this method is the safer option, as it is applicable whenever the feedforward control is good enough to not deviate from the reference too far. 356 | 357 | \FloatBarrier 358 | \section{Cart-Pole System} 359 | 360 | To demonstrate the broader applicability of this controller design, it will now be implemented for a second example. 361 | The chosen system is the cart-pole inverse pendulum seen in Figure~\ref{fig:pendulum_sketch}. 362 | 363 | \begin{figure}[ht] 364 | \centering 365 | \includegraphics[scale=1]{img/pendulum.pdf} 366 | \caption{Cart-pole inverse pendulum} 367 | \label{fig:pendulum_sketch} 368 | \end{figure} 369 | 370 | The state vector~$x$ consists of the cart position~$s$ and the angle~$\varphi$ as well as their first time derivatives, so 371 | $$ 372 | x = \begin{pmatrix}x_1\\x_2\\x_3\\x_4\end{pmatrix} := \begin{pmatrix}s\\\dot s\\\varphi\\\dot\varphi\end{pmatrix}\, , 373 | $$ 374 | while the input is the cart acceleration~$u = a$. 375 | The system equation already in state-space form then is: 376 | $$ 377 | \dot x = F(x, u) = \begin{pmatrix}x_2\\u\\x_4\\\frac{1}{l}(g \sin x_3+u\cos x_3)\end{pmatrix}\, . 378 | $$ 379 | From differentiation follows for the linearized system matrices 380 | $$ 381 | A(x^*, u^*) := \left.\frac{\partial F}{\partial x}\right\vert_{(x^*, u^*)}= \begin{pmatrix}0 & 1 & 0 & 0\\0 & 0 & 0 & 0\\0 & 0 & 0 & 1\\0 & 0 & \frac{1}{l}(g \cos x_3 - u \sin x_3) & 0\end{pmatrix} 382 | $$ 383 | and 384 | $$ 385 | B(x^*, u^*) := \left.\frac{\partial F}{\partial u}\right\vert_{(x^*, u^*)}= \begin{pmatrix}0\\1\\0\\\frac{\cos x_3}{l}\end{pmatrix}\, . 386 | $$ 387 | Converted to \py this system description becomes: 388 | \luainputlisting{../sim/01_lqr_pendulum.py}{defsystem} 389 | 390 | The task will be a swingup of the pendulum, meaning that it should move from the lower equilibrium~$x(0)=(0, 0, \pi, 0)$ to the upper equilibrium~$x(t_f) = (0, 0, 0, 0)$. 391 | How to plan trajectories like this is a separate topic that will not be discussed here. 392 | For now, assume that the necessary trajectory data is somehow known. 393 | 394 | The code trajectory contains a file \texttt{pendulum.csv} with this trajectory. 395 | The first column is a time vector with fixed step width of 10\,ms, the following four columns describe the state at each time step and the last column is the input signal. 396 | In \py this data can be loaded like this: 397 | \luainputlisting{../sim/01_lqr_pendulum.py}{loadcsv} 398 | All the other code doing the controller design and the simulation can be reused. 399 | 400 | Because the dynamics at the upper and lower position are obviously very different, it is to be expected that an LQR designed for time-invariant system approximation will struggle. 401 | If it assumes the system to be at the lower position it will not be able to stabilize it when it's actually at the upper one. 402 | Reversely, designing it for the upper position makes tracking during the swingup difficult. 403 | This last case was simulated in Figure~\ref{fig:pendulum_lti}. 404 | Even though the feedforward control was very precise, tracking does not work at all. 405 | 406 | \begin{figure}[ht] 407 | \centering 408 | \includegraphics[scale=1]{img/pendulum_lti.pdf} 409 | \caption{Pendulum swingup, LQR designed for linear time-invariant system at the upper equilibrium} 410 | \label{fig:pendulum_lti} 411 | \end{figure} 412 | 413 | In the previous example, this issues could be alleviated with the workaround of retuning the controller at every time step. 414 | Figure~\ref{fig:pendulum_pseudoltv} shows how this can also fail when the system is slightly more involved. 415 | Even though the pendulum arm reaches the upper position and stays there, the cart position starts drifting off halfway through the trajectory. 416 | 417 | \begin{figure}[ht] 418 | \centering 419 | \includegraphics[scale=1]{img/pendulum_pseudoltv.pdf} 420 | \caption{Pendulum swingup, LQR is retuned at every time step for the current desired state} 421 | \label{fig:pendulum_pseudoltv} 422 | \end{figure} 423 | 424 | Finally, an LQR properly designed for a linear time-variant system approximation was used in Figure~\ref{fig:pendulum_ltv}. 425 | At least in simulation tracking works with issues. 426 | Both during swingup and while remaining at the upper equilibrium the state follows the reference well. 427 | Furthermore the difference of the feedback matrix~$K(t)$ between this version and the previous ad-hoc solution now becomes obvious. 428 | 429 | \begin{figure}[ht] 430 | \centering 431 | \includegraphics[scale=1]{img/pendulum_ltv.pdf} 432 | \caption{Pendulum swingup, LQR designed for linear time-variant system approximation} 433 | \label{fig:pendulum_ltv} 434 | \end{figure} 435 | 436 | \printglossaries 437 | %\printbibliography 438 | 439 | \end{document} 440 | 441 | %%% Local Variables: 442 | %%% mode: latex 443 | %%% TeX-master: t 444 | %%% End: --------------------------------------------------------------------------------