├── .github └── workflows │ └── build.yml ├── .gitignore ├── .gitmodules ├── COPYING ├── LICENSE ├── MANIFEST.in ├── README.md ├── docs ├── Makefile ├── source │ ├── conf.py │ └── index.rst └── tunempc_paper.pdf ├── examples ├── __init__.py ├── awe_system │ ├── __init__.py │ ├── main.py │ ├── main_simulation.py │ ├── point_mass_model.py │ ├── prepare_inputs.py │ └── user_input.pkl ├── convex_lqr.py ├── cstr │ ├── __init__.py │ ├── cstr_model.py │ └── main.py ├── evaporation_process │ ├── __init__.py │ └── main.py └── unicycle │ ├── __init__.py │ └── main.py ├── external └── install_acados.sh ├── requirements.txt ├── setup.py ├── test └── test_processing.py └── tunempc ├── __init__.py ├── closed_loop_tools.py ├── convert.m ├── convexifier.py ├── logger.py ├── logging.conf ├── mtools.py ├── pmpc.py ├── pocp.py ├── preprocessing.py ├── sqp_method.py └── tuner.py /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | # This workflow will install Python dependencies, run tests and lint with a single version of Python 2 | # For more information see: https://help.github.com/actions/language-and-framework-guides/using-python-with-github-actions 3 | 4 | name: Build 5 | 6 | on: 7 | push: 8 | branches: [ master, develop ] 9 | pull_request: 10 | branches: [ master, develop ] 11 | 12 | jobs: 13 | build: 14 | 15 | runs-on: ubuntu-latest 16 | 17 | steps: 18 | - uses: actions/checkout@v2 19 | - name: Set up Python 3.6 20 | uses: actions/setup-python@v1 21 | with: 22 | python-version: 3.6 23 | - name: Install dependencies 24 | run: | 25 | python3 -m pip install --upgrade pip 26 | pip3 install -r requirements.txt 27 | - name: Install package 28 | run: | 29 | python3 setup.py install 30 | - name: Lint with flake8 31 | run: | 32 | pip3 install flake8 33 | # stop the build if there are Python syntax errors or undefined names 34 | flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics 35 | # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide 36 | flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics 37 | - name: Test with pytest 38 | run: | 39 | pip3 install pytest 40 | pytest 41 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .eggs/ 2 | dist/ 3 | tunempc.egg-info/ 4 | 5 | # python 6 | *.pyc 7 | 8 | # output files 9 | *.jpg 10 | *.png 11 | *.eps 12 | *.csv 13 | *.xml 14 | *.log 15 | *.p 16 | .idea/ 17 | *.c 18 | .pytest_cache/ 19 | build/ 20 | .history/ 21 | *.rst 22 | *.pkl 23 | .vscode/ 24 | 25 | # tex 26 | *.aux 27 | *.fdb_latexmk 28 | *.fls 29 | *.out 30 | *.nav 31 | *.snm 32 | *.toc 33 | *.pdf 34 | *.pytxcode 35 | *.gz 36 | figures/ 37 | *.pytxmcr 38 | *.pytxpyg 39 | *.auxlock 40 | *.bbl 41 | *.blg 42 | *.dvi 43 | 44 | # acados 45 | *.json 46 | c_generated_code/ 47 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "external/acados"] 2 | path = external/acados 3 | url = https://github.com/jdeschut/acados 4 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include tunempc/logging.conf 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TuneMPC 2 | 3 | ![Build](https://github.com/jdeschut/tunempc/workflows/Build/badge.svg) 4 | [![License: LGPL v3](https://img.shields.io/badge/License-LGPL%20v3-blue.svg)](https://www.gnu.org/licenses/lgpl-3.0) 5 | 6 | **TuneMPC** is a Python package for economic tuning of nonlinear model predictive control (NMPC) problems. 7 | 8 | More precisely, it implements a formal procedure that tunes a tracking (N)MPC scheme so that it is locally first-order equivalent to economic NMPC. 9 | For user-provided system dynamics, constraints and economic objective, **TuneMPC** enables automated computation of optimal steady states and periodic trajectories, and spits out corresponding tuned stage cost matrices. 10 | 11 | ## Installation 12 | 13 | **TuneMPC** requires Python 3.5/3.6/3.7. 14 | 15 | 1. Get a local copy of `tunempc`: 16 | 17 | ``` 18 | git clone https://github.com/jdeschut/tunempc.git 19 | ``` 20 | 21 | 2. Install the package with dependencies: 22 | 23 | ``` 24 | pip3 install /tunempc 25 | ``` 26 | 27 | ## Installation (optional) 28 | 29 | The following features enable additional features and enhance reliability and performance. 30 | 31 | 1. Install MOSEK SDP solver (free [academic licenses](https://www.mosek.com/products/academic-licenses/) available) 32 | 33 | ``` 34 | pip3 install -f https://download.mosek.com/stable/wheel/index.html Mosek==9.0.98 35 | ``` 36 | 37 | 2. Install the `acados` software package for generating fast and embedded TuneMPC solvers. 38 | 39 | ``` 40 | git submodule update --init --recursive 41 | cd external/acados 42 | mkdir build && cd build 43 | cmake -DACADOS_WITH_QPOASES=ON .. 44 | make install -j4 && cd .. 45 | pip3 install interfaces/acados_template 46 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"/external/acados/lib" 47 | ``` 48 | 49 | More detailed instructions can be found [here](https://github.com/jdeschut/acados/blob/master/interfaces/acados_template/README.md). 50 | 51 | 3. It is recommended to use HSL linear solvers as a plugin with IPOPT. 52 | In order to get the HSL solvers and render them visible to CasADi, follow these [instructions](https://github.com/casadi/casadi/wiki/Obtaining-HSL). 53 | 54 | ## Using TuneMPC 55 | 56 | The interface of **TuneMPC** is based on the symbolic modeling framework [CasADi](https://web.casadi.org/). 57 | Input functions should be given as CasADi `Function` objects. 58 | 59 | User-defined inputs: 60 | 61 | ```python 62 | import casadi as ca 63 | 64 | x = ca.MX.sym('x',) # states 65 | u = ca.MX.sym('u',) # controls 66 | 67 | f = ca.Function('f',[x,u],[],['x','u'],['xf']) # discrete system dynamics 68 | l = ca.Function('l',[x,u],[]) # economic stage cost 69 | h = ca.Function('h',[x,u],[]) # constraints >= 0 70 | 71 | p = # choose p=1 for steady-state 72 | ``` 73 | 74 | Basic application of **TuneMPC** with a few lines of code: 75 | 76 | ```python 77 | import tunempc 78 | 79 | tuner = tunempc.Tuner(f, l, h, p) # construct optimal control problem 80 | w_opt = tuner.solve_ocp() # compute optimal p-periodic trajectory 81 | [H, q] = tuner.convexify() # compute economically tuned stage cost matrices 82 | ``` 83 | 84 | Create a tracking MPC controller (terminal point constraint) based on the tuned stage cost matrices: 85 | 86 | ```python 87 | ctrl = tuner.create_mpc('tuned', N = ) 88 | u0 = ctrl.step(x0) # compute feedback law 89 | ``` 90 | 91 | (optional) Code-generate a fast and embeddable solver for this MPC controller using `acados` (see [installation instructions](#installation)): 92 | ```python 93 | ode = # ode or dae expression 94 | acados_ocp_solver, _ = ctrl.generate(ode) # generate solver 95 | u0 = ctrl.step_acados(x0) # solver can be called through python interface 96 | ``` 97 | 98 | For a more complete overview of the available functionality, have a look at the different application [examples](https://github.com/jdeschut/tunempc/tree/master/examples). 99 | 100 | ## Acknowledgments 101 | 102 | This project has received funding by DFG via Research Unit FOR 2401 and by an industrial project with the company [Kiteswarms Ltd](http://www.kiteswarms.com). 103 | 104 | ## Literature 105 | 106 | ### Software 107 | 108 | [TuneMPC - A Tool for Economic Tuning of Tracking (N)MPC Problems](https://github.com/jdeschut/tunempc/blob/master/docs/tunempc_paper.pdf) \ 109 | J. De Schutter, M. Zanon, M. Diehl \ 110 | IEEE Control Systems Letters 2020 111 | 112 | ### Theory 113 | 114 | [A Periodic Tracking MPC that is Locally Equivalent to Periodic Economic MPC](https://www.sciencedirect.com/science/article/pii/S2405896317328987) \ 115 | M. Zanon, S. Gros, M. Diehl \ 116 | IFAC 2017 World Congress 117 | 118 | [A tracking MPC formulation that is locally equivalent to economic MPC](https://cdn.syscop.de/publications/Zanon2016.pdf) \ 119 | M. Zanon, S. Gros, M. Diehl \ 120 | Journal of Process Control 2016 121 | 122 | ### CasADi 123 | 124 | [CasADi - A software framework for nonlinear optimization and optimal control](http://www.optimization-online.org/DB_FILE/2018/01/6420.pdf) \ 125 | J.A.E. Andersson, J. Gillis, G. Horn, J.B. Rawlings, M. Diehl \ 126 | Mathematical Programming Computation, 2018 127 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = build 9 | 10 | # User-friendly check for sphinx-build 11 | ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) 12 | $(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) 13 | endif 14 | 15 | # Internal variables. 16 | PAPEROPT_a4 = -D latex_paper_size=a4 17 | PAPEROPT_letter = -D latex_paper_size=letter 18 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source 19 | # the i18n builder cannot share the environment and doctrees with the others 20 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source 21 | 22 | .PHONY: help 23 | help: 24 | @echo "Please use \`make ' where is one of" 25 | @echo " html to make standalone HTML files" 26 | @echo " dirhtml to make HTML files named index.html in directories" 27 | @echo " singlehtml to make a single large HTML file" 28 | @echo " pickle to make pickle files" 29 | @echo " json to make JSON files" 30 | @echo " htmlhelp to make HTML files and a HTML help project" 31 | @echo " qthelp to make HTML files and a qthelp project" 32 | @echo " applehelp to make an Apple Help Book" 33 | @echo " devhelp to make HTML files and a Devhelp project" 34 | @echo " epub to make an epub" 35 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 36 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 37 | @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" 38 | @echo " text to make text files" 39 | @echo " man to make manual pages" 40 | @echo " texinfo to make Texinfo files" 41 | @echo " info to make Texinfo files and run them through makeinfo" 42 | @echo " gettext to make PO message catalogs" 43 | @echo " changes to make an overview of all changed/added/deprecated items" 44 | @echo " xml to make Docutils-native XML files" 45 | @echo " pseudoxml to make pseudoxml-XML files for display purposes" 46 | @echo " linkcheck to check all external links for integrity" 47 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 48 | @echo " coverage to run coverage check of the documentation (if enabled)" 49 | 50 | .PHONY: clean 51 | clean: 52 | rm -rf $(BUILDDIR)/* 53 | 54 | .PHONY: html 55 | html: 56 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 57 | @echo 58 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 59 | 60 | .PHONY: dirhtml 61 | dirhtml: 62 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 63 | @echo 64 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 65 | 66 | .PHONY: singlehtml 67 | singlehtml: 68 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 69 | @echo 70 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 71 | 72 | .PHONY: pickle 73 | pickle: 74 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 75 | @echo 76 | @echo "Build finished; now you can process the pickle files." 77 | 78 | .PHONY: json 79 | json: 80 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 81 | @echo 82 | @echo "Build finished; now you can process the JSON files." 83 | 84 | .PHONY: htmlhelp 85 | htmlhelp: 86 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 87 | @echo 88 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 89 | ".hhp project file in $(BUILDDIR)/htmlhelp." 90 | 91 | .PHONY: qthelp 92 | qthelp: 93 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 94 | @echo 95 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 96 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 97 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/tnmpc.qhcp" 98 | @echo "To view the help file:" 99 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/tnmpc.qhc" 100 | 101 | .PHONY: applehelp 102 | applehelp: 103 | $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp 104 | @echo 105 | @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." 106 | @echo "N.B. You won't be able to view it unless you put it in" \ 107 | "~/Library/Documentation/Help or install it in your application" \ 108 | "bundle." 109 | 110 | .PHONY: devhelp 111 | devhelp: 112 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 113 | @echo 114 | @echo "Build finished." 115 | @echo "To view the help file:" 116 | @echo "# mkdir -p $$HOME/.local/share/devhelp/tnmpc" 117 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/tnmpc" 118 | @echo "# devhelp" 119 | 120 | .PHONY: epub 121 | epub: 122 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 123 | @echo 124 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 125 | 126 | .PHONY: latex 127 | latex: 128 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 129 | @echo 130 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 131 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 132 | "(use \`make latexpdf' here to do that automatically)." 133 | 134 | .PHONY: latexpdf 135 | latexpdf: 136 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 137 | @echo "Running LaTeX files through pdflatex..." 138 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 139 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 140 | 141 | .PHONY: latexpdfja 142 | latexpdfja: 143 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 144 | @echo "Running LaTeX files through platex and dvipdfmx..." 145 | $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja 146 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 147 | 148 | .PHONY: text 149 | text: 150 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 151 | @echo 152 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 153 | 154 | .PHONY: man 155 | man: 156 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 157 | @echo 158 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 159 | 160 | .PHONY: texinfo 161 | texinfo: 162 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 163 | @echo 164 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 165 | @echo "Run \`make' in that directory to run these through makeinfo" \ 166 | "(use \`make info' here to do that automatically)." 167 | 168 | .PHONY: info 169 | info: 170 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 171 | @echo "Running Texinfo files through makeinfo..." 172 | make -C $(BUILDDIR)/texinfo info 173 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 174 | 175 | .PHONY: gettext 176 | gettext: 177 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 178 | @echo 179 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 180 | 181 | .PHONY: changes 182 | changes: 183 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 184 | @echo 185 | @echo "The overview file is in $(BUILDDIR)/changes." 186 | 187 | .PHONY: linkcheck 188 | linkcheck: 189 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 190 | @echo 191 | @echo "Link check complete; look for any errors in the above output " \ 192 | "or in $(BUILDDIR)/linkcheck/output.txt." 193 | 194 | .PHONY: doctest 195 | doctest: 196 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 197 | @echo "Testing of doctests in the sources finished, look at the " \ 198 | "results in $(BUILDDIR)/doctest/output.txt." 199 | 200 | .PHONY: coverage 201 | coverage: 202 | $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage 203 | @echo "Testing of coverage in the sources finished, look at the " \ 204 | "results in $(BUILDDIR)/coverage/python.txt." 205 | 206 | .PHONY: xml 207 | xml: 208 | $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml 209 | @echo 210 | @echo "Build finished. The XML files are in $(BUILDDIR)/xml." 211 | 212 | .PHONY: pseudoxml 213 | pseudoxml: 214 | $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml 215 | @echo 216 | @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." 217 | -------------------------------------------------------------------------------- /docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # tunempc documentation build configuration file, created by 4 | # sphinx-quickstart on Sat Oct 27 21:09:35 2018. 5 | # 6 | # This file is execfile()d with the current directory set to its 7 | # containing dir. 8 | # 9 | # Note that not all possible configuration values are present in this 10 | # autogenerated file. 11 | # 12 | # All configuration values have a default; values that are commented out 13 | # serve to show the default. 14 | 15 | import sys 16 | import os 17 | 18 | # If extensions (or modules to document with autodoc) are in another directory, 19 | # add these directories to sys.path here. If the directory is relative to the 20 | # documentation root, use os.path.abspath to make it absolute, like shown here. 21 | #sys.path.insert(0, os.path.abspath('.')) 22 | 23 | # -- General configuration ------------------------------------------------ 24 | 25 | # If your documentation needs a minimal Sphinx version, state it here. 26 | #needs_sphinx = '1.0' 27 | 28 | # Add any Sphinx extension module names here, as strings. They can be 29 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 30 | # ones. 31 | extensions = [ 32 | 'sphinx.ext.autodoc', 33 | ] 34 | 35 | # Add any paths that contain templates here, relative to this directory. 36 | templates_path = ['_templates'] 37 | 38 | # The suffix(es) of source filenames. 39 | # You can specify multiple suffix as a list of string: 40 | # source_suffix = ['.rst', '.md'] 41 | source_suffix = '.rst' 42 | 43 | # The encoding of source files. 44 | #source_encoding = 'utf-8-sig' 45 | 46 | # The master toctree document. 47 | master_doc = 'index' 48 | 49 | # General information about the project. 50 | project = u'tunempc' 51 | copyright = u'2018, Jochem De Schutter' 52 | author = u'Jochem De Schutter' 53 | 54 | # The version info for the project you're documenting, acts as replacement for 55 | # |version| and |release|, also used in various other places throughout the 56 | # built documents. 57 | # 58 | # The short X.Y version. 59 | version = u'0.1.0' 60 | # The full version, including alpha/beta/rc tags. 61 | release = u'0.1.0' 62 | 63 | # The language for content autogenerated by Sphinx. Refer to documentation 64 | # for a list of supported languages. 65 | # 66 | # This is also used if you do content translation via gettext catalogs. 67 | # Usually you set "language" from the command line for these cases. 68 | language = None 69 | 70 | # There are two options for replacing |today|: either, you set today to some 71 | # non-false value, then it is used: 72 | #today = '' 73 | # Else, today_fmt is used as the format for a strftime call. 74 | #today_fmt = '%B %d, %Y' 75 | 76 | # List of patterns, relative to source directory, that match files and 77 | # directories to ignore when looking for source files. 78 | exclude_patterns = [] 79 | 80 | # The reST default role (used for this markup: `text`) to use for all 81 | # documents. 82 | #default_role = None 83 | 84 | # If true, '()' will be appended to :func: etc. cross-reference text. 85 | #add_function_parentheses = True 86 | 87 | # If true, the current module name will be prepended to all description 88 | # unit titles (such as .. function::). 89 | #add_module_names = True 90 | 91 | # If true, sectionauthor and moduleauthor directives will be shown in the 92 | # output. They are ignored by default. 93 | #show_authors = False 94 | 95 | # The name of the Pygments (syntax highlighting) style to use. 96 | pygments_style = 'sphinx' 97 | 98 | # A list of ignored prefixes for module index sorting. 99 | #modindex_common_prefix = [] 100 | 101 | # If true, keep warnings as "system message" paragraphs in the built documents. 102 | #keep_warnings = False 103 | 104 | # If true, `todo` and `todoList` produce output, else they produce nothing. 105 | todo_include_todos = False 106 | 107 | 108 | # -- Options for HTML output ---------------------------------------------- 109 | 110 | # The theme to use for HTML and HTML Help pages. See the documentation for 111 | # a list of builtin themes. 112 | html_theme = 'classic' 113 | 114 | # Theme options are theme-specific and customize the look and feel of a theme 115 | # further. For a list of options available for each theme, see the 116 | # documentation. 117 | #html_theme_options = {} 118 | 119 | # Add any paths that contain custom themes here, relative to this directory. 120 | #html_theme_path = [] 121 | 122 | # The name for this set of Sphinx documents. If None, it defaults to 123 | # " v documentation". 124 | #html_title = None 125 | 126 | # A shorter title for the navigation bar. Default is the same as html_title. 127 | #html_short_title = None 128 | 129 | # The name of an image file (relative to this directory) to place at the top 130 | # of the sidebar. 131 | #html_logo = None 132 | 133 | # The name of an image file (relative to this directory) to use as a favicon of 134 | # the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 135 | # pixels large. 136 | #html_favicon = None 137 | 138 | # Add any paths that contain custom static files (such as style sheets) here, 139 | # relative to this directory. They are copied after the builtin static files, 140 | # so a file named "default.css" will overwrite the builtin "default.css". 141 | html_static_path = ['_static'] 142 | 143 | # Add any extra paths that contain custom files (such as robots.txt or 144 | # .htaccess) here, relative to this directory. These files are copied 145 | # directly to the root of the documentation. 146 | #html_extra_path = [] 147 | 148 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 149 | # using the given strftime format. 150 | #html_last_updated_fmt = '%b %d, %Y' 151 | 152 | # If true, SmartyPants will be used to convert quotes and dashes to 153 | # typographically correct entities. 154 | #html_use_smartypants = True 155 | 156 | # Custom sidebar templates, maps document names to template names. 157 | #html_sidebars = {} 158 | 159 | # Additional templates that should be rendered to pages, maps page names to 160 | # template names. 161 | #html_additional_pages = {} 162 | 163 | # If false, no module index is generated. 164 | #html_domain_indices = True 165 | 166 | # If false, no index is generated. 167 | #html_use_index = True 168 | 169 | # If true, the index is split into individual pages for each letter. 170 | #html_split_index = False 171 | 172 | # If true, links to the reST sources are added to the pages. 173 | #html_show_sourcelink = True 174 | 175 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 176 | #html_show_sphinx = True 177 | 178 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 179 | #html_show_copyright = True 180 | 181 | # If true, an OpenSearch description file will be output, and all pages will 182 | # contain a tag referring to it. The value of this option must be the 183 | # base URL from which the finished HTML is served. 184 | #html_use_opensearch = '' 185 | 186 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 187 | #html_file_suffix = None 188 | 189 | # Language to be used for generating the HTML full-text search index. 190 | # Sphinx supports the following languages: 191 | # 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' 192 | # 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' 193 | #html_search_language = 'en' 194 | 195 | # A dictionary with options for the search language support, empty by default. 196 | # Now only 'ja' uses this config value 197 | #html_search_options = {'type': 'default'} 198 | 199 | # The name of a javascript file (relative to the configuration directory) that 200 | # implements a search results scorer. If empty, the default will be used. 201 | #html_search_scorer = 'scorer.js' 202 | 203 | # Output file base name for HTML help builder. 204 | htmlhelp_basename = 'tunempcdoc' 205 | 206 | # -- Options for LaTeX output --------------------------------------------- 207 | 208 | latex_elements = { 209 | # The paper size ('letterpaper' or 'a4paper'). 210 | #'papersize': 'letterpaper', 211 | 212 | # The font size ('10pt', '11pt' or '12pt'). 213 | #'pointsize': '10pt', 214 | 215 | # Additional stuff for the LaTeX preamble. 216 | #'preamble': '', 217 | 218 | # Latex figure (float) alignment 219 | #'figure_align': 'htbp', 220 | } 221 | 222 | # Grouping the document tree into LaTeX files. List of tuples 223 | # (source start file, target name, title, 224 | # author, documentclass [howto, manual, or own class]). 225 | latex_documents = [ 226 | (master_doc, 'tunempc.tex', u'tunempc Documentation', 227 | u'Jochem De Schutter', 'manual'), 228 | ] 229 | 230 | # The name of an image file (relative to this directory) to place at the top of 231 | # the title page. 232 | #latex_logo = None 233 | 234 | # For "manual" documents, if this is true, then toplevel headings are parts, 235 | # not chapters. 236 | #latex_use_parts = False 237 | 238 | # If true, show page references after internal links. 239 | #latex_show_pagerefs = False 240 | 241 | # If true, show URL addresses after external links. 242 | #latex_show_urls = False 243 | 244 | # Documents to append as an appendix to all manuals. 245 | #latex_appendices = [] 246 | 247 | # If false, no module index is generated. 248 | #latex_domain_indices = True 249 | 250 | 251 | # -- Options for manual page output --------------------------------------- 252 | 253 | # One entry per manual page. List of tuples 254 | # (source start file, name, description, authors, manual section). 255 | man_pages = [ 256 | (master_doc, 'tunempc', u'tunempc Documentation', 257 | [author], 1) 258 | ] 259 | 260 | # If true, show URL addresses after external links. 261 | #man_show_urls = False 262 | 263 | 264 | # -- Options for Texinfo output ------------------------------------------- 265 | 266 | # Grouping the document tree into Texinfo files. List of tuples 267 | # (source start file, target name, title, author, 268 | # dir menu entry, description, category) 269 | texinfo_documents = [ 270 | (master_doc, 'tunempc', u'tunempc Documentation', 271 | author, 'tunempc', 'One line description of project.', 272 | 'Miscellaneous'), 273 | ] 274 | 275 | # Documents to append as an appendix to all manuals. 276 | #texinfo_appendices = [] 277 | 278 | # If false, no module index is generated. 279 | #texinfo_domain_indices = True 280 | 281 | # How to display URL addresses: 'footnote', 'no', or 'inline'. 282 | #texinfo_show_urls = 'footnote' 283 | 284 | # If true, do not generate a @detailmenu in the "Top" node's menu. 285 | #texinfo_no_detailmenu = False 286 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. tnmpc documentation master file, created by 2 | sphinx-quickstart on Sat Oct 27 21:09:35 2018. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to tnmpc's documentation! 7 | ================================= 8 | 9 | Contents: 10 | 11 | .. toctree:: 12 | :maxdepth: 2 13 | 14 | modules 15 | 16 | 17 | 18 | Indices and tables 19 | ================== 20 | 21 | * :ref:`genindex` 22 | * :ref:`modindex` 23 | * :ref:`search` 24 | 25 | -------------------------------------------------------------------------------- /docs/tunempc_paper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jdeschut/tunempc/5be052cb503fa8c7f4d8845d0ef1b51d7535691c/docs/tunempc_paper.pdf -------------------------------------------------------------------------------- /examples/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | -------------------------------------------------------------------------------- /examples/awe_system/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jdeschut/tunempc/5be052cb503fa8c7f4d8845d0ef1b51d7535691c/examples/awe_system/__init__.py -------------------------------------------------------------------------------- /examples/awe_system/main.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """ Example of a periodic, single-aircraft drag-mode airborne wind energy system. 24 | 25 | The user inputs are generated by the file "prepare_inputs.py" 26 | 27 | Example description found in: 28 | 29 | TuneMPC - A Tool for Economic Tuning of Tracking (N)MPC Problems 30 | J. De Schutter, M. Zanon, M. Diehl 31 | (pending approval) 32 | 33 | :author: Jochem De Schutter 34 | 35 | """ 36 | 37 | import tunempc 38 | import pickle 39 | 40 | # load user input 41 | with open('user_input.pkl','rb') as f: 42 | user_input = pickle.load(f) 43 | 44 | # set-up tuning problem 45 | tuner = tunempc.Tuner( 46 | f = user_input['f'], 47 | l = user_input['l'], 48 | h = user_input['h'], 49 | p = user_input['p'] 50 | ) 51 | 52 | # solve OCP 53 | wsol = tuner.solve_ocp(w0 = user_input['w0']) 54 | 55 | # convexify stage cost matrices 56 | Hc = tuner.convexify(solver='mosek') 57 | S = tuner.S 58 | 59 | sys = tuner.sys 60 | sys['vars'] = { 61 | 'x': sys['vars']['x'].shape, 62 | 'u': sys['vars']['u'].shape, 63 | 'us': sys['vars']['us'].shape 64 | } 65 | 66 | sol = { 67 | 'S': S, 68 | 'wsol': wsol, 69 | 'lam_g': tuner.pocp.lam_g, 70 | 'indeces_As': tuner.pocp.indeces_As, 71 | 'sys': sys, 72 | } 73 | 74 | SAVE = True 75 | if SAVE: 76 | with open('convex_reference.pkl','wb') as f: 77 | pickle.dump(sol,f) 78 | -------------------------------------------------------------------------------- /examples/awe_system/main_simulation.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """ Example of a periodic, single-aircraft drag-mode airborne wind energy system. 24 | 25 | The user inputs are generated by the file "prepare_inputs.py" 26 | 27 | The reference trajectory and convexified sensitivities are computed in "main.py". 28 | 29 | Example description found in: 30 | 31 | TuneMPC - A Tool for Economic Tuning of Tracking (N)MPC Problems 32 | J. De Schutter, M. Zanon, M. Diehl 33 | IEEE Control Systems Letters 2020 34 | 35 | :author: Jochem De Schutter 36 | 37 | """ 38 | 39 | import tunempc.pmpc as pmpc 40 | import tunempc.preprocessing as preprocessing 41 | import tunempc.mtools as mtools 42 | import tunempc.closed_loop_tools as clt 43 | import casadi as ca 44 | import casadi.tools as ct 45 | import numpy as np 46 | import pickle 47 | import collections 48 | import copy 49 | import matplotlib.pyplot as plt 50 | 51 | # load user input 52 | with open('user_input.pkl','rb') as f: 53 | user_input = pickle.load(f) 54 | 55 | # load convexified reference 56 | with open('convex_reference.pkl','rb') as f: 57 | sol = pickle.load(f) 58 | 59 | # add variables to sys again 60 | vars = collections.OrderedDict() 61 | for var in ['x','u','us']: 62 | vars[var] = ca.MX.sym(var, sol['sys']['vars'][var]) 63 | sol['sys']['vars'] = vars 64 | nx = sol['sys']['vars']['x'].shape[0] 65 | nu = sol['sys']['vars']['u'].shape[0] 66 | ns = sol['sys']['vars']['us'].shape[0] 67 | 68 | # set-up open-loop scenario 69 | Nmpc = 20 70 | alpha_steps = 20 71 | TUNEMPC_SIM = True # simulate with TuneMPC built-in MPC controllers 72 | ACADOS_SIM = False # simulate with acados code-generated MPC solvers 73 | 74 | # tether length 75 | l_t = np.sqrt( 76 | sol['wsol']['x',0][0]**2 + 77 | sol['wsol']['x',0][1]**2 + 78 | sol['wsol']['x',0][2]**2 79 | ) 80 | 81 | opts = {} 82 | # add projection operator for terminal constraint 83 | opts['p_operator'] = ca.Function( 84 | 'p_operator', 85 | [sol['sys']['vars']['x']], 86 | [ct.vertcat(sol['sys']['vars']['x'][1:3], 87 | sol['sys']['vars']['x'][4:])] 88 | ) 89 | 90 | # add MPC slacks to active constraints 91 | mpc_sys = preprocessing.add_mpc_slacks( 92 | sol['sys'], 93 | sol['lam_g'], 94 | sol['indeces_As'], 95 | slack_flag = 'active' 96 | ) 97 | # create controllers 98 | ctrls = {} 99 | 100 | # # economic MPC 101 | tuning = {'H': sol['S']['Hc'], 'q': sol['S']['q']} 102 | ctrls['EMPC'] = pmpc.Pmpc( 103 | N = Nmpc, 104 | sys = mpc_sys, 105 | cost = user_input['l'], 106 | wref = sol['wsol'], 107 | lam_g_ref = sol['lam_g'], 108 | sensitivities= sol['S'], 109 | options = opts 110 | ) 111 | 112 | # prepare tracking cost and initialization 113 | tracking_cost = mtools.tracking_cost(nx+nu+ns) 114 | lam_g0 = copy.deepcopy(sol['lam_g']) 115 | lam_g0['dyn'] = 0.0 116 | lam_g0['g'] = 0.0 117 | 118 | # standard tracking MPC 119 | tuningTn = {'H': [np.diag((nx+nu)*[1]+ns*[1e-10])]*user_input['p'], 'q': sol['S']['q']} 120 | ctrls['TMPC_1'] = pmpc.Pmpc( 121 | N = Nmpc, 122 | sys = mpc_sys, 123 | cost = tracking_cost, 124 | wref = sol['wsol'], 125 | tuning = tuningTn, 126 | lam_g_ref = lam_g0, 127 | sensitivities= sol['S'], 128 | options = opts 129 | ) 130 | # manually tuned tracking MPC 131 | Ht2 = [np.diag([0.1,0.1,0.1, 1.0, 1.0, 1.0, 1.0e3, 1.0, 100.0, 1.0, 1.0, 1.0, 1e-10, 1e-10, 1e-10])]*user_input['p'] 132 | tuningTn2 = {'H': Ht2, 'q': sol['S']['q']} 133 | ctrls['TMPC_2'] = pmpc.Pmpc( 134 | N = Nmpc, 135 | sys = mpc_sys, 136 | cost = tracking_cost, 137 | wref = sol['wsol'], 138 | tuning = tuningTn2, 139 | lam_g_ref = lam_g0, 140 | sensitivities= sol['S'], 141 | options = opts 142 | ) 143 | 144 | # tuned tracking MPC 145 | ctrls['TUNEMPC'] = pmpc.Pmpc( 146 | N = Nmpc, 147 | sys = mpc_sys, 148 | cost = tracking_cost, 149 | wref = sol['wsol'], 150 | tuning = tuning, 151 | lam_g_ref = lam_g0, 152 | sensitivities= sol['S'], 153 | options = opts 154 | ) 155 | 156 | if ACADOS_SIM: 157 | 158 | # get system dae 159 | alg = user_input['dyn'] 160 | 161 | # solver options 162 | opts = {} 163 | opts['integrator_type'] = 'IRK' # ERK, IRK, GNSF 164 | opts['nlp_solver_type'] = 'SQP' # SQP_RTI 165 | # opts['qp_solver_cond_N'] = Nmpc # ??? 166 | opts['print_level'] = 1 167 | opts['sim_method_num_steps'] = 1 168 | opts['tf'] = Nmpc*user_input['ts'] 169 | opts['nlp_solver_max_iter'] = 200 170 | opts['nlp_solver_step_length'] = 0.3 171 | opts['nlp_solver_tol_comp'] = 1e-5 172 | opts['nlp_solver_tol_eq'] = 1e-5 173 | opts['nlp_solver_tol_ineq'] = 1e-5 174 | opts['nlp_solver_tol_stat'] = 1e-5 175 | 176 | ctrls_acados = {} 177 | for ctrl_key in list(ctrls.keys()): 178 | if ctrl_key == 'EMPC': 179 | opts['hessian_approx'] = 'GAUSS_NEWTON' 180 | opts['qp_solver'] = 'PARTIAL_CONDENSING_HPIPM' 181 | else: 182 | opts['hessian_approx'] = 'GAUSS_NEWTON' 183 | opts['qp_solver'] = 'PARTIAL_CONDENSING_HPIPM' # faster than full condensing 184 | 185 | _, _ = ctrls[ctrl_key].generate(alg, opts = opts, name = 'awe_'+ctrl_key) 186 | ctrls_acados[ctrl_key+'_ACADOS'] = ctrls[ctrl_key] 187 | 188 | # initialize and set-up open-loop simulation 189 | alpha = np.linspace(-1.0, 1.0, alpha_steps+1) # deviation sweep grid 190 | dz = 8 # max. deviation 191 | x0 = sol['wsol']['x',0] 192 | tgrid = [1/user_input['p']*i for i in range(Nmpc)] 193 | tgridx = tgrid + [tgrid[-1]+1/user_input['p']] 194 | 195 | # optimal stage cost and constraints for comparison 196 | lOpt, hOpt = [], [] 197 | for k in range(Nmpc): 198 | lOpt.append(user_input['l'](sol['wsol']['x', k%user_input['p']], sol['wsol']['u',k%user_input['p']]).full()[0][0]) 199 | hOpt.append(user_input['h'](sol['wsol']['x', k%user_input['p']], sol['wsol']['u',k%user_input['p']]).full()) 200 | 201 | # open loop simulation 202 | import copy 203 | log = [] 204 | log_acados = [] 205 | for alph in alpha: 206 | x_init = copy.deepcopy(x0) 207 | x_init[2] = x_init[2] + alph*dz 208 | x_init[0] = np.sqrt(-x_init[2]**2-x_init[1]**2+(l_t)**2) 209 | x_init[5] = -(x_init[0]*x_init[3] + x_init[1]*x_init[4]) / x_init[2] 210 | if TUNEMPC_SIM: 211 | log.append(clt.check_equivalence(ctrls, user_input['l'], user_input['h'], x0, x_init-x0, [1.0])[-1]) 212 | if ACADOS_SIM: 213 | log_acados.append(clt.check_equivalence( 214 | ctrls_acados, 215 | user_input['l'], 216 | user_input['h'], 217 | x0, 218 | x_init-x0, 219 | [1.0], 220 | flag = 'acados')[-1]) 221 | for name in list(ctrls.keys()): 222 | ctrls[name].reset() 223 | 224 | # plotting options 225 | alpha_plot = -1 226 | lw = 2 227 | ctrls_colors = { 228 | 'EMPC': 'blue', 229 | 'TUNEMPC': 'green', 230 | 'TMPC_1': 'red', 231 | 'TMPC_2': 'orange' 232 | } 233 | ctrls_lstyle = { 234 | 'EMPC': 'solid', 235 | 'TUNEMPC': 'dashed', 236 | 'TMPC_1': 'dashdot', 237 | 'TMPC_2': 'dotted' 238 | } 239 | ctrls_markers = { 240 | 'EMPC': '.', 241 | 'TUNEMPC': 'o', 242 | 'TMPC_1': '^', 243 | 'TMPC_2': 'x' 244 | } 245 | 246 | if ACADOS_SIM: 247 | for ctrl_key in list(ctrls_acados.keys()): 248 | ctrls_colors[ctrl_key] = ctrls_colors[ctrl_key[:-7]] 249 | ctrls_lstyle[ctrl_key] = ctrls_lstyle[ctrl_key[:-7]] 250 | ctrls_markers[ctrl_key] = ctrls_markers[ctrl_key[:-7]] 251 | 252 | ctrls_list = [] 253 | if TUNEMPC_SIM: 254 | ctrls_list += list(ctrls.keys()) 255 | if ACADOS_SIM: 256 | ctrls_list += list(ctrls_acados.keys()) 257 | 258 | # plot feedback equivalence 259 | plt.figure(1) 260 | for name in ctrls_list: 261 | if name not in ['EMPC', 'EMPC_ACADOS']: 262 | if name[-6:] == 'ACADOS': 263 | plot_log = log_acados 264 | EMPC_key = 'EMPC_ACADOS' 265 | else: 266 | plot_log = log 267 | EMPC_key = 'EMPC' 268 | feedback_norm = [ 269 | np.linalg.norm( 270 | np.divide( 271 | np.array(plot_log[k]['u'][name][0]) - np.array(plot_log[k]['u'][EMPC_key][0]), 272 | np.array(plot_log[0]['u'][EMPC_key][0])) 273 | ) for k in range(len(alpha))] 274 | plt.plot( 275 | [dz*alph for alph in alpha], 276 | feedback_norm, 277 | marker = ctrls_markers[name], 278 | color = ctrls_colors[name], 279 | linestyle = ctrls_lstyle[name], 280 | markersize=2, 281 | linewidth=lw 282 | ) 283 | plt.grid(True) 284 | plt.legend(ctrls_list[1:]) 285 | plt.title(r'$\Delta \pi_0^{\star}(\hat{x}_0) \ [-]$') 286 | plt.xlabel(r'$\Delta z \ \mathrm{[m]}$') 287 | 288 | # plot stage cost deviation over time 289 | plt.figure(2) 290 | for name in ctrls_list: 291 | if name[-6:] == 'ACADOS': 292 | plot_log = log_acados 293 | else: 294 | plot_log = log 295 | stage_cost_dev = [x[0] - x[1] for x in zip(plot_log[alpha_plot]['l'][name],lOpt)] 296 | plt.step( 297 | tgrid, 298 | stage_cost_dev, 299 | color = ctrls_colors[name], 300 | linestyle = ctrls_lstyle[name], 301 | linewidth=lw, 302 | where='post') 303 | plt.legend(ctrls_list) 304 | plt.grid(True) 305 | plt.xlabel('t - [s]') 306 | plt.title('Stage cost deviation') 307 | plt.autoscale(enable=True, axis='x', tight=True) 308 | 309 | # plot state deviation over time 310 | plt.subplots(nx,1,sharex = True) 311 | for i in range(nx): 312 | plt.subplot(nx,1,i+1) 313 | if i == 0: 314 | plt.title('State deviation') 315 | if i == nx: 316 | plt.xlabel('t - [s]') 317 | 318 | for name in ctrls_list: 319 | if name[-6:] == 'ACADOS': 320 | plot_log = log_acados 321 | else: 322 | plot_log = log 323 | plt.plot( 324 | tgridx, 325 | [plot_log[alpha_plot]['x'][name][j][i] - sol['wsol']['x',j][i] for j in range(Nmpc+1)], 326 | color = ctrls_colors[name], 327 | linestyle = ctrls_lstyle[name], 328 | linewidth=lw) 329 | plt.plot(tgridx, [0.0 for j in range(Nmpc+1)], linestyle='--', color='black') 330 | plt.autoscale(enable=True, axis='x', tight=True) 331 | plt.grid(True) 332 | 333 | # plot transient cost vs. alpha 334 | plt.figure(4) 335 | transient_cost = {} 336 | for name in ctrls_list: 337 | if name[-6:] == 'ACADOS': 338 | plot_log = log_acados 339 | else: 340 | plot_log = log 341 | transient_cost[name] = [] 342 | for i in range(len(alpha)): 343 | transient_cost[name].append( 344 | sum([x[0] - x[1] for x in zip(plot_log[i]['l'][name],lOpt)]) 345 | ) 346 | plt.plot( 347 | alpha, 348 | transient_cost[name], 349 | marker = ctrls_markers[name], 350 | color = ctrls_colors[name], 351 | linestyle = ctrls_lstyle[name], 352 | markersize = 2, 353 | linewidth=lw) 354 | plt.grid(True) 355 | plt.legend(ctrls_list) 356 | plt.title('Transient cost') 357 | plt.xlabel(r'$\alpha \ \mathrm{[-]}$') 358 | 359 | if ACADOS_SIM: 360 | 361 | # plot time per iteration 362 | plt.figure(5) 363 | for name in ctrls_list: 364 | if name[-6:] == 'ACADOS': 365 | plot_log = log_acados 366 | plt.plot( 367 | alpha, 368 | [plot_log[k]['log'][name]['time_tot'][0]/plot_log[k]['log'][name]['sqp_iter'][0] for k in range(len(alpha))], 369 | marker = ctrls_markers[name], 370 | color = ctrls_colors[name[:-7]], 371 | linestyle = ctrls_lstyle[name], 372 | markersize = 2, 373 | linewidth = 2 374 | ) 375 | plt.grid(True) 376 | plt.legend(list(plot_log[0]['log'].keys())) 377 | plt.title("Time per iteration") 378 | plt.xlabel('alpha [-]') 379 | plt.ylabel('t [s]') 380 | 381 | from statistics import mean 382 | mean_timings = {'time_lin': [], 'time_qp_xcond': [], 'time_qp': [],'time_tot': []} 383 | for name in ctrls_list: 384 | if name[-6:] == 'ACADOS': 385 | plot_log = log_acados 386 | for timing in list(mean_timings.keys()): 387 | mean_timings[timing].append( 388 | mean([plot_log[k]['log'][name][timing][0][0]/plot_log[k]['log'][name]['sqp_iter'][0][0] for k in range(len(alpha))]) 389 | ) 390 | from tabulate import tabulate 391 | print(tabulate([ 392 | ['time_tot']+mean_timings['time_tot'], 393 | ['time_lin']+mean_timings['time_lin'], 394 | ['time_qp']+mean_timings['time_qp'], 395 | ['time_qp_xcond']+mean_timings['time_qp_xcond']], 396 | headers=[' ']+list(ctrls.keys()))) 397 | 398 | plt.show() 399 | -------------------------------------------------------------------------------- /examples/awe_system/point_mass_model.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """ TBD 24 | 25 | :author: Jochem De Schutter 26 | 27 | """ 28 | 29 | import numpy as np 30 | from casadi.tools import vertcat 31 | 32 | def data_dict(): 33 | 34 | data_dict = {} 35 | data_dict['name'] = 'point_mass' 36 | 37 | data_dict['geometry'] = geometry() # kite geometry 38 | data_dict['aero_deriv'] = aero_deriv() # stability derivatives 39 | 40 | 41 | # (optional: on-board battery model) 42 | coeff_min = np.array([0, -80*np.pi/180.0]) 43 | coeff_max = np.array([2, 80*np.pi/180.0]) 44 | data_dict['battery'] = battery_model_parameters(coeff_max, coeff_min) 45 | 46 | return data_dict 47 | 48 | def geometry(): 49 | 50 | geometry = {} 51 | # 'aerodynamic parameter identification for an airborne wind energy pumping system', licitra, williams, gillis, ghandchi, sierbling, ruiterkamp, diehl, 2017 52 | # 'numerical optimal trajectory for system in pumping mode described by differential algebraic equation (focus on ap2)' licitra, 2014 53 | geometry['s_ref'] = 500.0 # [m^2] 54 | geometry['m_k'] = 20*geometry['s_ref'] # [kg] 55 | geometry['ar'] = 10.0 56 | geometry['c_ref'] = np.sqrt(geometry['s_ref']/geometry['ar']) # [m] 57 | geometry['b_ref'] = np.sqrt(geometry['s_ref']*geometry['ar']) # [m] 58 | 59 | # dirty fix to get correct CI 60 | geometry['ar'] = 50.0 61 | 62 | geometry['j'] = np.array([[25., 0.0, 0.47], 63 | [0.0, 32., 0.0], 64 | [0.47, 0.0, 56.]]) 65 | 66 | geometry['length'] = geometry['b_ref'] # only for plotting 67 | geometry['height'] = geometry['b_ref'] / 5. # only for plotting 68 | geometry['delta_max'] = np.array([20., 30., 30.]) * np.pi / 180. 69 | geometry['ddelta_max'] = np.array([2., 2., 2.]) 70 | 71 | geometry['c_root'] = 1.4 * geometry['c_ref'] 72 | geometry['c_tip'] = 2. * geometry['c_ref'] - geometry['c_root'] 73 | 74 | geometry['fuselage'] = True 75 | geometry['wing'] = True 76 | geometry['tail'] = True 77 | geometry['wing_profile'] = None 78 | 79 | # tether attachment point 80 | geometry['r_tether'] = np.zeros((3,1)) 81 | 82 | return geometry 83 | 84 | def aero_deriv(): 85 | # 'numerical optimal trajectory for system in pumping mode described by differential algebraic equation (focus on ap2)' licitra, 2014 86 | 87 | aero_deriv = {} 88 | 89 | aero_deriv['CD0'] = 0.02 90 | 91 | return aero_deriv 92 | 93 | def set_options(options): 94 | 95 | options['params']['tether']['sigma_max'] = 3.9e9 96 | options['params']['tether']['f_sigma'] = 5.0 97 | options['params']['tether']['rho'] = 1450.0 98 | 99 | options['user_options']['wind']['u_ref'] = 10.0 100 | 101 | options['model']['aero']['three_dof']['coeff_max'] = [1.0, 80.0 * np.pi / 180.] 102 | options['model']['aero']['three_dof']['coeff_min'] = [0.0, -80.0 * np.pi / 180.] 103 | 104 | options['model']['model_bounds']['dcoeff_max'] = [5., 5.0] 105 | options['model']['model_bounds']['dcoeff_min'] = [-5., -5.0] 106 | options['model']['system_bounds']['u']['dkappa'] = [-1000.0, 1000.0] 107 | 108 | 109 | return options 110 | 111 | def battery_model_parameters(coeff_max, coeff_min): 112 | 113 | battery_model = {} 114 | 115 | # guessed values for battery model 116 | battery_model['flap_length'] = 0.2 117 | battery_model['flap_width'] = 0.1 118 | battery_model['max_flap_defl'] = 20.*(np.pi/180.) 119 | battery_model['min_flap_defl'] = -20.*(np.pi/180.) 120 | battery_model['c_dl'] = (battery_model['max_flap_defl'] - battery_model['min_flap_defl'])/(coeff_min[0] - coeff_max[0]) 121 | battery_model['c_dphi'] = (battery_model['max_flap_defl'] - battery_model['min_flap_defl'])/(coeff_min[1] - coeff_max[1]) 122 | battery_model['defl_lift_0'] = battery_model['min_flap_defl'] - battery_model['c_dl']*coeff_max[0] 123 | battery_model['defl_roll_0'] = battery_model['min_flap_defl'] - battery_model['c_dphi']*coeff_max[1] 124 | battery_model['voltage'] = 3.7 125 | battery_model['mAh'] = 5000. 126 | battery_model['charge'] = battery_model['mAh']*3600.*1e-3 127 | battery_model['number_of_cells'] = 15. 128 | battery_model['conversion_efficiency'] = 0.7 129 | battery_model['power_controller'] = 50. 130 | battery_model['power_electronics'] = 10. 131 | battery_model['charge_fraction'] = 1. 132 | 133 | return battery_model -------------------------------------------------------------------------------- /examples/awe_system/prepare_inputs.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """ awebox optimization toolbox is available at: https://github.com/awebox/awebox 24 | commit hash: 561f1726f28357e04b2e6a1163a1b30753670784 25 | 26 | :author: Jochem De Schutter 27 | 28 | """ 29 | 30 | import awebox as awe 31 | import awebox.tools.integrator_routines as awe_integrators 32 | import matplotlib.pyplot as plt 33 | import numpy as np 34 | import casadi as ca 35 | import casadi.tools as ct 36 | import pickle 37 | 38 | def generate_kite_model_and_orbit(N): 39 | 40 | import point_mass_model 41 | 42 | # make default options object 43 | options = awe.Options(True) 44 | 45 | # single kite with point-mass model 46 | options['user_options']['system_model']['architecture'] = {1:0} 47 | options['user_options']['system_model']['kite_dof'] = 3 48 | options['user_options']['kite_standard'] = point_mass_model.data_dict() 49 | 50 | # trajectory should be a single pumping cycle with initial number of five windings 51 | options['user_options']['trajectory']['type'] = 'power_cycle' 52 | options['user_options']['trajectory']['system_type'] = 'drag_mode' 53 | options['user_options']['trajectory']['lift_mode']['windings'] = 1 54 | 55 | # don't include induction effects, use simple tether drag 56 | options['user_options']['induction_model'] = 'not_in_use' 57 | options['user_options']['tether_drag_model'] = 'trivial' 58 | options['nlp']['n_k'] = N 59 | 60 | # get point mass model data 61 | options = point_mass_model.set_options(options) 62 | 63 | # initialize and optimize trial 64 | trial = awe.Trial(options, 'single_kite_drag_mode') 65 | trial.build() 66 | trial.optimize(final_homotopy_step='final') 67 | # trial.plot(['states','controls', 'constraints']) 68 | # plt.show() 69 | 70 | # extract model data 71 | sol = {} 72 | sol['model'] = trial.generate_optimal_model() 73 | sol['l_t'] = trial.optimization.V_opt['xd',0,'l_t'] 74 | 75 | # initial guess 76 | w_init = [] 77 | for k in range(N): 78 | w_init.append(trial.optimization.V_opt['xd',k][:-3]) 79 | w_init.append(trial.optimization.V_opt['u', k][3:6]) 80 | sol['w0'] = ca.vertcat(*w_init) 81 | 82 | return sol 83 | 84 | 85 | # discrete period of interest 86 | N = 40 87 | awe_sol = generate_kite_model_and_orbit(N) 88 | 89 | # remove tether variables 90 | model = awe_sol['model'] 91 | l_t = awe_sol['l_t'] 92 | x_shape = model['dae']['x'].shape 93 | x_shape = (x_shape[0]-3, x_shape[1]) 94 | x = ca.MX.sym('x',*x_shape) 95 | x_awe = ct.vertcat(x, l_t, 0.0, 0.0) 96 | 97 | # remove fictitious forces, tether jerk... 98 | u_shape = model['dae']['p'].shape 99 | u_shape = (u_shape[0]-4, u_shape[1]) 100 | u = ca.MX.sym('u',*u_shape) 101 | u_awe = ct.vertcat(0.0,0.0,0.0,u,0.0) 102 | 103 | # remove algebraic variable 104 | z = model['rootfinder'](0.1, x_awe, u_awe) 105 | nx = x.shape[0] 106 | nu = u_shape[0] 107 | constraints = ca.vertcat( 108 | -model['constraints'](x_awe, u_awe,z), 109 | -model['var_bounds_fun'](x_awe, u_awe,z) 110 | ) 111 | 112 | # remove redundant constraints 113 | constraints_new = [] 114 | for i in range(constraints.shape[0]): 115 | if True in ca.which_depends(constraints[i],ca.vertcat(x,u)): 116 | constraints_new.append(constraints[i]) 117 | 118 | # create integrator 119 | integrator = awe_integrators.rk4root( 120 | 'F', 121 | model['dae'], 122 | model['rootfinder'], 123 | {'tf': 1/N, 'number_of_finite_elements':10}) 124 | xf = integrator(x0=x_awe, p=u_awe, z0 = 0.1)['xf'][:-3] 125 | qf = integrator(x0=x_awe, p=u_awe, z0 = 0.1)['qf'] 126 | 127 | sys = { 128 | 'f' : ca.Function('F',[x,u],[xf,qf],['x0','p'],['xf','qf']), 129 | 'h' : ca.Function('h', [x,u], [ca.vertcat(*constraints_new)]) 130 | } 131 | 132 | # cost function 133 | power_output = -sys['f'](x0=x, p=u)['qf']/model['t_f']/1e3 134 | regularization = 1/2*1e-4*ct.mtimes(u.T,u) 135 | 136 | cost = ca.Function( 137 | 'cost', 138 | [x,u], 139 | [power_output + regularization] #+ extra_regularization 140 | ) 141 | 142 | # initial guess 143 | w0 = awe_sol['w0'] 144 | 145 | # save time-continuous dynamics 146 | xdot = ca.MX.sym('xdot', x.shape[0]) 147 | xdot_awe = ct.vertcat(xdot, 0.0, 0.0, 0.0)# remove l, ldot, lddot 148 | z = ca.MX.sym('z', model['dae']['z']['xa'].shape[0]) 149 | indeces = [*range(2,10)]+[*range(11,nx+3+z.shape[0])] # remove ldot, lddot, ldddot 150 | alg = model['dae']['alg'][indeces] 151 | alg_fun = ca.Function('alg_fun',[model['dae']['x'],model['dae']['p'],model['dae']['z']],[alg]) 152 | dyn = ca.Function( 153 | 'dae', 154 | [xdot,x,u,z], 155 | [alg_fun(x_awe, u_awe, ct.vertcat(xdot_awe, z))], 156 | ['xdot','x','u','z'], 157 | ['dyn']) 158 | 159 | # save user input info 160 | with open('user_input.pkl','wb') as f: 161 | pickle.dump({ 162 | 'f': sys['f'], 163 | 'l': cost, 164 | 'h': sys['h'], 165 | 'p': N, 166 | 'w0': w0, 167 | 'dyn': dyn, 168 | 'ts': model['t_f']/N 169 | },f) 170 | -------------------------------------------------------------------------------- /examples/awe_system/user_input.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jdeschut/tunempc/5be052cb503fa8c7f4d8845d0ef1b51d7535691c/examples/awe_system/user_input.pkl -------------------------------------------------------------------------------- /examples/convex_lqr.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """Implementation of the LQR example as proposed in 24 | 25 | Zanon et al. 26 | Indefinite linear MPC and approximated economic MPC for nonlinearsystems, (section 6). 27 | Journal of Process Control 2014 28 | 29 | :author: Jochem De Schutter 30 | 31 | """ 32 | 33 | import control 34 | import tunempc.convexifier as convexifier 35 | import numpy as np 36 | import matplotlib.pyplot as plt 37 | 38 | 39 | # system data 40 | A = np.matrix('-0.3319 0.7595 1.5399; -0.3393 0.1250 0.4245; -0.5090 0.9388 0.8864') 41 | B = np.matrix('0.1060; -1.3835; -0.1496') 42 | 43 | # (indefinite) cost matrices 44 | Q = np.matrix('-1.0029 -0.0896 1.1050; -0.0896 1.6790 -0.5762; 1.1050 -0.5762 -0.4381') 45 | N = np.matrix('-0.0420; 0.2112; -0.2832') 46 | R = np.matrix('0.6192') 47 | 48 | # convexify cost matrices 49 | dHc, dQc, dRc, dNc = convexifier.convexify(A,B,Q,R,N) 50 | 51 | # stabilizing LQR with tuned weighting matrices 52 | Pc, Ec, Kc = control.mateqn.dare(A, B, Q + dQc[0], R + dRc[0], N + dNc[0], np.eye(A.shape[0])) 53 | 54 | # stabilizing LQR 55 | P, E, K = control.mateqn.dare(A, B, Q, R, N, np.eye(A.shape[0])) 56 | 57 | # check equivalence 58 | assert np.linalg.norm(K - Kc) < 1e-5, 'Feedback laws should be identical.' 59 | 60 | # define closed-loop systems 61 | sys0 = control.ss(A- np.matmul(B,K), B, np.eye(A.shape[0]), np.zeros((A.shape[0],B.shape[1])),1) 62 | sysc = control.ss(A-np.matmul(B,Kc), B, np.eye(A.shape[0]), np.zeros((A.shape[0],B.shape[1])),1) 63 | 64 | # check equivalence visually 65 | T, Yout = control.initial_response(sys0,T=range(30),X0=np.matrix('1;0;0')) 66 | T, Youtc = control.initial_response(sysc,T=range(30),X0=np.matrix('1;0;0')) 67 | plt.plot(T, Yout[0]) 68 | plt.plot(T, Youtc[0]) 69 | plt.show() 70 | -------------------------------------------------------------------------------- /examples/cstr/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jdeschut/tunempc/5be052cb503fa8c7f4d8845d0ef1b51d7535691c/examples/cstr/__init__.py -------------------------------------------------------------------------------- /examples/cstr/cstr_model.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """System dynamics, constraints and objective of CSTR benchmarking example found in 24 | Chen, H., Stability and Robustness Considerations in Nonlinear Model Predictive Control, 1997. 25 | 26 | :author: Jochem De Schutter 27 | 28 | """ 29 | 30 | import numpy as np 31 | import casadi as ca 32 | import casadi.tools as ct 33 | 34 | def problem_data(): 35 | 36 | """ Problem data, numeric constants,... 37 | """ 38 | 39 | # physics 40 | data = {} 41 | data['k10'] = 1.287e12 42 | data['k20'] = 1.287e12 43 | data['k30'] = 9.043e9 44 | data['E1'] = -9758.3 45 | data['E2'] = -9758.3 46 | data['E3'] = -8560.0 47 | data['DH_AB'] = 4.2 48 | data['DH_BC'] = -11.0 49 | data['DH_AD'] = -41.85 50 | data['rho'] = 0.9342 51 | data['Cp'] = 3.01 52 | data['kw'] = 4032.0 53 | data['AR'] = 0.215 54 | data['VR'] = 10.0 55 | data['mK'] = 5.0 56 | data['CPK'] = 2.0 57 | data['cA0'] = 5.10 58 | data['theta0'] = 104.9 59 | 60 | # tuning 61 | data['rhoJ'] = 0.0 # regularization weight 62 | data['num_steps'] = 20 # number_of_finite_elements in integrator 63 | data['ts'] = 20 # sampling time 64 | data['integrator'] = 'rk' # numerical integrator 65 | 66 | return data 67 | 68 | def arrhenius(x,u,data,i): 69 | 70 | k = data['k{}0'.format(i)]*np.exp( 71 | data['E{}'.format(i)]/(x['theta']+273.15) 72 | ) 73 | 74 | return k 75 | 76 | def dynamics(x, u, data): 77 | 78 | """ System dynamics function (discrete time) 79 | """ 80 | 81 | # state derivative expression 82 | xdot = ca.vertcat( 83 | u['Vdot']*(data['cA0'] - x['cA']) - arrhenius(x,u,data,1)*x['cA'] - arrhenius(x,u,data,3)*x['cA']*x['cA'], 84 | -u['Vdot']*x['cB'] + arrhenius(x,u,data,1)*x['cA'] - arrhenius(x,u,data,2)*x['cB'], 85 | u['Vdot']*(data['theta0']-x['theta']) - 1.0/(data['rho']*data['Cp'])*( 86 | arrhenius(x,u,data,1)*x['cA']*data['DH_AB'] + 87 | arrhenius(x,u,data,2)*x['cB']*data['DH_BC'] + 88 | arrhenius(x,u,data,3)*x['cA']*x['cA']*data['DH_AD'] 89 | ) + data['kw']*data['AR']/(data['rho']*data['Cp']*data['VR'])*(x['thetaK']- x['theta']), 90 | 1.0/(data['mK']*data['CPK'])*(u['QdotK']+data['kw']*data['AR']*(x['theta']-x['thetaK'])) 91 | ) 92 | 93 | # create ode for integrator 94 | ode = {'x':x, 'p':u,'ode': xdot/3600} 95 | 96 | # integrator 97 | F = ca.integrator('F',data['integrator'],ode,{'tf':data['ts'],'number_of_finite_elements':data['num_steps']}) 98 | 99 | return [F, ode] 100 | 101 | def vars(): 102 | 103 | """ System states and controls 104 | """ 105 | 106 | x = ct.struct_symMX(['cA','cB','theta', 'thetaK']) 107 | u = ct.struct_symMX(['Vdot','QdotK']) 108 | 109 | return x, u 110 | 111 | def objective(x, u, data): 112 | 113 | """ Economic objective function 114 | """ 115 | 116 | # cost definition 117 | e_obj = - x['cB']/data['cA0'] 118 | 119 | # regularization 120 | reg = data['rhoJ']*( 121 | 0*(x['cA'] - 2.1402)**2 + \ 122 | 0*(x['cB'] - 1.0903)**2 + \ 123 | 0*(x['theta'] - 114.191)**2 + \ 124 | 0*(x['thetaK'] - 112.9066 )**2 + \ 125 | 1e-4*(u['Vdot']-14.19)**2 + \ 126 | 1e-4*(u['QdotK']-(-1113.5))**2 127 | ) 128 | 129 | return ca.Function('economic_cost',[x,u],[100*(e_obj+reg)]), ca.Function('cost_comp', [x,u], [e_obj, reg]) 130 | 131 | 132 | def tracking_cost(nw): 133 | 134 | """ Tracking cost function 135 | """ 136 | 137 | # reference parameters 138 | w = ca.MX.sym('w', (nw, 1)) 139 | wref = ca.MX.sym('wref', (nw, 1)) 140 | H = ca.MX.sym('H', (nw, nw)) 141 | q = ca.MX.sym('H', (nw, 1)) 142 | 143 | # cost definition 144 | dw = w - wref 145 | obj = 0.5*ct.mtimes(dw.T, ct.mtimes(H, dw)) + ct.mtimes(q.T,dw) 146 | 147 | return ca.Function('tracking_cost',[w, wref, H, q],[obj]) 148 | 149 | def constraints(x, u, data): 150 | 151 | """ Path inequality constraints function (convention h(x,u) >= 0) 152 | """ 153 | 154 | constr = ca.vertcat( 155 | u['Vdot'] - 5, 156 | 35.0 - u['Vdot'], 157 | u['QdotK'] + 9000.0, 158 | -u['QdotK'] 159 | ) 160 | 161 | return ca.Function('h', [x,u], [constr]) 162 | 163 | def initial_guess(): 164 | 165 | """ Initial guess for economic steady state optimization 166 | """ 167 | 168 | return ca.vertcat(2.1402, 1.0903, 114.191, 112.9066, 14.19, -1113.5) -------------------------------------------------------------------------------- /examples/cstr/main.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """Implementation of tuned steady-state tracking NMPC for the CSTR 24 | benchmarking example found in: 25 | 26 | Chen, H. et al., 27 | Nonlinear Predictive Control of a Benchmark CSTR 28 | Proc. 3rd European Control Conference ECC’95, Rome/Italy 29 | 30 | :author: Jochem De Schutter 31 | 32 | """ 33 | 34 | import tunempc 35 | import tunempc.pmpc as pmpc 36 | import tunempc.closed_loop_tools as clt 37 | import cstr_model as cstr 38 | import numpy as np 39 | import casadi as ca 40 | import casadi.tools as ct 41 | import matplotlib.pyplot as plt 42 | from statistics import mean 43 | 44 | # set-up system 45 | x, u = cstr.vars() 46 | data = cstr.problem_data() 47 | h = cstr.constraints(x, u, data) 48 | nx = x.shape[0] 49 | nu = u.shape[0] 50 | 51 | # simulation parameters 52 | Ts = 20 # sampling time 53 | Nsim = 70 # closed loop num of simulation steps 54 | N = 60 # nmpc horizon length 55 | 56 | # integrator options 57 | data['rhoJ'] = 1e-1 # regularization 58 | data['num_steps'] = 20 # number_of_finite_elements in integrator 59 | data['ts'] = Ts # sampling time 60 | data['integrator'] = 'rk' # numerical integrator 61 | 62 | # cost function 63 | cost, cost_comp = cstr.objective(x,u,data) 64 | 65 | # solve steady state problem 66 | tuner = tunempc.Tuner( 67 | f = cstr.dynamics(x, u, data)[0], 68 | l = cost, 69 | h = h, 70 | p = 1 71 | ) 72 | wsol = tuner.solve_ocp(w0 = cstr.initial_guess()) 73 | lOpt = cost(wsol['x',0], wsol['u',0]) 74 | Hc = tuner.convexify() 75 | S = tuner.S 76 | 77 | # prediction time grid 78 | tgrid = [Ts*k for k in range(N)] 79 | tgridx = [Ts*k for k in range(N+1)] 80 | 81 | # create controllers 82 | ctrls = {} 83 | 84 | # economic mpc controller 85 | ctrls['economic'] = tuner.create_mpc('economic',N) 86 | 87 | # normal tracking mpc controller 88 | tuningTn = {'H': [np.diag([0.2, 1.0, 0.5, 0.2, 0.5, 0.5])], 'q': S['q']} 89 | ctrls['tracking'] = tuner.create_mpc('tracking', N, tuning=tuningTn) 90 | 91 | # tuned tracking mpc controller 92 | ctrls['tuned'] = tuner.create_mpc('tuned', N) 93 | 94 | ACADOS_CODEGENERATE = False 95 | if ACADOS_CODEGENERATE: 96 | 97 | # get system ode 98 | ode = ca.Function('ode',[x.cat,u.cat],[cstr.dynamics(x,u,data)[1]['ode']]) 99 | 100 | # solver options 101 | opts = {} 102 | opts['integrator_type'] = 'ERK' 103 | opts['nlp_solver_type'] = 'SQP' # SQP_RTI 104 | opts['qp_solver_cond_N'] = 1 # ??? 105 | opts['print_level'] = 0 106 | opts['sim_method_num_steps'] = data['num_steps'] 107 | opts['tf'] = N*data['ts'] 108 | opts['nlp_solver_max_iter'] = 300 109 | 110 | ctrls_acados = {} 111 | for ctrl_key in list(ctrls.keys()): 112 | if ctrl_key == 'economic': 113 | opts['hessian_approx'] = 'EXACT' 114 | opts['nlp_solver_step_length'] = 0.7 115 | opts['qp_solver'] = 'PARTIAL_CONDENSING_HPIPM' 116 | else: 117 | opts['hessian_approx'] = 'GAUSS_NEWTON' 118 | opts['qp_solver'] = 'FULL_CONDENSING_HPIPM' 119 | 120 | _, _ = ctrls[ctrl_key].generate(ode, opts = opts, name = ctrl_key+'_cstr') 121 | ctrls_acados[ctrl_key+'_acados'] = ctrls[ctrl_key] 122 | 123 | # check equivalence 124 | alpha = np.linspace(-0.1, 1.0, 10) 125 | x0 = wsol['x',0] 126 | dx_diehl = np.array([1.0,0.5, 100.0, 100.0]) - wsol['x',0] 127 | 128 | # compute open-loop prediction for list of deviations in different state directions 129 | log = [] 130 | log_acados = [] 131 | for dist in [0]: 132 | dx = np.zeros((nx,1)) 133 | dx[dist] = dx_diehl[dist] 134 | log.append(clt.check_equivalence(ctrls, cost, tuner.sys['h'], x0, dx, alpha)) 135 | 136 | if ACADOS_CODEGENERATE: 137 | log_acados.append(clt.check_equivalence(ctrls_acados, cost, tuner.sys['h'], x0, dx, alpha, flag='acados')) 138 | 139 | fig_num = 1 140 | alpha_plot = -1 # alpha value of interest 141 | dx_plot = 0 # state direction of interest 142 | ctrl_list = list(ctrls.keys()) 143 | if ACADOS_CODEGENERATE: 144 | ctrl_list += list(ctrls_acados.keys()) 145 | for name in ctrl_list: 146 | 147 | if name[-6:] != 'acados': 148 | logg = log[dx_plot][alpha_plot] 149 | ls = '-' 150 | else: 151 | logg = log_acados[dx_plot][alpha_plot] 152 | ls = '--' 153 | 154 | # plot controls 155 | plt.figure(fig_num) 156 | for i in range(nu): 157 | plt.subplot(nu,1,i+1) 158 | plt.step(tgrid, [logg['u'][name][j][i] for j in range(len(tgrid))], where='post', linestyle = ls) 159 | plt.grid(True) 160 | plt.autoscale(enable=True, axis='x', tight=True) 161 | 162 | # plot stage cost deviation 163 | plt.figure(fig_num +1) 164 | stage_cost_dev = [x[0] - x[1] for x in zip(logg['l'][name],N*[lOpt])] 165 | plt.step(tgrid, stage_cost_dev, where='post', linestyle = ls) 166 | 167 | # plot state prediction 168 | plt.figure(fig_num +2) 169 | for i in range(nx): 170 | plt.subplot(nx,1,i+1) 171 | plt.plot(tgridx, [logg['x'][name][j][i] for j in range(len(tgridx))], linestyle = ls) 172 | plt.plot( 173 | tgridx, 174 | [wsol['x',0][i] for j in range(len(tgridx))], 175 | linestyle='--', 176 | color='black', 177 | label='_nolegend_') 178 | plt.autoscale(enable=True, axis='x', tight=True) 179 | plt.grid(True) 180 | 181 | # adjust control plot 182 | plt.figure(fig_num) 183 | plt.legend(ctrl_list) 184 | plt.subplot(nu,1,1) 185 | plt.title('Feedback controls') 186 | plt.subplot(nu,1,nu) 187 | plt.xlabel('t [s]') 188 | 189 | # adjust stage cost plot 190 | plt.figure(fig_num+1) 191 | plt.legend(ctrl_list) 192 | plt.grid(True) 193 | plt.xlabel('t [s]') 194 | plt.title('Stage cost deviation') 195 | plt.autoscale(enable=True, axis='x', tight=True) 196 | 197 | # adjust state plot 198 | plt.figure(fig_num+2) 199 | plt.subplot(nx,1,1) 200 | plt.legend(ctrl_list) 201 | plt.title('State trajectories') 202 | plt.subplot(nx,1,nx) 203 | plt.xlabel('t [s]') 204 | 205 | fig_num += 3 206 | 207 | # plot transient cost vs. alpha 208 | plt.figure(fig_num) 209 | transient_cost = {} 210 | for name in ctrl_list: 211 | 212 | if name[-6:] != 'acados': 213 | logg = log[dx_plot] 214 | ls = '-' 215 | else: 216 | logg = log_acados[dx_plot] 217 | ls = '--' 218 | 219 | transient_cost[name] = [] 220 | for i in range(len(alpha)): 221 | transient_cost[name].append( 222 | sum([x[0] - x[1] for x in zip(logg[i]['l'][name],N*[lOpt])]) 223 | ) 224 | plt.plot(alpha, transient_cost[name], marker = 'o', linestyle = ls) 225 | 226 | plt.grid(True) 227 | plt.legend(ctrl_list) 228 | plt.title('Transient cost') 229 | plt.xlabel('alpha [-]') 230 | 231 | plt.show() -------------------------------------------------------------------------------- /examples/evaporation_process/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jdeschut/tunempc/5be052cb503fa8c7f4d8845d0ef1b51d7535691c/examples/evaporation_process/__init__.py -------------------------------------------------------------------------------- /examples/evaporation_process/main.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """Evaporation process example as described in: 24 | 25 | A tracking MPC formulation that is locally equivalent to economic MPC 26 | M. Zanon, S. Gros, M. Diehl 27 | Journal of Process Control 2016 28 | (section 8) 29 | 30 | :author: Jochem De Schutter 31 | 32 | """ 33 | 34 | import tunempc 35 | import tunempc.pmpc 36 | import tunempc.closed_loop_tools as clt 37 | import numpy as np 38 | import casadi as ca 39 | import casadi.tools as ct 40 | import matplotlib.pyplot as plt 41 | 42 | def problem_data(): 43 | 44 | """ Problem data, numeric constants,... 45 | """ 46 | 47 | data = {} 48 | data['a'] = 0.5616 49 | data['b'] = 0.3126 50 | data['c'] = 48.43 51 | data['d'] = 0.507 52 | data['e'] = 55.0 53 | data['f'] = 0.1538 54 | data['g'] = 90.0 55 | data['h'] = 0.16 56 | 57 | data['M'] = 20.0 58 | data['C'] = 4.0 59 | data['UA2'] = 6.84 60 | data['Cp'] = 0.07 61 | data['lam'] = 38.5 62 | data['lams'] = 36.6 63 | data['F1'] = 10.0 64 | data ['X1'] = 5.0 65 | data['F3'] = 50.0 66 | data['T1'] = 40.0 67 | data['T200'] = 25.0 68 | 69 | return data 70 | 71 | def intermediate_vars(x, u, data): 72 | 73 | """ Intermediate model variables 74 | """ 75 | 76 | data['T2'] = data['a']*x['P2'] + data['b']*x['X2'] + data['c'] 77 | data['T3'] = data['d']*x['P2'] + data['e'] 78 | data['T100'] = data['f']*u['P100'] + data['g'] 79 | data['UA1'] = data['h']*(data['F1']+data['F3']) 80 | data['Q100'] = data['UA1']*(data['T100'] - data['T2']) 81 | data['F100'] = data['Q100']/data['lams'] 82 | data['Q200'] = data['UA2']*(data['T3']-data['T200'])/(1.0 + data['UA2']/(2.0*data['Cp']*u['F200'])) 83 | data['F5'] = data['Q200']/data['lam'] 84 | data['F4'] = (data['Q100']-data['F1']*data['Cp']*(data['T2']-data['T1']))/data['lam'] 85 | data['F2'] = data['F1'] - data['F4'] 86 | 87 | return data 88 | 89 | def dynamics(x, u, data): 90 | 91 | """ System dynamics function (discrete time) 92 | """ 93 | 94 | # state derivative expression 95 | xdot = ca.vertcat( 96 | (data['F1']*data['X1'] - data['F2']*x['X2'])/data['M'], 97 | (data['F4'] - data['F5'])/data['C'] 98 | ) 99 | 100 | # create ode for integrator 101 | ode = {'x':x, 'p':u,'ode': xdot} 102 | 103 | return [ca.integrator('F','collocation',ode,{'tf':1}), ode] 104 | 105 | def vars(): 106 | 107 | """ System states and controls 108 | """ 109 | 110 | x = ct.struct_symMX(['X2','P2']) 111 | u = ct.struct_symMX(['P100','F200']) 112 | 113 | return x, u 114 | 115 | def objective(x, u, data): 116 | 117 | """ Economic objective function 118 | """ 119 | 120 | # cost definition 121 | obj = 10.09*(data['F2']+data['F3']) + 600.0*data['F100'] + 0.6*u['F200'] 122 | 123 | return ca.Function('economic_cost',[x,u],[obj]) 124 | 125 | def constraints(x, u, data): 126 | 127 | """ Path inequality constraints function (convention h(x,u) >= 0) 128 | """ 129 | 130 | constr = ca.vertcat( 131 | x['X2'] - 25.0, 132 | x['P2'] - 40.0, 133 | 80.0 - x['P2'], 134 | 400.0 - u['P100'], 135 | 400.0 - u['F200'], 136 | ) 137 | 138 | return ca.Function('h', [x,u], [constr]) 139 | 140 | 141 | # set-up system 142 | x, u = vars() 143 | data = intermediate_vars(x,u, problem_data()) 144 | nx = x.shape[0] 145 | nu = u.shape[0] 146 | 147 | tuner = tunempc.Tuner( 148 | f = dynamics(x, u, data)[0], 149 | l = objective(x,u,data), 150 | h = constraints(x,u, data), 151 | p = 1 152 | ) 153 | 154 | # solve 155 | w0 = ca.vertcat(*[25.0, 49.743, 191.713, 215.888]) 156 | wsol = tuner.solve_ocp(w0) 157 | Hc = tuner.convexify(rho = 1e-3, force = False, solver='mosek') 158 | 159 | # nmpc horizon length 160 | N = 20 161 | 162 | # gradient 163 | S = tuner.S 164 | 165 | # economic mpc controller 166 | ctrls = {} 167 | sys = tuner.sys 168 | ctrls['economic'] = tuner.create_mpc('economic',N = N) 169 | 170 | # normal tracking mpc controller 171 | tuningTn = {'H': [np.diag([10.0, 10.0, 0.1, 0.1])], 'q': S['q']} 172 | ctrls['tracking'] = tuner.create_mpc('tracking',N = N, tuning = tuningTn) 173 | 174 | # tuned tracking mpc controller 175 | ctrls['tuned'] = tuner.create_mpc('tuned',N = N) 176 | 177 | # check feedback policy equivalence 178 | alpha = np.linspace(0.0, 1.0, 10) # sweep factor 179 | dP2 = 1.0 # initial state perturbation 180 | log = clt.check_equivalence(ctrls, objective(x,u,data), sys['h'], wsol['x',0], ca.vertcat(0.0, dP2), alpha) 181 | 182 | # plot feedback controls to check equivalence 183 | ctrl_name = u.keys() 184 | for name in list(ctrls.keys()): 185 | for i in range(nu): 186 | plt.figure(i) 187 | plt.plot([ 188 | a*dP2 for a in alpha], 189 | [log[j]['u'][name][0][i] - log[j]['u']['economic'][0][i] \ 190 | for j in range(len(alpha))]) 191 | plt.legend(list(ctrls.keys())) 192 | plt.xlabel('dP2') 193 | plt.ylabel('u0 - u0_economic: {}'.format(ctrl_name[i])) 194 | plt.title('Feedback policy deviation') 195 | plt.grid(True) 196 | 197 | # generate embedded solver 198 | ACADOS_CODEGENERATE = False 199 | if ACADOS_CODEGENERATE: 200 | 201 | # get system ode 202 | ode = ca.Function('ode',[x.cat,u.cat],[dynamics(x,u,data)[1]['ode']]) 203 | 204 | # solver options 205 | opts = {} 206 | opts['integrator_type'] = 'IRK' 207 | opts['nlp_solver_type'] = 'SQP' # SQP_RTI 208 | opts['qp_solver_cond_N'] = 1 209 | opts['print_level'] = 0 210 | opts['sim_method_num_steps'] = 20 211 | opts['tf'] = N # h = tf/N = 1 [s] 212 | # opts['nlp_solver_max_iter'] = 30 213 | # opts['nlp_solver_step_length'] = 0.9 214 | 215 | ctrls_acados = {} 216 | for ctrl_key in list(ctrls.keys()): 217 | if ctrl_key == 'economic': 218 | opts['hessian_approx'] = 'EXACT' 219 | opts['qp_solver'] = 'PARTIAL_CONDENSING_HPIPM' 220 | else: 221 | opts['hessian_approx'] = 'GAUSS_NEWTON' 222 | opts['qp_solver'] = 'FULL_CONDENSING_QPOASES' 223 | 224 | _, _ = ctrls[ctrl_key].generate(dae = ode, opts = opts, name = ctrl_key+'_evaporation') 225 | ctrls_acados[ctrl_key+'_acados'] = ctrls[ctrl_key] 226 | 227 | # recompute equivalence check 228 | log_acados = clt.check_equivalence( 229 | ctrls_acados, 230 | objective(x,u,data), 231 | sys['h'], 232 | wsol['x',0], 233 | ca.vertcat(0.0, dP2), 234 | alpha, 235 | flag = 'acados' 236 | ) 237 | 238 | # plot feedback controls to check equivalence 239 | plt.close('all') 240 | legend_list = [] 241 | for name in list(ctrls.keys()): 242 | legend_list += [name] 243 | for i in range(nu): 244 | plt.figure(i) 245 | plt.plot( 246 | [a*dP2 for a in alpha], 247 | [log[j]['u'][name][0][i] - log[j]['u']['economic'][0][i] \ 248 | for j in range(len(alpha))] 249 | ) 250 | plt.plot( 251 | [a*dP2 for a in alpha], 252 | [log_acados[j]['u'][name+'_acados'][0][i] - log[j]['u']['economic'][0][i] \ 253 | for j in range(len(alpha))], 254 | linestyle = '--') 255 | if i == 0: 256 | legend_list += [name+'_acados'] 257 | plt.legend(legend_list) 258 | plt.xlabel('dP2') 259 | plt.ylabel('u0 - u0_economic: {}'.format(ctrl_name[i])) 260 | plt.title('Feedback policy deviation') 261 | plt.grid(True) 262 | plt.show() 263 | -------------------------------------------------------------------------------- /examples/unicycle/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jdeschut/tunempc/5be052cb503fa8c7f4d8845d0ef1b51d7535691c/examples/unicycle/__init__.py -------------------------------------------------------------------------------- /examples/unicycle/main.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """Implementation of the periodic unicycle example as proposed in 24 | 25 | M. Zanon et al. 26 | A Periodic Tracking MPC that is Locally Equivalent to Periodic Economic NMPC (section 7) 27 | IFAC 2017 World Congress 28 | 29 | :author: Jochem De Schutter 30 | 31 | """ 32 | 33 | import tunempc 34 | import tunempc.pmpc as pmpc 35 | import numpy as np 36 | import casadi as ca 37 | import casadi.tools as ct 38 | import matplotlib.pyplot as plt 39 | 40 | def problemData(): 41 | 42 | """ Problem data, numeric constants,... 43 | """ 44 | 45 | data = {} 46 | data['rho'] = 0.001 47 | data['v'] = 1 48 | 49 | return data 50 | 51 | def dynamics(x, u, data, h = 1.0): 52 | 53 | """ System dynamics formulation (discrete time) 54 | """ 55 | # state derivative 56 | xdot = ca.vertcat( 57 | data['v']*x['ez'], 58 | data['v']*x['ey'], 59 | - u['u']*x['ey'] - data['rho']*x['ez']*(x['ez']**2 + x['ey']**2 - 1.0), 60 | u['u']*x['ez'] - data['rho']*x['ey']*(x['ez']**2 + x['ey']**2 - 1.0) 61 | ) 62 | 63 | # set-up ode system 64 | f = ca.Function('f',[x,u],[xdot]) 65 | ode = {'x':x, 'p':u,'ode': f(x,u)} 66 | 67 | return [ca.integrator('F','rk',ode,{'tf':h,'number_of_finite_elements':50}), ode] 68 | 69 | def vars(): 70 | 71 | """ System states and controls 72 | """ 73 | 74 | x = ct.struct_symMX(['z','y','ez','ey']) 75 | u = ct.struct_symMX(['u']) 76 | 77 | return x, u 78 | 79 | def objective(x, u, data): 80 | 81 | # cost definition 82 | obj = u['u']**2 + x['z']**2 + 5*x['y']**2 83 | 84 | return ca.Function('cost',[x,u],[obj]) 85 | 86 | # discretization 87 | T = 5 # [s] 88 | N = 30 89 | 90 | # set-up system 91 | x, u = vars() 92 | nx = x.shape[0] 93 | nu = u.shape[0] 94 | data = problemData() 95 | 96 | tuner = tunempc.Tuner( 97 | f = dynamics(x, u, data, h= T/N)[0], 98 | l = objective(x,u,data), 99 | p = N 100 | ) 101 | 102 | # initialization 103 | t = np.linspace(0,T,N+1) 104 | ez0 = list(map(np.cos, 2*np.pi*t/T)) 105 | ey0 = list(map(np.sin, 2*np.pi*t/T)) 106 | inv = list(map(lambda x, y: x**2 + y**2 - 1.0, ez0, ey0)) 107 | z0 = list(map(lambda x : data['v']*T/(2*np.pi)*np.sin(x), 2*np.pi*t/T)) 108 | y0 = list(map(lambda x : - data['v']*T/(2*np.pi)*np.cos(x), 2*np.pi*t/T)) 109 | 110 | # create initial guess 111 | w0 = tuner.pocp.w(0.0) 112 | for i in range(N): 113 | w0['x',i] = ct.vertcat(z0[i], y0[i],ez0[i], ey0[i]) 114 | w0['u'] = 2*np.pi/T 115 | 116 | wsol = tuner.solve_ocp(w0 = w0.cat) 117 | Hc = tuner.convexify(solver='mosek') 118 | S = tuner.S 119 | 120 | # add projection operator for terminal constraint 121 | sys = tuner.sys 122 | 123 | # mpc options 124 | opts = {} 125 | opts['p_operator'] = ca.Function( 126 | 'p_operator', 127 | [sys['vars']['x']], 128 | [sys['vars']['x'][0:3]] 129 | ) 130 | 131 | ctrls = {} 132 | 133 | # economic mpc controller 134 | opts['ipopt_presolve'] = True 135 | ctrls['EMPC'] = tuner.create_mpc('economic', N, opts=opts) 136 | 137 | # normal tracking mpc controller 138 | tuningTn = {'H': [np.diag([1.0, 1.0, 1.0, 1.0, 1.0])]*N, 'q': S['q']} 139 | ctrls['TMPC'] = tuner.create_mpc('tracking', N, opts=opts, tuning = tuningTn) 140 | 141 | # tuned tracking mpc controller 142 | ctrls['TUNEMPC'] = tuner.create_mpc('tuned', N, opts=opts) 143 | 144 | # list of controller names 145 | ctrl_list = list(ctrls.keys()) 146 | 147 | # generate embedded solver 148 | ACADOS_CODEGENERATE = False 149 | if ACADOS_CODEGENERATE: 150 | 151 | # get system ode 152 | ode = ca.Function('ode',[x.cat,u.cat],[dynamics(x,u,data, h=T/N)[1]['ode']]) 153 | 154 | # solver options 155 | opts = {} 156 | opts['qp_solver'] = 'PARTIAL_CONDENSING_HPIPM' # PARTIAL_CONDENSING_HPIPM 157 | opts['hessian_approx'] = 'GAUSS_NEWTON' 158 | opts['integrator_type'] = 'ERK' 159 | opts['nlp_solver_type'] = 'SQP' # SQP_RTI 160 | opts['qp_solver_cond_N'] = 1 # ??? 161 | opts['print_level'] = 0 162 | opts['sim_method_num_steps'] = 50 163 | opts['tf'] = T 164 | opts['nlp_solver_max_iter'] = 300 165 | opts['nlp_solver_step_length'] = 1.0 166 | 167 | for ctrl_key in list(ctrls.keys()): 168 | _, _ = ctrls[ctrl_key].generate(ode, opts = opts, name = 'unicycle_'+ctrl_key) 169 | ctrl_list += [ctrl_key+'_ACADOS'] 170 | 171 | # disturbance 172 | dist_z = [0.1, 0.1, 0.5, 0.5] 173 | Nstep = [1, 34, 69, 105] 174 | 175 | # plant simulator 176 | plant_sim = dynamics(x, u, data, h= T/N)[0] 177 | 178 | # initialize 179 | x, u, l = {}, {}, {} 180 | for ctrl_key in ctrl_list: 181 | x[ctrl_key] = wsol['x',0] 182 | u[ctrl_key] = [] 183 | l[ctrl_key] = [] 184 | 185 | # closed-loop simulation 186 | Nsim = 250 187 | tgrid = [T/N*i for i in range(Nsim)] 188 | for k in range(Nsim): 189 | 190 | print('Closed-loop simulation step: {}/{}'.format(k+1,Nsim)) 191 | 192 | # reference stage cost 193 | lOpt = tuner.l(wsol['x', k%N], wsol['u',k%N]) 194 | 195 | for ctrl_key in ctrl_list: 196 | 197 | # add disturbance 198 | if k in Nstep: 199 | dist = dist_z[Nstep.index(k)] 200 | x[ctrl_key][0] += dist 201 | 202 | # compute feedback 203 | print('Compute {} feedback...'.format(ctrl_key)) 204 | if ctrl_key[-6:] == 'ACADOS': 205 | u[ctrl_key].append(ctrls[ctrl_key[:-7]].step_acados(x[ctrl_key])) 206 | else: 207 | u[ctrl_key].append(ctrls[ctrl_key].step(x[ctrl_key])) 208 | l[ctrl_key].append(tuner.l(x[ctrl_key], u[ctrl_key][-1])-lOpt) 209 | 210 | # forward sim 211 | x[ctrl_key] = plant_sim(x0 = x[ctrl_key], p = u[ctrl_key][-1])['xf'] 212 | 213 | # plot feedback controls to check equivalence 214 | for ctrl_key in ctrl_list: 215 | for i in range(nu): 216 | plt.figure(i) 217 | plt.step(tgrid,[u[ctrl_key][j][i] - wsol['u',j%N][i] for j in range(len(tgrid))]) 218 | plt.autoscale(enable=True, axis='x', tight=True) 219 | plt.grid(True) 220 | legend = ctrl_list 221 | plt.legend(legend) 222 | plt.title('Feedback control deviation') 223 | 224 | plt.figure(nu) 225 | for ctrl_key in ctrl_list: 226 | plt.step(tgrid,l[ctrl_key]) 227 | plt.legend(legend) 228 | plt.autoscale(enable=True, axis='x', tight=True) 229 | plt.grid(True) 230 | plt.title('Stage cost deviation') 231 | 232 | plt.show() 233 | -------------------------------------------------------------------------------- /external/install_acados.sh: -------------------------------------------------------------------------------- 1 | # rm -r acados/build 2 | git submodule update --init --recursive 3 | cd acados 4 | mkdir -p build && cd build 5 | cmake -DACADOS_WITH_QPOASES=ON .. 6 | make install -j4 && cd .. 7 | sudo pip3 install interfaces/acados_template 8 | #make shared_library 9 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | scipy 3 | casadi==3.5.1 4 | matplotlib 5 | picos==1.2.0.post32 -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with awebox; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | 23 | from setuptools import setup, find_packages 24 | 25 | import sys 26 | print(sys.version_info) 27 | 28 | if sys.version_info < (3,5): 29 | sys.exit('Python version 3.5 or later required. Exiting.') 30 | 31 | setup(name='tunempc', 32 | version='0.1.0', 33 | python_requires='>=3.5, <3.8', 34 | description='A tool for economic tuning of tracking (N)MPC problems', 35 | url='https://github.com/jdeschut/tunempc', 36 | author='Jochem De Schutter', 37 | author_email='jochem.de.schutter@imtek.de', 38 | license='LGPLv3.0', 39 | packages = find_packages(), 40 | include_package_data = True, 41 | # setup_requires=['setuptools_scm'], 42 | # use_scm_version={ 43 | # "fallback_version": "0.1-local", 44 | # "root": "../..", 45 | # "relative_to": __file__ 46 | # }, 47 | install_requires=[ 48 | 'numpy', 49 | 'scipy', 50 | 'casadi==3.5.1', 51 | 'matplotlib', 52 | 'picos==1.2.0.post32', 53 | ], 54 | ) 55 | -------------------------------------------------------------------------------- /test/test_processing.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | """ 23 | Test routines for preprocessing module 24 | 25 | :author: Jochem De Schutter 26 | """ 27 | 28 | import tunempc.preprocessing as preprocessing 29 | import casadi as ca 30 | import collections 31 | 32 | def test_input_formatting_no_constraints(): 33 | 34 | """ Test formatting of user provided input w/o constraints. 35 | """ 36 | 37 | x = ca.MX.sym('x',1) 38 | u = ca.MX.sym('u',2) 39 | 40 | # case of no constraints 41 | sys = {} 42 | sys['f'] = ca.Function('f',[x,u],[x]) 43 | sys = preprocessing.input_formatting(sys) 44 | 45 | assert 'h' not in sys, 'non-existent lin. inequalites are being created' 46 | assert 'g' not in sys, 'non-existent nonlin. inequalities are being created' 47 | assert 'vars' in sys, 'system variables are not being created' 48 | assert sys['vars']['x'].shape == x.shape, 'x-variable dimensions are incorrect' 49 | assert sys['vars']['u'].shape == u.shape, 'u-variable dimensions are incorrect' 50 | 51 | def test_input_formatting_lin_constraints(): 52 | 53 | """ Test formatting of user provided input with linear constraints. 54 | """ 55 | 56 | x = ca.MX.sym('x',1) 57 | u = ca.MX.sym('u',2) 58 | 59 | # case of linear constraints 60 | sys = {} 61 | sys['f'] = ca.Function('f',[x,u],[x]) 62 | h = ca.Function('h',[x,u],[ca.vertcat(x+u[0], u[1])]) 63 | sys['h'] = h 64 | sys = preprocessing.input_formatting(sys) 65 | 66 | assert 'h' in sys, 'lin. inequalites are being removed' 67 | assert sys['h'].size1_out(0) == 2, 'lin inequalities dimension is altered incorrectly' 68 | assert 'g' not in sys, 'non-existent nonlin. inequalities are being created' 69 | assert 'vars' in sys, 'system variables are not being created' 70 | assert sys['vars']['x'].shape == x.shape, 'x-variable dimensions are incorrect' 71 | assert sys['vars']['u'].shape == u.shape, 'u-variable dimensions are incorrect' 72 | 73 | 74 | def test_input_formatting_mixed_constraints(): 75 | 76 | """ Test formatting of user provided input with linear constraints. 77 | """ 78 | 79 | x = ca.MX.sym('x',1) 80 | u = ca.MX.sym('u',2) 81 | 82 | # case of mixed constraints 83 | sys = {} 84 | sys['f'] = ca.Function('f',[x,u],[x]) 85 | h = ca.Function('h',[x,u],[ca.vertcat(x+u[0], u[1], x**2*u[0])]) 86 | sys['h'] = h 87 | sys = preprocessing.input_formatting(sys) 88 | 89 | assert 'h' in sys, 'lin. inequalites are being removed' 90 | assert sys['h'].size1_out(0) == 3, 'lin inequalities have wrong dimension' 91 | assert 'g' in sys, 'nonlin. inequalities are not being created' 92 | assert sys['g'].size1_out(0) == 1, 'nonlin inequalities have wrong dimension' 93 | assert 'vars' in sys, 'system variables are not being created' 94 | assert sys['vars']['x'].shape == x.shape, 'x-variable dimensions are incorrect' 95 | assert sys['vars']['u'].shape == u.shape, 'u-variable dimensions are incorrect' 96 | assert sys['vars']['us'].shape == (1,1), 'us-variable dimensions are incorrect' 97 | 98 | x_num = 1.0 99 | u_num = ca.vertcat(0.0, 2.0) 100 | us_num = ca.vertcat(3.0) 101 | h_eval = sys['h'](x_num, u_num, us_num).full() 102 | g_eval = sys['g'](x_num, u_num, us_num).full() 103 | assert ca.vertsplit(h_eval) == [1.0, 2.0, 3.0] 104 | assert ca.vertsplit(g_eval) == [-3.0] 105 | 106 | def test_add_mpc_slacks_no_constraints(): 107 | 108 | """ Test automatic soft constraint generation if no constraints are present. 109 | """ 110 | 111 | x = ca.MX.sym('x',1) 112 | u = ca.MX.sym('u',2) 113 | 114 | # case of no constraints 115 | sys = {} 116 | sys['f'] = ca.Function('f',[x,u],[x]) 117 | sys['vars'] = collections.OrderedDict() 118 | sys['vars']['x'] = x 119 | sys['vars']['u'] = u 120 | sys = preprocessing.add_mpc_slacks(sys, None, None, slack_flag = 'active') 121 | 122 | assert 'h' not in sys, 'non-existent lin. inequalities are being created' 123 | assert 'usc' not in sys['vars'], 'non-existent slack variables are being created' 124 | 125 | def test_add_mpc_slacks_no_active_constraints(): 126 | 127 | """ Test automatic soft constraint generation if no active constraints are present. 128 | """ 129 | 130 | x = ca.MX.sym('x',1) 131 | u = ca.MX.sym('u',2) 132 | 133 | # case of no active constraints 134 | sys = {} 135 | sys['f'] = ca.Function('f',[x,u],[x]) 136 | h = ca.Function('h',[x,u],[ca.vertcat(x+u[0], u[1])]) 137 | sys['h'] = h 138 | sys['vars'] = collections.OrderedDict() 139 | sys['vars']['x'] = x 140 | sys['vars']['u'] = u 141 | active_set = [[],[]] 142 | sys = preprocessing.add_mpc_slacks(sys, None, active_set, slack_flag = 'active') 143 | 144 | assert 'h' in sys, 'lin. inequalities are being removed' 145 | assert 'usc' not in sys['vars'], 'non-existent slack variables are being created' 146 | 147 | def test_add_mpc_slacks_active_constraints(): 148 | 149 | """ Test automatic soft constraint generation if active constraints are present. 150 | """ 151 | 152 | x = ca.MX.sym('x',1) 153 | u = ca.MX.sym('u',2) 154 | 155 | # case of no active constraints 156 | sys = {} 157 | sys['f'] = ca.Function('f',[x,u],[x]) 158 | h = ca.Function('h',[x,u],[ca.vertcat(x+u[0], u[1])]) 159 | sys['h'] = h 160 | sys['vars'] = collections.OrderedDict() 161 | sys['vars']['x'] = x 162 | sys['vars']['u'] = u 163 | 164 | lam_g_struct = ca.tools.struct_symMX([ 165 | ca.tools.entry('h', shape = (2,1), repeat = 2) 166 | ]) 167 | lam_g = lam_g_struct(0.0) 168 | lam_g['h',0,0] = -5.0 169 | active_set = [[0],[]] 170 | sys = preprocessing.add_mpc_slacks(sys, lam_g, active_set, slack_flag = 'active') 171 | 172 | assert 'h' in sys, 'lin. inequalities are being removed' 173 | assert 'usc' in sys['vars'], 'slack variables are not being created' 174 | assert sys['vars']['usc'].shape[0] == 1, 'slack variables are not being created' 175 | assert 'scost' in sys, 'slack cost is not being created' 176 | 177 | x_num = 1.0 178 | u_num = ca.vertcat(0.0, 2.0) 179 | usc_num = ca.vertcat(3.0) 180 | h_eval = sys['h'](x_num, u_num, usc_num).full() 181 | scost_eval = sys['scost'].full() 182 | print(scost_eval) 183 | assert ca.vertsplit(h_eval) == [4.0, 2.0, 3.0] 184 | assert scost_eval[0][0] == 5000. -------------------------------------------------------------------------------- /tunempc/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | from .tuner import Tuner 23 | from .pocp import Pocp -------------------------------------------------------------------------------- /tunempc/closed_loop_tools.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | """ 23 | :author: Jochem De Schutter 24 | """ 25 | 26 | import numpy as np 27 | import matplotlib.pyplot as plt 28 | from tunempc.logger import Logger 29 | 30 | def check_equivalence(controllers, cost, h, x0, dx, alpha, flag = 'tunempc'): 31 | 32 | """ Check local equivalence of different controllers. 33 | """ 34 | 35 | Logger.logger.info(60*'=') 36 | Logger.logger.info(15*' '+'Compare feedback policies...') 37 | Logger.logger.info(60*'=') 38 | Logger.logger.info('') 39 | 40 | log = [] 41 | 42 | # compute feedback law in direction dx for different alpha values 43 | for alph in alpha: 44 | 45 | # determine initial condition 46 | print('alpha: {}'.format(alph)) 47 | x_init = x0 + alph*dx 48 | 49 | log.append(initialize_log(controllers, x_init)) 50 | 51 | for name in list(controllers.keys()): 52 | 53 | # compute feedback law and store results 54 | print('Compute MPC feedback for controller {}'.format(name)) 55 | if flag == 'tunempc': 56 | u0 = controllers[name].step(x_init) 57 | wsol = controllers[name].w_sol 58 | elif flag == 'acados': 59 | u0 = controllers[name].step_acados(x_init) 60 | wsol = controllers[name].w_sol_acados 61 | log[-1]['log'][name] = controllers[name].log_acados 62 | log[-1]['u'][name] = wsol['u',:] 63 | log[-1]['x'][name] = wsol['x',:] 64 | log[-1]['l'][name] = [cost(wsol['x',k],wsol['u',k]).full()[0][0] for k in range(len(wsol['u',:]))] 65 | log[-1]['h'][name] = [h(wsol['x',k],wsol['u',k]).full()[0][0] for k in range(len(wsol['u',:]))] 66 | 67 | # reset controller 68 | controllers[name].reset() 69 | 70 | return log 71 | 72 | def closed_loop_sim(controllers, cost, h, F, x0, N, flag = 'tunempc'): 73 | 74 | """ Perform closed-loop simulations for different controllers starting from x0 75 | """ 76 | 77 | Logger.logger.info(60*'=') 78 | Logger.logger.info(10*' '+'Compute closed-loop responses...') 79 | Logger.logger.info(60*'=') 80 | Logger.logger.info('') 81 | 82 | log = initialize_log(controllers, x0) 83 | 84 | for i in range(N): 85 | 86 | Logger.logger.info('======================================') 87 | Logger.logger.info('Closed-loop simulation step {} of {}'.format(i, N)) 88 | Logger.logger.info('======================================') 89 | 90 | for name in list(controllers.keys()): 91 | 92 | # compute feedback law and store results 93 | print('Compute MPC feedback for controller {}'.format(name)) 94 | if flag == 'tunempc': 95 | log['u'][name].append(controllers[name].step(log['x'][name][-1])) 96 | elif flag == 'acados': 97 | log['u'][name].append(controllers[name].step_acados(log['x'][name][-1])) 98 | log['l'][name].append(cost(log['x'][name][-1], log['u'][name][-1]).full()[0][0]) 99 | log['h'][name].append(h(log['x'][name][-1], log['u'][name][-1]).full()) 100 | 101 | # simulate 102 | log['x'][name].append(F(x0 = log['x'][name][-1], p = log['u'][name][-1])['xf']) 103 | 104 | return log 105 | 106 | def initialize_log(controllers, x0=None): 107 | 108 | # initialize log 109 | log = {'u':{},'l':{},'h':{}, 'log':{}} 110 | 111 | if x0 is not None: 112 | log['x'] = {} 113 | 114 | for name in list(controllers.keys()): 115 | for log_key in log.keys(): 116 | log[log_key][name] = [] 117 | 118 | if x0 is not None: 119 | log['x'][name] = [x0] 120 | 121 | return log -------------------------------------------------------------------------------- /tunempc/convert.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of TuneMPC. 3 | % 4 | % TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | % Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | % 7 | % TuneMPC is free software; you can redistribute it and/or 8 | % modify it under the terms of the GNU Lesser General Public 9 | % License as published by the Free Software Foundation; either 10 | % version 3 of the License, or (at your option) any later version. 11 | % 12 | % TuneMPC is distributed in the hope that it will be useful, 13 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | % Lesser General Public License for more details. 16 | % 17 | % You should have received a copy of the GNU Lesser General Public 18 | % License along with TuneMPC; if not, write to the Free Software Foundation, 19 | % Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | % 21 | 22 | % function that converts general DAE model to GNSF-structured model 23 | % @author: Jonathan Frey 24 | function convert(json_filename) 25 | dae_json_to_gnsf_json( json_filename ); 26 | end -------------------------------------------------------------------------------- /tunempc/convexifier.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """ Convexification code 24 | 25 | :author: Jochem De Schutter 26 | 27 | """ 28 | 29 | import picos 30 | import numpy as np 31 | import tunempc.mtools as mtools 32 | from functools import reduce 33 | import tunempc.preprocessing as preprocessing 34 | from tunempc.logger import Logger 35 | 36 | def convexify(A, B, Q, R, N, G = None, C = None, opts = {'rho':1e-3, 'solver':'mosek','force': False}): 37 | 38 | """ Convexify the indefinite Hessian "H" of the system with the discrete time dynamics 39 | 40 | .. math:: 41 | 42 | x_{k+1} = A x_k + B u_k 43 | 44 | so that the solution of the LQR problem based on the convexified 45 | Hessian "H + dH" yields the same trajectory as the LQR-solution 46 | of the indefinite problem. 47 | 48 | :param A: system matrix 49 | :param B: input matrix 50 | :param Q: weighting matrix Q (nx,nx) 51 | :param R: weighting matrix R (nu,nu) 52 | :param N: weighting matrix N (nx,nu) 53 | :param C: jacobian of active constraints at steady state (nc, nx+nu) 54 | :param G: jacobian of equality constraints at steady state (ng, nx+nu) 55 | :param opts: tuning options 56 | 57 | :return: Convexified Hessian supplement "dH". 58 | """ 59 | 60 | # perform input checks 61 | arg = {**locals()} 62 | del arg['opts'] 63 | 64 | if arg['C'] is None: 65 | del arg['C'] 66 | Logger.logger.info('Convexifier called w/o active constraints at steady state') 67 | if arg['G'] is None: 68 | del arg['G'] 69 | 70 | # check inputs 71 | arg = preprocessing.input_checks(arg) 72 | 73 | # extract steady-state period 74 | period = len(arg['A']) 75 | Logger.logger.info('Convexify Hessians along {:d}-periodic steady state trajectory.'.format(period)) 76 | 77 | # extract dimensions 78 | nx = arg['A'][0].shape[0] 79 | nu = arg['B'][0].shape[1] 80 | 81 | # check if hessian is already convex! 82 | min_eigval = list(map(lambda q,r,n: np.min(np.linalg.eigvals(mtools.buildHessian(q,r,n))), arg['Q'], arg['R'], arg['N'])) 83 | if min(min_eigval) > 0: 84 | Logger.logger.info('Provided hessian(s) are already positive definite. No convexification needed!') 85 | return np.zeros((nx+nu,nx+nu)), np.zeros((nx,nx)), np.zeros((nu,nu)), np.zeros((nx,nu)) 86 | 87 | # solver verbosity 88 | if Logger.logger.getEffectiveLevel() < 20: 89 | opts['verbose'] = 1 90 | else: 91 | opts['verbose'] = 0 92 | 93 | Logger.logger.info('Construct SDP...') 94 | Logger.logger.info('') 95 | Logger.logger.info(50*'*') 96 | 97 | # perform autoscaling 98 | scaling = autoScaling(arg['Q'], arg['R'], arg['N']) 99 | 100 | # define model 101 | M = setUpModelPicos(**arg, constr = False) 102 | 103 | Logger.logger.info('Step 1: (\u03B7_F = 0), (\u03B7_T = 0)') 104 | 105 | # solve 106 | constraint_contribution = False 107 | M = solveSDP(M, opts) 108 | 109 | status, dHc, dQc, dRc, dNc = check_convergence(M, scaling, **arg, constr = constraint_contribution) 110 | 111 | if status in ['Optimal', 'Feasible']: 112 | 113 | Logger.logger.info('EQUIVALENCE TYPE A') 114 | Logger.logger.info(50*'*') 115 | 116 | if status == 'Infeasible' and 'C' in arg: 117 | 118 | Logger.logger.info(50*'*') 119 | Logger.logger.info('Step 2: (\u03B7_F = 1), (\u03B7_T = 0)') 120 | 121 | # create model with active constraint regularisation 122 | M = setUpModelPicos(**arg, rho = opts['rho'], constr = True) 123 | 124 | # solve again 125 | constraint_contribution = True 126 | M = solveSDP(M, opts) 127 | 128 | status, dHc, dQc, dRc, dNc = check_convergence(M, scaling, **arg, constr = constraint_contribution) 129 | 130 | if status in ['Optimal', 'Feasible']: 131 | 132 | Logger.logger.info('EQUIVALENCE TYPE B') 133 | Logger.logger.info(50*'*') 134 | 135 | if status == 'Infeasible': 136 | 137 | Logger.logger.warning('!! Strict dissipativity does not hold locally !!') 138 | Logger.logger.warning('!! The provided indefinite LQ MPC problem is not stabilising !!') 139 | Logger.logger.warning(50*'*') 140 | 141 | if opts['force']: 142 | 143 | Logger.logger.info('Step 3: (\u03B7_F = 1), (\u03B7_T = 1)') 144 | Logger.logger.info('Enforcing convexification...') 145 | 146 | M = setUpModelPicos(**arg, rho = opts['rho'], constr = constraint_contribution, force = True) 147 | M = solveSDP(M, opts) 148 | status, dHc, dQc, dRc, dNc = check_convergence(M, scaling, **arg, constr = constraint_contribution, force = True) 149 | 150 | Logger.logger.warning(50*'*') 151 | 152 | else: 153 | 154 | Logger.logger.warning('Consider operating the system at another orbit of different period p') 155 | Logger.logger.warning('Convexification and stabilization of the MPC scheme can be enforced by enabling "force"-flag.') 156 | Logger.logger.warning('In this case there are no guarantees of (local, first-order) equivalence.') 157 | raise ValueError('Convexification is not possible if the system is not optimally operated at the optimal orbit.') 158 | 159 | Logger.logger.info('') 160 | Logger.logger.info('Hessians convexified.') 161 | Logger.logger.info('') 162 | 163 | return dHc, dQc, dRc, dNc 164 | 165 | def convexHessianSuppl(A, B, Q, R, N, dP, G = None, Fg = None, C = None, F = None, T = None): 166 | 167 | """ Construct the convexified Hessian Supplement 168 | 169 | :param A: system matrix 170 | :param B: input matrix 171 | :param Q: weighting matrix Q (nx,nx) 172 | :param R: weighting matrix R (nu,nu) 173 | :param N: weighting matrix N (nx,nu) 174 | :param dP: ... 175 | 176 | :return: convexified Hessian supplement 177 | """ 178 | 179 | period = len(dP) 180 | nx = A[0].shape[0] # state dimension 181 | 182 | dHc, dQc, dRc, dNc = [], [], [], [] 183 | 184 | for i in range(period): 185 | 186 | # unpack dP 187 | dP1 = dP[i] 188 | dP2 = dP[(i+1)%period] 189 | 190 | # convexify 191 | Qco = np.squeeze(np.dot(np.dot(A[i].T, dP2), A[i]) - dP1) 192 | Rco = np.dot(np.dot(B[i].T, dP2), B[i]) 193 | Nco = np.dot(np.dot(B[i].T, dP2.T), A[i]).T 194 | Hco = mtools.buildHessian(Qco, Rco, Nco) 195 | 196 | if G: 197 | Hco = Hco + np.dot(np.dot(G[i].T,np.diagflat(Fg[i])),G[i]) 198 | if F: 199 | if C[i] is not None: 200 | nc = C[i].shape[0] 201 | Hco = Hco + np.dot(np.dot(C[i].T, np.diagflat(F[i])), C[i]) 202 | if T: 203 | Hco = Hco + T[i] 204 | 205 | # symmetrize 206 | dHc.append(mtools.symmetrize(Hco)) 207 | dQc.append(dHc[i][:nx,:nx]) 208 | dRc.append(dHc[i][nx:,nx:]) 209 | dNc.append(dHc[i][:nx,nx:]) 210 | 211 | return dHc, dQc, dRc, dNc 212 | 213 | def setUpModelPicos(A, B, Q, R, N, G = None, C = None, rho = 1e-3, constr = True, force = False): 214 | 215 | """ Description 216 | :param A: 217 | :param B: 218 | :param Q: 219 | :param R: 220 | :param N: 221 | :param dP: 222 | :param C: 223 | :param constr: 224 | 225 | :rtype: 226 | """ 227 | 228 | # extract data 229 | nx = A[0].shape[0] 230 | nu = B[0].shape[1] 231 | period = len(A) 232 | 233 | # create model 234 | M = picos.Problem() 235 | 236 | # scaling 237 | scaling = autoScaling(Q,R,N) 238 | 239 | # model variables 240 | alpha = M.add_variable('alpha', 1, vtype='continuous') 241 | beta = M.add_variable('beta', 1, vtype='continuous') 242 | dP = [M.add_variable('dP'+str(i), (nx,nx), vtype='symmetric') for i in range(period)] 243 | 244 | # alpha should be positive 245 | M.add_constraint(alpha > 1e-8) 246 | # M.add_constraint(alpha < 1e8/scaling['alpha']) 247 | 248 | # add regularisation in range space of equality constraints 249 | if G is not None: 250 | ng = G[0].shape[0] 251 | Fg = [M.add_variable('Fg'+str(i), (ng,1), vtype='continuous') for i in range(period)] 252 | for i in range(period): 253 | M.add_constraint(Fg[i] > 0) 254 | else: 255 | Fg = None 256 | 257 | # add regularisation in range space of active inequality constraints 258 | if (C is not None) and (constr is True): # TODO check if this is correct 259 | F = [] 260 | for i in range(period): 261 | if C[i] is not None: 262 | nc = C[i].shape[0] # number of active constraints at index 263 | F.append(M.add_variable('F'+str(i), (nc,1), vtype='continuous')) 264 | M.add_constraint(F[i] > 0) 265 | else: 266 | F.append(None) 267 | 268 | # force regularisation 269 | if force: 270 | T = [] 271 | for i in range(period): 272 | T.append(M.add_variable('T'+str(i), (nx+nu,nx+nu), vtype='symmetric')) 273 | M.add_constraint(T[i] > 0) 274 | 275 | # objective 276 | obj = beta # minimize condition number 277 | if constr: 278 | for i in range(period): 279 | if F[i] is not None: 280 | obj = picos.sum(obj, abs(rho*F[i])) 281 | if G is not None: 282 | obj = picos.sum(obj, abs(rho*Fg[i])) 283 | if force: 284 | for i in range(period): 285 | obj = picos.sum(obj, abs(rho*T[i])) 286 | 287 | M.set_objective('min', obj) 288 | 289 | # formulate convexified Hessian expression 290 | if not constr and not force: 291 | HcE = [convexHessianExprPicos(Q, R, N, A, B, dP, alpha, scaling, G = G, Fg = Fg, index = i) \ 292 | for i in range(period)] 293 | elif constr and not force: 294 | HcE = [convexHessianExprPicos(Q, R, N, A, B, dP, alpha, scaling, G = G, Fg = Fg, C = C, F = F, index = i) \ 295 | for i in range(period)] 296 | elif not constr and force: 297 | HcE = [convexHessianExprPicos(Q, R, N, A, B, dP, alpha, scaling, G = G, Fg = Fg, T = T, index = i) \ 298 | for i in range(period)] 299 | else: 300 | HcE = [convexHessianExprPicos(Q, R, N, A, B, dP, alpha, scaling, G = G, Fg = Fg, C = C, F = F, T = T, index = i) \ 301 | for i in range(period)] 302 | 303 | # formulate constraints 304 | for i in range(period): 305 | M.add_constraint((HcE[i] - np.eye(nx+nu))/scaling['alpha'] >> 0) 306 | M.add_constraint((scaling['beta']*beta * np.eye(nx+nu) - HcE[i])/scaling['alpha'] >> 0) 307 | 308 | return M 309 | 310 | def convexHessianExprPicos(Q, R, N, A, B, dP, alpha, scaling, index, G = None, C = None, F = None, Fg = None, T = None): 311 | 312 | """ Construct the Picos symbolic expression of the convexified Hessian 313 | 314 | :param H: non-convex Hessian 315 | :param A: system matrix 316 | :param B: input matrix 317 | :param dP: ... 318 | 319 | :return: Picos Expression of the convexified Hessian 320 | """ 321 | 322 | # set-up 323 | period = len(dP) 324 | 325 | H = scaling['alpha']*alpha*mtools.buildHessian(Q[index], R[index], N[index]) 326 | 327 | A = A[index] 328 | B = B[index] 329 | 330 | if C is not None: 331 | CM = C[index] 332 | if G is not None: 333 | GM = G[index] 334 | 335 | dP1 = scaling['dP']*dP[index] 336 | dP2 = scaling['dP']*dP[(index+1)%period] 337 | 338 | # convexify 339 | dQ = A.T*dP2*A - dP1 340 | dN = A.T*dP2*B 341 | dNT = B.T*dP2.T*A 342 | dR = B.T*dP2*B 343 | dH = (dQ & dN) // (dN.T & dR) 344 | 345 | # add constraints contribution 346 | if G is not None: 347 | dH = dH + GM.T*picos.diag(scaling['F']*Fg[index])*GM 348 | if F is not None: 349 | if F[index] is not None: 350 | dH = dH + CM.T*picos.diag(scaling['F']*F[index])*CM 351 | 352 | # add free regularisation 353 | if T is not None: 354 | if T[index] is not None: 355 | dH = dH + scaling['T']*T[index] 356 | 357 | return H+dH 358 | 359 | def solveSDP(M, opts): 360 | 361 | Logger.logger.info('solving SDP...') 362 | 363 | M.solve(solver = opts['solver'], verbose = opts['verbose']) 364 | 365 | if M.status == 'optimal': 366 | Logger.logger.debug('SDP solution:') 367 | Logger.logger.debug('alpha: {}'.format(str(M.variables['alpha'].value))) 368 | Logger.logger.debug('beta: {}'.format(str((M.variables['beta'].value)))) 369 | else: 370 | Logger.logger.debug('solution status: {} ...'.format(M.status)) 371 | 372 | return M 373 | 374 | def autoScaling(Q, R, N): 375 | 376 | """ Compute scaling factors for SDP variables, objective and constraints. 377 | """ 378 | 379 | # build hessians 380 | H = [mtools.buildHessian(Q[k], R[k], N[k]) for k in range(len(Q))] 381 | 382 | # get mininmum absolute value of eigenvalues 383 | min_eig = 1e10 384 | max_eig = 0.0 385 | for k in range(len(H)): 386 | eigvalsk = np.abs(np.linalg.eigvals(H[k])) 387 | min_eig = min([min_eig, np.min(eigvalsk[np.nonzero(eigvalsk)])]) 388 | max_eig = max([max_eig, np.max(eigvalsk[np.nonzero(eigvalsk)])]) 389 | 390 | # maximum condition number 391 | max_cond = max_eig/min_eig 392 | 393 | scaling = { 394 | 'alpha': 1/min_eig, 395 | 'beta': max_cond, 396 | 'dP': 1/min_eig, 397 | 'F': 1/min_eig, 398 | 'T': 1/min_eig 399 | } 400 | 401 | return scaling 402 | 403 | def check_convergence(M, scaling, A, B, Q, R, N, G = None, Fg = None, C = None, constr = False, force = False): 404 | 405 | # build convex hessian list 406 | dP = [scaling['dP']*np.array(M.variables['dP'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \ 407 | for i in range(len(A))] 408 | 409 | if G is not None: 410 | Fg = [scaling['F']*np.array(M.variables['Fg'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \ 411 | for i in range(len(A))] 412 | 413 | if not constr and not force: 414 | dHc, dQc, dRc, dNc = convexHessianSuppl( A, B, Q, R, N, dP, G = G, Fg = Fg) 415 | if constr: 416 | F = [] 417 | for i in range(len(A)): 418 | if 'F'+str(i) in M.variables: 419 | F.append(scaling['F']*np.array(M.variables['F'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value)) 420 | else: 421 | F.append(None) 422 | if not force: 423 | dHc, dQc, dRc, dNc = convexHessianSuppl( A, B, Q, R, N, dP, G = G, Fg = Fg, C = C, F = F) 424 | else: 425 | T = [scaling['T']*np.array(M.variables['T'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \ 426 | for i in range(len(A))] 427 | dHc, dQc, dRc, dNc = convexHessianSuppl( A, B, Q, R, N, dP, G = G, Fg = Fg, C = C, F = F, T = T) 428 | else: 429 | if force: 430 | T = [scaling['T']*np.array(M.variables['T'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \ 431 | for i in range(len(A))] 432 | dHc, dQc, dRc, dNc = convexHessianSuppl( A, B, Q, R, N, dP, G = G, Fg = Fg, T = T) 433 | 434 | # add hessian supplements 435 | Hc = [mtools.buildHessian(Q[k], R[k], N[k]) + dHc[k] for k in range(len(dHc))] 436 | 437 | # compute eigenvalues 438 | min_eigenvalue = min([np.min(np.linalg.eigvals(Hk)) for Hk in Hc]) 439 | max_eigenvalue = max([np.max(np.linalg.eigvals(Hk)) for Hk in Hc]) 440 | max_cond = max([np.linalg.cond(Hk) for Hk in Hc]) 441 | 442 | if min_eigenvalue > 0.0: 443 | if M.status == 'optimal': 444 | status = 'Optimal' 445 | else: 446 | status = 'Feasible' 447 | Logger.logger.info('{} solution found.'.format(status)) 448 | Logger.logger.info('Maximum condition number: {}'.format(max_cond)) 449 | Logger.logger.info('Minimum eigenvalue: {}'.format(min_eigenvalue)) 450 | else: 451 | status = 'Infeasible' 452 | Logger.logger.info('SDP solver status: {}'.format(M.status)) 453 | Logger.logger.info('Minimum eigenvalue: {}'.format(min_eigenvalue)) 454 | Logger.logger.info('!! Problem infeasible !!') 455 | 456 | return status, dHc, dQc, dRc, dNc -------------------------------------------------------------------------------- /tunempc/logger.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """ 24 | Manage tunempc logging 25 | """ 26 | 27 | import logging.config 28 | import os 29 | 30 | def singleton(cls): 31 | instances = {} 32 | def get_instance(): 33 | if cls not in instances: 34 | instances[cls] = cls() 35 | return instances[cls] 36 | return get_instance() 37 | 38 | @singleton 39 | class Logger(): 40 | def __init__(self): 41 | 42 | config_file = 'logging.conf' 43 | default_config_file = os.path.join(os.path.abspath(os.path.dirname(__file__)), config_file) 44 | if os.path.exists(config_file): 45 | logging.config.fileConfig(config_file) 46 | else: 47 | assert os.path.exists(default_config_file), 'path {} does not exist'.format(default_config_file) 48 | logging.config.fileConfig(default_config_file) 49 | logger = logging.getLogger('tunempc') 50 | self.logger = logger 51 | -------------------------------------------------------------------------------- /tunempc/logging.conf: -------------------------------------------------------------------------------- 1 | [loggers] 2 | keys=root,tunempc 3 | 4 | [handlers] 5 | keys=consoleHandler,fileHandler 6 | 7 | [formatters] 8 | keys=consoleFormatter,fileFormatter 9 | 10 | [logger_root] 11 | level=ERROR 12 | handlers=consoleHandler 13 | qualname=root 14 | propagate=0 15 | 16 | [logger_tunempc] 17 | level=INFO 18 | handlers=consoleHandler,fileHandler 19 | qualname=tunempc 20 | propagate=0 21 | 22 | [handler_consoleHandler] 23 | class=StreamHandler 24 | level=INFO 25 | formatter=consoleFormatter 26 | args=(sys.stdout,) 27 | 28 | [handler_fileHandler] 29 | class=FileHandler 30 | level=ERROR 31 | formatter=fileFormatter 32 | args=('tunempc.log','w',None,True) 33 | 34 | [formatter_consoleFormatter] 35 | # format=%(levelname)s: %(message)s 36 | format=%(message)s 37 | datefmt= 38 | 39 | [formatter_fileFormatter] 40 | format=%(levelname)s: %(message)s 41 | datefmt= 42 | -------------------------------------------------------------------------------- /tunempc/mtools.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | #!/usr/bin/python3 23 | """ Matrix operation tools. 24 | 25 | :author: Jochem De Schutter 26 | 27 | """ 28 | 29 | import numpy as np 30 | import casadi as ca 31 | import casadi.tools as ct 32 | 33 | def symmetrize(S): 34 | """Symmetrize matrix S 35 | """ 36 | return (S + S.T)/2.0 37 | 38 | def buildHessian(Q, R, N): 39 | """ Build the Hessian matrix based on submatrices. 40 | """ 41 | return np.vstack((np.hstack((Q, N)),np.hstack((N.T, R)))) 42 | 43 | def tracking_cost(nw): 44 | """ Create tracking cost function for variables nw 45 | """ 46 | 47 | # reference parameters 48 | w = ca.MX.sym('w', (nw, 1)) 49 | wref = ca.MX.sym('wref', (nw, 1)) 50 | H = ca.MX.sym('H', (nw, nw)) 51 | q = ca.MX.sym('H', (nw, 1)) 52 | 53 | # cost definition 54 | dw = w - wref 55 | obj = 0.5*ct.mtimes(dw.T, ct.mtimes(H, dw)) + ct.mtimes(q.T,dw) 56 | 57 | return ca.Function('tracking_cost',[w, wref, H, q],[obj]) -------------------------------------------------------------------------------- /tunempc/pocp.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | """ 23 | Periodic OCP routines 24 | 25 | :author: Jochem De Schutter 26 | """ 27 | 28 | import casadi.tools as ct 29 | import casadi as ca 30 | import numpy as np 31 | import itertools 32 | import collections 33 | import tunempc.sqp_method as sqp_method 34 | from tunempc.logger import Logger 35 | 36 | class Pocp(object): 37 | 38 | def __init__(self, sys, cost, period = 1): 39 | 40 | """ Constructor 41 | """ 42 | 43 | # store system dynamics 44 | self.__F = sys['f'] 45 | 46 | # store linear inequality constraints 47 | if 'h' in sys: 48 | self.__h = sys['h'] 49 | else: 50 | self.__h = None 51 | 52 | # store slacked nonlinear inequality constraints 53 | if 'g' in sys: 54 | self.__gnl = sys['g'] 55 | else: 56 | self.__gnl = None 57 | 58 | # store system variables 59 | self.__vars = sys['vars'] 60 | self.__nx = sys['vars']['x'].shape[0] 61 | self.__nu = sys['vars']['u'].shape[0] 62 | if 'us' in sys['vars']: # slacks 63 | self.__ns = sys['vars']['us'].shape[0] 64 | 65 | # story economic cost function 66 | self.__cost = cost 67 | 68 | # desired steady state periods 69 | self.__N = period 70 | 71 | # construct OCP with sensitivities 72 | self.__parallelization = 'openmp' 73 | self.__mu_tresh = 1e-15 # treshold for active constraint detection 74 | self.__reg_slack = 1e-4 # slack regularization 75 | self.__construct_solver() 76 | self.__construct_sensitivity_funcs() 77 | 78 | def __construct_solver(self): 79 | 80 | """ Construct periodic NLP and solver. 81 | """ 82 | 83 | # system variables and dimensions 84 | x = self.__vars['x'] 85 | u = self.__vars['u'] 86 | 87 | variables_entry = ( 88 | ct.entry('x', shape = (self.__nx,), repeat = self.__N), 89 | ct.entry('u', shape = (self.__nu,), repeat = self.__N) 90 | ) 91 | 92 | if 'us' in self.__vars: 93 | variables_entry += ( 94 | ct.entry('us', shape = (self.__ns,), repeat = self.__N), 95 | ) 96 | 97 | # nlp variables + bounds 98 | w = ct.struct_symMX([variables_entry]) 99 | 100 | self.__lbw = w(-np.inf) 101 | self.__ubw = w(np.inf) 102 | 103 | # prepare dynamics and path constraints entry 104 | constraints_entry = (ct.entry('dyn', shape = (self.__nx,), repeat = self.__N),) 105 | if self.__h is not None: 106 | if type(self.__h) is not list: 107 | constraints_entry += (ct.entry('h', shape = self.__h.size1_out(0), repeat = self.__N),) 108 | else: 109 | # TODO: allow for changing dimension of h 110 | constraints_entry += (ct.entry('h', shape = self.__h[0].size1_out(0), repeat = self.__N),) 111 | 112 | if self.__gnl is not None: 113 | if type(self.__gnl) is not list: 114 | constraints_entry += (ct.entry('g', shape = self.__gnl.size1_out(0), repeat = self.__N),) 115 | else: 116 | # TODO: allow for changing dimension of g 117 | constraints_entry += (ct.entry('g', shape = self.__gnl[0].size1_out(0), repeat = self.__N),) 118 | 119 | # create general constraints structure 120 | g_struct = ct.struct_symMX([ 121 | constraints_entry, 122 | ]) 123 | 124 | # create symbolic constraint expressions 125 | map_args = collections.OrderedDict() 126 | map_args['x0'] = ct.horzcat(*w['x']) 127 | map_args['p'] = ct.horzcat(*w['u']) 128 | 129 | # evaluate function dynamics 130 | if type(self.__F) == list: 131 | F_constr = [self.__F[i](x0 = w['x',i], p = w['u',i])['xf'] for i in range(len(self.__F))] 132 | else: 133 | F_constr = ct.horzsplit(self.__F.map(self.__N, self.__parallelization)(**map_args)['xf']) 134 | 135 | # generate constraints 136 | constr = collections.OrderedDict() 137 | constr['dyn'] = [a - b for a,b in zip(F_constr, w['x',1:]+[w['x',0]])] 138 | 139 | if 'us' in self.__vars: 140 | map_args['us'] = ct.horzcat(*w['us']) 141 | if self.__h is not None: 142 | if type(self.__h) == list: 143 | constr['h'] = [self.__h[i](*[[*map_args.values()][j][:,i] for j in range(len(map_args))]) for i in range(len(self.__h))] 144 | else: 145 | constr['h'] = ct.horzsplit(self.__h.map(self.__N,self.__parallelization)(*map_args.values())) 146 | if self.__gnl is not None: 147 | if type(self.__gnl) == list: 148 | constr['g'] = [self.__gnl[i](*[[*map_args.values()][j][:,i] for j in range(len(map_args))]) for i in range(len(self.__gnl))] 149 | else: 150 | constr['g'] = ct.horzsplit(self.__gnl.map(self.__N,self.__parallelization)(*map_args.values())) 151 | 152 | # interleaving of constraints 153 | repeated_constr = list(itertools.chain.from_iterable(zip(*constr.values()))) 154 | 155 | # fill in constraint structure 156 | self.__g = g_struct(ca.vertcat(*repeated_constr)) 157 | 158 | # constraint bounds 159 | self.__lbg = g_struct(np.zeros(self.__g.shape)) 160 | self.__ubg = g_struct(np.zeros(self.__g.shape)) 161 | 162 | if self.__h is not None: 163 | self.__ubg['h',:] = np.inf 164 | 165 | # nlp cost 166 | if type(self.__cost) == list: 167 | f = sum([self.__cost[k](w['x',k], w['u',k]) for k in range(self.__N)]) 168 | else: 169 | cost_map_fun = self.__cost.map(self.__N,self.__parallelization) 170 | f = ca.sum2(cost_map_fun(map_args['x0'], map_args['p'])) 171 | 172 | # add phase fixing cost 173 | self.__construct_phase_fixing_cost() 174 | alpha = ca.MX.sym('alpha') 175 | x0star = ca.MX.sym('x0star', self.__nx, 1) 176 | f += self.__phase_fix_fun(alpha, x0star, w['x',0]) 177 | 178 | # add slack regularization 179 | # if 'us' in self.__vars: 180 | # f += self.__reg_slack*ct.mtimes(ct.vertcat(*w['us']).T,ct.vertcat(*w['us'])) 181 | 182 | # NLP parameters 183 | p = ca.vertcat(alpha, x0star) 184 | self.__w = w 185 | self.__g_fun = ca.Function('g_fun',[w,p],[self.__g]) 186 | 187 | # create IP-solver 188 | prob = {'f': f, 'g': self.__g, 'x': w, 'p': p} 189 | opts = {'ipopt':{'linear_solver':'ma57'},'expand':False} 190 | if Logger.logger.getEffectiveLevel() > 10: 191 | opts['ipopt']['print_level'] = 0 192 | opts['print_time'] = 0 193 | opts['ipopt']['sb'] = 'yes' 194 | 195 | self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts) 196 | 197 | # create SQP-solver 198 | prob['lbg'] = self.__lbg 199 | prob['ubg'] = self.__ubg 200 | self.__sqp_solver = sqp_method.Sqp(prob) 201 | 202 | 203 | return None 204 | 205 | def solve(self, w0 = None): 206 | 207 | """ 208 | Solve periodic OCP 209 | """ 210 | 211 | # initialize 212 | if w0 is None: 213 | w0 = self.__w(0.0) 214 | 215 | # no phase fix cost 216 | self.__alpha = 0.0 217 | self.__x0star = np.zeros((self.__nx,1)) 218 | p = ca.vertcat( 219 | self.__alpha, 220 | self.__x0star 221 | ) 222 | 223 | # solve OCP 224 | Logger.logger.info('IPOPT pre-solve...') 225 | self.__sol = self.__solver( 226 | x0 = w0, 227 | lbx = self.__lbw, 228 | ubx = self.__ubw, 229 | lbg = self.__lbg, 230 | ubg = self.__ubg, 231 | p = p 232 | ) 233 | 234 | # fix phase 235 | if self.__N > 1: 236 | # prepare 237 | wsol = self.__w(self.__sol['x']) 238 | self.__alpha = 0.1 239 | self.__x0star = wsol['x',0] 240 | p = ca.vertcat( 241 | self.__alpha, 242 | self.__x0star 243 | ) 244 | # solve 245 | Logger.logger.info('IPOPT pre-solve with phase-fix...') 246 | self.__sol = self.__solver( 247 | x0 = self.__sol['x'], 248 | lbx = self.__lbw, 249 | ubx = self.__ubw, 250 | lbg = self.__lbg, 251 | ubg = self.__ubg, 252 | p = p 253 | ) 254 | 255 | # solve with SQP (with active set QP solver) to retrieve active set 256 | Logger.logger.info('Solve with active-set based SQP method...') 257 | self.__sol = self.__sqp_solver.solve(self.__sol['x'], p, self.__sol['lam_g']) 258 | 259 | return self.__w(self.__sol['x']) 260 | 261 | def get_sensitivities(self): 262 | 263 | """ 264 | Extract NLP sensitivities evaluated at the solution. 265 | """ 266 | 267 | # solution 268 | wsol = self.__w(self.__sol['x']) 269 | 270 | # extract multipliers 271 | lam_g = self.__g(self.__sol['lam_g']) 272 | self.__lam_g = lam_g 273 | 274 | map_args = collections.OrderedDict() 275 | map_args['x'] = ct.horzcat(*wsol['x']) 276 | map_args['u'] = ct.horzcat(*wsol['u']) 277 | map_args['lam_g'] = ct.horzcat(*lam_g['dyn']) 278 | 279 | if 'us' in self.__vars: 280 | map_args['us'] = ct.horzcat(*wsol['us']) 281 | map_args['lam_s'] = ct.horzcat(*lam_g['g']) 282 | 283 | # sensitivity dict 284 | S = {} 285 | 286 | # dynamics sensitivities 287 | S['A'] = np.split(self.__jac_Fx( 288 | map_args['x'], 289 | map_args['u'] 290 | ).full(), self.__N, axis = 1) 291 | 292 | S['B'] = np.split(self.__jac_Fu( 293 | map_args['x'], 294 | map_args['u'] 295 | ).full(), self.__N, axis = 1) 296 | 297 | # extract active constraints 298 | if self.__h is not None: 299 | 300 | # add slacks to function args if necessary 301 | if 'us' in self.__vars: 302 | args = [map_args['x'],map_args['u'],map_args['us']] 303 | else: 304 | args = [map_args['x'],map_args['u']] 305 | 306 | # compute gradient of constraints 307 | mu_s = lam_g['h'] 308 | S['C'] = np.split(self.__jac_h(*args).full(), self.__N, axis = 1) 309 | if type(self.__h) != list: 310 | S['e'] = np.split(self.__h(*args).full(), self.__N, axis = 1) 311 | else: 312 | S['e'] = [self.__h[k](*[args[j][:,k] for j in range(len(args))]) for k in range(self.__N)] 313 | if 'g' in lam_g.keys(): 314 | lam_s = lam_g['g'] 315 | S['G'] = np.split(self.__jac_g(*args).full(), self.__N, axis = 1) 316 | if type(self.__gnl) == list: 317 | S['r'] = [self.__gnl[k](*[args[j][:,k] for j in range(len(args))]) for k in range(self.__N)] 318 | else: 319 | S['r'] = np.split(self.__gnl(*args).full(), self.__N, axis = 1) 320 | else: 321 | S['G'] = None 322 | S['r'] = None 323 | 324 | # retrieve active set 325 | C_As = [] 326 | self.__indeces_As = [] 327 | for k in range(self.__N): 328 | C_active = [] 329 | index_active = [] 330 | for i in range(mu_s[k].shape[0]): 331 | if np.abs(mu_s[k][i].full()) > self.__mu_tresh: 332 | C_active.append(S['C'][k][i,:]) 333 | index_active.append(i) 334 | if len(C_active) > 0: 335 | C_As.append(ca.horzcat(*C_active).full().T) 336 | else: 337 | C_As.append(None) 338 | self.__indeces_As.append(index_active) 339 | S['C_As'] = C_As 340 | 341 | else: 342 | S['C'] = None 343 | S['C_As'] = None 344 | S['G'] = None 345 | S['e'] = None 346 | S['r'] = None 347 | self.__indeces_As = None 348 | 349 | # compute hessian of lagrangian 350 | H = self.__sol['S']['H'] 351 | M = self.__nx + self.__nu 352 | if 'us' in self.__vars: 353 | M += self.__ns 354 | S['H'] = [H[i*M:(i+1)*M,i*M:(i+1)*M] for i in range(self.__N)] 355 | 356 | # compute cost function gradient 357 | if self.__h is not None: 358 | S['q'] = [- ca.mtimes(lam_g['h',i].T,S['C'][i]) for i in range(self.__N)] 359 | else: 360 | S['q'] = [np.zeros((1,self.__nx+self.__nu)) for i in range(self.__N)] 361 | 362 | return S 363 | 364 | def __construct_sensitivity_funcs(self): 365 | 366 | """ Construct functions for NLP sensitivity evaluations 367 | """ 368 | 369 | # system variables 370 | x, u = self.__vars['x'], self.__vars['u'] 371 | nx = x.shape[0] 372 | wk = ca.vertcat(x,u) 373 | if 'us' in self.__vars: 374 | us = self.__vars['us'] 375 | uhat = ca.vertcat(u, us) 376 | wk = ca.vertcat(wk, us) 377 | else: 378 | uhat = u 379 | 380 | # dynamics sensitivities 381 | if type(self.__F) != list: 382 | x_next = self.__F(x0 = x, p = u)['xf'] # symbolic integrator evaluation 383 | self.__jac_Fx = ca.Function('jac_Fx',[x,u],[ca.jacobian(x_next,x)]).map(self.__N) 384 | self.__jac_Fu = ca.Function('jac_Fu',[x,u],[ca.jacobian(x_next,uhat)]).map(self.__N) 385 | else: 386 | x_map = ca.MX.sym('x_map', self.__nx, self.__N) 387 | u_map = ca.MX.sym('u_map', self.__nu, self.__N) 388 | x_next = [self.__F[k](x0 = x_map[:,k], p = u_map[:,k])['xf'] for k in range(self.__N)] 389 | jac_Fx = [ca.jacobian(x_next[k],x_map)[:,k*self.__nx:(k+1)*self.__nx] for k in range(self.__N)] 390 | jac_Fu = [ca.jacobian(x_next[k],u_map)[:,k*self.__nu:(k+1)*self.__nu] for k in range(self.__N)] 391 | if 'us' in self.__vars: 392 | jac_Fu = [ct.horzcat(jac_Fu[k], ca.MX.zeros(self.__nx, self.__ns)) for k in range(self.__N)] 393 | self.__jac_Fx = ca.Function('jac_Fx',[x_map,u_map],[ct.horzcat(*jac_Fx)]) 394 | self.__jac_Fu = ca.Function('jac_Fu',[x_map,u_map],[ct.horzcat(*jac_Fu)]) 395 | 396 | # constraints sensitivities 397 | if self.__h is not None: 398 | if type(self.__h) != list: 399 | if 'us' in self.__vars: 400 | constr = self.__h(x,u,us) # symbolic constraint evaluation 401 | self.__jac_h = ca.Function('jac_h',[x,u,us], [ca.jacobian(constr,wk)]).map(self.__N) 402 | else: 403 | constr = self.__h(x,u) 404 | self.__jac_h = ca.Function('jac_h',[x,u], [ca.jacobian(constr,wk)]).map(self.__N) 405 | else: 406 | x_map = ca.MX.sym('x_map', self.__nx, self.__N) 407 | u_map = ca.MX.sym('u_map', self.__nu, self.__N) 408 | if 'us' in self.__vars: 409 | us_map = ca.MX.sym('us_map', self.__ns, self.__N) 410 | constr = [self.__h[k](x_map[:,k],u_map[:,k],us_map[:,k]) for k in range(self.__N)] 411 | jac_hx = [ca.jacobian(constr[k],x_map)[:,k*self.__nx:(k+1)*self.__nx] for k in range(self.__N)] 412 | jac_hu = [ca.jacobian(constr[k],u_map)[:,k*self.__nu:(k+1)*self.__nu] for k in range(self.__N)] 413 | jac_hus = [ca.jacobian(constr[k],us_map)[:,k*self.__ns:(k+1)*self.__ns] for k in range(self.__N)] 414 | jac_h = [ct.horzcat(jac_hx[k], jac_hu[k], jac_hus[k]) for k in range(self.__N)] 415 | self.__jac_h = ca.Function('jac_h', [x_map, u_map, us_map], [ct.horzcat(*jac_h)]) 416 | else: 417 | constr = [self.__h[k](x_map[:,k],u_map[:,k]) for k in range(self.__N)] 418 | jac_hx = [ca.jacobian(constr[k],x_map)[:,k*self.__nx:(k+1)*self.__nx] for k in range(self.__N)] 419 | jac_hu = [ca.jacobian(constr[k],u_map)[:,k*self.__nu:(k+1)*self.__nu] for k in range(self.__N)] 420 | jac_h = [ct.horzcat(jac_hx[k], jac_hu[k]) for k in range(self.__N)] 421 | self.__jac_h = ca.Function('jac_h', [x_map, u_map], [ct.horzcat(*jac_h)]) 422 | 423 | if self.__gnl is not None: 424 | if type(self.__gnl) == list: 425 | constr = [self.__gnl[k](x_map[:,k],u_map[:,k],us_map[:,k]) for k in range(self.__N)] 426 | jac_gx = [ca.jacobian(constr[k],x_map)[:,k*self.__nx:(k+1)*self.__nx] for k in range(self.__N)] 427 | jac_gu = [ca.jacobian(constr[k],u_map)[:,k*self.__nu:(k+1)*self.__nu] for k in range(self.__N)] 428 | jac_gus = [ca.jacobian(constr[k],us_map)[:,k*self.__ns:(k+1)*self.__ns] for k in range(self.__N)] 429 | jac_g = [ct.horzcat(jac_gx[k], jac_gu[k], jac_gus[k]) for k in range(self.__N)] 430 | self.__jac_g = ca.Function('jac_g', [x_map, u_map, us_map], [ct.horzcat(*jac_g)]) 431 | else: 432 | constr = self.__gnl(x,u,us) 433 | self.__jac_g = ca.Function('jac_g',[x,u,us], [ca.jacobian(constr,wk)]).map(self.__N) 434 | 435 | 436 | return None 437 | 438 | def __construct_phase_fixing_cost(self): 439 | 440 | """ 441 | Construct phase fixing cost function part. 442 | Only applied to initial state. 443 | """ 444 | 445 | alpha = ca.MX.sym('alpha') 446 | x0star = ca.MX.sym('x0star', self.__nx, 1) 447 | x0 = ca.MX.sym('x0star', self.__nx, 1) 448 | phase_fix = 0.5*alpha * ca.mtimes((x0 - x0star).T,x0 - x0star) 449 | self.__phase_fix_fun = ca.Function('phase_fix', [alpha, x0star, x0], [phase_fix]) 450 | 451 | return None 452 | 453 | @property 454 | def w(self): 455 | "OCP variables structure" 456 | return self.__w 457 | 458 | @property 459 | def indeces_As(self): 460 | "Active constraints indeces along trajectory" 461 | return self.__indeces_As 462 | 463 | @property 464 | def lam_g(self): 465 | "Constraints lagrange multipliers" 466 | return self.__lam_g 467 | 468 | @property 469 | def g(self): 470 | "Constraints" 471 | return self.__g 472 | 473 | @property 474 | def g_fun(self): 475 | "Constraints function" 476 | return self.__g_fun -------------------------------------------------------------------------------- /tunempc/preprocessing.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | """ 23 | Preprocessing routines of provided user data 24 | 25 | :author: Jochem De Schutter 26 | """ 27 | 28 | import casadi as ca 29 | import casadi.tools as ct 30 | import itertools 31 | import numpy as np 32 | import collections 33 | from tunempc.logger import Logger 34 | 35 | def input_formatting(sys): 36 | 37 | # TODO: perform some tests? 38 | 39 | # system dimensions 40 | if type(sys['f']) == list: 41 | fsize = sys['f'][0] 42 | else: 43 | fsize = sys['f'] 44 | nx = fsize.size1_in(0) 45 | nu = fsize.size1_in(1) 46 | sys['vars'] = collections.OrderedDict() 47 | sys['vars']['x'] = ca.MX.sym('x',nx) 48 | sys['vars']['u'] = ca.MX.sym('u',nu) 49 | 50 | # process path constraints 51 | if 'h' in sys: 52 | 53 | # detect and sort out nonlinear inequalities 54 | sys['g'], sys['h'] = detect_nonlinear_inequalities(sys['h']) 55 | 56 | if sys['g'].count(None) == len(sys['g']): 57 | del(sys['g']) 58 | else: 59 | # add slacks to system variables 60 | sys['vars']['us'] = [] 61 | for k in range(len(sys['g'])): 62 | if sys['g'][k] is not None: 63 | ns = sys['g'][k].size1_in(2) 64 | else: 65 | ns = 0 66 | 67 | # TODO: allow for varying dimension of g 68 | sys['vars']['us'] = ca.MX.sym('us',ns) 69 | 70 | # unpack lists for len == 1 71 | if len(sys['h']) == 1: 72 | sys['h'] = sys['h'][0] 73 | if 'g' in sys: 74 | sys['g'] = sys['g'][0] 75 | 76 | return sys 77 | 78 | def detect_nonlinear_inequalities(h): 79 | 80 | # make constraints "time-varying" 81 | if type(h) is not list: 82 | h = [h] 83 | 84 | # initialize update inequality list 85 | h_new = [] 86 | g_new = [] 87 | 88 | # iterate over constraints 89 | for k in range(len(h)): 90 | x = ca.MX.sym('x', h[k].size1_in(0)) 91 | u = ca.MX.sym('u', h[k].size1_in(1)) 92 | h_expr = h[k](x,u) 93 | 94 | # sort constraints according to (non-)linearity 95 | h_nlin = [] 96 | h_lin = [] 97 | for i in range(h_expr.shape[0]): 98 | if True in ca.which_depends(h_expr[i],ca.vertcat(x,u),2): 99 | h_nlin.append(h_expr[i]) 100 | else: 101 | h_lin.append(h_expr[i]) 102 | 103 | # update control vector (add slacks for nonlinear inequalities) 104 | if len(h_nlin) > 0: 105 | 106 | # function for nonlinear slacked inequalities 107 | s = ca.MX.sym('s', len(h_nlin)) 108 | g_new.append(ca.Function('g',[x,u,s], [ca.vertcat(*h_nlin) - s])) 109 | 110 | # function for linear inequalities 111 | h_lin.append(s) # slacks > 0 112 | h_new.append(ca.Function('h', [x,u,s], [ca.vertcat(*h_lin)])) 113 | 114 | else: 115 | g_new.append(None) 116 | h_new.append(h[k]) 117 | 118 | return g_new, h_new 119 | 120 | def add_mpc_slacks(sys, lam_g, active_set, slack_flag = 'active'): 121 | 122 | """ 123 | Add slacks to (all, active or none of) the linear inequality constraints 124 | in order to implement soft constraints in the MPC controller. 125 | """ 126 | 127 | if ('h' not in sys) or (slack_flag == 'none'): 128 | return sys 129 | 130 | active_constraints = set(itertools.chain(*active_set)) 131 | slack_condition = lambda x: (slack_flag == 'all') or \ 132 | ((slack_flag == 'active') and (x in active_constraints)) 133 | h_args = sys['vars'].values() 134 | h_expr = sys['h'](*h_args) 135 | 136 | slacks = list(map(slack_condition, range(h_expr.shape[0]))) 137 | if sum(slacks) > 0: 138 | usc = ca.MX.sym('usc', sum(slacks)) 139 | slack_cost = [] 140 | h_slack = [] 141 | j = 0 142 | for i in range(h_expr.shape[0]): 143 | if slacks[i]: 144 | h_slack.append(usc[j]) 145 | slack_cost.append(1e3*np.max(-ca.vertcat(*lam_g['h',:,i]).full())) 146 | j = j+1 147 | else: 148 | h_slack.append(0.0) 149 | 150 | h_new = ca.vertcat(h_expr + ca.vertcat(*h_slack), usc) 151 | sys['h'] = ca.Function('h', [*h_args, usc], [h_new]) 152 | sys['scost'] = ct.vertcat(*slack_cost) 153 | sys['vars']['usc'] = usc 154 | 155 | return sys 156 | 157 | def input_checks(arg): 158 | 159 | """ 160 | Input checks for provided convexification matrices A, B, Q, R, N, G, (C). 161 | 162 | - Check if all provided matrices are of same type (list vs. single matrix) 163 | - Check if size of matrices is consistent troughout time 164 | - Return matrices as lists 165 | """ 166 | 167 | msg1 = "Input arguments should be of same type!" 168 | assert (all(type(argument)==type(arg['A']) for key, argument in arg.items())), msg1 169 | 170 | if type(arg['A']) == list: 171 | msg2 = "Input data lists should have same length!" 172 | assert (all(len(argument)==len(arg['A']) for key, argument in arg.items())), msg2 173 | else: 174 | for key in list(arg.keys()): 175 | arg[key] = [arg[key]] 176 | 177 | # check matrix size consistency 178 | msg3 = "Data matrices should have same size along trajectory." 179 | for key, argument in arg.items(): 180 | if key != 'C': 181 | assert (all(mat.shape==argument[0].shape for mat in argument)), msg3 182 | 183 | # TODO: check A and B, Q, R, N on consistency 184 | 185 | return arg -------------------------------------------------------------------------------- /tunempc/sqp_method.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | """ 23 | SQP method 24 | 25 | :author: Jochem De Schutter (2019), based on implementation by Mario Zanon (2016). 26 | """ 27 | 28 | 29 | import casadi.tools as ct 30 | import casadi as ca 31 | import numpy as np 32 | from scipy.linalg import eig 33 | from scipy.linalg import null_space 34 | from tunempc.logger import Logger 35 | 36 | class Sqp(object): 37 | 38 | def __init__(self, problem, options = {}): 39 | 40 | """ Constructor 41 | """ 42 | 43 | # problem data 44 | self.__w = problem['x'] 45 | self.__f = problem['f'] 46 | self.__g = problem['g'] 47 | self.__p = problem['p'] 48 | self.__lbg = problem['lbg'] 49 | self.__ubg = problem['ubg'] 50 | 51 | # default settings 52 | self.__options = { 53 | 'regularization': 'reduced', 54 | 'regularization_tol': 1e-8, 55 | 'tol': 1e-6, 56 | 'lam_tresh': 1e-8, 57 | 'max_ls_iter': 300, 58 | 'ls_step_factor': 0.8, 59 | 'hessian_approximation': 'exact', 60 | 'max_iter': 2000 61 | } 62 | 63 | # overwrite default 64 | for opt in options: 65 | self.__options[opt] = options[opt] 66 | 67 | self.__construct_sensitivities() 68 | self.__construct_qp_solver() 69 | 70 | return None 71 | 72 | def __construct_sensitivities(self): 73 | 74 | """ Construct NLP sensitivities 75 | """ 76 | 77 | # convenience 78 | w = self.__w 79 | p = self.__p 80 | 81 | # cost function 82 | self.__f_fun = ca.Function('f_fun',[w,p], [self.__f]) 83 | self.__jacf_fun = ca.Function('jacf_fun',[w,p], [ca.jacobian(self.__f,self.__w)]) 84 | 85 | # constraints 86 | self.__g_fun = ca.Function('g_fun',[w,p],[self.__g]) 87 | self.__jacg_fun = ca.Function('jacg_fun',[w,p], [ca.jacobian(self.__g,self.__w)]) 88 | self.__gzeros = np.zeros((self.__g.shape[0],1)) 89 | 90 | # exact hessian 91 | lam_g = ca.MX.sym('lam_g',self.__g.shape) 92 | lag = self.__f + ct.mtimes(lam_g.T, self.__g) 93 | self.__jlag_fun = ca.Function('jLag',[w,p,lam_g],[ca.jacobian(lag,w)]) 94 | 95 | if self.__options['hessian_approximation'] == 'exact': 96 | self.__H_fun = ca.Function('H_fun',[w,p,lam_g],[ca.hessian(lag,w)[0]]) 97 | else: 98 | self.__H_fun = self.__options['hessian_approximation'] 99 | 100 | def __construct_qp_solver(self): 101 | 102 | Hlag = self.__H_fun(self.__w, self.__p, ca.MX.sym('lam_g',self.__g.shape)) 103 | jacg = self.__jacg_fun(self.__w, self.__p) 104 | 105 | # qp cost function and constraints 106 | qp = { 107 | 'h': Hlag.sparsity(), 108 | 'a': jacg.sparsity() 109 | } 110 | 111 | # qp options 112 | opts = { 113 | 'enableEqualities':True, 114 | 'printLevel':'none', 115 | 'sparse': True, 116 | 'enableInertiaCorrection':True, 117 | 'enableCholeskyRefactorisation':1, 118 | # 'enableRegularisation': True, 119 | # 'epsRegularisation': 1e-6 120 | # 'enableFlippingBounds':True 121 | # 'enableFarBounds': False, 122 | # 'enableFlippingBounds': True, 123 | # 'epsFlipping': 1e-8, 124 | # 'initialStatusBounds': 'inactive', 125 | } 126 | 127 | self.__solver = ca.conic( 128 | 'qpsol', 129 | 'qpoases', 130 | qp, 131 | opts 132 | ) 133 | 134 | return None 135 | 136 | def solve(self, w0, p0, lam_g_ip): 137 | 138 | """ Vanilla SQP-method, no line-search. 139 | """ 140 | 141 | # Pre-filter multipliers from interior-point method 142 | lam_g0 = self.__prefilter_lam_g(lam_g_ip) 143 | 144 | # Perform SQP iterations 145 | converged = self.__check_convergence(w0,p0,lam_g0,0.0, 0) 146 | converged = False 147 | 148 | k = 0 149 | while not converged: 150 | 151 | # evaluate constraints residuals 152 | g0 = self.__g_fun(w0,p0) 153 | 154 | # regularize hessian 155 | H = self.__regularize_hessian(w0, p0, lam_g0) 156 | 157 | # qp matrices 158 | qp_p = { 159 | 'h': H, 160 | 'g': self.__jacf_fun(w0,p0), 161 | 'a': self.__jacg_fun(w0,p0), 162 | 'lba': self.__lbg - g0, 163 | 'uba': self.__ubg - g0, 164 | 'lam_a0': lam_g0 165 | } 166 | 167 | # solve qp 168 | res = self.__solver(**qp_p) 169 | 170 | # perform line-search 171 | dw = self.__linesearch(w0, p0, res['x'].full()) 172 | 173 | # check convergence 174 | w0 = w0 + dw 175 | lam_g0 = res['lam_a'] 176 | 177 | k += 1 178 | converged = self.__check_convergence(w0,p0,lam_g0,dw, k) 179 | 180 | # Solution sanity check 181 | S = self.__postprocessing(w0,p0,lam_g0, k) 182 | 183 | return {'x':w0, 'lam_g': lam_g0, 'S': S} 184 | 185 | def __postprocessing(self, w0, p0, lam_g0, iter): 186 | 187 | """ Perform sanity checks on solution and compute sensitivities 188 | """ 189 | 190 | # compute sensitivities 191 | H = self.__H_fun(w0,p0,lam_g0) 192 | 193 | # check if reduced hessian is PD 194 | jacg_active, as_idx = self.__active_constraints_jacobian(w0,p0,lam_g0) 195 | Z = null_space(jacg_active) 196 | Hred = ct.mtimes(ct.mtimes(Z.T, H),Z) 197 | 198 | # check eigenvalues of reduced hessian 199 | if Hred.shape[0] > 0: 200 | min_eigval = np.min(np.linalg.eigvals(Hred)) 201 | assert min_eigval > self.__options['regularization_tol'], 'Reduced Hessian is not positive definite!' 202 | 203 | # retrieve active set changes w.r.t initial guess 204 | nAC = len([k for k in self.__as_idx_init if k not in as_idx]) 205 | nAC += len([k for k in as_idx if k not in self.__as_idx_init]) 206 | 207 | # save stats 208 | info = self.__solver.stats() 209 | self.__stats = { 210 | 'x': w0, 211 | 'lam_g': lam_g0, 212 | 'p0': p0, 213 | 'iter_count': iter, 214 | 'f': self.__f_fun(w0,p0), 215 | 't_wall_total': info['t_wall_solver'], 216 | 'return_status': info['return_status'], 217 | 'nAC': nAC, 218 | 'nAS': len(as_idx) 219 | } 220 | 221 | return {'H': H} 222 | 223 | def __prefilter_lam_g(self, lam_g0): 224 | 225 | """ Prefilter multipliers obtained from the interior-point solver 226 | for use in the active-set SQP-method. Due to limited accuracy of 227 | the IP-solver, multipliers of passive constraints are not exactly zero. 228 | """ 229 | 230 | import copy 231 | lam_g0 = copy.deepcopy(lam_g0) 232 | 233 | # filter multipliers with treshold value 234 | for i in range(lam_g0.shape[0]): 235 | if abs(lam_g0[i]) < self.__options['lam_tresh']: 236 | lam_g0[i] = 0.0 237 | 238 | return lam_g0 239 | 240 | def __check_convergence(self, w0, p0, lam_g0, dw, k): 241 | 242 | """ Check convergence of SQP algorithm and print stats. 243 | """ 244 | 245 | # dual infeasibility 246 | dual_infeas = np.linalg.norm(self.__jlag_fun(w0,p0,lam_g0).full(),np.inf) 247 | 248 | if k == 0: 249 | f0 = self.__f_fun(w0,p0) 250 | g0 = self.__g_fun(w0,p0) 251 | lbg = self.__lbg.cat.full() 252 | ubg = self.__ubg.cat.full() 253 | infeas = np.linalg.norm( 254 | abs(np.array([*map(min,np.array(g0) - lbg,self.__gzeros)]) + 255 | np.array([*map(max,np.array(g0) - ubg,self.__gzeros)])), 256 | np.inf) 257 | 258 | self.__ls_filter = np.array([[ f0, infeas ]]) 259 | self.__alpha = 0.0 260 | self.__reg = 0.0 261 | _, self.__as_idx_init = self.__active_constraints_jacobian(w0,p0,lam_g0) 262 | 263 | # print stats 264 | if k%10 == 0: 265 | Logger.logger.debug('iter\tf\t\tstep\t\tinf_du\t\tinf_pr\t\talpha\t\treg') 266 | Logger.logger.debug('{:3d}\t{:.4e}\t{:.4e}\t{:.4e}\t{:.4e}\t{:.2e}\t{:.2e}'.format( 267 | k, 268 | self.__ls_filter[-1,0], 269 | np.linalg.norm(dw), 270 | dual_infeas, 271 | self.__ls_filter[-1,1], 272 | self.__alpha, 273 | self.__reg)) 274 | 275 | # check convergence 276 | if (self.__ls_filter[-1,1] < self.__options['tol'] and 277 | dual_infeas < self.__options['tol']): 278 | converged = True 279 | if k != 0: 280 | Logger.logger.debug('Optimal solution found.') 281 | elif k == self.__options['max_iter']: 282 | converged = True 283 | Logger.logger.debug('Maximum number of iterations exceeded.') 284 | else: 285 | converged = False 286 | 287 | return converged 288 | 289 | def __linesearch(self, w0, p0, dw): 290 | 291 | alpha = 1.0 292 | wnew = w0 + alpha*dw 293 | 294 | # filter method 295 | f0 = self.__f_fun(wnew,p0) 296 | g0 = self.__g_fun(wnew,p0) 297 | lbg = self.__lbg.cat.full() 298 | ubg = self.__ubg.cat.full() 299 | infeas = np.linalg.norm( 300 | abs(np.array([*map(min,np.array(g0) - lbg,self.__gzeros)]) + 301 | np.array([*map(max,np.array(g0) - ubg,self.__gzeros)])), 302 | np.inf) 303 | 304 | for ls_k in range(self.__options['max_ls_iter']): 305 | check = np.array(list(map(int,float(f0) > self.__ls_filter[:,0]))) \ 306 | + np.array(list(map(int, infeas > self.__ls_filter[:,1] ))) 307 | check = check.tolist() 308 | check = [check[i] > 1.5 for i in range(len(check))] 309 | if sum(check) > 1: # Allow to worsen the filter for max 0 iterations 310 | alpha *= self.__options['ls_step_factor'] 311 | # iter_btls += 1 312 | wnew = w0 + alpha*dw 313 | f0 = self.__f_fun(wnew,p0) 314 | g0 = self.__g_fun(wnew,p0) 315 | infeas = np.linalg.norm( 316 | abs(np.array([*map(min,np.array(g0) - lbg,self.__gzeros)]) + 317 | np.array([*map(max,np.array(g0) - ubg,self.__gzeros)])), 318 | np.inf) 319 | else: 320 | break 321 | 322 | self.__alpha = alpha 323 | self.__ls_filter = np.append( self.__ls_filter, np.array([[ f0, infeas ]]), axis=0 ) 324 | 325 | return alpha*dw 326 | 327 | def __regularize_hessian(self, w0, p0, lam_g): 328 | 329 | # evaluate hessian 330 | H = self.__H_fun(w0,p0,lam_g).full() 331 | 332 | # eigenvalue tolerance 333 | tol = self.__options['regularization_tol'] 334 | 335 | if self.__options['regularization'] == 'reduced': 336 | 337 | # active constraints jacobian 338 | jacg_active, _ = self.__active_constraints_jacobian(w0,p0,lam_g) 339 | 340 | # compute reduced hessian 341 | Z = null_space(jacg_active) 342 | Hr = np.dot(np.dot(Z.T, H), Z) 343 | 344 | # compute eigenvalues 345 | if Hr.shape[0] != 0: 346 | eva, evec = eig(Hr) 347 | regularize = (min(eva.real) < tol) 348 | else: 349 | regularize = False 350 | 351 | # check if regularization is needed 352 | if regularize: 353 | 354 | # detect non-positive eigenvalues 355 | evmod = eva*0 356 | for i in range(len(eva)): 357 | if eva[i] < tol: 358 | evmod[i] = tol 359 | else: 360 | evmod[i] = eva[i] 361 | 362 | # eigenvalue regularization vector 363 | deva = evmod - eva 364 | self._reg = max(deva.real) 365 | dHr = np.dot(np.dot(evec,np.diag(deva)),np.linalg.inv(evec)) 366 | H = H + np.dot(np.dot( Z, dHr), Z.T ) 367 | 368 | # make sure that the Hessian is symmetric 369 | H = (H.real + H.real.T)/2.0 370 | 371 | # check regularized reduced hessian to be sure 372 | rH = np.dot(np.dot( Z.T, H ), Z) 373 | eva_r, evec = eig(rH) 374 | for e in eva_r: 375 | if e < tol/1e2: 376 | print('Regularization of reduced Hessian failed. Eigenvalue: {}'.format(e)) 377 | 378 | 379 | elif self.__options['regularization'] == 'full': 380 | 381 | # comput eigenvaluescc 382 | eva, evec = eig(H) 383 | 384 | # detect non-positive eigenvalues 385 | evmod = eva*0 386 | for i in range(len(eva)): 387 | if eva[i] < tol: 388 | evmod[i] = tol 389 | else: 390 | evmod[i] = eva[i] 391 | 392 | # eigenvalue regularization vector 393 | deva = evmod - eva 394 | self.__reg = max(deva.real) 395 | H = H + np.dot(np.dot(evec,np.diag(deva)),np.linalg.inv(evec)) 396 | 397 | # make sure that the Hessian is symmetric 398 | H = (H.real + H.real.T)/2.0 399 | 400 | else: 401 | self.__reg = 0.0 402 | 403 | return H 404 | 405 | def __active_constraints_jacobian(self, w0, p0, lam_g): 406 | 407 | # evaluate constraints jacobian 408 | jacg = self.__jacg_fun(w0,p0).full() 409 | 410 | # constraint bounds 411 | bounds = (self.__ubg.cat - self.__lbg.cat).full() 412 | 413 | # equality constraints 414 | eq_idx = [i for i, e in enumerate(bounds) if e == 0] 415 | jacg_active = jacg[eq_idx,:] 416 | 417 | # inequality constraints 418 | ineq_idx = [i for i, e in enumerate(bounds) if e != 0] 419 | as_idx = [] 420 | for i in ineq_idx: 421 | if lam_g[i] != 0: 422 | jacg_active = np.append(jacg_active,[jacg[i,:]], axis = 0) 423 | as_idx.append(i) 424 | 425 | return jacg_active, as_idx 426 | 427 | @property 428 | def ls_filter(self): 429 | return self.__ls_filter 430 | 431 | @property 432 | def stats(self): 433 | return self.__stats -------------------------------------------------------------------------------- /tunempc/tuner.py: -------------------------------------------------------------------------------- 1 | # 2 | # This file is part of TuneMPC. 3 | # 4 | # TuneMPC -- A Tool for Economic Tuning of Tracking (N)MPC Problems. 5 | # Copyright (C) 2020 Jochem De Schutter, Mario Zanon, Moritz Diehl (ALU Freiburg). 6 | # 7 | # TuneMPC is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # 12 | # TuneMPC is distributed in the hope that it will be useful, 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | # Lesser General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU Lesser General Public 18 | # License along with TuneMPC; if not, write to the Free Software Foundation, 19 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 20 | # 21 | # 22 | """ 23 | Tuner high level class 24 | 25 | :author: Jochem De Schutter 26 | """ 27 | 28 | import tunempc.preprocessing as preprocessing 29 | import tunempc.pocp as pocp 30 | import tunempc.convexifier as convexifier 31 | import tunempc.pmpc as pmpc 32 | import tunempc.mtools as mtools 33 | import casadi as ca 34 | import casadi.tools as ct 35 | import numpy as np 36 | import copy 37 | from tunempc.logger import Logger 38 | 39 | class Tuner(object): 40 | 41 | def __init__(self, f, l, h = None, p = 1): 42 | 43 | """ Constructor 44 | """ 45 | 46 | # print license information 47 | self.__log_license_info() 48 | 49 | Logger.logger.info(60*'=') 50 | Logger.logger.info(18*' '+'Create Tuner instance...') 51 | Logger.logger.info(60*'=') 52 | Logger.logger.info('') 53 | 54 | # construct system 55 | sys = {'f': f} 56 | if h is not None: 57 | sys['h'] = h 58 | 59 | # detect nonlinear constraints 60 | Logger.logger.info('Detect nonlinear constraints...') 61 | 62 | self.__sys = preprocessing.input_formatting(sys) 63 | 64 | # problem dimensions 65 | self.__nx = sys['vars']['x'].shape[0] 66 | self.__nu = sys['vars']['u'].shape[0] 67 | self.__nw = self.__nx + self.__nu 68 | self.__p = p 69 | if 'us' in sys['vars'].keys(): 70 | self.__nus = sys['vars']['us'].shape[0] 71 | else: 72 | self.__nus = 0 73 | 74 | # construct p-periodic OCP 75 | if self.__p == 1: 76 | Logger.logger.info('Construct steady-state optimization problem...') 77 | else: 78 | Logger.logger.info('Construct {}-periodic optimal control problem...'.format(self.__p)) 79 | 80 | 81 | self.__l = l 82 | self.__ocp = pocp.Pocp(sys = sys, cost = l, period = p) 83 | 84 | Logger.logger.info('') 85 | Logger.logger.info('Tuner instance created:') 86 | self.__log_problem_dimensions() 87 | 88 | return None 89 | 90 | def solve_ocp(self, w0 = None, lam0 = None): 91 | 92 | """ Solve periodic optimal control problem. 93 | """ 94 | 95 | # check input dimensions 96 | w0_shape = self.__p*(self.__nx+self.__nu) 97 | assert w0.shape[0] == w0_shape, \ 98 | 'Incorrect dimensions of input variable w0: expected {}x1, \ 99 | but reveived {}x{}'.format(w0_shape, w0.shape[0], w0.shape[1]) 100 | 101 | # initialize 102 | w_init = self.__ocp.w(0.0) 103 | if w0 is not None: 104 | 105 | for k in range(self.__p): 106 | 107 | # initialize primal variables 108 | w_init['x',k] = w0[k*self.__nw: k*self.__nw+self.__nx] 109 | w_init['u',k] = w0[k*self.__nw+self.__nx:(k+1)*self.__nw] 110 | 111 | # initialize slacks 112 | if 'g' in self.__sys: 113 | if type(self.__sys['g']) == list: 114 | w_init['us',k] = self.__sys['g'][k](w_init['x',k], w_init['u',k],0.0) 115 | else: 116 | w_init['us',k] = self.__sys['g'](w_init['x',k], w_init['u',k],0.0) 117 | 118 | # solve OCP 119 | Logger.logger.info(60*'=') 120 | Logger.logger.info(15*' '+'Solve optimization problem...') 121 | Logger.logger.info(60*'=') 122 | Logger.logger.info('') 123 | 124 | self.__w_sol = self.__ocp.solve(w0=w_init) 125 | self.__S = self.__ocp.get_sensitivities() 126 | self.__sys['S'] = self.__S 127 | 128 | Logger.logger.info('') 129 | Logger.logger.info('Optimization problem solved.') 130 | Logger.logger.info('') 131 | 132 | return self.__w_sol 133 | 134 | def convexify(self, rho = 1.0, force = False, solver = 'cvxopt'): 135 | 136 | """ Compute positive definite stage cost matrices for a tracking NMPC 137 | scheme that is locally first-order equivalent to economic MPC. 138 | """ 139 | 140 | 141 | # extract POCP sensitivities at optimal solution 142 | S = self.__S 143 | 144 | # extract Q, R, N 145 | Q = [S['H'][i][:self.__nx,:self.__nx] for i in range(self.__p)] 146 | R = [S['H'][i][self.__nx:,self.__nx:] for i in range(self.__p)] 147 | N = [S['H'][i][:self.__nx, self.__nx:] for i in range(self.__p)] 148 | 149 | # convexifier options 150 | opts = {'rho': rho, 'solver': solver, 'force': force} 151 | 152 | Logger.logger.info(60*'=') 153 | Logger.logger.info(15*' '+'Convexify Lagrangian Hessians...') 154 | Logger.logger.info(60*'=') 155 | Logger.logger.info('') 156 | 157 | dHc, _, _, _ = convexifier.convexify(S['A'], S['B'], Q, R, N, C = S['C_As'], G = S['G'], opts=opts) 158 | S['Hc'] = [(S['H'][i] + dHc[i]).full() for i in range(self.__p)] # build tuned MPC hessian 159 | 160 | return S['Hc'] 161 | 162 | def create_mpc(self, mpc_type, N, opts = {}, tuning = None): 163 | 164 | """ Create MPC controller of user-defined type and horizon 165 | for given system dynamics, constraints and cost function. 166 | """ 167 | 168 | if mpc_type not in ['economic','tuned','tracking']: 169 | raise ValueError('Provided MPC type not supported.') 170 | 171 | if 'slack_flag' in opts.keys(): 172 | mpc_sys = preprocessing.add_mpc_slacks( 173 | copy.deepcopy(self.__sys), 174 | self.__ocp.lam_g, 175 | self.__ocp.indeces_As, 176 | slack_flag = opts['slack_flag'] 177 | ) 178 | else: 179 | mpc_sys = self.__sys 180 | 181 | if mpc_type == 'economic': 182 | mpc = pmpc.Pmpc(N = N, sys = mpc_sys, cost = self.__l, wref = self.__w_sol, 183 | lam_g_ref=self.__ocp.lam_g, sensitivities = self.__S, options=opts) 184 | else: 185 | cost = mtools.tracking_cost(self.__nx+self.__nu+self.__nus) 186 | lam_g0 = copy.deepcopy(self.__ocp.lam_g) 187 | lam_g0['dyn'] = 0.0 188 | if 'g' in lam_g0.keys(): 189 | lam_g0['g'] = 0.0 190 | 191 | if mpc_type == 'tracking': 192 | if tuning is None: 193 | raise ValueError('Tracking type MPC controller requires user-provided tuning!') 194 | elif mpc_type == 'tuned': 195 | tuning = {'H': self.__S['Hc'], 'q': self.__S['q']} 196 | mpc = pmpc.Pmpc(N = N, sys = mpc_sys, cost = cost, wref = self.__w_sol, 197 | tuning = tuning, lam_g_ref=lam_g0, sensitivities = self.__S, options=opts) 198 | 199 | return mpc 200 | 201 | def __log_license_info(self): 202 | 203 | """ Print tunempc license info 204 | """ 205 | 206 | license_info = [] 207 | license_info += [80*'+'] 208 | license_info += ['This is TuneMPC, a tool for economic tuning of tracking (N)MPC problems.'] 209 | license_info += ['TuneMPC is free software; you can redistribute it and/or modify it under the terms'] 210 | license_info += ['of the GNU Lesser General Public License as published by the Free Software Foundation'] 211 | license_info += ['license. More information can be found at http://github.com/jdeschut/tunempc.'] 212 | license_info += [80*'+'] 213 | 214 | Logger.logger.info('') 215 | for line in license_info: 216 | Logger.logger.info(line) 217 | Logger.logger.info('') 218 | 219 | return None 220 | 221 | def __log_problem_dimensions(self): 222 | 223 | """ Logging of problem dimensions 224 | """ 225 | 226 | Logger.logger.info('') 227 | Logger.logger.info('Number of states:................: {}'.format(self.__nx)) 228 | Logger.logger.info('Number of controls:..............: {}'.format(self.__nu)) 229 | if ('h' in self.__sys) and type(self.__sys['h']) != list: 230 | Logger.logger.info('Number of linear constraints:....: {}'.format(self.__sys['h'].size1_out(0)-self.__nus)) 231 | Logger.logger.info('Number of nonlinear constraints:.: {}'.format(self.__nus)) 232 | Logger.logger.info('Steady-state period:.............: {}'.format(self.__p)) 233 | Logger.logger.info('') 234 | 235 | return None 236 | 237 | @property 238 | def pocp(self): 239 | "Periodic optimal control problem" 240 | return self.__ocp 241 | 242 | @property 243 | def sys(self): 244 | "Reformulated system dynamics" 245 | return self.__sys 246 | 247 | @property 248 | def p(self): 249 | "Period" 250 | return self.__p 251 | 252 | @property 253 | def w_sol(self): 254 | "Optimal periodic orbit" 255 | return self.__w_sol 256 | 257 | @property 258 | def l(self): 259 | "Economic cost function" 260 | return self.__l 261 | 262 | @property 263 | def S(self): 264 | "Sensitivity information" 265 | return self.__S --------------------------------------------------------------------------------