├── .gitignore ├── .idea ├── .gitignore └── dictionaries │ └── marco.xml ├── LICENSE.md ├── README.md ├── README_PI.md ├── doc ├── latex │ ├── main.pdf │ ├── main.tex │ ├── pyMPC_formula_1.txt │ └── pyMPC_formula_2.txt └── sphinx │ ├── Makefile │ └── source │ ├── code.rst │ ├── conf.py │ ├── index.rst │ ├── math.rst │ ├── project.rst │ └── sphyinx_install ├── examples ├── example_accelerate_brake.py ├── example_inverted_pendulum.ipynb ├── example_inverted_pendulum.py ├── example_inverted_pendulum_kalman.ipynb ├── example_inverted_pendulum_kalman.py ├── example_point_mass.ipynb ├── example_point_mass.py └── img │ ├── cart_pole.png │ └── cart_pole.svg ├── pyMPC ├── .idea │ ├── misc.xml │ ├── modules.xml │ ├── pyMPC.iml │ ├── vcs.xml │ └── workspace.xml ├── __init__.py ├── kalman.py ├── mpc.py └── mpc_no_slack.py ├── setup.py └── test_scripts ├── alternative ├── example_unconstrained_MPC.py └── unconstrained.py ├── cvx_mpc_reference_governor.py ├── cvx_mpc_reference_governor_du.py ├── cvx_mpc_reference_governor_du_mimo.py ├── cvx_mpc_simple.py ├── disturbance.ipynb ├── example_inverted_pendulum.py ├── example_inverted_pendulum_1ms.py ├── example_inverted_pendulum_disturbance.py ├── example_inverted_pendulum_ode.py ├── example_inverted_pendulum_reference.py ├── example_mpc_function.py ├── kalman ├── MPC_observer.py ├── example_estimator_save_predictions.py ├── example_inverted_pendulum_disturbance.py ├── example_inverted_pendulum_disturbance_test.py ├── example_inverted_pendulum_estimator.py ├── img │ ├── cart_pole.png │ └── cart_pole.svg └── kalman.py ├── main.py ├── main_cvxpy.py ├── main_cvxpy_du.py ├── main_cvxpy_du_Nc.py ├── main_cvxpy_du_nocnst.py ├── main_cvxpy_simple.py ├── main_du.py ├── main_du_Nc.py ├── main_matlab_yalmip.m ├── main_simple.py ├── reference_governor ├── cvx_mpc_reference_governor_du_mimo_slack.py ├── oscillating_dynamics.py └── system_dynamics.py └── verify_MPC.py /.gitignore: -------------------------------------------------------------------------------- 1 | /.ipynb_checkpoints/ 2 | -------------------------------------------------------------------------------- /.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /workspace.xml -------------------------------------------------------------------------------- /.idea/dictionaries/marco.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | kalman 5 | 6 | 7 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Marco Forgione 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pyMPC 2 | 3 | Linear Constrained Model Predictive Control (MPC) in Python: 4 | 5 | 6 | 7 | where 8 | 9 | ## Requirements 10 | 11 | pyMPC requires the following packages: 12 | * numpy 13 | * scipy 14 | * [OSQP](https://osqp.org/) 15 | * matplotlib 16 | 17 | ## Installation 18 | 19 | ### Stable version from PyPI 20 | 21 | Run the command 22 | 23 | ``` 24 | pip install python-mpc 25 | ``` 26 | This will install the [stable version](https://pypi.org/project/python-mpc/0.1.1/) of pyMPC from the PyPI package repository. 27 | 28 | ### Latest version from GitHub 29 | 1. Get a local copy the pyMPC project. For instance, run 30 | ``` 31 | git clone https://github.com/forgi86/pyMPC.git 32 | ``` 33 | in a terminal to clone the project using git. Alternatively, download the zipped pyMPC project from [this link](https://github.com/forgi86/pyMPC/zipball/master) and extract it in a local folder 34 | 35 | 2. Install pyMPC by running 36 | ``` 37 | pip install -e . 38 | ``` 39 | in the pyMPC project root folder (where the file setup.py is located). 40 | 41 | ## Supported platforms 42 | 43 | We successfully tested pyMPC on the following platforms: 44 | * Windows 10 on a PC with x86-64 CPU 45 | * Ubuntu 18.04 LTS on a PC with x86-64 CPU 46 | * Raspbian Buster on a Raspberry PI 3 rev B 47 | 48 | Detailed instructions for the Raspberry PI platform are available [here](README_PI.md). 49 | 50 | ## Usage 51 | 52 | This code snippets illustrates the use of the MPCController class: 53 | 54 | ``` 55 | from pyMPC.mpc import MPCController 56 | 57 | K = MPCController(Ad,Bd,Np=20, x0=x0,xref=xref,uminus1=uminus1, 58 | Qx=Qx, QxN=QxN, Qu=Qu,QDu=QDu, 59 | xmin=xmin,xmax=xmax,umin=umin,umax=umax,Dumin=Dumin,Dumax=Dumax) 60 | K.setup() 61 | 62 | ... 63 | 64 | xstep = x0 65 | for i in range(nsim): 66 | uMPC = K.output() 67 | xstep = Ad.dot(xstep) + Bd.dot(uMPC) # system simulation steps 68 | K.update(xstep) # update with measurement 69 | ``` 70 | Full working examples are given in the [examples](examples) folder: 71 | * [Point mass with input force and friction](examples/example_point_mass.ipynb) 72 | * [Inverted pendulum on a cart](examples/example_inverted_pendulum.ipynb) 73 | * [Inverted pendulum on a cart with kalman filter](examples/example_inverted_pendulum_kalman.ipynb) 74 | 75 | ## Contributing 76 | 77 | I am slowly adding new functionalities to pyMPC according to my research needs. If you also wanna contribute, feel free to write me an email: marco.forgione@idsia.ch 78 | 79 | ## Citing 80 | 81 | If you find this project useful, we encourage you to 82 | 83 | * Star this repository :star: 84 | * Cite the [paper](https://arxiv.org/pdf/1911.13021) 85 | ``` 86 | @inproceedings{forgione2020efficient, 87 | title={Efficient Calibration of Embedded {MPC}}, 88 | author={Forgione, Marco and Piga, Dario and Bemporad, Alberto}, 89 | booktitle={Proc. of the 21st IFAC World Congress 2020, Berlin, Germany, July 12-17 2020}, 90 | year={2020} 91 | } 92 | ``` 93 | -------------------------------------------------------------------------------- /README_PI.md: -------------------------------------------------------------------------------- 1 | ## pyMPC on Raspberry PI 2 | 3 | pyMPC can run on Raspberry PI (we tested model 3B). This procedure may also work on other versions and maybe other ARM machines, but we haven't tested yet. 4 | 5 | ## Installation procedure 6 | 7 | We assume you already have Raspbian installed on the Raspberry PI. 8 | 9 | 1. Install the following packages via apt 10 | ``` 11 | apt-get install git 12 | apt-get install cmake 13 | ``` 14 | 15 | 2. Install the berryconda conda distribution 16 | ``` 17 | wget https://github.com/jjhelmus/berryconda/releases/download/v2.0.0/Berryconda3-2.0.0-Linux-armv7l.sh 18 | chmod +x Berryconda3-2.0.0-Linux-armv7l.sh 19 | ./Berryconda3-2.0.0-Linux-armv7l.sh 20 | ``` 21 | In the installation process, you will be asked for an installation folder. The default /home/pi/berryconda3 is fine 22 | 23 | 3. Install required packages in your new (berry)conda distribtion 24 | ``` 25 | cd /home/pi/berryconda3 26 | ./conda install numpy scipy matplotlib 27 | ./conda install -c numba numba 28 | ./pip install control 29 | apt-get install cmake 30 | ./pip install osqp 31 | ``` 32 | 33 | 4. Get a local copy the pyMPC project 34 | ``` 35 | git clone https://github.com/forgi86/pyMPC.git 36 | ``` 37 | 38 | 5. Install pyMPC. In the folder where you cloned the project, run 39 | 40 | ``` 41 | pip install -e . 42 | ``` 43 | -------------------------------------------------------------------------------- /doc/latex/main.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/pyMPC/291db149554767a035fcb01df3fed7a6b3fe60e4/doc/latex/main.pdf -------------------------------------------------------------------------------- /doc/latex/pyMPC_formula_1.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | \begin{align*} &\min_{u_0, \dots, u_{N_{p}-1},x_0,\dots,x_{N_p}} \sum_{k=0}^{N_p-1} \bigg[ \big(x_k - x_{ref}\big)^\top Q_x\big(x_k - x_{ref}\big) + \big(u_k - u_{ref}\big)^\top Q_u \big(u_k - u_{ref}\big) + \Delta u_k^\top Q_{\Delta} \Delta u_k \bigg ] + \big(x_{N_p} - x_{ref}\big)^\top Q_{x_{N_p}}\big(x_{N_p} - x_{ref}\big) \\ &\text{subject to} \nonumber\\ &x_{k+1} = Ax_k + B u_k \label{eq:linear_dynamics} \\ &u_{min} \leq u_k \leq u_{max}\\ &x_{min} \leq x_k \leq x_{max}\\ &\Delta u_{min} \leq \Delta u_k \leq \Delta u_{max}\\ &u_{-1} = \bar u \\ &x_0 = \bar x \end{align*} 6 | -------------------------------------------------------------------------------- /doc/latex/pyMPC_formula_2.txt: -------------------------------------------------------------------------------- 1 | . 2 | 3 | 4 | \Delta u_k = u_k - u_{k-1} 5 | -------------------------------------------------------------------------------- /doc/sphinx/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SOURCEDIR = source 8 | BUILDDIR = build 9 | 10 | # Put it first so that "make" without argument is like "make help". 11 | help: 12 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 13 | 14 | .PHONY: help Makefile 15 | 16 | # Catch-all target: route all unknown targets to Sphinx using the new 17 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 18 | %: Makefile 19 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /doc/sphinx/source/code.rst: -------------------------------------------------------------------------------- 1 | .. pyMPC documentation master file, created by 2 | sphinx-quickstart on Tue May 28 18:47:14 2019. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Code documentation 7 | ================================= 8 | 9 | 10 | .. automodule:: mpc 11 | :members: 12 | 13 | .. automodule:: kalman 14 | :members: 15 | 16 | -------------------------------------------------------------------------------- /doc/sphinx/source/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # Configuration file for the Sphinx documentation builder. 4 | # 5 | # This file does only contain a selection of the most common options. For a 6 | # full list see the documentation: 7 | # http://www.sphinx-doc.org/en/master/config 8 | 9 | # -- Path setup -------------------------------------------------------------- 10 | 11 | # If extensions (or modules to document with autodoc) are in another directory, 12 | # add these directories to sys.path here. If the directory is relative to the 13 | # documentation root, use os.path.abspath to make it absolute, like shown here. 14 | # 15 | import os 16 | import sys 17 | sys.path.insert(0, os.path.abspath('../../../pyMPC')) 18 | 19 | 20 | # -- Project information ----------------------------------------------------- 21 | 22 | project = 'pyMPC' 23 | copyright = '2019, Marco Forgione' 24 | author = 'Marco Forgione' 25 | 26 | # The short X.Y version 27 | version = '' 28 | # The full version, including alpha/beta/rc tags 29 | release = '0.1' 30 | 31 | 32 | # -- General configuration --------------------------------------------------- 33 | 34 | # If your documentation needs a minimal Sphinx version, state it here. 35 | # 36 | # needs_sphinx = '1.0' 37 | 38 | # Add any Sphinx extension module names here, as strings. They can be 39 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 40 | # ones. 41 | extensions = [ 42 | 'sphinx.ext.autodoc', 43 | 'sphinx.ext.mathjax', 44 | 'numpydoc' 45 | ] 46 | 47 | # Add any paths that contain templates here, relative to this directory. 48 | templates_path = ['_templates'] 49 | 50 | # The suffix(es) of source filenames. 51 | # You can specify multiple suffix as a list of string: 52 | # 53 | # source_suffix = ['.rst', '.md'] 54 | source_suffix = '.rst' 55 | 56 | # The master toctree document. 57 | master_doc = 'index' 58 | 59 | # The language for content autogenerated by Sphinx. Refer to documentation 60 | # for a list of supported languages. 61 | # 62 | # This is also used if you do content translation via gettext catalogs. 63 | # Usually you set "language" from the command line for these cases. 64 | language = None 65 | 66 | # List of patterns, relative to source directory, that match files and 67 | # directories to ignore when looking for source files. 68 | # This pattern also affects html_static_path and html_extra_path. 69 | exclude_patterns = [] 70 | 71 | # The name of the Pygments (syntax highlighting) style to use. 72 | pygments_style = None 73 | 74 | 75 | # -- Options for HTML output ------------------------------------------------- 76 | 77 | # The theme to use for HTML and HTML Help pages. See the documentation for 78 | # a list of builtin themes. 79 | # 80 | html_theme = 'alabaster' 81 | 82 | # Theme options are theme-specific and customize the look and feel of a theme 83 | # further. For a list of options available for each theme, see the 84 | # documentation. 85 | # 86 | # html_theme_options = {} 87 | 88 | # Add any paths that contain custom static files (such as style sheets) here, 89 | # relative to this directory. They are copied after the builtin static files, 90 | # so a file named "default.css" will overwrite the builtin "default.css". 91 | html_static_path = ['_static'] 92 | 93 | # Custom sidebar templates, must be a dictionary that maps document names 94 | # to template names. 95 | # 96 | # The default sidebars (for documents that don't match any pattern) are 97 | # defined by theme itself. Builtin themes are using these templates by 98 | # default: ``['localtoc.html', 'relations.html', 'sourcelink.html', 99 | # 'searchbox.html']``. 100 | # 101 | # html_sidebars = {} 102 | 103 | 104 | # -- Options for HTMLHelp output --------------------------------------------- 105 | 106 | # Output file base name for HTML help builder. 107 | htmlhelp_basename = 'pyMPCdoc' 108 | 109 | 110 | # -- Options for LaTeX output ------------------------------------------------ 111 | 112 | latex_elements = { 113 | # The paper size ('letterpaper' or 'a4paper'). 114 | # 115 | # 'papersize': 'letterpaper', 116 | 117 | # The font size ('10pt', '11pt' or '12pt'). 118 | # 119 | # 'pointsize': '10pt', 120 | 121 | # Additional stuff for the LaTeX preamble. 122 | # 123 | # 'preamble': '', 124 | 125 | # Latex figure (float) alignment 126 | # 127 | # 'figure_align': 'htbp', 128 | } 129 | 130 | # Grouping the document tree into LaTeX files. List of tuples 131 | # (source start file, target name, title, 132 | # author, documentclass [howto, manual, or own class]). 133 | latex_documents = [ 134 | (master_doc, 'pyMPC.tex', 'pyMPC Documentation', 135 | 'Marco Forgione', 'manual'), 136 | ] 137 | 138 | 139 | # -- Options for manual page output ------------------------------------------ 140 | 141 | # One entry per manual page. List of tuples 142 | # (source start file, name, description, authors, manual section). 143 | man_pages = [ 144 | (master_doc, 'pympc', 'pyMPC Documentation', 145 | [author], 1) 146 | ] 147 | 148 | 149 | # -- Options for Texinfo output ---------------------------------------------- 150 | 151 | # Grouping the document tree into Texinfo files. List of tuples 152 | # (source start file, target name, title, author, 153 | # dir menu entry, description, category) 154 | texinfo_documents = [ 155 | (master_doc, 'pyMPC', 'pyMPC Documentation', 156 | author, 'pyMPC', 'One line description of project.', 157 | 'Miscellaneous'), 158 | ] 159 | 160 | 161 | # -- Options for Epub output ------------------------------------------------- 162 | 163 | # Bibliographic Dublin Core info. 164 | epub_title = project 165 | 166 | # The unique identifier of the text. This can be a ISBN number 167 | # or the project homepage. 168 | # 169 | # epub_identifier = '' 170 | 171 | # A unique identification for the text. 172 | # 173 | # epub_uid = '' 174 | 175 | # A list of files that should not be packed into the epub file. 176 | epub_exclude_files = ['search.html'] 177 | 178 | numpydoc_show_class_members = False 179 | # -- Extension configuration ------------------------------------------------- 180 | -------------------------------------------------------------------------------- /doc/sphinx/source/index.rst: -------------------------------------------------------------------------------- 1 | .. pyMPC documentation master file, created by 2 | sphinx-quickstart on Tue May 28 18:47:14 2019. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | pyMPC 7 | ===== 8 | --------------------------------------------- 9 | A python library for Model Predictive Control 10 | --------------------------------------------- 11 | 12 | pyMPC is an open-source python library for Model Predictive Control (MPC). 13 | The project is hosted on this `GitHub repository `_. 14 | 15 | Requirements 16 | ------------ 17 | 18 | In order to run pyMPC, you need a python 3.x environment and the following packages: 19 | 20 | * `numpy `_ 21 | * `scipy `_ 22 | * `OSQP `_ 23 | * `matplotlib `_ 24 | 25 | The installation procedure below should take case of installing pyMPC with all its dependencies. 26 | 27 | Installation 28 | ------------ 29 | 1. Copy or clone the pyMPC project into a local folder. For instance, run 30 | 31 | .. code-block:: bash 32 | 33 | git clone https://github.com/forgi86/pyMPC.git 34 | 35 | from the command line 36 | 37 | 2. Navigate to your local pyMPC folder 38 | 39 | .. code-block:: bash 40 | 41 | cd PYMPC_LOCAL_FOLDER 42 | 43 | where PYMPC_LOCAL_FOLDER is the folder where you have just downloaded the code in step 2 44 | 45 | 3. Install pyMPC in your python environment: run 46 | 47 | .. code-block:: bash 48 | 49 | pip install -e . 50 | 51 | from the command line, in the working folder PYMPC_LOCAL_FOLDER 52 | 53 | Usage 54 | ----- 55 | 56 | This code snippets illustrates the use of the MPCController class: 57 | 58 | .. code-block:: python 59 | 60 | from pyMPC.mpc import MPCController 61 | 62 | ... 63 | 64 | K = MPCController(Ad,Bd,Np=20, x0=x0,xref=xref,uminus1=uminus1, 65 | Qx=Qx, QxN=QxN, Qu=Qu,QDu=QDu, 66 | xmin=xmin,xmax=xmax,umin=umin,umax=umax,Dumin=Dumin,Dumax=Dumax) 67 | K.setup() 68 | 69 | ... 70 | 71 | xstep = x0 72 | for i in range(nsim): 73 | uMPC = K.output() 74 | xstep = Ad.dot(xstep) + Bd.dot(uMPC) # system simulation steps 75 | K.update(xstep) # update with measurement 76 | 77 | 78 | Examples 79 | -------- 80 | 81 | Full working examples are given in the `examples `_ folder on git: 82 | * `Point mass, full state feedback `_ 83 | * `Cart-pole system, full state feedback `_ 84 | * `Cart-pole system, with Kalman Filter `_ 85 | 86 | 87 | Content 88 | ------- 89 | .. toctree:: 90 | :maxdepth: 2 91 | 92 | code 93 | math 94 | 95 | 96 | 97 | Indices and tables 98 | ------------------ 99 | 100 | * :ref:`genindex` 101 | * :ref:`modindex` 102 | * :ref:`search` 103 | -------------------------------------------------------------------------------- /doc/sphinx/source/math.rst: -------------------------------------------------------------------------------- 1 | The Math 2 | ================================= 3 | 4 | pyMPC solves the following optimization problem: 5 | 6 | .. math:: 7 | \newcommand{\MPC}{\mathrm{MPC}} 8 | \newcommand{\varx}{\mathbf{x}} 9 | \newcommand{\varu}{\mathbf{u}} 10 | \newcommand{\slack}{\epsilon} 11 | \newcommand{\QxN}{Q_{x_N}} 12 | \newcommand{\Qx}{Q_{x}} 13 | \newcommand{\Qu}{Q_{u}} 14 | \newcommand{\Qdu}{Q_{\Delta u}} 15 | \newcommand{\Np}{{N_p}} 16 | \newcommand{\Nc}{{N_c}} 17 | \newcommand{\blkdiag}{\text{blkdiag}} 18 | 19 | .. math:: 20 | 21 | \begin{multline} 22 | \arg \min_{\mathbf{x},\mathbf{u}} 23 | \big(x_\Np - x_{ref}\big)^\top Q_x \big(x_\Np - x_{ref}\big) + 24 | \bigg [ 25 | \sum_{k=0}^{\Np-1} \big(x_k - x_{ref}\big)^\top Q_x \big(x_k - x_{ref}\big) + 26 | \big(u_k - u_{ref}\big)^\top Q_u \big(u_k - u_{ref}\big) + 27 | \Delta u_k^\top Q_u \Delta u_k 28 | \bigg ] 29 | \end{multline}. 30 | 31 | Under the hood, pyMPC transforms the MPC optimization problem above in a form that can be solved using a standard QP 32 | solver: 33 | 34 | .. math:: 35 | 36 | \begin{align} 37 | &\min \frac{1}{2} x_{q}^\top P + q^\top x_{q} \\ 38 | &\text{subject to} \nonumber \\ 39 | &l_{b} \leq Ax \leq u_{b} 40 | \end{align} 41 | 42 | -------------------------------------------------------------------------------- /doc/sphinx/source/project.rst: -------------------------------------------------------------------------------- 1 | The pyMPC Project 2 | ================================= 3 | 4 | The aim of the pyMPC project is to provide a Model Predictive Control (MPC) library in python. 5 | -------------------------------------------------------------------------------- /doc/sphinx/source/sphyinx_install: -------------------------------------------------------------------------------- 1 | sudo add-apt-repository ppa:builds/sphinxsearch-stable 2 | sudo apt-get install sphinxsearch 3 | -------------------------------------------------------------------------------- /examples/example_accelerate_brake.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.sparse as sparse 3 | import time 4 | import matplotlib.pyplot as plt 5 | from pyMPC.mpc import MPCController 6 | from scipy.integrate import ode 7 | 8 | 9 | if __name__ == '__main__': 10 | # Constants # 11 | Ts = 0.2 # sampling time (s) 12 | M = 2 # mass (Kg) 13 | b = 0.3 # friction coefficient (N*s/m) 14 | 15 | # Continuous-time matrices (just for reference) 16 | Ac = np.array([ 17 | [0.0, 1.0], 18 | [0, -b/M]] 19 | ) 20 | Bc = np.array([ 21 | [0.0], 22 | [1/M] 23 | ]) 24 | 25 | Bc = np.c_[Bc, Bc] # Acceleration and Brake input 26 | 27 | def f_ODE(t,x,u): 28 | der = Ac @ x + Bc @ u 29 | return der 30 | 31 | [nx, nu] = Bc.shape # number of states and number or inputs 32 | 33 | # Simple forward euler discretization 34 | Ad = np.eye(nx) + Ac*Ts 35 | Bd = Bc*Ts 36 | 37 | 38 | # Reference input and states 39 | pref = 7.0 40 | vref = 0.0 41 | xref = np.array([pref, vref]) # reference state 42 | uref = np.array([0.0]) # reference input 43 | uminus1 = np.array([0.0, 0.0]) # input at time step negative one - used to penalize the first delta u at time instant 0. Could be the same as uref. 44 | 45 | # Constraints 46 | xmin = np.array([-100.0, -0.8]) 47 | xmax = np.array([100.0, 0.8]) 48 | 49 | umin = np.array([0, -1.0]) # Accelerator is positive, brake is negative 50 | umax = np.array([0.5, 0]) 51 | 52 | Dumin = np.array([-np.inf, -np.inf]) 53 | Dumax = np.array([np.inf, np.inf]) 54 | 55 | # Objective function 56 | Qx = sparse.diags([10.0, 0.0]) # Quadratic cost for states x0, x1, ..., x_N-1 57 | QxN = sparse.diags([10.0, 0.0]) # Quadratic cost for xN 58 | Qu = sparse.diags([0.5, 0.2]) # Quadratic cost for u0, u1, ...., u_N-1 59 | QDu = sparse.diags([0.5, 0.2]) # Quadratic cost for Du0, Du1, ...., Du_N-1 60 | 61 | # Initial state 62 | x0 = np.array([0.1, 0.2]) # initial state 63 | system_dyn = ode(f_ODE).set_integrator('vode', method='bdf') 64 | system_dyn.set_initial_value(x0, 0) 65 | system_dyn.set_f_params(0.0) 66 | 67 | # Prediction horizon 68 | Np = 20 69 | 70 | K = MPCController(Ad, Bd, Np=Np, x0=x0, xref=xref, uminus1=uminus1, 71 | Qx=Qx, QxN=QxN, Qu=Qu, QDu=QDu, 72 | xmin=xmin, xmax=xmax, umin=umin, umax=umax, Dumin=Dumin, Dumax=Dumax) 73 | K.setup() 74 | 75 | # Simulate in closed loop 76 | [nx, nu] = Bd.shape # number of states and number or inputs 77 | len_sim = 15 # simulation length (s) 78 | nsim = int(len_sim/Ts) # simulation length(timesteps) 79 | xsim = np.zeros((nsim,nx)) 80 | usim = np.zeros((nsim,nu)) 81 | tcalc = np.zeros((nsim,1)) 82 | tsim = np.arange(0,nsim)*Ts 83 | 84 | 85 | 86 | xstep = x0 87 | uMPC = uminus1 88 | 89 | time_start = time.time() 90 | for i in range(nsim): 91 | xsim[i, :] = xstep 92 | 93 | # MPC update and step. Could be in just one function call 94 | time_start = time.time() 95 | K.update(xstep, uMPC) # update with measurement 96 | uMPC = K.output() # MPC step (u_k value) 97 | tcalc[i,:] = time.time() - time_start 98 | usim[i,:] = uMPC 99 | 100 | #xstep = Ad.dot(xstep) + Bd.dot(uMPC) # Real system step (x_k+1 value) 101 | system_dyn.set_f_params(uMPC) # set current input value to uMPC 102 | system_dyn.integrate(system_dyn.t + Ts) 103 | xstep = system_dyn.y 104 | 105 | 106 | time_sim = time.time() - time_start 107 | fig,axes = plt.subplots(3,1, figsize=(10,10)) 108 | axes[0].plot(tsim, xsim[:,0], "k", label='p') 109 | axes[0].plot(tsim, xref[0]*np.ones(np.shape(tsim)), "r--", label="pref") 110 | axes[0].set_title("Position (m)") 111 | 112 | axes[1].plot(tsim, xsim[:,1], label="v") 113 | axes[1].plot(tsim, xref[1]*np.ones(np.shape(tsim)), "r--", label="vref") 114 | axes[1].set_title("Velocity (m/s)") 115 | 116 | axes[2].plot(tsim, usim[:, 0], label="u1") 117 | axes[2].plot(tsim, usim[:, 1], label="u2") 118 | 119 | axes[2].plot(tsim, uref*np.ones(np.shape(tsim)), "r--", label="uref") 120 | axes[2].set_title("Force (N)") 121 | 122 | for ax in axes: 123 | ax.grid(True) 124 | ax.legend() 125 | 126 | plt.figure() 127 | plt.hist(tcalc*1000) 128 | plt.grid(True) 129 | 130 | -------------------------------------------------------------------------------- /examples/example_inverted_pendulum.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.sparse as sparse 3 | import time 4 | import matplotlib.pyplot as plt 5 | from pyMPC.mpc import MPCController 6 | 7 | if __name__ == '__main__': 8 | 9 | # Constants # 10 | M = 0.5 11 | m = 0.2 12 | b = 0.1 13 | ftheta = 0.1 14 | l = 0.3 15 | g = 9.81 16 | 17 | Ts = 50e-3 18 | 19 | Ac =np.array([[0, 1, 0, 0], 20 | [0, -b/M, -(g*m)/M, (ftheta*m)/M], 21 | [0, 0, 0, 1], 22 | [0, b/(M*l), (M*g + g*m)/(M*l), -(M*ftheta + ftheta*m)/(M*l)]]) 23 | 24 | Bc = np.array([ 25 | [0.0], 26 | [1.0/M], 27 | [0.0], 28 | [-1/(M*l)] 29 | ]) 30 | 31 | [nx, nu] = Bc.shape # number of states and number or inputs 32 | 33 | # Simple forward euler discretization 34 | Ad = np.eye(nx) + Ac*Ts 35 | Bd = Bc*Ts 36 | 37 | # Reference input and states 38 | xref = np.array([0.3, 0.0, 0.0, 0.0]) # reference state 39 | uref = np.array([0.0]) # reference input 40 | uminus1 = np.array([0.0]) # input at time step negative one - used to penalize the first delta u at time instant 0. Could be the same as uref. 41 | 42 | # Constraints 43 | xmin = np.array([-1.0, -100, -100, -100]) 44 | xmax = np.array([0.3, 100.0, 100, 100]) 45 | 46 | umin = np.array([-20]) 47 | umax = np.array([20]) 48 | 49 | Dumin = np.array([-5]) 50 | Dumax = np.array([5]) 51 | 52 | # Objective function weights 53 | Qx = sparse.diags([0.3, 0, 1.0, 0]) # Quadratic cost for states x0, x1, ..., x_N-1 54 | QxN = sparse.diags([0.3, 0, 1.0, 0]) # Quadratic cost for xN 55 | Qu = 0.0 * sparse.eye(1) # Quadratic cost for u0, u1, ...., u_N-1 56 | QDu = 0.01 * sparse.eye(1) # Quadratic cost for Du0, Du1, ...., Du_N-1 57 | 58 | # Initial state 59 | phi0 = 15*2*np.pi/360 60 | x0 = np.array([0, 0, phi0, 0]) # initial state 61 | 62 | # Prediction horizon 63 | Np = 20 64 | 65 | K = MPCController(Ad,Bd,Np=Np, x0=x0,xref=xref,uminus1=uminus1, 66 | Qx=Qx, QxN=QxN, Qu=Qu,QDu=QDu, 67 | xmin=xmin,xmax=xmax,umin=umin,umax=umax,Dumin=Dumin,Dumax=Dumax, 68 | eps_feas=1e3) 69 | K.setup() 70 | 71 | # Simulate in closed loop 72 | [nx, nu] = Bd.shape # number of states and number or inputs 73 | len_sim = 20 # simulation length (s) 74 | nsim = int(len_sim/Ts) # simulation length(timesteps) 75 | xsim = np.zeros((nsim,nx)) 76 | usim = np.zeros((nsim,nu)) 77 | tsim = np.arange(0,nsim)*Ts 78 | 79 | time_start = time.time() 80 | 81 | xstep = x0 82 | uMPC = uminus1 83 | for i in range(nsim): 84 | xsim[i,:] = xstep 85 | 86 | # MPC update and step. Could be in just one function call 87 | K.update(xstep, uMPC) # update with measurement 88 | uMPC = K.output() # MPC step (u_k value) 89 | usim[i,:] = uMPC 90 | 91 | # System simulation step 92 | F = uMPC 93 | v = xstep[1] 94 | theta = xstep[2] 95 | omega = xstep[3] 96 | der = np.zeros(nx) 97 | der[0] = v 98 | der[1] = (m*l*np.sin(theta)*omega**2 -m*g*np.sin(theta)*np.cos(theta) + m*ftheta*np.cos(theta)*omega + F - b*v)/(M+m*(1-np.cos(theta)**2)); 99 | der[2] = omega 100 | der[3] = ((M+m)*(g*np.sin(theta) - ftheta*omega) - m*l*omega**2*np.sin(theta)*np.cos(theta) -(F-b*v)*np.cos(theta))/(l*(M + m*(1-np.cos(theta)**2)) ); 101 | # Forward euler step 102 | xstep = xstep + der*Ts 103 | 104 | time_sim = time.time() - time_start 105 | 106 | fig,axes = plt.subplots(3, 1, figsize=(10, 10)) 107 | axes[0].plot(tsim, xsim[:, 0], "k", label='p') 108 | axes[0].plot(tsim, xref[0]*np.ones(np.shape(tsim)), "r--", label="p_ref") 109 | axes[0].set_title("Position (m)") 110 | 111 | axes[1].plot(tsim, xsim[:, 2]*360/2/np.pi, label="phi") 112 | axes[1].plot(tsim, xref[2]*360/2/np.pi*np.ones(np.shape(tsim)), "r--", label="phi_ref") 113 | axes[1].set_title("Angle (deg)") 114 | 115 | axes[2].plot(tsim, usim[:, 0], label="u") 116 | axes[2].plot(tsim, uref*np.ones(np.shape(tsim)), "r--", label="u_ref") 117 | axes[2].set_title("Force (N)") 118 | 119 | for ax in axes: 120 | ax.grid(True) 121 | ax.legend() 122 | 123 | 124 | -------------------------------------------------------------------------------- /examples/example_inverted_pendulum_kalman.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.sparse as sparse 3 | import time 4 | import matplotlib.pyplot as plt 5 | from pyMPC.mpc import MPCController 6 | from scipy.integrate import ode 7 | from pyMPC.kalman import kalman_design_simple, LinearStateEstimator 8 | 9 | if __name__ == '__main__': 10 | 11 | # Constants # 12 | M = 0.5 13 | m = 0.2 14 | b = 0.1 15 | ftheta = 0.1 16 | l = 0.3 17 | g = 9.81 18 | 19 | Ts = 5e-3 20 | 21 | Ac =np.array([[0, 1, 0, 0], 22 | [0, -b/M, -(g*m)/M, (ftheta*m)/M], 23 | [0, 0, 0, 1], 24 | [0, b/(M*l), (M*g + g*m)/(M*l), -(M*ftheta + ftheta*m)/(M*l)]]) 25 | 26 | Bc = np.array([ 27 | [0.0], 28 | [1.0/M], 29 | [0.0], 30 | [-1/(M*l)] 31 | ]) 32 | 33 | Cc = np.array([[1., 0., 0., 0.], 34 | [0., 0., 1., 0.]]) 35 | 36 | Dc = np.zeros((2, 1)) 37 | 38 | [nx, nu] = Bc.shape # number of states and number or inputs 39 | ny = np.shape(Cc)[0] 40 | 41 | # Nonlinear dynamics ODE 42 | def f_ODE(t,x,u): 43 | #print(x) 44 | F = u 45 | v = x[1] 46 | theta = x[2] 47 | omega = x[3] 48 | der = np.zeros(nx) 49 | der[0] = v 50 | der[1] = (m * l * np.sin(theta) * omega ** 2 - m * g * np.sin(theta) * np.cos(theta) + m * ftheta * np.cos( 51 | theta) * omega + F - b * v) / (M + m * (1 - np.cos(theta) ** 2)) 52 | der[2] = omega 53 | der[3] = ((M + m) * (g * np.sin(theta) - ftheta * omega) - m * l * omega ** 2 * np.sin(theta) * np.cos( 54 | theta) - ( 55 | F - b * v) * np.cos(theta)) / (l * (M + m * (1 - np.cos(theta) ** 2))) 56 | return der 57 | 58 | # Simple forward euler discretization 59 | Ad = np.eye(nx) + Ac*Ts 60 | Bd = Bc*Ts 61 | Cd = Cc 62 | Dd = Dc 63 | 64 | # Standard deviation of the measurement noise on position and angle 65 | std_npos = 0*0.005 66 | std_nphi = 0*0.005 67 | 68 | # Reference input and states 69 | xref = np.array([0.3, 0.0, 0.0, 0.0]) # reference state 70 | uref = np.array([0.0]) # reference input 71 | uminus1 = np.array([0.0]) # input at time step negative one - used to penalize the first delta u at time instant 0. Could be the same as uref. 72 | 73 | # Constraints 74 | xmin = np.array([-1.0, -100, -100, -100]) 75 | xmax = np.array([1.0, 100.0, 100, 100]) 76 | 77 | umin = np.array([-20]) 78 | umax = np.array([20]) 79 | 80 | Dumin = np.array([-5]) 81 | Dumax = np.array([5]) 82 | 83 | # Objective function weights 84 | Qx = sparse.diags([0.3, 0, 1.0, 0]) # Quadratic cost for states x0, x1, ..., x_N-1 85 | QxN = sparse.diags([0.3, 0, 1.0, 0]) # Quadratic cost for xN 86 | Qu = 0.0 * sparse.eye(1) # Quadratic cost for u0, u1, ...., u_N-1 87 | QDu = 0.01 * sparse.eye(1) # Quadratic cost for Du0, Du1, ...., Du_N-1 88 | 89 | # Initialize simulation system 90 | phi0 = 15*2*np.pi/360 91 | x0 = np.array([0, 0, phi0, 0]) # initial state 92 | system_dyn = ode(f_ODE).set_integrator('vode', method='bdf') 93 | system_dyn.set_initial_value(x0, 0) 94 | system_dyn.set_f_params(0.0) 95 | 96 | # Basic Kalman filter design 97 | Q_kal = 10 * np.eye(nx) 98 | R_kal = np.eye(ny) 99 | L, P, W = kalman_design_simple(Ad, Bd, Cd, Dd, Q_kal, R_kal, type='filter') 100 | x0_est = x0 101 | KF = LinearStateEstimator(x0_est, Ad, Bd, Cd, Dd, L) 102 | 103 | # Prediction horizon 104 | Np = 200 105 | 106 | # Initialize controller 107 | K = MPCController(Ad,Bd,Np=Np, x0=x0,xref=xref,uminus1=uminus1, 108 | Qx=Qx, QxN=QxN, Qu=Qu,QDu=QDu, 109 | xmin=xmin,xmax=xmax,umin=umin,umax=umax,Dumin=Dumin,Dumax=Dumax, 110 | eps_feas = 1e3) 111 | K.setup() 112 | 113 | # Simulate in closed loop 114 | [nx, nu] = Bd.shape # number of states and number or inputs 115 | len_sim = 10 # simulation length (s) 116 | nsim = int(len_sim / Ts) # simulation length(timesteps) 117 | x_vec = np.zeros((nsim, nx)) 118 | #x_vec_EA = np.zeros((nsim, nx)) 119 | 120 | y_vec = np.zeros((nsim, ny)) 121 | y_meas_vec = np.zeros((nsim, ny)) 122 | y_est_vec = np.zeros((nsim, ny)) 123 | x_est_vec = np.zeros((nsim, nx)) 124 | u_vec = np.zeros((nsim, nu)) 125 | t_vec = np.arange(0, nsim) * Ts 126 | t_MPC_CPU = np.arange(0, nsim) * Ts 127 | x_MPC_pred = np.zeros((nsim, Np+1, nx)) # on-line predictions from the Kalman Filter 128 | 129 | time_start = time.time() 130 | 131 | uMPC = uminus1 132 | y_step = None 133 | ymeas_step = None 134 | #x_step = x0 135 | for i in range(nsim): 136 | # Output for step i 137 | # System 138 | y_step = Cd.dot(system_dyn.y) # y[i] from the system 139 | ymeas_step = y_step 140 | ymeas_step[0] += std_npos * np.random.randn() 141 | ymeas_step[1] += std_nphi * np.random.randn() 142 | # Estimator 143 | 144 | # time_MPC_start = time.time() 145 | uMPC, infoMPC = K.output(return_x_seq=True) # u[i] = k(\hat x[i]) possibly computed at time instant -1 146 | # t_MPC_CPU[i] = time.time() - time_MPC_start 147 | 148 | x_MPC_pred[i, :, :] = infoMPC['x_seq'] # x_MPC_pred[i,i+1,...| possibly computed at time instant -1] 149 | 150 | # Save output for step i 151 | y_vec[i, :] = y_step # y[i] 152 | y_meas_vec[i,:] = ymeas_step # y_meas[i] 153 | x_vec[i, :] = system_dyn.y # x[i] 154 | y_est_vec[i,:] = KF.y # \hat y[i|i-1] 155 | x_est_vec[i, :] = KF.x # \hat x[i|i-1] 156 | u_vec[i, :] = uMPC # u[i] 157 | # x_vec_EA[i,:] = x_step 158 | 159 | # Update i+1 160 | # System 161 | system_dyn.set_f_params(uMPC) # set current input value to uMPC 162 | system_dyn.integrate(system_dyn.t + Ts) 163 | #x_step = system_dyn.y 164 | # der = f_ODE(0,x_step,uMPC) 165 | #x_step = x_step + der * Ts # true system evolves to x[i+1] 166 | #system_dyn.set_initial_value(x_step, 0) 167 | 168 | # Kalman filter: update and predict 169 | KF.update(ymeas_step) # update \hat x[i|i-1] to \hat x[i|i] updated using ymeas[i] 170 | KF.predict(uMPC) # predict \hat x[i+1|i] using u[i] 171 | 172 | # MPC update for step i+1 173 | time_MPC_start = time.time() 174 | K.update(KF.x, uMPC) # update with measurement (and possibly pre-compute u[i+1]) 175 | t_MPC_CPU[i] = time.time() - time_MPC_start 176 | 177 | time_sim = time.time() - time_start 178 | 179 | fig,axes = plt.subplots(3,1, figsize=(10,10)) 180 | axes[0].plot(t_vec, x_vec[:,0], "k", label='p') 181 | axes[0].plot(t_vec, xref[0]*np.ones(np.shape(t_vec)), "r--", label="p_ref") 182 | #axes[0].plot(t_vec, x_vec_EA[:,0]*np.ones(np.shape(t_vec)), "r--", label="p_EA") 183 | axes[0].set_title("Position (m)") 184 | 185 | axes[1].plot(t_vec, x_vec[:,2]*360/2/np.pi, label="phi") 186 | axes[1].plot(t_vec, xref[2]*360/2/np.pi*np.ones(np.shape(t_vec)), "r--", label="phi_ref") 187 | #axes[1].plot(t_vec, x_vec_EA[:,2]*np.ones(np.shape(t_vec)), "r--", label="p_EA") 188 | axes[1].set_title("Angle (deg)") 189 | 190 | axes[2].plot(t_vec, u_vec[:,0], label="u") 191 | axes[2].plot(t_vec, uref*np.ones(np.shape(t_vec)), "r--", label="u_ref") 192 | axes[2].set_title("Force (N)") 193 | 194 | for ax in axes: 195 | ax.grid(True) 196 | ax.legend() 197 | 198 | 199 | fig,ax = plt.subplots(1,1, figsize=(5,5)) 200 | ax.hist(t_MPC_CPU*1000, bins=100) 201 | ax.grid(True) 202 | ax.set_xlabel('MPC computation CPU time (ms)') 203 | -------------------------------------------------------------------------------- /examples/example_point_mass.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.sparse as sparse 3 | import time 4 | import matplotlib.pyplot as plt 5 | from pyMPC.mpc import MPCController 6 | from scipy.integrate import ode 7 | 8 | 9 | if __name__ == '__main__': 10 | # Constants # 11 | Ts = 0.2 # sampling time (s) 12 | M = 2 # mass (Kg) 13 | b = 0.3 # friction coefficient (N*s/m) 14 | 15 | # Continuous-time matrices (just for reference) 16 | Ac = np.array([ 17 | [0.0, 1.0], 18 | [0, -b/M]] 19 | ) 20 | Bc = np.array([ 21 | [0.0], 22 | [1/M] 23 | ]) 24 | 25 | def f_ODE(t,x,u): 26 | der = Ac @ x + Bc @ u 27 | return der 28 | 29 | [nx, nu] = Bc.shape # number of states and number or inputs 30 | 31 | # Simple forward euler discretization 32 | Ad = np.eye(nx) + Ac*Ts 33 | Bd = Bc*Ts 34 | 35 | 36 | # Reference input and states 37 | pref = 7.0 38 | vref = 0.0 39 | xref = np.array([pref, vref]) # reference state 40 | uref = np.array([0.0]) # reference input 41 | uminus1 = np.array([0.0]) # input at time step negative one - used to penalize the first delta u at time instant 0. Could be the same as uref. 42 | 43 | # Constraints 44 | xmin = np.array([-100.0, -100.0]) 45 | xmax = np.array([100.0, 100.0]) 46 | 47 | umin = np.array([-1.2]) 48 | umax = np.array([1.2]) 49 | 50 | Dumin = np.array([-2e-1]) 51 | Dumax = np.array([2e-1]) 52 | 53 | # Objective function 54 | Qx = sparse.diags([0.5, 0.1]) # Quadratic cost for states x0, x1, ..., x_N-1 55 | QxN = sparse.diags([0.5, 0.1]) # Quadratic cost for xN 56 | Qu = 2.0 * sparse.eye(1) # Quadratic cost for u0, u1, ...., u_N-1 57 | QDu = 10.0 * sparse.eye(1) # Quadratic cost for Du0, Du1, ...., Du_N-1 58 | 59 | # Initial state 60 | x0 = np.array([0.1, 0.2]) # initial state 61 | system_dyn = ode(f_ODE).set_integrator('vode', method='bdf') 62 | system_dyn.set_initial_value(x0, 0) 63 | system_dyn.set_f_params(0.0) 64 | 65 | # Prediction horizon 66 | Np = 20 67 | 68 | K = MPCController(Ad,Bd,Np=Np, x0=x0,xref=xref,uminus1=uminus1, 69 | Qx=Qx, QxN=QxN, Qu=Qu,QDu=QDu, 70 | xmin=xmin,xmax=xmax,umin=umin,umax=umax,Dumin=Dumin,Dumax=Dumax) 71 | K.setup() 72 | 73 | # Simulate in closed loop 74 | [nx, nu] = Bd.shape # number of states and number or inputs 75 | len_sim = 15 # simulation length (s) 76 | nsim = int(len_sim/Ts) # simulation length(timesteps) 77 | xsim = np.zeros((nsim,nx)) 78 | usim = np.zeros((nsim,nu)) 79 | tcalc = np.zeros((nsim,1)) 80 | tsim = np.arange(0,nsim)*Ts 81 | 82 | 83 | 84 | xstep = x0 85 | uMPC = uminus1 86 | 87 | time_start = time.time() 88 | for i in range(nsim): 89 | xsim[i,:] = xstep 90 | 91 | # MPC update and step. Could be in just one function call 92 | time_start = time.time() 93 | K.update(xstep, uMPC) # update with measurement 94 | uMPC = K.output() # MPC step (u_k value) 95 | tcalc[i,:] = time.time() - time_start 96 | usim[i,:] = uMPC 97 | 98 | #xstep = Ad.dot(xstep) + Bd.dot(uMPC) # Real system step (x_k+1 value) 99 | system_dyn.set_f_params(uMPC) # set current input value to uMPC 100 | system_dyn.integrate(system_dyn.t + Ts) 101 | xstep = system_dyn.y 102 | 103 | 104 | time_sim = time.time() - time_start 105 | fig,axes = plt.subplots(3,1, figsize=(10,10)) 106 | axes[0].plot(tsim, xsim[:,0], "k", label='p') 107 | axes[0].plot(tsim, xref[0]*np.ones(np.shape(tsim)), "r--", label="pref") 108 | axes[0].set_title("Position (m)") 109 | 110 | axes[1].plot(tsim, xsim[:,1], label="v") 111 | axes[1].plot(tsim, xref[1]*np.ones(np.shape(tsim)), "r--", label="vref") 112 | axes[1].set_title("Velocity (m/s)") 113 | 114 | axes[2].plot(tsim, usim[:,0], label="u") 115 | axes[2].plot(tsim, uref*np.ones(np.shape(tsim)), "r--", label="uref") 116 | axes[2].set_title("Force (N)") 117 | 118 | for ax in axes: 119 | ax.grid(True) 120 | ax.legend() 121 | 122 | plt.figure() 123 | plt.hist(tcalc*1000) 124 | plt.grid(True) 125 | 126 | -------------------------------------------------------------------------------- /examples/img/cart_pole.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/pyMPC/291db149554767a035fcb01df3fed7a6b3fe60e4/examples/img/cart_pole.png -------------------------------------------------------------------------------- /pyMPC/.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 40 | 41 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /pyMPC/.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /pyMPC/.idea/pyMPC.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | -------------------------------------------------------------------------------- /pyMPC/.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /pyMPC/.idea/workspace.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 40 | 41 | 42 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 |