├── .github └── workflows │ └── python-publish.yml ├── CITATIONS.bib ├── CODE_OF_CONDUCT.md ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── _config.yml ├── docs └── README.md ├── images ├── Logo_ISTRC_Green_English.png ├── logo.png └── logo_abc.png ├── pyproject.toml ├── requirements.txt └── src └── PyDiffGame ├── ContinuousPyDiffGame.py ├── DiscretePyDiffGame.py ├── LQR.py ├── Objective.py ├── PyDiffGame.py ├── PyDiffGameLQRComparison.py ├── __init__.py └── examples ├── InvertedPendulumComparison.py ├── MassesWithSpringsComparison.py ├── PVTOL.py ├── PVTOLComparison.py ├── QuadRotorControl.py └── figures ├── 2 ├── 2-players_large_1.png ├── 2-players_large_2.png ├── LQR_large_1.png ├── LQR_large_2.png └── two_masses_tikz.png ├── 4 ├── 4-players_large_1.png ├── 4-players_large_2.png ├── LQR_large_1.png └── LQR_large_2.png ├── 8 ├── 8-players_large_1.png ├── 8-players_large_2.png ├── LQR_large_1.png └── LQR_large_2.png ├── PVTOL ├── PVTOL1.png ├── PVTOL10.png ├── PVTOL100.png └── PVTOL1000.png ├── PVTOL0001.png ├── PVTOL001.png ├── PVTOL01.png └── PVTOL1.png /.github/workflows/python-publish.yml: -------------------------------------------------------------------------------- 1 | # This workflow will upload a Python Package to PyPI when a release is created 2 | # For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python#publishing-to-package-registries 3 | 4 | # This workflow uses actions that are not certified by GitHub. 5 | # They are provided by a third-party and are governed by 6 | # separate terms of service, privacy policy, and support 7 | # documentation. 8 | 9 | name: Upload Python Package 10 | 11 | on: 12 | release: 13 | types: [published] 14 | 15 | permissions: 16 | contents: read 17 | 18 | jobs: 19 | release-build: 20 | runs-on: ubuntu-latest 21 | 22 | steps: 23 | - uses: actions/checkout@v4 24 | 25 | - uses: actions/setup-python@v5 26 | with: 27 | python-version: "3.10" 28 | 29 | - name: Build release distributions 30 | run: | 31 | # NOTE: put your own distribution build steps here. 32 | python -m pip install build 33 | python -m build 34 | 35 | - name: Upload distributions 36 | uses: actions/upload-artifact@v4 37 | with: 38 | name: release-dists 39 | path: dist/ 40 | 41 | pypi-publish: 42 | runs-on: ubuntu-latest 43 | needs: 44 | - release-build 45 | permissions: 46 | # IMPORTANT: this permission is mandatory for trusted publishing 47 | id-token: write 48 | 49 | # Dedicated environments with protections for publishing are strongly recommended. 50 | # For more information, see: https://docs.github.com/en/actions/deployment/targeting-different-environments/using-environments-for-deployment#deployment-protection-rules 51 | environment: 52 | name: pypi 53 | # OPTIONAL: uncomment and update to include your PyPI project URL in the deployment status: 54 | url: https://pypi.org/project/PyDiffGame/ 55 | # 56 | # ALTERNATIVE: if your GitHub Release name is the PyPI project version string 57 | # ALTERNATIVE: exactly, uncomment the following line instead: 58 | # url: https://pypi.org/project/YOURPROJECT/${{ github.event.release.name }} 59 | 60 | steps: 61 | - name: Retrieve release distributions 62 | uses: actions/download-artifact@v4 63 | with: 64 | name: release-dists 65 | path: dist/ 66 | 67 | - name: Publish release distributions to PyPI 68 | uses: pypa/gh-action-pypi-publish@release/v1 69 | with: 70 | packages-dir: dist/ 71 | -------------------------------------------------------------------------------- /CITATIONS.bib: -------------------------------------------------------------------------------- 1 | @software{pydiffgame_package, 2 | author = {Kricheli, Joshua Shay and Sadon, Aviran}, 3 | title = {{PyDiffGame}}, 4 | howpublished = {\url{https://krichelj.github.io/PyDiffGame/}}, 5 | year = {2021}, 6 | } 7 | 8 | @inproceedings{pydiffgame_paper, 9 | author={Kricheli, Joshua Shay and Sadon, Aviran and Arogeti, Shai and Regev, Shimon and Weiss, Gera}, 10 | booktitle={29th Mediterranean Conference on Control and Automation (MED 2021)}, 11 | title={{Composition of Dynamic Control Objectives Based on Differential Games}}, 12 | year={2021}, 13 | volume={}, 14 | number={}, 15 | pages={298-304}, 16 | doi={10.1109/MED51440.2021.9480269}} 17 | 18 | @mastersthesis{kricheli2022differential, 19 | title={{Differential Games for Compositional Handling of Competing Control Tasks}}, 20 | author={Kricheli, Joshua Shay}, 21 | year={2022}, 22 | school={Ben-Gurion University of the Negev} 23 | } -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | We as members, contributors, and leaders pledge to make participation in our 6 | community a harassment-free experience for everyone, regardless of age, body 7 | size, visible or invisible disability, ethnicity, sex characteristics, gender 8 | identity and expression, level of experience, education, socio-economic status, 9 | nationality, personal appearance, race, religion, or sexual identity 10 | and orientation. 11 | 12 | We pledge to act and interact in ways that contribute to an open, welcoming, 13 | diverse, inclusive, and healthy community. 14 | 15 | ## Our Standards 16 | 17 | Examples of behavior that contributes to a positive environment for our 18 | community include: 19 | 20 | * Demonstrating empathy and kindness toward other people 21 | * Being respectful of differing opinions, viewpoints, and experiences 22 | * Giving and gracefully accepting constructive feedback 23 | * Accepting responsibility and apologizing to those affected by our mistakes, 24 | and learning from the experience 25 | * Focusing on what is best not just for us as individuals, but for the 26 | overall community 27 | 28 | Examples of unacceptable behavior include: 29 | 30 | * The use of sexualized language or imagery, and sexual attention or 31 | advances of any kind 32 | * Trolling, insulting or derogatory comments, and personal or political attacks 33 | * Public or private harassment 34 | * Publishing others' private information, such as a physical or email 35 | address, without their explicit permission 36 | * Other conduct which could reasonably be considered inappropriate in a 37 | professional setting 38 | 39 | ## Enforcement Responsibilities 40 | 41 | Community leaders are responsible for clarifying and enforcing our standards of 42 | acceptable behavior and will take appropriate and fair corrective action in 43 | response to any behavior that they deem inappropriate, threatening, offensive, 44 | or harmful. 45 | 46 | Community leaders have the right and responsibility to remove, edit, or reject 47 | comments, commits, code, wiki edits, issues, and other contributions that are 48 | not aligned to this Code of Conduct, and will communicate reasons for moderation 49 | decisions when appropriate. 50 | 51 | ## Scope 52 | 53 | This Code of Conduct applies within all community spaces, and also applies when 54 | an individual is officially representing the community in public spaces. 55 | Examples of representing our community include using an official e-mail address, 56 | posting via an official social media account, or acting as an appointed 57 | representative at an online or offline event. 58 | 59 | ## Enforcement 60 | 61 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 62 | reported to the community leaders responsible for enforcement at 63 | . 64 | All complaints will be reviewed and investigated promptly and fairly. 65 | 66 | All community leaders are obligated to respect the privacy and security of the 67 | reporter of any incident. 68 | 69 | ## Enforcement Guidelines 70 | 71 | Community leaders will follow these Community Impact Guidelines in determining 72 | the consequences for any action they deem in violation of this Code of Conduct: 73 | 74 | ### 1. Correction 75 | 76 | **Community Impact**: Use of inappropriate language or other behavior deemed 77 | unprofessional or unwelcome in the community. 78 | 79 | **Consequence**: A private, written warning from community leaders, providing 80 | clarity around the nature of the violation and an explanation of why the 81 | behavior was inappropriate. A public apology may be requested. 82 | 83 | ### 2. Warning 84 | 85 | **Community Impact**: A violation through a single incident or series 86 | of actions. 87 | 88 | **Consequence**: A warning with consequences for continued behavior. No 89 | interaction with the people involved, including unsolicited interaction with 90 | those enforcing the Code of Conduct, for a specified period of time. This 91 | includes avoiding interactions in community spaces as well as external channels 92 | like social media. Violating these terms may lead to a temporary or 93 | permanent ban. 94 | 95 | ### 3. Temporary Ban 96 | 97 | **Community Impact**: A serious violation of community standards, including 98 | sustained inappropriate behavior. 99 | 100 | **Consequence**: A temporary ban from any sort of interaction or public 101 | communication with the community for a specified period of time. No public or 102 | private interaction with the people involved, including unsolicited interaction 103 | with those enforcing the Code of Conduct, is allowed during this period. 104 | Violating these terms may lead to a permanent ban. 105 | 106 | ### 4. Permanent Ban 107 | 108 | **Community Impact**: Demonstrating a pattern of violation of community 109 | standards, including sustained inappropriate behavior, harassment of an 110 | individual, or aggression toward or disparagement of classes of individuals. 111 | 112 | **Consequence**: A permanent ban from any sort of public interaction within 113 | the community. 114 | 115 | ## Attribution 116 | 117 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], 118 | version 2.0, available at 119 | https://www.contributor-covenant.org/version/2/0/code_of_conduct.html. 120 | 121 | Community Impact Guidelines were inspired by [Mozilla's code of conduct 122 | enforcement ladder](https://github.com/mozilla/diversity). 123 | 124 | [homepage]: https://www.contributor-covenant.org 125 | 126 | For answers to common questions about this code of conduct, see the FAQ at 127 | https://www.contributor-covenant.org/faq. Translations are available at 128 | https://www.contributor-covenant.org/translations. 129 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contribution Guidelines 2 | 3 | This repo is part of a research conducted at Ben Gurion University, 4 | and is thus open source. We would love to receive your input! 5 | 6 | Please ensure your pull request adheres to the following guidelines: 7 | 8 | - Search previous suggestions before making a new one, as yours may be a duplicate. 9 | - Make sure your link is useful before submitting. 10 | - Make an individual pull request for each suggestion. 11 | - New categories or improvements to the existing categorization are welcome. 12 | - Check your spelling and grammar. 13 | - Make sure your text editor is set to remove trailing whitespace. 14 | - The pull request and commit should have a useful title. 15 | 16 | Thank you for your suggestions! 17 | 18 | Joshua Shay Kricheli 19 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021-2024 Joshua Shay Kricheli, Dr. Aviran Sadon, Dr. Shai Arogeti and Prof. Gera Weiss 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 |

2 | Logo 3 |

4 | 5 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![Upload Python Package](https://github.com/krichelj/PyDiffGame/actions/workflows/python-publish.yml/badge.svg)](https://github.com/krichelj/PyDiffGame/actions/workflows/python-publish.yml) [![pages-build-deployment](https://github.com/krichelj/PyDiffGame/actions/workflows/pages/pages-build-deployment/badge.svg)](https://github.com/krichelj/PyDiffGame/actions/workflows/pages/pages-build-deployment) 6 | 7 | 8 | * [What is this?](#what-is-this) 9 | * [Installation](#installation) 10 | * [Input Parameters](#input-parameters) 11 | * [Tutorial](#tutorial) 12 | * [Authors](#authors) 13 | * [Acknowledgments](#acknowledgments) 14 | 15 | # What is this? 16 | 17 | [`PyDiffGame`](https://github.com/krichelj/PyDiffGame) is a Python implementation of a Nash Equilibrium solution to Differential Games, based on a reduction of Game Hamilton-Bellman-Jacobi (GHJB) equations to Game Algebraic and Differential Riccati equations, associated with Multi-Objective Dynamical Control Systems\ 18 | The method relies on the formulation given in: 19 | 20 | - The thesis work "_Differential Games for Compositional Handling of Competing Control Tasks_" 21 | ([Research Gate](https://www.researchgate.net/publication/359819808_Differential_Games_for_Compositional_Handling_of_Competing_Control_Tasks)) 22 | 23 | - The conference article "_Composition of Dynamic Control Objectives Based on Differential Games_" 24 | ([IEEE](https://ieeexplore.ieee.org/document/9480269) | 25 | [Research Gate](https://www.researchgate.net/publication/353452024_Composition_of_Dynamic_Control_Objectives_Based_on_Differential_Games)) 26 | 27 | The package was tested for Python >= 3.10. 28 | 29 | # Installation 30 | 31 | To install this package run this from the command prompt: 32 | ``` 33 | pip install PyDiffGame 34 | ``` 35 | 36 | The package was tested for Python >= 3.10, along with the listed packages versions in [`requirments.txt`](https://github.com/krichelj/PyDiffGame/blob/master/requirements.txt) 37 | 38 | # Input Parameters 39 | 40 | The package defines an abstract class [`PyDiffGame.py`](https://github.com/krichelj/PyDiffGame/blob/master/src/PyDiffGame/PyDiffGame.py). An object of this class represents an instance of differential game. 41 | The input parameters to instantiate a `PyDiffGame` object are: 42 | 43 | * `A` : `np.array` of shape $(n,n)$ 44 | >System dynamics matrix 45 | * `B` : `np.array` of shape $(n, m_1 + ... + m_N)$, optional 46 | >Input matrix for all virtual control objectives 47 | * `Bs` : `Sequence` of `np.array` objects of len $(N)$, each array $B_i$ of shape $(n,m_i)$, optional 48 | >Input matrices for each virtual control objective 49 | * `Qs` : `Sequence` of `np.array` objects of len $(N)$, each array $Q_i$ of shape $(n,n)$, optional 50 | >State weight matrices for each virtual control objective 51 | * `Rs` : `Sequence` of `np.array` objects of len $(N)$, each array $R_i$ of shape $(m_i,m_i)$, optional 52 | >Input weight matrices for each virtual control objective 53 | * `Ms` : `Sequence` of `np.array` objects of len $(N)$, each array $M_i$ of shape $(m_i,m)$, optional 54 | >Decomposition matrices for each virtual control objective 55 | * `objectives` : `Sequence` of `Objective` objects of len $(N)$, each $O_i$ specifying $Q_i, R_i$ and $M_i$, optional 56 | >Desired objectives for the game 57 | * `x_0` : `np.array` of len $(n)$, optional 58 | >Initial state vector 59 | * `x_T` : `np.array` of len $(n)$, optional 60 | >Final state vector, in case of signal tracking 61 | * `T_f` : positive `float`, optional 62 | >System dynamics horizon. Should be given in the case of finite horizon 63 | * `P_f` : `list` of `np.array` objects of len $(N)$, each array $P_{f_i}$ of shape $(n,n)$, optional, default = uncoupled solution of `scipy's solve_are` 64 | > 65 | >Final condition for the Riccati equation array. Should be given in the case of finite horizon 66 | * `state_variables_names` : `Sequence` of `str` objects of len $(N)$, optional 67 | >The state variables' names to display when plotting 68 | * `show_legend` : `boolean`, optional 69 | >Indicates whether to display a legend in the plots 70 | * `state_variables_names` : `Sequence` of `str` objects of len $(n)$, optional 71 | >The state variables' names to display 72 | * `epsilon_x` : `float` in the interval $(0,1)$, optional 73 | >Numerical convergence threshold for the state vector of the system 74 | * `epsilon_P` : `float` in the interval $(0,1)$, optional 75 | >Numerical convergence threshold for the matrices P_i 76 | * `L` : positive `int`, optional 77 | >Number of data points 78 | * `eta` : positive `int`, optional 79 | >The number of last matrix norms to consider for convergence 80 | * `debug` : `boolean`, optional 81 | >Indicates whether to display debug information 82 | 83 | 84 | # Tutorial 85 | 86 | To demonstrate the use of the package, we provide a few running examples. 87 | Consider the following system of masses and springs: 88 | 89 | 90 |

91 | 92 |

93 | 94 | The performance of the system under the use of the suggested method is compared with that of a Linear Quadratic Regulator (LQR). For that purpose, class named [`PyDiffGameLQRComparison`](https://github.com/krichelj/PyDiffGame/blob/master/src/PyDiffGame/PyDiffGameLQRComparison.py) is defined. A comparison of a system should subclass this class. 95 | As an example, for the masses and springs system, consider the following instantiation of an [`MassesWithSpringsComparison`](https://github.com/krichelj/PyDiffGame/blob/master/src/PyDiffGame/examples/MassesWithSpringsComparison.py) object: 96 | 97 | ```python 98 | import numpy as np 99 | from PyDiffGame.examples.MassesWithSpringsComparison import MassesWithSpringsComparison 100 | 101 | N = 2 102 | k = 10 103 | m = 50 104 | r = 1 105 | epsilon_x = 10e-8 106 | epsilon_P = 10e-8 107 | q = [[500, 2000], [500, 250]] 108 | 109 | x_0 = np.array([10 * i for i in range(1, N + 1)] + [0] * N) 110 | x_T = x_0 * 10 if N == 2 else np.array([(10 * i) ** 3 for i in range(1, N + 1)] + [0] * N) 111 | T_f = 25 112 | 113 | masses_with_springs = MassesWithSpringsComparison(N=N, 114 | m=m, 115 | k=k, 116 | q=q, 117 | r=r, 118 | x_0=x_0, 119 | x_T=x_T, 120 | T_f=T_f, 121 | epsilon_x=epsilon_x, 122 | epsilon_P=epsilon_P) 123 | ``` 124 | 125 | Consider the constructor of the class `MassesWithSpringsComparison`: 126 | 127 | ```python 128 | import numpy as np 129 | from typing import Sequence, Optional 130 | 131 | from PyDiffGame.PyDiffGame import PyDiffGame 132 | from PyDiffGame.PyDiffGameLQRComparison import PyDiffGameLQRComparison 133 | from PyDiffGame.Objective import GameObjective, LQRObjective 134 | 135 | 136 | class MassesWithSpringsComparison(PyDiffGameLQRComparison): 137 | def __init__(self, 138 | N: int, 139 | m: float, 140 | k: float, 141 | q: float | Sequence[float] | Sequence[Sequence[float]], 142 | r: float, 143 | Ms: Optional[Sequence[np.array]] = None, 144 | x_0: Optional[np.array] = None, 145 | x_T: Optional[np.array] = None, 146 | T_f: Optional[float] = None, 147 | epsilon_x: Optional[float] = PyDiffGame.epsilon_x_default, 148 | epsilon_P: Optional[float] = PyDiffGame.epsilon_P_default, 149 | L: Optional[int] = PyDiffGame.L_default, 150 | eta: Optional[int] = PyDiffGame.eta_default): 151 | I_N = np.eye(N) 152 | Z_N = np.zeros((N, N)) 153 | 154 | M_masses = m * I_N 155 | K = k * (2 * I_N - np.array([[int(abs(i - j) == 1) for j in range(N)] for i in range(N)])) 156 | M_masses_inv = np.linalg.inv(M_masses) 157 | 158 | M_inv_K = M_masses_inv @ K 159 | 160 | if Ms is None: 161 | eigenvectors = np.linalg.eig(M_inv_K)[1] 162 | Ms = [eigenvector.reshape(1, N) for eigenvector in eigenvectors] 163 | 164 | A = np.block([[Z_N, I_N], 165 | [-M_inv_K, Z_N]]) 166 | B = np.block([[Z_N], 167 | [M_masses_inv]]) 168 | 169 | Qs = [np.diag([0.0] * i + [q] + [0.0] * (N - 1) + [q] + [0.0] * (N - i - 1)) 170 | if isinstance(q, (int, float)) else 171 | np.diag([0.0] * i + [q[i]] + [0.0] * (N - 1) + [q[i]] + [0.0] * (N - i - 1)) for i in range(N)] 172 | 173 | M = np.concatenate(Ms, 174 | axis=0) 175 | 176 | assert np.all(np.abs(np.linalg.inv(M) - M.T) < 10e-12) 177 | 178 | Q_mat = np.kron(a=np.eye(2), 179 | b=M) 180 | 181 | Qs = [Q_mat.T @ Q @ Q_mat for Q in Qs] 182 | 183 | Rs = [np.array([r])] * N 184 | R_lqr = 1 / 4 * r * I_N 185 | Q_lqr = q * np.eye(2 * N) if isinstance(q, (int, float)) else np.diag(2 * q) 186 | 187 | state_variables_names = ['x_{' + str(i) + '}' for i in range(1, N + 1)] + \ 188 | ['\\dot{x}_{' + str(i) + '}' for i in range(1, N + 1)] 189 | args = {'A': A, 190 | 'B': B, 191 | 'x_0': x_0, 192 | 'x_T': x_T, 193 | 'T_f': T_f, 194 | 'state_variables_names': state_variables_names, 195 | 'epsilon_x': epsilon_x, 196 | 'epsilon_P': epsilon_P, 197 | 'L': L, 198 | 'eta': eta} 199 | 200 | lqr_objective = [LQRObjective(Q=Q_lqr, 201 | R_ii=R_lqr)] 202 | game_objectives = [GameObjective(Q=Q, 203 | R_ii=R, 204 | M_i=M_i) for Q, R, M_i in zip(Qs, Rs, Ms)] 205 | games_objectives = [lqr_objective, 206 | game_objectives] 207 | 208 | super().__init__(args=args, 209 | M=M, 210 | games_objectives=games_objectives, 211 | continuous=True) 212 | ``` 213 | 214 | Finally, consider calling the `masses_with_springs` object as follows: 215 | 216 | ```python 217 | output_variables_names = ['$\\frac{x_1 + x_2}{\\sqrt{2}}$', 218 | '$\\frac{x_2 - x_1}{\\sqrt{2}}$', 219 | '$\\frac{\\dot{x}_1 + \\dot{x}_2}{\\sqrt{2}}$', 220 | '$\\frac{\\dot{x}_2 - \\dot{x}_1}{\\sqrt{2}}$'] 221 | 222 | masses_with_springs(plot_state_spaces=True, 223 | plot_Mx=True, 224 | output_variables_names=output_variables_names, 225 | save_figure=True) 226 | ``` 227 | 228 | This will result in the following plot that compares the two systems performance for a differential game vs an LQR: 229 | 230 |

231 | 232 | 233 |

234 | 235 | 236 | And when tweaking the weights by setting 237 | 238 | ```python 239 | qs = [[500, 5000]] 240 | ``` 241 | 242 | we have: 243 | 244 |

245 | 246 | 247 |

248 | 249 | # Authors 250 | 251 | If you use this work, please cite our paper: 252 | ``` 253 | @inproceedings{pydiffgame_paper, 254 | author={Kricheli, Joshua Shay and Sadon, Aviran and Arogeti, Shai and Regev, Shimon and Weiss, Gera}, 255 | booktitle={29th Mediterranean Conference on Control and Automation (MED 2021)}, 256 | title={{Composition of Dynamic Control Objectives Based on Differential Games}}, 257 | year={2021}, 258 | volume={}, 259 | number={}, 260 | pages={298-304}, 261 | doi={10.1109/MED51440.2021.9480269}} 262 | ``` 263 | 264 | Further details can be found in the [citation document](CITATIONS.bib). 265 | 266 | # Acknowledgments 267 | 268 | This research was supported in part by the Leona M. and Harry B. Helmsley Charitable Trust through the '_Agricultural, Biological and Cognitive Robotics Initiative_' ('ABC') and by the Marcus Endowment Fund both at Ben-Gurion University of the Negev, Israel. 269 | This research was also supported by The '_Israeli Smart Transportation Research Center_' ('ISTRC') by The Technion and Bar-Ilan Universities, Israel. 270 | 271 |

272 | 273 | 274 | 275 |   276 | 277 | 278 | 279 | 280 | BGU Logo 281 | 282 | 283 |   284 |   285 | 286 | 287 | 288 | 289 | Helmsley Charitable Trust 290 | 291 | 292 |

293 | -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | plugins: 2 | - jekyll-relative-links 3 | relative_links: 4 | enabled: true 5 | collections: true 6 | include: 7 | - CONTRIBUTING.md 8 | - README.md 9 | - CODE_OF_CONDUCT.md 10 | - LICENSE -------------------------------------------------------------------------------- /docs/README.md: -------------------------------------------------------------------------------- 1 |

2 | Logo 3 |

4 | 5 | [![pages-build-deployment](https://github.com/krichelj/PyDiffGame/actions/workflows/pages/pages-build-deployment/badge.svg)](https://github.com/krichelj/PyDiffGame/actions/workflows/pages/pages-build-deployment) 6 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) 7 | 8 | * [What is this?](#what-is-this) 9 | * [Local Installation](#local-installation) 10 | * [Input Parameters](#input-parameters) 11 | * [Tutorial](#tutorial) 12 | * [Acknowledgments](#acknowledgments) 13 | 14 | # What is this? 15 | 16 | [`PyDiffGame`](https://github.com/krichelj/PyDiffGame) is a Python implementation of a Nash Equilibrium solution to Differential Games, based on a reduction of Game Hamilton-Bellman-Jacobi (GHJB) equations to Game Algebraic and Differential Riccati equations, associated with Multi-Objective Dynamical Control Systems\ 17 | The method relies on the formulation given in: 18 | 19 | - The thesis work "_Differential Games for Compositional Handling of Competing Control Tasks_" 20 | ([Research Gate](https://www.researchgate.net/publication/359819808_Differential_Games_for_Compositional_Handling_of_Competing_Control_Tasks)) 21 | 22 | - The conference article "_Composition of Dynamic Control Objectives Based on Differential Games_" 23 | ([IEEE](https://ieeexplore.ieee.org/document/9480269) | 24 | [Research Gate](https://www.researchgate.net/publication/353452024_Composition_of_Dynamic_Control_Objectives_Based_on_Differential_Games)) 25 | 26 | If you use this work, please cite our paper: 27 | ``` 28 | @conference{med_paper, 29 | author={Kricheli, Joshua Shay and Sadon, Aviran and Arogeti, Shai and Regev, Shimon and Weiss, Gera}, 30 | booktitle={29th Mediterranean Conference on Control and Automation (MED)}, 31 | title={{Composition of Dynamic Control Objectives Based on Differential Games}}, 32 | year={2021}, 33 | pages={298-304}, 34 | doi={10.1109/MED51440.2021.9480269}} 35 | ``` 36 | 37 | # Installation 38 | 39 | To install this package run this from the command prompt: 40 | ``` 41 | pip install PyDiffGame 42 | ``` 43 | 44 | The package was tested for Python >= 3.10, along with the listed packages versions in [`requirments.txt`](https://github.com/krichelj/PyDiffGame/blob/master/requirements.txt) 45 | 46 | # Input Parameters 47 | 48 | The package defines an abstract class [`PyDiffGame.py`](https://github.com/krichelj/PyDiffGame/blob/master/src/PyDiffGame/PyDiffGame.py). An object of this class represents an instance of differential game. 49 | The input parameters to instantiate a `PyDiffGame` object are: 50 | 51 | * `A` : `np.array` of shape $(n,n)$ 52 | >System dynamics matrix 53 | * `B` : `np.array` of shape $(n, m_1 + ... + m_N)$, optional 54 | >Input matrix for all virtual control objectives 55 | * `Bs` : `Sequence` of `np.array` objects of len $(N)$, each array $B_i$ of shape $(n,m_i)$, optional 56 | >Input matrices for each virtual control objective 57 | * `Qs` : `Sequence` of `np.array` objects of len $(N)$, each array $Q_i$ of shape $(n,n)$, optional 58 | >State weight matrices for each virtual control objective 59 | * `Rs` : `Sequence` of `np.array` objects of len $(N)$, each array $R_i$ of shape $(m_i,m_i)$, optional 60 | >Input weight matrices for each virtual control objective 61 | * `Ms` : `Sequence` of `np.array` objects of len $(N)$, each array $M_i$ of shape $(m_i,m)$, optional 62 | >Decomposition matrices for each virtual control objective 63 | * `objectives` : `Sequence` of `Objective` objects of len $(N)$, each $O_i$ specifying $Q_i, R_i$ and $M_i$, optional 64 | >Desired objectives for the game 65 | * `x_0` : `np.array` of len $(n)$, optional 66 | >Initial state vector 67 | * `x_T` : `np.array` of len $(n)$, optional 68 | >Final state vector, in case of signal tracking 69 | * `T_f` : positive `float`, optional 70 | >System dynamics horizon. Should be given in the case of finite horizon 71 | * `P_f` : `list` of `np.array` objects of len $(N)$, each array $P_{f_i}$ of shape $(n,n)$, optional, default = uncoupled solution of `scipy's solve_are` 72 | > 73 | >Final condition for the Riccati equation array. Should be given in the case of finite horizon 74 | * `state_variables_names` : `Sequence` of `str` objects of len $(N)$, optional 75 | >The state variables' names to display when plotting 76 | * `show_legend` : `boolean`, optional 77 | >Indicates whether to display a legend in the plots 78 | * `state_variables_names` : `Sequence` of `str` objects of len $(n)$, optional 79 | >The state variables' names to display 80 | * `epsilon_x` : `float` in the interval $(0,1)$, optional 81 | >Numerical convergence threshold for the state vector of the system 82 | * `epsilon_P` : `float` in the interval $(0,1)$, optional 83 | >Numerical convergence threshold for the matrices P_i 84 | * `L` : positive `int`, optional 85 | >Number of data points 86 | * `eta` : positive `int`, optional 87 | >The number of last matrix norms to consider for convergence 88 | * `debug` : `boolean`, optional 89 | >Indicates whether to display debug information 90 | 91 | 92 | # Tutorial 93 | 94 | To demonstrate the use of the package, we provide a few running examples. 95 | Consider the following system of masses and springs: 96 | 97 | 98 |

99 | 100 |

101 | 102 | The performance of the system under the use of the suggested method is compared with that of a Linear Quadratic Regulator (LQR). For that purpose, class named [`PyDiffGameLQRComparison`](https://github.com/krichelj/PyDiffGame/blob/master/src/PyDiffGame/PyDiffGameLQRComparison.py) is defined. A comparison of a system should subclass this class. 103 | As an example, for the masses and springs system, consider the following instantiation of an [`MassesWithSpringsComparison`](https://github.com/krichelj/PyDiffGame/blob/master/src/PyDiffGame/examples/MassesWithSpringsComparison.py) object: 104 | 105 | ```python 106 | import numpy as np 107 | from PyDiffGame.examples.MassesWithSpringsComparison import MassesWithSpringsComparison 108 | 109 | N = 2 110 | k = 10 111 | m = 50 112 | r = 1 113 | epsilon_x = 10e-8 114 | epsilon_P = 10e-8 115 | q = [[500, 2000], [500, 250]] 116 | 117 | x_0 = np.array([10 * i for i in range(1, N + 1)] + [0] * N) 118 | x_T = x_0 * 10 if N == 2 else np.array([(10 * i) ** 3 for i in range(1, N + 1)] + [0] * N) 119 | T_f = 25 120 | 121 | masses_with_springs = MassesWithSpringsComparison(N=N, 122 | m=m, 123 | k=k, 124 | q=q, 125 | r=r, 126 | x_0=x_0, 127 | x_T=x_T, 128 | T_f=T_f, 129 | epsilon_x=epsilon_x, 130 | epsilon_P=epsilon_P) 131 | ``` 132 | 133 | Consider the constructor of the class `MassesWithSpringsComparison`: 134 | 135 | ```python 136 | import numpy as np 137 | from typing import Sequence, Optional 138 | 139 | from PyDiffGame.PyDiffGame import PyDiffGame 140 | from PyDiffGame.PyDiffGameLQRComparison import PyDiffGameLQRComparison 141 | from PyDiffGame.Objective import GameObjective, LQRObjective 142 | 143 | 144 | class MassesWithSpringsComparison(PyDiffGameLQRComparison): 145 | def __init__(self, 146 | N: int, 147 | m: float, 148 | k: float, 149 | q: float | Sequence[float] | Sequence[Sequence[float]], 150 | r: float, 151 | Ms: Optional[Sequence[np.array]] = None, 152 | x_0: Optional[np.array] = None, 153 | x_T: Optional[np.array] = None, 154 | T_f: Optional[float] = None, 155 | epsilon_x: Optional[float] = PyDiffGame.epsilon_x_default, 156 | epsilon_P: Optional[float] = PyDiffGame.epsilon_P_default, 157 | L: Optional[int] = PyDiffGame.L_default, 158 | eta: Optional[int] = PyDiffGame.eta_default): 159 | I_N = np.eye(N) 160 | Z_N = np.zeros((N, N)) 161 | 162 | M_masses = m * I_N 163 | K = k * (2 * I_N - np.array([[int(abs(i - j) == 1) for j in range(N)] for i in range(N)])) 164 | M_masses_inv = np.linalg.inv(M_masses) 165 | 166 | M_inv_K = M_masses_inv @ K 167 | 168 | if Ms is None: 169 | eigenvectors = np.linalg.eig(M_inv_K)[1] 170 | Ms = [eigenvector.reshape(1, N) for eigenvector in eigenvectors] 171 | 172 | A = np.block([[Z_N, I_N], 173 | [-M_inv_K, Z_N]]) 174 | B = np.block([[Z_N], 175 | [M_masses_inv]]) 176 | 177 | Qs = [np.diag([0.0] * i + [q] + [0.0] * (N - 1) + [q] + [0.0] * (N - i - 1)) 178 | if isinstance(q, (int, float)) else 179 | np.diag([0.0] * i + [q[i]] + [0.0] * (N - 1) + [q[i]] + [0.0] * (N - i - 1)) for i in range(N)] 180 | 181 | M = np.concatenate(Ms, 182 | axis=0) 183 | 184 | assert np.all(np.abs(np.linalg.inv(M) - M.T) < 10e-12) 185 | 186 | Q_mat = np.kron(a=np.eye(2), 187 | b=M) 188 | 189 | Qs = [Q_mat.T @ Q @ Q_mat for Q in Qs] 190 | 191 | Rs = [np.array([r])] * N 192 | R_lqr = 1 / 4 * r * I_N 193 | Q_lqr = q * np.eye(2 * N) if isinstance(q, (int, float)) else np.diag(2 * q) 194 | 195 | state_variables_names = ['x_{' + str(i) + '}' for i in range(1, N + 1)] + \ 196 | ['\\dot{x}_{' + str(i) + '}' for i in range(1, N + 1)] 197 | args = {'A': A, 198 | 'B': B, 199 | 'x_0': x_0, 200 | 'x_T': x_T, 201 | 'T_f': T_f, 202 | 'state_variables_names': state_variables_names, 203 | 'epsilon_x': epsilon_x, 204 | 'epsilon_P': epsilon_P, 205 | 'L': L, 206 | 'eta': eta} 207 | 208 | lqr_objective = [LQRObjective(Q=Q_lqr, 209 | R_ii=R_lqr)] 210 | game_objectives = [GameObjective(Q=Q, 211 | R_ii=R, 212 | M_i=M_i) for Q, R, M_i in zip(Qs, Rs, Ms)] 213 | games_objectives = [lqr_objective, 214 | game_objectives] 215 | 216 | super().__init__(args=args, 217 | M=M, 218 | games_objectives=games_objectives, 219 | continuous=True) 220 | ``` 221 | 222 | Finally, consider calling the `masses_with_springs` object as follows: 223 | 224 | ```python 225 | output_variables_names = ['$\\frac{x_1 + x_2}{\\sqrt{2}}$', 226 | '$\\frac{x_2 - x_1}{\\sqrt{2}}$', 227 | '$\\frac{\\dot{x}_1 + \\dot{x}_2}{\\sqrt{2}}$', 228 | '$\\frac{\\dot{x}_2 - \\dot{x}_1}{\\sqrt{2}}$'] 229 | 230 | masses_with_springs(plot_state_spaces=True, 231 | plot_Mx=True, 232 | output_variables_names=output_variables_names, 233 | save_figure=True) 234 | ``` 235 | 236 | Refer 237 | This will result in the following plot that compares the two systems performance for a differential game vs an LQR: 238 | 239 |

240 | 241 | 242 |

243 | 244 | 245 | And when tweaking the weights by setting 246 | 247 | ```python 248 | qs = [[500, 5000]] 249 | ``` 250 | 251 | we have: 252 | 253 |

254 | 255 | 256 |

257 | 258 | 259 | # Acknowledgments 260 | 261 | This research was supported in part by the Leona M. and Harry B. Helmsley Charitable Trust through the Agricultural, Biological and Cognitive Robotics Initiative and by the Marcus Endowment Fund both at Ben-Gurion University of the Negev, Israel. 262 | This research was also supported by The Israeli Smart Transportation Research Center (ISTRC) by The Technion and Bar-Ilan Universities, Israel. 263 | 264 |

265 | 266 | 267 | 268 |   269 | 270 | BGU Logo 271 | 272 |   273 |   274 | 275 | Helmsley Charitable Trust 276 | 277 |

278 | -------------------------------------------------------------------------------- /images/Logo_ISTRC_Green_English.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/images/Logo_ISTRC_Green_English.png -------------------------------------------------------------------------------- /images/logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/images/logo.png -------------------------------------------------------------------------------- /images/logo_abc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/images/logo_abc.png -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["hatchling"] 3 | build-backend = "hatchling.build" 4 | 5 | [project] 6 | name = "PyDiffGame" 7 | version = "1.0.0" 8 | authors = [ 9 | { name="Joshua Shay Kricheli", email="skricheli2@gmail.com" } 10 | ] 11 | description = "PyDiffGame is a Python implementation of a Nash Equilibrium solution to Differential Games, based on a reduction of Game Hamilton-Bellman-Jacobi (GHJB) equations to Game Algebraic and Differential Riccati equations, associated with Multi-Objective Dynamical Control Systems" 12 | readme = "README.md" 13 | requires-python = ">=3.10" 14 | classifiers = [ 15 | "Programming Language :: Python :: 3", 16 | "License :: OSI Approved :: MIT License", 17 | "Operating System :: OS Independent", 18 | ] 19 | 20 | [project.urls] 21 | "Homepage" = "https://krichelj.github.io/PyDiffGame/" 22 | "Bug Tracker" = "https://github.com/krichelj/PyDiffGame/issues" 23 | 24 | # Specify where your source code is located 25 | [tool.hatch.build.targets.wheel] 26 | packages = ["src/PyDiffGame"] # Replace with the actual path to your package 27 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | matplotlib>=3.7.0 2 | numpy>=1.24.2 3 | scipy>=1.8.0 4 | setuptools>=61.0 5 | tqdm>=4.63.0 6 | sympy~=1.11.1 -------------------------------------------------------------------------------- /src/PyDiffGame/ContinuousPyDiffGame.py: -------------------------------------------------------------------------------- 1 | import contextlib 2 | import os 3 | import sys 4 | import numpy as np 5 | from scipy.integrate import odeint 6 | from numpy.linalg import eigvals, inv 7 | from typing import Sequence, Optional, Union 8 | 9 | from PyDiffGame.PyDiffGame import PyDiffGame 10 | from PyDiffGame.Objective import Objective 11 | 12 | 13 | class ContinuousPyDiffGame(PyDiffGame): 14 | """ 15 | Continuous differential game base class 16 | 17 | 18 | Considers control design according to the system: 19 | dx(t)/dt = A x(t) + sum_{j=1}^N B_j v_j(t) 20 | """ 21 | 22 | def __init__(self, 23 | A: np.array, 24 | B: Optional[np.array] = None, 25 | Bs: Optional[Sequence[np.array]] = None, 26 | Qs: Optional[Sequence[np.array]] = None, 27 | Rs: Optional[Sequence[np.array]] = None, 28 | Ms: Optional[Sequence[np.array]] = None, 29 | objectives: Optional[Sequence[Objective]] = None, 30 | x_0: Optional[np.array] = None, 31 | x_T: Optional[np.array] = None, 32 | T_f: Optional[float] = None, 33 | P_f: Optional[Union[Sequence[np.array], np.array]] = None, 34 | show_legend: Optional[bool] = True, 35 | state_variables_names: Optional[Sequence[str]] = None, 36 | epsilon_x: Optional[float] = PyDiffGame._epsilon_x_default, 37 | epsilon_P: Optional[float] = PyDiffGame._epsilon_P_default, 38 | L: Optional[int] = PyDiffGame._L_default, 39 | eta: Optional[int] = PyDiffGame._eta_default, 40 | debug: Optional[bool] = False): 41 | 42 | super().__init__(A=A, 43 | B=B, 44 | Bs=Bs, 45 | Qs=Qs, 46 | Rs=Rs, 47 | Ms=Ms, 48 | objectives=objectives, 49 | x_0=x_0, 50 | x_T=x_T, 51 | T_f=T_f, 52 | P_f=P_f, 53 | show_legend=show_legend, 54 | state_variables_names=state_variables_names, 55 | epsilon_x=epsilon_x, 56 | epsilon_P=epsilon_P, 57 | L=L, 58 | eta=eta, 59 | debug=debug) 60 | 61 | self.__dx_dt = None 62 | 63 | def __solve_N_coupled_diff_riccati(_: float, P_t: np.array) -> np.array: 64 | """ 65 | odeint Coupled Differential Matrix Riccati Equations Solver function 66 | 67 | Parameters 68 | ---------- 69 | _: float 70 | Integration point in time 71 | P_t: numpy array of numpy 2-d arrays, of len(N), of shape(n, n) 72 | Current integrated solution 73 | 74 | Returns 75 | ---------- 76 | dP_tdt: numpy array of numpy 2-d arrays, of len(N), of shape(n, n) 77 | Current calculated value for the time-derivative of the matrices P_t 78 | """ 79 | P_t = [(P_t[i * self._P_size:(i + 1) * self._P_size]).reshape(self._n, self._n) for i in range(self._N)] 80 | dP_tdt = np.zeros((self._P_size,)) 81 | A_cl_t = self._A - sum([(B_j @ inv(R_j) if R_j.ndim > 1 else B_j / R_j) @ B_j.T @ P_t_j 82 | for B_j, R_j, P_t_j in zip(self._Bs, self._Rs, P_t)]) 83 | 84 | for i in range(self._N): 85 | P_t_i = P_t[i] 86 | B_i = self._Bs[i] 87 | R_ii = self._Rs[i] 88 | Q_i = self._Qs[i] 89 | 90 | dP_t_idt = - A_cl_t.T @ P_t_i - P_t_i @ A_cl_t - Q_i - P_t_i @ (B_i @ inv(R_ii) 91 | if R_ii.ndim > 1 92 | else B_i / R_ii) @ B_i.T @ P_t_i 93 | 94 | dP_t_idt = dP_t_idt.reshape(self._P_size) 95 | dP_tdt = dP_t_idt if not i else np.concatenate((dP_tdt, dP_t_idt), axis=0) 96 | 97 | dP_tdt = np.ravel(dP_tdt) 98 | 99 | return dP_tdt 100 | 101 | self._ode_func = __solve_N_coupled_diff_riccati 102 | 103 | def _update_K_from_last_state(self, t: int): 104 | """ 105 | After the matrices P_i are obtained, this method updates the controllers at time t by the rule: 106 | K_i(t) = R^{-1}_ii B^T_i P_i(t) 107 | 108 | Parameters 109 | ---------- 110 | t: int 111 | Current point in time 112 | """ 113 | 114 | P_t = [(self._P[t][i * self._P_size:(i + 1) * self._P_size]).reshape(self._n, self._n) for i in range(self._N)] 115 | self._K = [(inv(R_ii) @ B_i.T if R_ii.ndim > 1 else 1 / R_ii * B_i.T) @ P_i 116 | for R_ii, B_i, P_i in zip(self._Rs, self._Bs, P_t)] 117 | 118 | def _get_K_i(self, i: int) -> np.array: 119 | return self._K[i] 120 | 121 | def _get_P_f_i(self, i: int) -> np.array: 122 | if len(self._P_f) == self._N: 123 | return self._P_f[i] 124 | 125 | return self._P_f[i * self._P_size:(i + 1) * self._P_size].reshape(self._n, self._n) 126 | 127 | def _update_Ps_from_last_state(self): 128 | """ 129 | With P_f as the terminal condition, evaluates the matrices P_i by backwards-solving this set of 130 | coupled time-continuous Riccati differential equations: 131 | 132 | dP_idt = - A^T P_i(t) - P_i(t) A - Q_i + P_i(t) sum_{j=1}^N B_j R^{-1}_{jj} B^T_j P_j(t) + 133 | [sum_{j=1, j!=i}^N P_j(t) B_j R^{-1}_{jj} B^T_j] P_i(t) 134 | """ 135 | 136 | with stdout_redirected(): 137 | self._P = odeint(func=self._ode_func, 138 | y0=np.ravel(self._P_f), 139 | t=self._backward_time, 140 | tfirst=True) 141 | 142 | def _solve_finite_horizon(self): 143 | """ 144 | Considers the following set of finite-horizon cost functions: 145 | J_i = x(T_f)^T F_i(T_f) x(T_f) + int_{t=0}^T_f [x(t)^T Q_i x(t) + sum_{j=1}^N u_j(t)^T R_{ij} u_j(t)] dt 146 | 147 | In the continuous-time case, the matrices P_i can be solved for, unrelated to the controllers K_i. 148 | Then when simulating the state space progression, the controllers K_i can be computed as functions of P_i 149 | for each time interval 150 | """ 151 | 152 | self._update_Ps_from_last_state() 153 | self._converged = True 154 | 155 | def _solve_infinite_horizon(self): 156 | """ 157 | Considers the following finite-horizon cost functions: 158 | J_i = int_{t=0}^infty [x(t)^T Q_i x(t) + sum_{j=1}^N u_j(t)^T R_{ij} u_j(t)] dt 159 | """ 160 | 161 | self._converge_DREs_to_AREs() 162 | 163 | def is_A_cl_stable(self) -> bool: 164 | """ 165 | Tests Lyapunov stability of the closed loop: 166 | A continuous dynamic system governed by the matrix A_cl has Lyapunov stability iff: 167 | Re(eig) <= 0 forall eigenvalues of A_cl and there is at most one real eigenvalue at the origin 168 | """ 169 | 170 | closed_loop_eigenvalues = eigvals(self._A_cl) 171 | closed_loop_eigenvalues_real_values = [eig.real for eig in closed_loop_eigenvalues] 172 | num_of_zeros = [int(v) for v in closed_loop_eigenvalues_real_values].count(0) 173 | non_positive_eigenvalues = all([eig_real_value <= 0 for eig_real_value in closed_loop_eigenvalues_real_values]) 174 | at_most_one_zero_eigenvalue = num_of_zeros <= 1 175 | stability = non_positive_eigenvalues and at_most_one_zero_eigenvalue 176 | 177 | return stability 178 | 179 | @PyDiffGame._post_convergence 180 | def _solve_state_space(self): 181 | """ 182 | Propagates the game forward through time and solves for it by solving the continuous differential equation: 183 | dx(t)/dt = A_cl(t) x(t) 184 | In each step, the controllers K_i are calculated with the values of P_i evaluated beforehand. 185 | """ 186 | 187 | t = 0 188 | 189 | def state_diff_eqn(x_t: np.array, 190 | _: float) -> np.array: 191 | """ 192 | Scipy's odeint State Variables Solver function 193 | 194 | Parameters 195 | ---------- 196 | x_t: numpy 1-d array of shape(n) 197 | Current integrated state variables 198 | _: float 199 | Current time 200 | 201 | Returns 202 | ---------- 203 | dx_t_dt: numpy 1-d array of shape(n) 204 | Current calculated value for the time-derivative of the state variable vector x_t 205 | """ 206 | 207 | nonlocal t 208 | 209 | self._update_K_from_last_state(t=t) 210 | self._update_A_cl_from_last_state() 211 | dx_t_dt = self._A_cl @ ((x_t - self._x_T) if self._x_T is not None else x_t) 212 | dx_t_dt = dx_t_dt.ravel() 213 | 214 | if t < self._L - 1: 215 | t += 1 216 | 217 | return dx_t_dt 218 | 219 | with stdout_redirected(): 220 | self._x = odeint(func=state_diff_eqn, 221 | y0=self._x_0, 222 | t=self._forward_time) 223 | 224 | def _simulate_curr_x_T(self) -> np.array: 225 | 226 | def state_diff_eqn(x_t: np.array, 227 | _: float) -> np.array: 228 | self._update_K_from_last_state(t=-1) 229 | self._update_A_cl_from_last_state() 230 | dx_t_dt = self._A_cl @ ((x_t - self._x_T) if self._x_T is not None else x_t) 231 | 232 | return dx_t_dt.ravel() 233 | 234 | with stdout_redirected(): 235 | simulated_x_T = odeint(func=state_diff_eqn, 236 | y0=self._x_0, 237 | t=self._forward_time)[-1] 238 | 239 | return simulated_x_T 240 | 241 | 242 | def fileno(file_or_fd): 243 | fd = getattr(file_or_fd, 'fileno', lambda: file_or_fd)() 244 | if not isinstance(fd, int): 245 | raise ValueError("Expected a file (`.fileno()`) or a file descriptor") 246 | 247 | return fd 248 | 249 | 250 | @contextlib.contextmanager 251 | def stdout_redirected(to=os.devnull, 252 | stdout=None): 253 | """ 254 | https://stackoverflow.com/a/22434262/190597 (J.F. Sebastian) 255 | """ 256 | if stdout is None: 257 | stdout = sys.stdout 258 | 259 | stdout_fd = fileno(stdout) 260 | # copy stdout_fd before it is overwritten 261 | # NOTE: `copied` is inheritable on Windows when duplicating a standard stream 262 | with os.fdopen(os.dup(stdout_fd), 'wb') as copied: 263 | stdout.flush() # flush library buffers that dup2 knows nothing about 264 | try: 265 | os.dup2(fileno(to), stdout_fd) # $ exec >&to 266 | except ValueError: # filename 267 | with open(to, 'wb') as to_file: 268 | os.dup2(to_file.fileno(), stdout_fd) # $ exec > to 269 | try: 270 | yield stdout # allow code to be run with the redirected stdout 271 | finally: 272 | # restore stdout to its previous value 273 | # NOTE: dup2 makes stdout_fd inheritable unconditionally 274 | stdout.flush() 275 | os.dup2(copied.fileno(), stdout_fd) # $ exec >&copied 276 | -------------------------------------------------------------------------------- /src/PyDiffGame/DiscretePyDiffGame.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy as sp 3 | 4 | from typing import Sequence, Optional, Union 5 | 6 | from PyDiffGame.PyDiffGame import PyDiffGame 7 | from PyDiffGame.Objective import Objective 8 | 9 | 10 | class DiscretePyDiffGame(PyDiffGame): 11 | """ 12 | Discrete differential game base class 13 | 14 | 15 | Considers control design according to the system: 16 | x[k+1] = A_tilda x[k] + sum_{j=1}^N B_j_tilda v_j[k] 17 | 18 | where A_tilda and each B_j_tilda are in discrete form, meaning they correlate to a discrete system, 19 | which, at the sampling points, is assumed to be equal to some equivalent continuous system of the form: 20 | dx(t)/dt = A x(t) + sum_{j=1}^N B_j v_j(t) 21 | 22 | Parameters 23 | ---------- 24 | is_input_discrete: boolean, optional, default = False 25 | Indicates whether the input matrices A, B, Q, R are in discrete form or whether they need to be discretized 26 | """ 27 | 28 | def __init__(self, 29 | A: np.array, 30 | B: Optional[np.array] = None, 31 | Bs: Optional[Sequence[np.array]] = None, 32 | Qs: Optional[Sequence[np.array]] = None, 33 | Rs: Optional[Sequence[np.array]] = None, 34 | Ms: Optional[Sequence[np.array]] = None, 35 | objectives: Optional[Sequence[Objective]] = None, 36 | is_input_discrete: Optional[bool] = False, 37 | x_0: Optional[np.array] = None, 38 | x_T: Optional[np.array] = None, 39 | T_f: Optional[float] = None, 40 | P_f: Optional[Union[Sequence[np.array], np.array]] = None, 41 | show_legend: Optional[bool] = True, 42 | state_variables_names: Optional[Sequence] = None, 43 | epsilon_x: Optional[float] = PyDiffGame._epsilon_x_default, 44 | epsilon_P: Optional[float] = PyDiffGame._epsilon_P_default, 45 | L: Optional[int] = PyDiffGame._L_default, 46 | eta: Optional[int] = PyDiffGame._eta_default, 47 | debug: Optional[bool] = False): 48 | 49 | super().__init__(A=A, 50 | B=B, 51 | Bs=Bs, 52 | Qs=Qs, 53 | Rs=Rs, 54 | Ms=Ms, 55 | objectives=objectives, 56 | x_0=x_0, 57 | x_T=x_T, 58 | T_f=T_f, 59 | P_f=P_f, 60 | show_legend=show_legend, 61 | state_variables_names=state_variables_names, 62 | epsilon_x=epsilon_x, 63 | epsilon_P=epsilon_P, 64 | L=L, 65 | eta=eta, 66 | debug=debug 67 | ) 68 | 69 | if not is_input_discrete: 70 | self.__discretize_game() 71 | 72 | self.__K_rows_num = sum([B_i.shape[1] for B_i in self._Bs]) 73 | 74 | def __solve_for_K_k(K_k_previous: np.array, 75 | k_1: int) -> np.array: 76 | """ 77 | fsolve Controllers Solver function 78 | 79 | Parameters 80 | ---------- 81 | K_k_previous: numpy array 82 | The previous estimation for the controllers at the k'th sample point 83 | k_1: int 84 | The k+1 index 85 | 86 | Returns 87 | ---------- 88 | dP_tdt: list of numpy 2-d arrays, of len(N), of shape(n, n) 89 | Current calculated value for the time-derivative of the matrices P_t 90 | """ 91 | 92 | K_k_previous = K_k_previous.reshape((self._N, 93 | self.__K_rows_num, 94 | self._n)) 95 | P_k_1 = self._P[k_1] 96 | K_k = np.zeros_like(K_k_previous) 97 | 98 | for i in range(self._N): 99 | i_indices = self.__get_indices_tuple(i) 100 | K_k_previous_i = K_k_previous[i_indices] 101 | P_k_1_i = P_k_1[i] 102 | R_ii = self._R[i] 103 | B_i = self._Bs[i] 104 | B_i_T = B_i.T 105 | 106 | A_cl_k_i = self._A 107 | 108 | for j in range(self._N): 109 | if j != i: 110 | B_j = self._Bs[j] 111 | j_indices = self.__get_indices_tuple(j) 112 | K_k_previous_j = K_k_previous[j_indices] 113 | A_cl_k_i = A_cl_k_i - B_j @ K_k_previous_j 114 | 115 | K_k_i = K_k_previous_i - np.linalg.inv(R_ii + B_i_T @ P_k_1_i @ B_i) @ B_i_T @ P_k_1_i @ A_cl_k_i 116 | K_k[i_indices] = K_k_i 117 | 118 | return K_k.ravel() 119 | 120 | self.f_solve_func = __solve_for_K_k 121 | 122 | def __discretize_game(self): 123 | """ 124 | The input to the discrete game can either be given in continuous or discrete form. 125 | If it is not in discrete form, the model gets discretized using this method in the following manner: 126 | A = exp(delta_T A) 127 | B_i = int_{t=0}^delta_T exp(tA) dt B_i 128 | 129 | The performance index gets discretized in the following manner: 130 | Q_i = Q_i * delta_T 131 | R_i = Q_i / delta_T 132 | """ 133 | 134 | A_tilda = np.exp(self._A * self._delta) 135 | e_AT = sp.integrate.quad(lambda T: np.array([ 136 | np.exp(t * self._A) for t in T]).swapaxes(0, 2).swapaxes(0, 1), 137 | a=0, 138 | b=self._delta)[0] 139 | 140 | self._A = A_tilda 141 | B_tilda = np.array(e_AT) 142 | self._Bs = [B_tilda @ B_i for B_i in self._Bs] 143 | self._Q = [Q_i * self._delta for Q_i in self._Q] 144 | self._R = [R_i / self._delta for R_i in self._Rs] 145 | 146 | def __simpson_integrate_Q(self): 147 | """ 148 | Use Simpson's approximation for the definite integral of the state cost function term: 149 | x(t) ^T Q_f_i x(t) + int_{t=0}^T_f x(t) ^T Q_i x(t) ~ sum_{k=0)^K x(t) ^T Q_i_k x(t) 150 | 151 | where: 152 | Q_i_0 = 1 / 3 * Q_i 153 | Q_i_1, ..., Q_i_K-1 = 4 / 3 * Q_i 154 | Q_i_2, ..., Q_i_K-2 = 2 / 3 * Q_i 155 | """ 156 | Q = np.array(self._Q) 157 | self._Q = np.zeros((self._L, 158 | self._N, 159 | self._n, 160 | self._n)) 161 | self._Q[0] = 1 / 3 * Q 162 | self._Q[1::2] = 4 / 3 * Q 163 | self._Q[::2] = 2 / 3 * Q 164 | 165 | def __initialize_finite_horizon(self): 166 | """ 167 | Initializes the calculated parameters for the finite horizon case by the following steps: 168 | - The matrices P_i and controllers K_i are initialized randomly for all sample points 169 | - The closed-loop dynamics matrix A_cl is first set to zero for all sample points 170 | - The terminal conditions P_f_i are set to Q_i 171 | - The resulting closed-loop dynamics matrix A_cl for the last sampling point is updated 172 | - The state is initialized with its initial value, if given 173 | """ 174 | 175 | self._P = np.random.rand(self._L, 176 | self._N, 177 | self._n, 178 | self._n) 179 | self._K = np.random.rand(self._L, 180 | self._N, 181 | self.__K_rows_num, 182 | self._n) 183 | self._A_cl = np.zeros((self._L, 184 | self._n, 185 | self._n)) 186 | 187 | self._P[-1] = np.array(self._Q).reshape((self._N, 188 | self._n, 189 | self._n)) 190 | self._update_A_cl_from_last_state(k=-1) 191 | self._x = np.zeros((self._L, 192 | self._n)) 193 | self._x[0] = self._x_0 194 | 195 | def __get_K_i_shape(self, 196 | i: int) -> range: 197 | """ 198 | Returns the i'th controller shape indices 199 | 200 | Parameters 201 | ---------- 202 | i: int 203 | The desired controller index 204 | """ 205 | 206 | K_i_offset = sum([self._Bs[j].shape[1] for j in range(i)]) if i else 0 207 | first_K_i_index = K_i_offset 208 | second_K_i_index = first_K_i_index + self._Bs[i].shape[1] 209 | 210 | return range(first_K_i_index, second_K_i_index) 211 | 212 | def __get_indices_tuple(self, i: int): 213 | return i, self.__get_K_i_shape(i) 214 | 215 | def _update_K_from_last_state(self, 216 | k_1: int): 217 | """ 218 | After initializing, in order to solve for the controllers K_i and matrices P_i, 219 | we start by solving for the controllers by the update rule: 220 | K_i[k] = [ R_ii + B_i^T P_i[k+1] B_i ] ^ {-1} B_i^T P_i[k+1] [ A - sum_{j=1, j!=i}^N B_j K_j[k] ] 221 | 222 | Parameters 223 | ---------- 224 | k_1: int 225 | The current k+1 sample index 226 | """ 227 | 228 | K_k_1 = self._K[k_1] 229 | K_k_0 = np.random.rand(*K_k_1.shape) 230 | K_k = sp.optimize.fsolve(func=self.f_solve_func, 231 | x0=K_k_0, 232 | args=tuple([k_1])) 233 | K_k_reshaped = np.array(K_k).reshape((self._N, 234 | self.__K_rows_num, 235 | self._n)) 236 | 237 | k = k_1 - 1 238 | 239 | if self._debug: 240 | f_K_k = self.f_solve_func(K_k_previous=K_k, k_1=k) 241 | close_to_zero_array = np.isclose(f_K_k, np.zeros_like(K_k)) 242 | is_close_to_zero = all(close_to_zero_array) 243 | print(f"The current solution is {'NOT ' if not is_close_to_zero else ''}close to zero" 244 | f"\nK_k:\n{K_k_reshaped}\nf_K_k:\n{f_K_k}") 245 | 246 | self._K[k] = K_k_reshaped 247 | 248 | def _get_K_i(self, 249 | i: int, 250 | k: int) -> np.array: 251 | return self._K[k][self.__get_indices_tuple(i)] 252 | 253 | def _get_P_f_i(self, 254 | i: int) -> np.array: 255 | return self._P_f[i] 256 | 257 | def _update_Ps_from_last_state(self, 258 | k_1: int): 259 | """ 260 | Updates the matrices P_i with 261 | A_cl[k] = A - sum_{i=1}^N B_i K_i[k] 262 | 263 | Parameters 264 | ---------- 265 | k_1: int 266 | The current k'th sample index 267 | """ 268 | 269 | k = k_1 - 1 270 | P_k_1 = self._P[k_1] 271 | K_k = self._K[k] 272 | A_cl_k = self._A_cl[k] 273 | P_k = np.zeros_like(P_k_1) 274 | 275 | for i in range(self._N): 276 | i_indices = self.__get_indices_tuple(i) 277 | K_k_i = K_k[i_indices] 278 | K_k_i_T = K_k_i.T 279 | P_k_1_i = P_k_1[i] 280 | R_ii = self._R[i] 281 | Q_i = self._Q[i] 282 | 283 | P_k[i] = A_cl_k.T @ P_k_1_i @ A_cl_k + (K_k_i_T @ R_ii if R_ii.ndim > 1 else K_k_i_T * R_ii) @ K_k_i + Q_i 284 | 285 | self._P[k] = P_k 286 | 287 | def is_A_cl_stable(self, 288 | k: int) -> bool: 289 | """ 290 | Tests Lyapunov stability of the closed loop: 291 | A discrete dynamic system governed by the matrix A_cl has Lyapunov stability iff: 292 | |eig| <= 1 forall eigenvalues of A_cl 293 | """ 294 | 295 | A_cl_k = self._A_cl[k] 296 | A_cl_k_eigenvalues = np.linalg.eigvals(A_cl_k) 297 | A_cl_k_eigenvalues_absolute_values = [abs(eig) for eig in A_cl_k_eigenvalues] 298 | 299 | if self._debug: 300 | print(A_cl_k_eigenvalues_absolute_values) 301 | 302 | stability = all([eig_norm < 1 for eig_norm in A_cl_k_eigenvalues_absolute_values]) 303 | 304 | return stability 305 | 306 | def _solve_finite_horizon(self): 307 | """ 308 | Solves the system with the finite-horizon cost functions: 309 | J_i = sum_{k=1}^T_f [ x[k]^T Q_i x[k] + sum_{j=1}^N u_j[k]^T R_{ij} u_j[k] ] 310 | 311 | In the discrete-time case, the matrices P_i have to be solved simultaneously with the controllers K_i 312 | """ 313 | 314 | pass 315 | 316 | def _solve_infinite_horizon(self): 317 | """ 318 | Solves the system with the infinite-horizon cost functions: 319 | J_i = sum_{k=1}^infty [ x[k]^T Q_i x[k] + sum_{j=1}^N u_j[k]^T R_{ij} u_j[k] ] 320 | """ 321 | 322 | self.__initialize_finite_horizon() 323 | 324 | for backwards_index, t in enumerate(self._backward_time[:-1]): 325 | k_1 = self._L - 1 - backwards_index 326 | k = k_1 - 1 327 | 328 | if self._debug: 329 | print('#' * 30 + f' t = {round(t, 3)} ' + '#' * 30) 330 | 331 | self._update_K_from_last_state(k_1=k_1) 332 | 333 | if self._debug: 334 | print(f'K_k+1:\n{self._K[k_1]}') 335 | 336 | self._update_A_cl_from_last_state(k=k) 337 | 338 | if self._debug: 339 | print(f'A_cl_k:\n{self._A_cl[k]}') 340 | stable_k = self.is_A_cl_stable(k) 341 | print(f"The system is {'NOT ' if not stable_k else ''}stable") 342 | 343 | self._update_Ps_from_last_state(k_1=k_1) 344 | 345 | @PyDiffGame._post_convergence 346 | def _solve_state_space(self): 347 | """ 348 | Propagates the game through time and solves for it by solving the discrete difference equation: 349 | x[k+1] = A_cl[k] x[k] 350 | In each step, the controllers K_i are calculated with the values of P_i evaluated beforehand. 351 | """ 352 | 353 | x_1_k = self._x_0 354 | 355 | for k in range(1, self._L - 1): 356 | A_cl_k = self._A_cl[k] 357 | x_k = A_cl_k @ x_1_k 358 | self._x[k] = x_k 359 | x_1_k = x_k 360 | -------------------------------------------------------------------------------- /src/PyDiffGame/LQR.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from typing import Sequence, Optional 3 | from abc import ABC 4 | 5 | from PyDiffGame.PyDiffGame import PyDiffGame 6 | from PyDiffGame.ContinuousPyDiffGame import ContinuousPyDiffGame 7 | from PyDiffGame.DiscretePyDiffGame import DiscretePyDiffGame 8 | from PyDiffGame.Objective import LQRObjective 9 | 10 | 11 | class LQR(PyDiffGame, ABC): 12 | """ 13 | LQR abstract base class 14 | 15 | 16 | Considers a differential game with only one objective 17 | """ 18 | 19 | def __init__(self, 20 | A: np.array, 21 | B: np.array, 22 | objective: LQRObjective, 23 | x_0: Optional[np.array] = None, 24 | x_T: Optional[np.array] = None, 25 | T_f: Optional[float] = None, 26 | P_f: Optional[np.array] = None, 27 | show_legend: Optional[bool] = True, 28 | state_variables_names: Optional[Sequence[str]] = None, 29 | epsilon_x: Optional[float] = PyDiffGame._epsilon_x_default, 30 | L: Optional[int] = PyDiffGame._L_default, 31 | eta: Optional[int] = PyDiffGame._eta_default, 32 | debug: Optional[bool] = False): 33 | super().__init__(A=A, 34 | B=B, 35 | objectives=[objective], 36 | x_0=x_0, 37 | x_T=x_T, 38 | T_f=T_f, 39 | P_f=P_f, 40 | show_legend=show_legend, 41 | state_variables_names=state_variables_names, 42 | epsilon_x=epsilon_x, 43 | L=L, 44 | eta=eta, 45 | debug=debug) 46 | 47 | 48 | class ContinuousLQR(LQR, ContinuousPyDiffGame): 49 | """ 50 | Continuous LQR marker class 51 | 52 | 53 | Considers control design according to the system: 54 | dx(t)/dt = A x(t) + B u(t) 55 | """ 56 | 57 | pass 58 | 59 | 60 | class DiscreteLQR(LQR, DiscretePyDiffGame): 61 | """ 62 | Discrete LQR marker class 63 | 64 | 65 | Considers control design according to the system: 66 | x[k+1] = A_tilda x[k] + B_tilda u[k] 67 | 68 | where A_tilda and B_tilda are in discrete form, meaning they correlate to a discrete system, 69 | which, at the sampling points, is assumed to be equal to some equivalent continuous system of the form: 70 | dx(t)/dt = A x(t) + B u(t) 71 | """ 72 | 73 | pass 74 | -------------------------------------------------------------------------------- /src/PyDiffGame/Objective.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from abc import ABC 3 | 4 | 5 | class Objective(ABC): 6 | """ 7 | Objective abstract base class 8 | 9 | 10 | Parameters 11 | ---------- 12 | Q: 2-d np.array of shape(n, n) 13 | Cost function state weights for the objective 14 | R_ii: 2-d np.array of shape(m_i, m_i) 15 | Cost function input weights for the objective with regard to the i'th input 16 | """ 17 | 18 | def __init__(self, 19 | Q: np.array, 20 | R_ii: np.array): 21 | self._Q = Q 22 | self._R_ii = R_ii 23 | 24 | @property 25 | def Q(self) -> np.array: 26 | return self._Q 27 | 28 | @property 29 | def R(self) -> np.array: 30 | return self._R_ii 31 | 32 | 33 | class LQRObjective(Objective): 34 | """ 35 | LQR objective marker class 36 | """ 37 | 38 | pass 39 | 40 | 41 | class GameObjective(Objective): 42 | """ 43 | Game objective class 44 | 45 | 46 | Parameters 47 | ---------- 48 | M_i: 2-d np.array of shape(m_i, m) 49 | Division matrix for the i'th virtual input 50 | """ 51 | 52 | def __init__(self, 53 | Q: np.array, 54 | R_ii: np.array, 55 | M_i: np.array): 56 | self.__M_i = M_i 57 | super().__init__(Q=Q, 58 | R_ii=R_ii) 59 | 60 | @property 61 | def M_i(self) -> np.array: 62 | return self.__M_i 63 | -------------------------------------------------------------------------------- /src/PyDiffGame/PyDiffGame.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import os 4 | from pathlib import Path 5 | from tqdm import tqdm 6 | import numpy as np 7 | import math 8 | import matplotlib.pyplot as plt 9 | import scipy as sp 10 | import sympy 11 | import warnings 12 | from typing import Callable, Final, ClassVar, Optional, Sequence, Union 13 | from abc import ABC, abstractmethod 14 | 15 | from PyDiffGame.Objective import Objective, GameObjective, LQRObjective 16 | 17 | 18 | class PyDiffGame(ABC, Callable, Sequence): 19 | """ 20 | Differential game abstract base class 21 | 22 | 23 | Parameters 24 | ---------- 25 | 26 | A: np.array of shape(n, n) 27 | System dynamics matrix 28 | B: np.array of shape(n, sum_{i=}1^N m_i), optional 29 | Input matrix for all virtual control objectives 30 | Bs: Sequence of np.arrays of len(N), each array B_i of shape(n, m_i), optional 31 | Input matrices for each virtual control objective 32 | Qs: Sequence of np.arrays of len(N), each array Q_i of shape(n, n), optional 33 | State weight matrices for each virtual control objective 34 | Rs: Sequence of np.arrays of len(N), each array R_i of shape(m_i, m_i), optional 35 | Input weight matrices for each virtual control objective 36 | Ms: Sequence of np.arrays of len(N), each array M_i of shape(m_i, m), optional 37 | Decomposition matrices for each virtual control objective 38 | objectives: Sequence of Objective objects of len(N), optional 39 | Desired objectives for the game, each O_i specifying Q_i R_i and M_i 40 | x_0: np.array of shape(n), optional 41 | Initial state vector 42 | x_T: np.array of shape(n), optional 43 | Final state vector, in case of signal tracking 44 | T_f: positive float, optional 45 | System dynamics horizon. Should be given in the case of finite horizon 46 | P_f: Sequence of np.arrays of len(N), each array P_f_i of shape(n, n), optional 47 | default = uncoupled solution of scipy's solve_are 48 | Final condition for the Riccati equation array. Should be given in the case of finite horizon 49 | show_legend: bool, optional 50 | Indicates whether to display a legend in the plots 51 | state_variables_names: Sequence of str objects of len(n), optional 52 | The state variables' names to display when plotting 53 | epsilon_x: float in the interval (0,1), optional 54 | Numerical convergence threshold for the state vector of the system 55 | epsilon_P: float in the interval (0,1), optional 56 | Numerical convergence threshold for the matrices P_i 57 | L: positive int, optional 58 | Number of data points 59 | eta: positive int, optional 60 | The number of last matrix norms to consider for convergence 61 | debug: bool, optional 62 | Indicates whether to display debug information 63 | """ 64 | 65 | # class fields 66 | __T_f_default: Final[ClassVar[int]] = 20 67 | _epsilon_x_default: Final[ClassVar[float]] = 10e-7 68 | _epsilon_P_default: Final[ClassVar[float]] = 10e-7 69 | _eigvals_tol: Final[ClassVar[float]] = 10e-7 70 | _L_default: Final[ClassVar[int]] = 1000 71 | _eta_default: Final[ClassVar[int]] = 3 72 | _g: Final[ClassVar[float]] = 9.81 73 | __x_max_convergence_iterations: Final[ClassVar[int]] = 25 74 | __p_max_convergence_iterations: Final[ClassVar[int]] = 25 75 | 76 | _default_figures_path = Path(fr'{os.getcwd()}/figures') 77 | _default_figures_filename = 'image' 78 | 79 | @classmethod 80 | @property 81 | def epsilon_x_default(cls) -> float: 82 | return cls._epsilon_x_default 83 | 84 | @classmethod 85 | @property 86 | def epsilon_P_default(cls) -> float: 87 | return cls._epsilon_P_default 88 | 89 | @classmethod 90 | @property 91 | def L_default(cls) -> int: 92 | return cls._L_default 93 | 94 | @classmethod 95 | @property 96 | def eta_default(cls) -> float: 97 | return cls._eta_default 98 | 99 | @classmethod 100 | @property 101 | def g(cls) -> float: 102 | return cls._g 103 | 104 | @classmethod 105 | @property 106 | def default_figures_path(cls) -> Path: 107 | return cls._default_figures_path 108 | 109 | @classmethod 110 | @property 111 | def default_figures_filename(cls) -> str: 112 | return cls._default_figures_filename 113 | 114 | def __init__(self, 115 | A: np.array, 116 | B: Optional[np.array] = None, 117 | Bs: Optional[Sequence[np.array]] = None, 118 | Qs: Optional[Sequence[np.array]] = None, 119 | Rs: Optional[Sequence[np.array]] = None, 120 | Ms: Optional[Sequence[np.array]] = None, 121 | objectives: Optional[Sequence[Objective]] = None, 122 | x_0: Optional[np.array] = None, 123 | x_T: Optional[np.array] = None, 124 | T_f: Optional[float] = None, 125 | P_f: Optional[Union[Sequence[np.array], np.array]] = None, 126 | show_legend: Optional[bool] = True, 127 | state_variables_names: Optional[Sequence[str]] = None, 128 | epsilon_x: Optional[float] = _epsilon_x_default, 129 | epsilon_P: Optional[float] = _epsilon_P_default, 130 | L: Optional[int] = _L_default, 131 | eta: Optional[int] = _eta_default, 132 | debug: Optional[bool] = False): 133 | 134 | self.__continuous = 'ContinuousPyDiffGame' in [c.__name__ for c in 135 | set(type(self).__bases__).union({self.__class__})] 136 | self._A = A 137 | self._B = B 138 | self._Bs = Bs 139 | self._n = self._A.shape[0] 140 | self._m = self._B.shape[1] if self._B is not None else self._Bs[0].shape[1] 141 | self.__objectives = objectives 142 | self._N = len(objectives) if objectives else len(Qs) 143 | self._P_size = self._n ** 2 144 | self._Qs = Qs if Qs else [o.Q for o in objectives] 145 | self._Rs = Rs if Rs else [o.R for o in objectives] 146 | self._x_0 = x_0 147 | self._x_T = x_T 148 | self.__infinite_horizon = T_f is None 149 | self._T_f = self.__T_f_default if T_f is None else T_f 150 | 151 | self._Ms = Ms 152 | 153 | if self._Ms is None and self.__objectives is not None: 154 | self._Ms = [o.M_i for o in self.__objectives if isinstance(o, GameObjective)] 155 | 156 | self._M = None 157 | 158 | if self._Bs is None: 159 | if self._Ms is not None and len(self._Ms): 160 | self._M = np.concatenate(self._Ms, axis=0) 161 | try: 162 | self._M_inv = np.linalg.inv(self._M) 163 | except np.linalg.LinAlgError: 164 | self._M_inv = np.linalg.pinv(self._M) 165 | l = 0 166 | self._Bs = [] 167 | 168 | for M_i in self._Ms: 169 | m_i = M_i.shape[0] 170 | M_i_tilde = self._M_inv[:, l:l + m_i] 171 | BM_i_tilde = self._B @ M_i_tilde 172 | B_i = BM_i_tilde.reshape((self._n, m_i)) 173 | self._Bs += [B_i] 174 | l += m_i 175 | else: 176 | self._Bs = [B] 177 | 178 | self._P_f = self.__get_are_P_f() if P_f is None else P_f 179 | # self._P_f = [np.eye(self._n)] * self._N if P_f is None else P_f 180 | self._L = L 181 | self._delta = self._T_f / self._L 182 | self.__show_legend = show_legend 183 | self.__state_variables_names = state_variables_names 184 | self._forward_time = np.linspace(start=0, 185 | stop=self._T_f, 186 | num=self._L) 187 | self._backward_time = self._forward_time[::-1] 188 | self._A_cl = np.empty_like(self._A) 189 | self._P = [] 190 | self._K = [] 191 | self.__epsilon_x = epsilon_x 192 | self.__epsilon_P = epsilon_P 193 | self._x = self._x_0 194 | self._x_non_linear = None 195 | self.__eta = eta 196 | self._converged = False 197 | self._debug = debug 198 | self._fig = None 199 | 200 | self._verify_input() 201 | 202 | def _verify_input(self): 203 | """ 204 | Input checking method 205 | 206 | Raises 207 | ------ 208 | 209 | ValueError 210 | If any of the class parameters don't meet the specified requirements 211 | """ 212 | 213 | if self._N == 0: 214 | raise ValueError('At least one objective must be specified') 215 | if self._N > 1 and self._Ms and not all([M_i.shape[0] == R_ii.shape[0] == R_ii.shape[1] 216 | if R_ii.ndim > 1 else M_i.shape[0] == R_ii.shape[0] 217 | for M_i, R_ii in zip(self._Ms, self._Rs)]): 218 | raise ValueError('R must be a Sequence of square numpy arrays with shape ' 219 | 'corresponding to the second dimensions of the arrays in M') 220 | 221 | if not 1 > self.__epsilon_x > 0: 222 | raise ValueError('The state convergence tolerance must be in the open interval (0,1)') 223 | if not 1 > self.__epsilon_P > 0: 224 | raise ValueError('The convergence tolerance of the matrix P must be in the open interval (0,1)') 225 | 226 | if self._A.shape != (self._n, self._n): 227 | raise ValueError(f'A must be a square numpy array with shape nxn = {self._n}x{self._n}') 228 | if self._B is not None: 229 | if self._B.shape[0] != self._n: 230 | raise ValueError(f'B must be a numpy array with n = {self._n} rows') 231 | if not self.is_fully_controllable(): 232 | warnings.warn("Warning: The given system is not fully controllable") 233 | else: 234 | if not all([B_i.shape[0] == self._n and B_i.shape[1] == R_ii.shape[0] == R_ii.shape[1] 235 | if R_ii.ndim > 1 else B_i.shape[1] == R_ii.shape[0] for B_i, R_ii in zip(self._Bs, self._Rs)]): 236 | raise ValueError('R must be a Sequence of square numpy arrays with shape ' 237 | 'corresponding to the second dimensions of the arrays in Bs') 238 | if not all([np.all(np.linalg.eigvals(R_ii) > 0) if R_ii.ndim > 1 else R_ii > 0 239 | for R_ii in self._Rs]): 240 | raise ValueError('R must be a Sequence of square positive definite numpy arrays') 241 | if not all([Q_i.shape == (self._n, self._n) for Q_i in self._Qs]): 242 | raise ValueError(f'Q must be a Sequence of square positive semi-definite numpy arrays with shape nxn ' 243 | f'= {self._n}x{self._n}') 244 | if not all([all([e_i >= 0 or PyDiffGame._eigvals_tol > abs(e_i) >= 0 for e_i in np.linalg.eigvals(Q_i)]) 245 | for Q_i in self._Qs]): 246 | raise ValueError('Q must contain positive semi-definite numpy arrays') 247 | 248 | if self._x_0 is not None and self._x_0.shape != (self._n,): 249 | raise ValueError(f'x_0 must be a numpy array with length n = {self._n}') 250 | if self._x_T is not None and self._x_T.shape != (self._n,): 251 | raise ValueError(f'x_T must be a numpy array with length n= {self._n}') 252 | if self._T_f is not None and self._T_f <= 0: 253 | raise ValueError('T_f must be a positive real number') 254 | if not all([P_f_i.shape[0] == P_f_i.shape[1] == self._n for P_f_i in self._P_f]): 255 | raise ValueError(f'P_f must be a Sequence of positive semi-definite numpy arrays with shape nxn = ' 256 | f'{self._n}x{self._n}') 257 | # if not all([eig >= 0 for eig_set in [np.linalg.eigvals(P_f_i) for P_f_i in self._P_f] for eig in eig_set]): 258 | # warnings.warn("Warning: there is a matrix in P_f that has negative eigenvalues. 259 | # Convergence may not occur") 260 | if self.__state_variables_names and len(self.__state_variables_names) != self._n: 261 | raise ValueError(f'The parameter state_variables_names must be of length n = {self._n}') 262 | 263 | if self._L <= 0: 264 | raise ValueError('The number of data points must be a positive integer') 265 | if self.__eta <= 0: 266 | raise ValueError('The number of last matrix norms to consider for convergence must be a positive integer') 267 | 268 | def is_fully_controllable(self) -> bool: 269 | """ 270 | Tests full controllability of an LTI system by the equivalent condition of full rank 271 | of the controllability Gramian 272 | 273 | Returns 274 | ------- 275 | 276 | fully_controllable : bool 277 | Whether the system is fully controllable 278 | """ 279 | 280 | controllability_matrix = np.concatenate([self._B] + 281 | [np.linalg.matrix_power(self._A, i) @ self._B 282 | for i in range(1, self._n)], 283 | axis=1) 284 | controllability_matrix_rank = np.linalg.matrix_rank(A=controllability_matrix, 285 | tol=10e-5) 286 | fully_controllable = controllability_matrix_rank == self._n 287 | 288 | return fully_controllable 289 | 290 | def is_LQR(self) -> bool: 291 | """ 292 | Indicates whether the game has only one objective and is just a regular LQR 293 | 294 | Returns 295 | ------- 296 | 297 | is_lqr : bool 298 | Whether the system is LQR 299 | """ 300 | 301 | return len(self) == 1 302 | 303 | def __print_eigenvalues(self): 304 | """ 305 | Prints the eigenvalues of the matrices Q_i and R_ii for all 1 <= i <= N 306 | with the greatest common divisor of their elements d as a parameter 307 | """ 308 | 309 | d = sympy.symbols('d') 310 | 311 | for matrix in self._Rs + self._Qs: 312 | curr_gcd = math.gcd(*matrix.ravel()) 313 | sympy_matrix = sympy.Matrix((matrix / curr_gcd).astype(int)) * d 314 | eigenvalues_multiset = sympy_matrix.eigenvals(multiple=True) 315 | print(f"{repr(sympy_matrix)}: {eigenvalues_multiset}\n") 316 | 317 | def __print_characteristic_polynomials(self): 318 | """ 319 | Prints the characteristic polynomials of the matrices Q_i and R_ii for all 1 <= i <= N 320 | as a function of L and with the greatest common divisor of all their elements d as a parameter 321 | """ 322 | 323 | d = sympy.symbols('d') 324 | L = sympy.symbols('L') 325 | 326 | for matrix in self._Rs + self._Qs: 327 | curr_gcd = math.gcd(*matrix.ravel()) 328 | sympy_matrix = sympy.Matrix((matrix / curr_gcd).astype(int)) * d 329 | characteristic_polynomial = sympy_matrix.charpoly(x=L).as_expr() 330 | factored_characteristic_polynomial_expression = sympy.factor(f=characteristic_polynomial) 331 | print(f"{repr(sympy_matrix)}: {factored_characteristic_polynomial_expression}\n") 332 | 333 | def _converge_DREs_to_AREs(self): 334 | """ 335 | Solves the game as backwards convergence of the differential 336 | finite-horizon game for repeated consecutive steps until the matrix norm converges 337 | """ 338 | 339 | x_converged = False 340 | P_converged = False 341 | last_eta_norms = [] 342 | curr_iteration_T_f = self._T_f 343 | x_T = self._x_T if self._x_T is not None else np.zeros_like(self._x_0) 344 | x_convergence_iterations = 0 345 | 346 | with tqdm(total=self.__x_max_convergence_iterations) as x_progress_bar: 347 | while (not x_converged) and x_convergence_iterations < self.__x_max_convergence_iterations: 348 | x_convergence_iterations += 1 349 | P_convergence_iterations = 0 350 | 351 | while (not P_converged) and P_convergence_iterations < self.__p_max_convergence_iterations: 352 | P_convergence_iterations += 1 353 | self._backward_time = np.linspace(start=self._T_f, 354 | stop=self._T_f - self._delta, 355 | num=self._L) 356 | self._update_Ps_from_last_state() 357 | self._P_f = self._P[-1] 358 | last_eta_norms += [np.linalg.norm(self._P_f)] 359 | 360 | if len(last_eta_norms) > self.__eta: 361 | last_eta_norms.pop(0) 362 | 363 | if len(last_eta_norms) == self.__eta: 364 | P_converged = all([abs(norm_i - norm_i1) < self.__epsilon_P for norm_i, norm_i1 365 | in zip(last_eta_norms, last_eta_norms[1:])]) 366 | self._T_f -= self._delta 367 | 368 | self._T_f = curr_iteration_T_f 369 | 370 | if self._x_0 is not None: 371 | curr_x_T = self._simulate_curr_x_T() 372 | x_T_difference = x_T - curr_x_T 373 | x_T_difference_norm = x_T_difference.T @ x_T_difference 374 | curr_x_T_norm = curr_x_T.T @ curr_x_T 375 | 376 | if curr_x_T_norm > 0: 377 | convergence_ratio = x_T_difference_norm / curr_x_T_norm 378 | x_converged = convergence_ratio < self.__epsilon_x 379 | 380 | curr_iteration_T_f += 1 381 | self._T_f = curr_iteration_T_f 382 | self._forward_time = np.linspace(start=0, 383 | stop=self._T_f, 384 | num=self._L) 385 | else: 386 | x_converged = True 387 | 388 | x_progress_bar.update(1) 389 | 390 | def _post_convergence(f: Callable) -> Callable: 391 | """ 392 | A decorator static-method to apply on methods that need only be called after convergence 393 | 394 | Parameters 395 | ---------- 396 | 397 | f: Callable 398 | The method requiring convergence 399 | 400 | Returns 401 | ---------- 402 | 403 | decorated_f: Callable 404 | A decorated version of the method requiring convergence 405 | """ 406 | 407 | def decorated_f(self: PyDiffGame, 408 | *args, 409 | **kwargs): 410 | if not self._converged: 411 | raise RuntimeError('Must first simulate the differential game') 412 | return f(self, 413 | *args, 414 | **kwargs) 415 | 416 | return decorated_f 417 | 418 | @_post_convergence 419 | def __plot_temporal_variables(self, 420 | t: np.array, 421 | temporal_variables: np.array, 422 | is_P: bool, 423 | title: Optional[str] = None, 424 | output_variables_names: Optional[Sequence[str]] = None, 425 | two_state_spaces: Optional[bool] = False): 426 | """ 427 | Displays plots for the state variables with respect to time and the convergence of the values of P 428 | 429 | Parameters 430 | ---------- 431 | 432 | t: np.array array of len(L) 433 | The time axis information to plot 434 | temporal_variables: np.arrays of shape(L, n) 435 | The y-axis information to plot 436 | is_P: bool 437 | Indicates whether to accommodate plotting for all the values in P_i or just for x 438 | title: str, optional 439 | The plot title to display 440 | two_state_spaces: bool, optional 441 | Indicates whether to plot two state spaces 442 | """ 443 | 444 | self._fig = plt.figure(dpi=150) 445 | self._fig.set_size_inches(8, 6) 446 | plt.plot(t[:temporal_variables.shape[0]], temporal_variables) 447 | plt.xlabel('Time $[s]$', fontsize=18) 448 | plt.xticks(fontsize=18) 449 | plt.yticks(fontsize=18) 450 | plt.subplots_adjust(wspace=0) 451 | 452 | if not is_P and max(np.max(temporal_variables, axis=0)) > 1e3: 453 | plt.ticklabel_format(style='sci', axis='y', scilimits=(0, 0)) 454 | plt.gca().yaxis.get_offset_text().set_size(18) 455 | 456 | 457 | if title: 458 | plt.title(title) 459 | 460 | if self.__show_legend: 461 | if is_P: 462 | labels = ['${P' + str(i) + '}_{' + str(j) + str(k) + '}$' for i in range(1, self._N + 1) 463 | for j in range(1, self._n + 1) for k in range(1, self._n + 1)] 464 | elif output_variables_names is not None: 465 | labels = output_variables_names 466 | elif self.__state_variables_names is not None: 467 | if two_state_spaces: 468 | labels = [f'${name}_{1}$' for name in self.__state_variables_names] + \ 469 | [f'${name}_{2}$' for name in self.__state_variables_names] 470 | else: 471 | labels = [f'${name}$' for name in self.__state_variables_names] 472 | else: 473 | labels = ["${\mathbf{x}}_{" + str(j) + "}$" for j in range(1, self._n + 1)] 474 | 475 | plt.legend(labels=labels, 476 | loc='upper left' if is_P else 'right', 477 | ncol=4 if self._n > 8 else 2, 478 | prop={'size': 26}, 479 | bbox_to_anchor=(1, 0.55), 480 | framealpha=0.3 481 | ) 482 | 483 | plt.grid() 484 | plt.show() 485 | 486 | def __get_temporal_state_variables_title(self, 487 | linear_system: Optional[bool] = True, 488 | two_state_spaces: Optional[bool] = False) -> str: 489 | """ 490 | Returns the title of a temporal figure to plot 491 | 492 | Parameters 493 | ---------- 494 | 495 | linear_system: bool, optional 496 | Whether the system is linear 497 | two_state_spaces: bool, optional 498 | Whether the plot is of two state spaces 499 | 500 | Returns 501 | ------- 502 | 503 | title : str 504 | The title of the figure 505 | """ 506 | 507 | if two_state_spaces: 508 | title = f"{self._N}-Player Game" 509 | else: 510 | title = f"{('' if linear_system else 'Nonlinear, ')}" \ 511 | f"{('Continuous' if self.__continuous else 'Discrete')}, " \ 512 | f"{('Infinite' if self.__infinite_horizon else 'Finite')} Horizon" \ 513 | f"{(f', {self._N}-Player Game' if self._N > 1 else ' LQR')}" \ 514 | f"{f', {self._L} Sampling Points'}" 515 | 516 | return title 517 | 518 | def plot_state_variables(self, 519 | state_variables: np.array, 520 | linear_system: Optional[bool] = True, 521 | output_variables_names: Optional[Sequence[str]] = None, ): 522 | """ 523 | Displays plots for the state variables with respect to time 524 | 525 | Parameters 526 | ---------- 527 | 528 | state_variables: np.array of shape(L, n) 529 | The temporal state variables y-axis information to plot 530 | linear_system: bool, optional 531 | Indicates whether the system is linear or not 532 | output_variables_names: optional 533 | Names of the output variables 534 | """ 535 | 536 | self.__plot_temporal_variables(t=self._forward_time, 537 | temporal_variables=state_variables, 538 | is_P=False, 539 | # title=self.__get_temporal_state_variables_title(linear_system=linear_system), 540 | output_variables_names=output_variables_names) 541 | 542 | def __plot_x(self): 543 | """ 544 | Plots the state vector variables wth respect to time 545 | """ 546 | 547 | self.plot_state_variables(state_variables=self._x) 548 | 549 | def plot_y(self, 550 | C: np.array, 551 | output_variables_names: Optional[Sequence[str]] = None, 552 | save_figure: Optional[bool] = False, 553 | figure_path: Optional[Union[str, Path]] = _default_figures_filename, 554 | figure_filename: Optional[str] = _default_figures_filename): 555 | """ 556 | Plots an output vector y = C x^T wth respect to time 557 | 558 | Parameters 559 | ---------- 560 | 561 | C: np.array array of shape(p, n) 562 | The output coefficients with respect to state 563 | output_variables_names: Sequence of len(p) 564 | Names of output variables 565 | save_figure: bool 566 | Whether to save the figure or not 567 | figure_path: str or Path, optional, default = figures sub-folder of the current directory 568 | The desired saved figure's file path 569 | figure_filename: str, optional 570 | The desired saved figure's filename 571 | """ 572 | 573 | y = C @ self._x.T 574 | self.plot_state_variables(state_variables=y.T, 575 | output_variables_names=output_variables_names) 576 | if save_figure: 577 | self.__save_figure(figure_path=figure_path, 578 | figure_filename=figure_filename) 579 | 580 | @_post_convergence 581 | def __save_figure(self, 582 | figure_path: Optional[Union[str, Path]] = _default_figures_filename, 583 | figure_filename: Optional[str] = _default_figures_filename): 584 | """ 585 | Saves the current figure 586 | 587 | Parameters 588 | ---------- 589 | 590 | figure_path: str or Path, optional, default = figures sub-folder of the current directory 591 | The desired saved figure's file path 592 | figure_filename: str, optional 593 | The desired saved figure's filename 594 | """ 595 | 596 | if self._x_0 is None: 597 | raise RuntimeError('No parameter x_0 was defined to illustrate a figure') 598 | 599 | if figure_filename == PyDiffGame._default_figures_filename: 600 | figure_filename += '0' 601 | 602 | if not figure_path.is_dir(): 603 | os.mkdir(figure_path) 604 | 605 | figure_full_filename = Path(fr'{figure_path}/{figure_filename}.png') 606 | 607 | while figure_full_filename.is_file(): 608 | curr_name = figure_full_filename.name 609 | curr_num = int(curr_name.split('.png')[0][-1]) 610 | next_num = curr_num + 1 611 | figure_full_filename = Path(fr'{figure_path}/image{next_num}.png') 612 | 613 | self._fig.savefig(figure_full_filename) 614 | 615 | def __augment_two_state_space_vectors(self, 616 | other: PyDiffGame, 617 | non_linear: bool = False) -> (np.array, np.array): 618 | """ 619 | Plots the state variables of two converged state spaces 620 | 621 | Parameters 622 | ---------- 623 | 624 | other: PyDiffGame 625 | Other converged differential game to augment the current one with 626 | non_linear: bool, optional 627 | Indicates whether to augment the non-linear states or not 628 | 629 | Returns 630 | ---------- 631 | 632 | longer_period: np.array of len(max(self.L, other.L)) 633 | The longer time period of the two games 634 | augmented_state_space: np.array of shape(max(self.L, other.L), self.n + other.n) 635 | Augmented state space with interpolated and extrapolated values for both games 636 | """ 637 | 638 | f_self = sp.interpolate.interp1d(x=self._forward_time, 639 | y=self._x if not non_linear else self._x_non_linear.T, 640 | axis=0, 641 | fill_value="extrapolate") 642 | 643 | f_other = sp.interpolate.interp1d(x=other.forward_time, 644 | y=other.x if not non_linear else other._x_non_linear.T, 645 | axis=0, 646 | fill_value="extrapolate") 647 | 648 | longer_period = self._forward_time if max(self._forward_time) > max(other.forward_time) \ 649 | else other.forward_time 650 | 651 | self_state_space = f_self(longer_period) 652 | other_state_space = f_other(longer_period) 653 | 654 | augmented_state_space = np.concatenate((self_state_space, other_state_space), axis=1) 655 | 656 | return longer_period, augmented_state_space 657 | 658 | def plot_two_state_spaces(self, 659 | other: PyDiffGame, 660 | non_linear: bool = False): 661 | """ 662 | Plots the state variables of two converged state spaces 663 | 664 | Parameters 665 | ---------- 666 | 667 | other: PyDiffGame 668 | Other converged differential game to plot along with the current one 669 | non_linear: bool, optional 670 | Indicates whether to plot the non-linear states or not 671 | """ 672 | 673 | longer_period, augmented_state_space = self.__augment_two_state_space_vectors(other=other, 674 | non_linear=non_linear) 675 | 676 | self_title = self.__get_temporal_state_variables_title(two_state_spaces=True) 677 | other_title = other.__get_temporal_state_variables_title(two_state_spaces=True) 678 | 679 | self.__plot_temporal_variables(t=longer_period, 680 | temporal_variables=augmented_state_space, 681 | is_P=False, 682 | title=f"{self_title} " + "$(T_{f_1} = " + 683 | f"{self._T_f})$ \nvs\n {other_title} " + "$(T_{f_2} = " + f"{other.T_f})$", 684 | two_state_spaces=True 685 | ) 686 | 687 | def __get_are_P_f(self) -> list[np.array]: 688 | """ 689 | Solves the uncoupled set of algebraic Riccati equations to use as initial guesses for fsolve and odeint 690 | 691 | Returns 692 | ---------- 693 | 694 | P_f: list of numpy arrays, of len(N), of shape(n, n), solution of scipy's solve_are 695 | Final condition for the Riccati equation matrix 696 | """ 697 | 698 | are_solver = sp.linalg.solve_continuous_are if self.__continuous else sp.linalg.solve_discrete_are 699 | P_f = [] 700 | 701 | for B_i, Q_i, R_ii in zip(self._Bs, self._Qs, self._Rs): 702 | try: 703 | P_f_i = are_solver(self._A, B_i, Q_i, R_ii) 704 | except (np.linalg.LinAlgError, ValueError): 705 | P_f_i = Q_i 706 | 707 | P_f += [P_f_i] 708 | 709 | return P_f 710 | 711 | @abstractmethod 712 | def _update_K_from_last_state(self, 713 | *args, 714 | **kwargs): 715 | """ 716 | Updates the controllers K after forward propagation of the state through time 717 | """ 718 | 719 | pass 720 | 721 | @abstractmethod 722 | def _get_K_i(self, 723 | *args, 724 | **kwargs) -> np.array: 725 | """ 726 | Returns the i'th element of the currently calculated controllers 727 | 728 | Returns 729 | ---------- 730 | 731 | K_i: numpy array of numpy arrays, of len(N), of shape(n, n), solution of scipy's solve_are 732 | Final condition for the Riccati equation matrix 733 | """ 734 | 735 | pass 736 | 737 | @abstractmethod 738 | def _get_P_f_i(self, 739 | i: int) -> np.array: 740 | """ 741 | Returns the i'th element of the final condition matrices P_f 742 | 743 | Parameters 744 | ---------- 745 | 746 | i: int 747 | The required index 748 | 749 | Returns 750 | ---------- 751 | 752 | P_f_i: numpy array of shape(n, n) 753 | i'th element of the final condition matrices P_f 754 | """ 755 | 756 | pass 757 | 758 | def _update_A_cl_from_last_state(self, 759 | k: Optional[int] = None): 760 | """ 761 | Updates the closed-loop dynamics with the updated controllers based on the relation: 762 | A_cl = A - sum_{i=1}^N B_i K_i 763 | 764 | Parameters 765 | ---------- 766 | 767 | k: int, optional 768 | The current k'th sample index, in case the controller is time-dependant. 769 | In this case the update rule is: 770 | A_cl[k] = A - sum_{i=1}^N B_i K_i[k] 771 | """ 772 | 773 | A_cl = self._A - sum([self._Bs[i] @ (self._get_K_i(i, k) if k is not None else self._get_K_i(i)) 774 | for i in range(self._N)]) 775 | if k is not None: 776 | self._A_cl[k] = A_cl 777 | else: 778 | self._A_cl = A_cl 779 | 780 | @abstractmethod 781 | def _update_Ps_from_last_state(self, 782 | *args, 783 | **kwargs): 784 | """ 785 | Updates the matrices {P_i}_{i=1}^N after forward propagation of the state through time 786 | """ 787 | 788 | pass 789 | 790 | @abstractmethod 791 | def is_A_cl_stable(self, 792 | *args, 793 | **kwargs) -> bool: 794 | """ 795 | Tests Lyapunov stability of the closed loop 796 | 797 | Returns 798 | ---------- 799 | 800 | is_stable: bool 801 | Indicates whether the system has Lyapunov stability or not 802 | """ 803 | 804 | pass 805 | 806 | @abstractmethod 807 | def _solve_finite_horizon(self): 808 | """ 809 | Solves for the finite horizon case 810 | """ 811 | 812 | pass 813 | 814 | @_post_convergence 815 | def __plot_finite_horizon_convergence(self): 816 | """ 817 | Plots the convergence of the values for the matrices P_i 818 | """ 819 | 820 | self.__plot_temporal_variables(t=self._backward_time, 821 | temporal_variables=self._P, 822 | is_P=True) 823 | 824 | def __solve_and_plot_finite_horizon(self): 825 | """ 826 | Solves for the finite horizon case and plots the convergence of the values for the matrices P_i 827 | """ 828 | 829 | self._solve_finite_horizon() 830 | self.__plot_finite_horizon_convergence() 831 | 832 | @abstractmethod 833 | def _solve_infinite_horizon(self): 834 | """ 835 | Solves for the infinite horizon case using the finite horizon case 836 | """ 837 | 838 | pass 839 | 840 | @abstractmethod 841 | @_post_convergence 842 | def _solve_state_space(self): 843 | """ 844 | Propagates the game through time and solves for it 845 | """ 846 | 847 | pass 848 | 849 | def __solve_game(self): 850 | if self.__infinite_horizon: 851 | self._solve_infinite_horizon() 852 | else: 853 | self.__solve_and_plot_finite_horizon() 854 | 855 | self._converged = True 856 | 857 | def __solve_game_and_simulate_state_space(self): 858 | """ 859 | Propagates the game through time, solves for it and plots the state with respect to time 860 | """ 861 | 862 | self.__solve_game() 863 | 864 | if self._x_0 is not None: 865 | self._solve_state_space() 866 | 867 | def __solve_game_simulate_state_space_and_plot(self, 868 | plot_Mx: Optional[bool] = False, 869 | M: Optional[np.array] = None, 870 | output_variables_names: Optional[Sequence[str]] = None): 871 | """ 872 | Propagates the game through time, solves for it and plots the state with respect to time 873 | """ 874 | 875 | self.__solve_game_and_simulate_state_space() 876 | 877 | if self._x_0 is not None: 878 | self.__plot_x() 879 | 880 | if plot_Mx and self._n % self._m == 0: 881 | C = np.kron(a=np.eye(int(self._n / self._m)), 882 | b=M) 883 | self.plot_y(C=C, 884 | output_variables_names=output_variables_names) 885 | 886 | def _simulate_curr_x_T(self) -> np.array: 887 | """ 888 | Evaluates the current value of the state space vector at the terminal time T_f 889 | 890 | Returns 891 | ---------- 892 | 893 | x_T_f: numpy array of shape(n) 894 | Evaluated terminal state vector x(T_f) 895 | """ 896 | 897 | pass 898 | 899 | @_post_convergence 900 | def __get_x_l_tilde(self, 901 | x: np.array, 902 | l: int) -> np.array: 903 | """ 904 | Returns the state difference at the lth segment 905 | 906 | Parameters 907 | ---------- 908 | 909 | x: numpy array of shape(n) 910 | The solved state vector to consider 911 | l: int 912 | The desired segment 913 | 914 | Returns 915 | ---------- 916 | 917 | x_l_tilde: numpy array of shape(n) 918 | State difference at the lth segment 919 | """ 920 | 921 | x_l = x[l] 922 | x_l_tilde = self.x_T - x_l 923 | 924 | return x_l_tilde 925 | 926 | @_post_convergence 927 | def __get_v_i_l_tilde(self, 928 | x: np.array, 929 | i: int, 930 | l: int, 931 | v_i_T: np.array) -> np.array: 932 | """ 933 | Returns the ith virtual input difference at the lth segment 934 | 935 | Parameters 936 | ---------- 937 | 938 | x: numpy array of shape(n) 939 | The solved state vector to consider 940 | i: int 941 | The desired input index 942 | l: int 943 | The desired segment 944 | v_i_T: numpy array of shape(m_i) 945 | The terminal value of the ith input 946 | 947 | Returns 948 | ---------- 949 | 950 | v_i_l_tilde: numpy array of shape(m_i) 951 | ith virtual input difference at the lth segment 952 | """ 953 | 954 | x_l = x[l] 955 | 956 | K_i_l = self._get_K_i(i) if self.__continuous else self._get_K_i(i, l) 957 | v_i_l = - K_i_l @ x_l 958 | v_i_l_tilde = v_i_T - v_i_l 959 | 960 | return v_i_l_tilde 961 | 962 | @_post_convergence 963 | def __get_u_l_tilde(self, 964 | x: np.array, 965 | l: int, 966 | x_l_tilde: np.array) -> np.array: 967 | """ 968 | Returns the actual input difference at the lth segment 969 | 970 | Parameters 971 | ---------- 972 | 973 | x: numpy array of shape(n) 974 | The solved state vector to consider 975 | l: int 976 | The desired segment 977 | x_l_tilde: numpy array of shape(n) 978 | The state difference at the lth segment 979 | 980 | Returns 981 | ---------- 982 | 983 | u_l_tilde: numpy array of shape(m) 984 | Actual input difference at the lth segment 985 | """ 986 | 987 | if self.is_LQR(): 988 | k_T = self._get_K_i(0) if self.__continuous else self._get_K_i(0, self._L - 1) 989 | u_l_tilde = - k_T @ x_l_tilde 990 | else: 991 | v_l_tilde = [] 992 | 993 | for i in range(self._N): 994 | k_i_T = self._get_K_i(i) if self.__continuous else self._get_K_i(i, self._L - 1) 995 | v_i_T = - k_i_T @ self.x_T 996 | 997 | v_i_l_tilde = self.__get_v_i_l_tilde(x=x, 998 | i=i, 999 | l=l, 1000 | v_i_T=v_i_T) 1001 | 1002 | v_l_tilde += [v_i_l_tilde] 1003 | 1004 | v_l_tilde = np.array(v_l_tilde) 1005 | u_l_tilde = self._M_inv @ v_l_tilde 1006 | 1007 | return u_l_tilde 1008 | 1009 | @_post_convergence 1010 | def __get_x_and_u_l_tilde(self, 1011 | x: np.array, 1012 | l: int) -> (np.array, np.array): 1013 | """ 1014 | Returns the state and actual input difference at the lth segment 1015 | 1016 | Parameters 1017 | ---------- 1018 | 1019 | x: numpy array of shape(n) 1020 | The solved state vector to consider 1021 | l: int 1022 | The desired segment 1023 | 1024 | Returns 1025 | ---------- 1026 | 1027 | x_l_tilde: numpy array of shape(n) 1028 | State difference at the lth segment 1029 | u_l_tilde: numpy array of shape(m) 1030 | Actual input difference at the lth segment 1031 | """ 1032 | 1033 | x_l_tilde = self.__get_x_l_tilde(x=x, 1034 | l=l) 1035 | u_l_tilde = self.__get_u_l_tilde(x=x, 1036 | l=l, 1037 | x_l_tilde=x_l_tilde) 1038 | 1039 | return x_l_tilde, u_l_tilde 1040 | 1041 | def __get_x(self, 1042 | non_linear: bool) -> np.array: 1043 | """ 1044 | Returns the desired state vector to consider 1045 | 1046 | Parameters 1047 | ---------- 1048 | 1049 | non_linear: bool 1050 | Whether to return the non-linear state 1051 | 1052 | Returns 1053 | ---------- 1054 | 1055 | x: numpy array of shape(n) 1056 | Desired state vector 1057 | """ 1058 | 1059 | x = self._x if not non_linear else self._x_non_linear.T 1060 | assert len(x) == self._L 1061 | 1062 | return x 1063 | 1064 | @_post_convergence 1065 | def get_cost(self, 1066 | lqr_objective: Objective, 1067 | non_linear: Optional[bool] = False, 1068 | x_only: Optional[bool] = False) -> float: 1069 | """ 1070 | Calculates the cost functionals values using the formula: 1071 | J = int_{t=0}^T_f J(t) dt = 1072 | int_{t=0}^T_f [ x_tilde(t)^T Q x_tilde(t) + u_tilde^T(t) R u_tilde(t) ) ] dt 1073 | 1074 | and the corresponding Trapezoidal rule approximation: 1075 | J ~ delta / 2 sum_{l=1}^{L} [ J(l) + J(l - 1) ] 1076 | = delta * [ sum_{l=1}^{L-1} J(l) + 1/2 ( J(0) + J(L) ) ] 1077 | 1078 | Parameters 1079 | ---------- 1080 | 1081 | lqr_objective: LQRObjective 1082 | LQR objective to get Q and R from 1083 | non_linear: bool, optional 1084 | Indicates whether to calculate the cost with respect to the nonlinear state 1085 | x_only: bool, optional 1086 | Indicates whether to calculate the cost only with respect to x(t) 1087 | 1088 | Returns 1089 | ---------- 1090 | 1091 | log_cost: float 1092 | game cost 1093 | """ 1094 | 1095 | x = self.__get_x(non_linear=non_linear) 1096 | 1097 | cost = 0 1098 | Q = lqr_objective.Q 1099 | R = lqr_objective.R 1100 | 1101 | for l in range(0, self._L): 1102 | x_l_tilde = self.__get_x_l_tilde(x=x, 1103 | l=l) 1104 | cost_l = float(x_l_tilde.T @ Q @ x_l_tilde) 1105 | 1106 | if not x_only: 1107 | u_l_tilde = self.__get_u_l_tilde(x=x, 1108 | l=l, 1109 | x_l_tilde=x_l_tilde) 1110 | cost_l += float(u_l_tilde.T @ R @ u_l_tilde) 1111 | 1112 | if l in [0, self._L - 1]: 1113 | cost_l *= 0.5 1114 | 1115 | cost += cost_l 1116 | 1117 | return cost 1118 | 1119 | @_post_convergence 1120 | def get_agnostic_costs(self, 1121 | non_linear: Optional[bool] = False) -> float: 1122 | """ 1123 | Calculates the agnostic functional value using the formula: 1124 | J_agnostic = int_{t=0}^T_f J_agnostic(t) dt = 1125 | int_{t=0}^T_f [ ||x_tilde(t)||^2 + ||u_tilde(t)||^2 ] dt = 1126 | int_{t=0}^T_f [ x_tilde(t)^T x_tilde(t) + u_tilde(t)^T u_tilde(t) ] dt 1127 | 1128 | and the corresponding Trapezoidal rule approximation: 1129 | J ~ delta / 2 sum_{l=1}^L [ J(l) + J(l - 1) ] = 1130 | delta * [ sum_{l=1}^{L-1} J(l) + 1/2 ( J(0) + J(L) ) ] 1131 | 1132 | Parameters 1133 | ---------- 1134 | 1135 | non_linear: bool, optional 1136 | Indicates whether to calculate the cost with respect to the nonlinear state 1137 | 1138 | Returns 1139 | ---------- 1140 | 1141 | log_cost: float 1142 | agnostic game cost 1143 | """ 1144 | 1145 | x = self.__get_x(non_linear=non_linear) 1146 | 1147 | cost = sum((0.5 if l in [0, self._L - 1] else 1) * sum(y_l.T @ y_l 1148 | for y_l in self.__get_x_and_u_l_tilde(x=x, 1149 | l=l)) 1150 | for l in range(0, self._L)) * self._delta 1151 | 1152 | return cost 1153 | 1154 | def __call__(self, 1155 | plot_state_space: Optional[bool] = True, 1156 | plot_Mx: Optional[bool] = False, 1157 | M: Optional[np.array] = None, 1158 | output_variables_names: Optional[Sequence[str]] = None, 1159 | save_figure: Optional[bool] = False, 1160 | figure_path: Optional[Union[str, Path]] = _default_figures_path, 1161 | figure_filename: Optional[str] = _default_figures_filename, 1162 | print_characteristic_polynomials: Optional[bool] = False, 1163 | print_eigenvalues: Optional[bool] = False): 1164 | """ 1165 | Runs the game simulation 1166 | 1167 | Parameters 1168 | ---------- 1169 | 1170 | plot_state_space: bool, optional 1171 | Whether to plot the solved game state space 1172 | save_figure: bool, optional 1173 | Whether to save the state space plot figure 1174 | print_characteristic_polynomials: bool, optional 1175 | Whether to print the characteristic polynomials of the matrices Q_i and R_ii for all 1 <= i <= N 1176 | print_eigenvalues: bool, optional 1177 | Whether to print the eigenvalues of the matrices Q_i and R_ii for all 1 <= i <= N 1178 | 1179 | """ 1180 | 1181 | if print_characteristic_polynomials: 1182 | self.__print_characteristic_polynomials() 1183 | if print_eigenvalues: 1184 | self.__print_eigenvalues() 1185 | 1186 | if plot_state_space: 1187 | self.__solve_game_simulate_state_space_and_plot(plot_Mx=plot_Mx, 1188 | M=M, 1189 | output_variables_names=output_variables_names) 1190 | if save_figure: 1191 | self.__save_figure(figure_path=figure_path, 1192 | figure_filename=figure_filename) 1193 | else: 1194 | self.__solve_game_and_simulate_state_space() 1195 | 1196 | def __getitem__(self, 1197 | i: int) -> Objective: 1198 | """ 1199 | Returns the i'th objective of the game 1200 | 1201 | Parameters 1202 | ---------- 1203 | 1204 | i: int 1205 | The objective index to consider 1206 | 1207 | Returns 1208 | ---------- 1209 | 1210 | o_i: Objective 1211 | i'th objective of the game 1212 | """ 1213 | 1214 | return self.__objectives[i] 1215 | 1216 | def __len__(self) -> int: 1217 | """ 1218 | We define the length of a differential game to be its number of objectives (N) 1219 | 1220 | Returns 1221 | ---------- 1222 | 1223 | len: int 1224 | The length of the game 1225 | """ 1226 | 1227 | return self._N 1228 | 1229 | @property 1230 | def P(self) -> list[np.array]: 1231 | return self._P 1232 | 1233 | @property 1234 | def K(self) -> list[np.array]: 1235 | return self._K 1236 | 1237 | @property 1238 | def x_0(self) -> np.array: 1239 | return self._x_0 1240 | 1241 | @property 1242 | def x_T(self) -> np.array: 1243 | return self._x_T 1244 | 1245 | @property 1246 | def x(self) -> np.array: 1247 | return self._x 1248 | 1249 | @property 1250 | def forward_time(self) -> np.array: 1251 | return self._forward_time 1252 | 1253 | @property 1254 | def T_f(self) -> float: 1255 | return self._T_f 1256 | 1257 | @property 1258 | def L(self) -> int: 1259 | return self._L 1260 | 1261 | @property 1262 | def Bs(self) -> Sequence: 1263 | return self._Bs 1264 | 1265 | @property 1266 | def M_inv(self) -> np.array: 1267 | return self._M_inv 1268 | 1269 | @property 1270 | def epsilon(self) -> float: 1271 | return self.__epsilon_x 1272 | 1273 | _post_convergence = staticmethod(_post_convergence) 1274 | -------------------------------------------------------------------------------- /src/PyDiffGame/PyDiffGameLQRComparison.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from tqdm import tqdm 3 | from time import time 4 | from pathlib import Path 5 | import itertools 6 | from concurrent.futures import ProcessPoolExecutor 7 | 8 | import inspect 9 | from typing import Sequence, Any, Optional, Callable, Union 10 | from abc import ABC 11 | 12 | from PyDiffGame.PyDiffGame import PyDiffGame 13 | from PyDiffGame.ContinuousPyDiffGame import ContinuousPyDiffGame 14 | from PyDiffGame.DiscretePyDiffGame import DiscretePyDiffGame 15 | from PyDiffGame.Objective import GameObjective 16 | 17 | 18 | class PyDiffGameLQRComparison(ABC, Callable, Sequence): 19 | """ 20 | Differential games comparison abstract base class 21 | 22 | 23 | Parameters 24 | ---------- 25 | 26 | args: sequence of 2-d np.arrays of len(N), each array B_i of shape(n, m_i) 27 | System input matrices for each control objective 28 | games_objectives: Sequence of Sequences of GameObjective objects 29 | Each game objectives for the comparison 30 | continuous: boolean, optional 31 | Indicates whether to consider a continuous or discrete control design 32 | """ 33 | 34 | def __init__(self, 35 | args: dict[str, Any], 36 | games_objectives: Sequence[Sequence[GameObjective]], 37 | M: np.array = None, 38 | continuous: bool = True): 39 | game_class = ContinuousPyDiffGame if continuous else DiscretePyDiffGame 40 | 41 | self.__args = args 42 | self.__verify_input() 43 | 44 | self._games = {} 45 | self._lqr_objective = None 46 | self.__M = M 47 | 48 | for i, game_i_objectives in enumerate(games_objectives): 49 | game_i = game_class(**self.__args | {'objectives': [o for o in game_i_objectives]}) 50 | self._games[i] = game_i 51 | 52 | if game_i.is_LQR(): 53 | self._lqr_objective = game_i_objectives[0] 54 | 55 | def __verify_input(self): 56 | """ 57 | Input checking method 58 | 59 | Raises 60 | ------ 61 | 62 | Case-specific errors 63 | """ 64 | 65 | for mat in ['A', 'B']: 66 | mat_name = 'dynamics' if mat == 'A' else 'input' 67 | if mat not in self.__args.keys(): 68 | raise ValueError(f"Passed arguments must contain the {mat_name} matrix {mat}") 69 | if isinstance(self.__args[mat], type(np.array)): 70 | raise ValueError(f'The {mat_name} matrix {mat} must be a numpy array') 71 | 72 | def are_fully_controllable(self): 73 | """ 74 | Tests each game's full controllability as an LTI system 75 | """ 76 | 77 | return all(game.is_fully_controllable() for game in self._games.values()) 78 | 79 | def plot_two_state_spaces(self, 80 | i: Optional[int] = 0, 81 | j: Optional[int] = 1, 82 | non_linear: bool = False): 83 | """ 84 | Plot the state spaces of two games 85 | """ 86 | 87 | self._games[i].plot_two_state_spaces(other=self._games[j], 88 | non_linear=non_linear) 89 | 90 | def __simulate_non_linear_system(self, 91 | i: int, 92 | plot: bool = False) -> np.array: 93 | """ 94 | Simulates the corresponding non-linear system's progression through time 95 | """ 96 | 97 | pass 98 | 99 | def __run_animation(self, 100 | i: int): 101 | """ 102 | Animates the state progression of the system 103 | """ 104 | 105 | pass 106 | 107 | def __call__(self, 108 | plot_state_spaces: Optional[bool] = False, 109 | plot_Mx: Optional[bool] = False, 110 | output_variables_names: Optional[Sequence[str]] = None, 111 | save_figure: Optional[bool] = False, 112 | figure_path: Optional[Union[str, Path]] = PyDiffGame.default_figures_path, 113 | figure_filename: Optional[Union[str, Callable[[PyDiffGame], str]]] = 114 | PyDiffGame.default_figures_filename, 115 | run_animations: Optional[bool] = True, 116 | print_characteristic_polynomials: Optional[bool] = False, 117 | print_eigenvalues: Optional[bool] = False): 118 | """ 119 | Runs the comparison 120 | """ 121 | 122 | for i, game_i in self._games.items(): 123 | game_i(plot_state_space=plot_state_spaces, 124 | plot_Mx=plot_Mx, 125 | M=self.__M, 126 | output_variables_names=output_variables_names, 127 | save_figure=save_figure, 128 | figure_path=figure_path, 129 | figure_filename=figure_filename if isinstance(figure_filename, str) else figure_filename(game_i), 130 | print_characteristic_polynomials=print_characteristic_polynomials, 131 | print_eigenvalues=print_eigenvalues) 132 | 133 | if run_animations: 134 | self.__run_animation(i=i) 135 | 136 | @staticmethod 137 | def run_multiprocess(multiprocess_worker_function: Callable, 138 | values: Sequence[Sequence]): 139 | t_start = time() 140 | combos = list(itertools.product(*values)) 141 | 142 | with ProcessPoolExecutor() as executor: 143 | submittals = {str(combo): executor.submit(multiprocess_worker_function, *combo) for combo in combos} 144 | 145 | delimiter = ', ' 146 | names = inspect.signature(multiprocess_worker_function).parameters.keys() 147 | 148 | for combo, submittal in tqdm(iterable=submittals.items(), total=len(submittals)): 149 | values = combo[1:-1].split(delimiter) 150 | print_str = f"{delimiter.join([f'{n}={v}' for n, v in zip(names, values)])}" 151 | print(print_str) 152 | submittal.result() 153 | 154 | print(f'Total time: {time() - t_start}') 155 | 156 | def __getitem__(self, 157 | i: int) -> PyDiffGame: 158 | """ 159 | Returns the i'th game 160 | """ 161 | 162 | return self._games[i] 163 | 164 | def __len__(self) -> int: 165 | """ 166 | We define the length of a comparison to be its number of games 167 | """ 168 | 169 | return len(self._games) 170 | -------------------------------------------------------------------------------- /src/PyDiffGame/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/__init__.py -------------------------------------------------------------------------------- /src/PyDiffGame/examples/InvertedPendulumComparison.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import numpy as np 4 | import scipy as sp 5 | from time import time 6 | import matplotlib 7 | import matplotlib.pyplot as plt 8 | 9 | from typing import Optional 10 | 11 | from PyDiffGame.PyDiffGame import PyDiffGame 12 | from PyDiffGame.PyDiffGameLQRComparison import PyDiffGameLQRComparison 13 | from PyDiffGame.Objective import GameObjective, LQRObjective 14 | 15 | 16 | class InvertedPendulumComparison(PyDiffGameLQRComparison): 17 | def __init__(self, 18 | m_c: float, 19 | m_p: float, 20 | p_L: float, 21 | q: float, 22 | r: Optional[float] = 1, 23 | x_0: Optional[np.array] = None, 24 | x_T: Optional[np.array] = None, 25 | T_f: Optional[float] = None, 26 | epsilon_x: Optional[float] = PyDiffGame.epsilon_x_default, 27 | epsilon_P: Optional[float] = PyDiffGame.epsilon_P_default, 28 | L: Optional[int] = PyDiffGame.L_default, 29 | eta: Optional[int] = PyDiffGame.eta_default): 30 | self.__m_c = m_c 31 | self.__m_p = m_p 32 | self.__p_L = p_L 33 | self.__l = self.__p_L / 2 # CoM of uniform rod 34 | self.__I = 1 / 12 * self.__m_p * self.__p_L ** 2 # center mass moment of inertia of uniform rod 35 | 36 | # # original linear system 37 | linearized_D = self.__m_c * self.__m_p * self.__l ** 2 + self.__I * (self.__m_c + self.__m_p) 38 | a32 = self.__m_p * PyDiffGame.g * self.__l ** 2 / linearized_D 39 | a42 = self.__m_p * PyDiffGame.g * self.__l * (self.__m_c + self.__m_p) / linearized_D 40 | A = np.array([[0, 0, 1, 0], 41 | [0, 0, 0, 1], 42 | [0, a32, 0, 0], 43 | [0, a42, 0, 0]]) 44 | 45 | b21 = (m_p * self.__l ** 2 + self.__I) / linearized_D 46 | b31 = m_p * self.__l / linearized_D 47 | b22 = b31 48 | b32 = (m_c + m_p) / linearized_D 49 | B = np.array([[0, 0], 50 | [0, 0], 51 | [b21, b22], 52 | [b31, b32]]) 53 | 54 | M1 = B[2, :].reshape(1, 2) 55 | M2 = B[3, :].reshape(1, 2) 56 | Ms = [M1, M2] 57 | 58 | Q_x = q * np.array([[1, 0, 2, 0], 59 | [0, 0, 0, 0], 60 | [2, 0, 4, 0], 61 | [0, 0, 0, 0]]) 62 | Q_theta = q * np.array([[0, 0, 0, 0], 63 | [0, 1, 0, 2], 64 | [0, 0, 0, 0], 65 | [0, 2, 0, 4]]) 66 | Q_lqr = Q_theta + Q_x 67 | Qs = [Q_x, Q_theta] 68 | 69 | R_lqr = np.diag([r] * 2) 70 | Rs = [np.array([r])] * 2 71 | 72 | self.__origin = (0.0, 0.0) 73 | 74 | state_variables_names = ['x', 75 | '\\theta', 76 | '\\dot{x}', 77 | '\\dot{\\theta}'] 78 | 79 | args = {'A': A, 80 | 'B': B, 81 | 'x_0': x_0, 82 | 'x_T': x_T, 83 | 'T_f': T_f, 84 | 'state_variables_names': state_variables_names, 85 | 'epsilon_x': epsilon_x, 86 | 'epsilon_P': epsilon_P, 87 | 'L': L, 88 | 'eta': eta, 89 | 'force_finite_horizon': T_f is not None} 90 | 91 | lqr_objective = [LQRObjective(Q=Q_lqr, R_ii=R_lqr)] 92 | game_objectives = [GameObjective(Q=Q, R_ii=R, M_i=M_i) for Q, R, M_i in zip(Qs, Rs, Ms)] 93 | games_objectives = [lqr_objective, game_objectives] 94 | 95 | super().__init__(args=args, 96 | games_objectives=games_objectives, 97 | continuous=True) 98 | 99 | def __simulate_non_linear_system(self, 100 | i: int, 101 | plot: bool = False) -> np.array: 102 | game = self._games[i] 103 | K = game.K 104 | x_T = game.x_T 105 | 106 | def nonlinear_state_space(_, x_t: np.array) -> np.array: 107 | x_t = x_t - x_T 108 | 109 | if game.is_LQR(): 110 | u_t = - K[0] @ x_t 111 | F_t, M_t = u_t.T 112 | else: 113 | K_x, K_theta = K 114 | v_x = - K_x @ x_t 115 | v_theta = - K_theta @ x_t 116 | v = np.array([v_x, v_theta]) 117 | F_t, M_t = game.M_inv @ v 118 | 119 | x, theta, x_dot, theta_dot = x_t 120 | 121 | theta_ddot = 1 / ( 122 | self.__m_p * self.__l ** 2 + self.__I - (self.__m_p * self.__l) ** 2 * np.cos(theta) ** 2 / 123 | (self.__m_p + self.__m_c)) * (M_t - self.__m_p * self.__l * 124 | (np.cos(theta) / (self.__m_p + self.__m_c) * 125 | (F_t + self.__m_p * self.__l * np.sin(theta) 126 | * theta_dot ** 2) + PyDiffGame.g * np.sin(theta))) 127 | x_ddot = 1 / (self.__m_p + self.__m_c) * (F_t + self.__m_p * self.__l * (np.sin(theta) * theta_dot ** 2 - 128 | np.cos(theta) * theta_ddot)) 129 | if isinstance(theta_ddot, np.ndarray): 130 | theta_ddot = theta_ddot[0] 131 | x_ddot = x_ddot[0] 132 | 133 | non_linear_x = np.array([x_dot, theta_dot, x_ddot, theta_ddot], 134 | dtype=float) 135 | 136 | return non_linear_x 137 | 138 | pendulum_state = sp.integrate.solve_ivp(fun=nonlinear_state_space, 139 | t_span=[0.0, game.T_f], 140 | y0=game.x_0, 141 | t_eval=game.forward_time, 142 | rtol=game.epsilon) 143 | 144 | Y = pendulum_state.y 145 | 146 | if plot: 147 | game.plot_state_variables(state_variables=Y.T, 148 | linear_system=False) 149 | 150 | return Y 151 | 152 | def __run_animation(self, 153 | i: int) -> (matplotlib.lines.Line2D, matplotlib.patches.Rectangle): 154 | game = self._games[i] 155 | game._x_non_linear = self.__simulate_non_linear_system(i=i, 156 | plot=True) 157 | x_t, theta_t, x_dot_t, theta_dot_t = game._x_non_linear 158 | 159 | pendulumArm = matplotlib.lines.Line2D(xdata=self.__origin, 160 | ydata=self.__origin, 161 | color='r') 162 | cart = matplotlib.patches.Rectangle(xy=self.__origin, 163 | width=0.5, 164 | height=0.15, 165 | color='b') 166 | 167 | fig = plt.figure() 168 | x_max = max(abs(max(x_t)), abs(min(x_t))) 169 | square_side = 1.1 * min(max(self.__p_L, x_max), 3 * self.__p_L) 170 | 171 | ax = fig.add_subplot(111, 172 | aspect='equal', 173 | xlim=(-square_side, square_side), 174 | ylim=(-square_side, square_side), 175 | title=f"Inverted Pendulum {'LQR' if game.is_LQR() else 'Game'} Simulation") 176 | 177 | def init() -> (matplotlib.lines.Line2D, matplotlib.patches.Rectangle): 178 | ax.add_patch(cart) 179 | ax.add_line(pendulumArm) 180 | 181 | return pendulumArm, cart 182 | 183 | def animate(i: int) -> (matplotlib.lines.Line2D, matplotlib.patches.Rectangle): 184 | x_i, theta_i = x_t[i], theta_t[i] 185 | pendulum_x_coordinates = [x_i, x_i + self.__p_L * np.sin(theta_i)] 186 | pendulum_y_coordinates = [0, - self.__p_L * np.cos(theta_i)] 187 | pendulumArm.set_xdata(x=pendulum_x_coordinates) 188 | pendulumArm.set_ydata(y=pendulum_y_coordinates) 189 | 190 | cart_x_y = [x_i - cart.get_width() / 2, - cart.get_height()] 191 | cart.set_xy(xy=cart_x_y) 192 | 193 | return pendulumArm, cart 194 | 195 | ax.grid() 196 | t0 = time() 197 | animate(0) 198 | t1 = time() 199 | 200 | frames = game.L 201 | interval = game.T_f - (t1 - t0) 202 | 203 | anim = matplotlib.animationFuncAnimation(fig=fig, 204 | func=animate, 205 | init_func=init, 206 | frames=frames, 207 | interval=interval, 208 | blit=True) 209 | plt.show() 210 | 211 | 212 | def multiprocess_worker_function(x_T: float, 213 | theta_0: float, 214 | m_c: float, 215 | m_p: float, 216 | p_L: float, 217 | q: float, 218 | epsilon_x: float, 219 | epsilon_P: float) -> int: 220 | x_T = np.array([x_T, # x 221 | theta_0, # theta 222 | 0, # x_dot 223 | 0] # theta_dot 224 | ) 225 | x_0 = np.zeros_like(x_T) 226 | 227 | inverted_pendulum_comparison = \ 228 | InvertedPendulumComparison(m_c=m_c, 229 | m_p=m_p, 230 | p_L=p_L, 231 | q=q, 232 | x_0=x_0, 233 | x_T=x_T, 234 | epsilon_x=epsilon_x, 235 | epsilon_P=epsilon_P) # game class 236 | is_max_lqr = \ 237 | inverted_pendulum_comparison(plot_state_spaces=False, 238 | run_animations=False 239 | ) 240 | 241 | # inverted_pendulum_comparison.plot_two_state_spaces(non_linear=True) 242 | return int(is_max_lqr) 243 | 244 | 245 | if __name__ == '__main__': 246 | x_Ts = [10 ** p for p in [2]] 247 | theta_Ts = [np.pi / 2 + np.pi / n for n in [10]] 248 | m_cs = [10 ** p for p in [1, 2]] 249 | m_ps = [10 ** p for p in [0, 1, 2]] 250 | p_Ls = [10 ** p for p in [0, 1]] 251 | qs = [10 ** p for p in [-2, -1, 0, 1]] 252 | epsilon_xs = [10 ** (-7)] 253 | epsilon_Ps = [10 ** (-3)] 254 | params = [x_Ts, theta_Ts, m_cs, m_ps, p_Ls, qs, epsilon_xs, epsilon_Ps] 255 | 256 | PyDiffGameLQRComparison.run_multiprocess(multiprocess_worker_function=multiprocess_worker_function, 257 | values=params) 258 | -------------------------------------------------------------------------------- /src/PyDiffGame/examples/MassesWithSpringsComparison.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from typing import Sequence, Optional, Union 3 | 4 | from PyDiffGame.PyDiffGame import PyDiffGame 5 | from PyDiffGame.PyDiffGameLQRComparison import PyDiffGameLQRComparison 6 | from PyDiffGame.Objective import GameObjective, LQRObjective 7 | 8 | 9 | class MassesWithSpringsComparison(PyDiffGameLQRComparison): 10 | def __init__(self, 11 | N: int, 12 | m: float, 13 | k: float, 14 | q: Union[float, Sequence[float], Sequence[Sequence[float]]], 15 | r: float, 16 | Ms: Optional[Sequence[np.array]] = None, 17 | x_0: Optional[np.array] = None, 18 | x_T: Optional[np.array] = None, 19 | T_f: Optional[float] = None, 20 | epsilon_x: Optional[float] = PyDiffGame.epsilon_x_default, 21 | epsilon_P: Optional[float] = PyDiffGame.epsilon_P_default, 22 | L: Optional[int] = PyDiffGame.L_default, 23 | eta: Optional[int] = PyDiffGame.eta_default): 24 | I_N = np.eye(N) 25 | Z_N = np.zeros((N, N)) 26 | 27 | M_masses = m * I_N 28 | K = k * (2 * I_N - np.array([[int(abs(i - j) == 1) for j in range(N)] for i in range(N)])) 29 | M_masses_inv = np.linalg.inv(M_masses) 30 | 31 | M_inv_K = M_masses_inv @ K 32 | 33 | if Ms is None: 34 | eigenvectors = np.linalg.eig(M_inv_K)[1] 35 | Ms = [eigenvector.reshape(1, N) for eigenvector in eigenvectors] 36 | 37 | A = np.block([[Z_N, I_N], 38 | [-M_inv_K, Z_N]]) 39 | B = np.block([[Z_N], 40 | [M_masses_inv]]) 41 | 42 | Qs = [np.diag([0.0] * i + [q] + [0.0] * (N - 1) + [q] + [0.0] * (N - i - 1)) 43 | if isinstance(q, (int, float)) else 44 | np.diag([0.0] * i + [q[i]] + [0.0] * (N - 1) + [q[i]] + [0.0] * (N - i - 1)) for i in range(N)] 45 | 46 | M = np.concatenate(Ms, 47 | axis=0) 48 | 49 | assert np.all(np.abs(np.linalg.inv(M) - M.T) < 10e-12) 50 | 51 | Q_mat = np.kron(a=np.eye(2), 52 | b=M) 53 | 54 | Qs = [Q_mat.T @ Q @ Q_mat for Q in Qs] 55 | 56 | Rs = [np.array([r])] * N 57 | R_lqr = 1 / 4 * r * I_N 58 | Q_lqr = q * np.eye(2 * N) if isinstance(q, (int, float)) else np.diag(2 * q) 59 | 60 | state_variables_names = ['x_{' + str(i) + '}' for i in range(1, N + 1)] + \ 61 | ['\\dot{x}_{' + str(i) + '}' for i in range(1, N + 1)] 62 | args = {'A': A, 63 | 'B': B, 64 | 'x_0': x_0, 65 | 'x_T': x_T, 66 | 'T_f': T_f, 67 | 'state_variables_names': state_variables_names, 68 | 'epsilon_x': epsilon_x, 69 | 'epsilon_P': epsilon_P, 70 | 'L': L, 71 | 'eta': eta} 72 | 73 | lqr_objective = [LQRObjective(Q=Q_lqr, 74 | R_ii=R_lqr)] 75 | game_objectives = [GameObjective(Q=Q, 76 | R_ii=R, 77 | M_i=M_i) for Q, R, M_i in zip(Qs, Rs, Ms)] 78 | games_objectives = [lqr_objective, 79 | game_objectives] 80 | 81 | self.figure_filename_generator = lambda g: ('LQR' if g.is_LQR() else f'{N}-players') + \ 82 | f'_large_{1 if q[0] > q[1] else 2}' 83 | 84 | super().__init__(args=args, 85 | M=M, 86 | games_objectives=games_objectives, 87 | continuous=True) 88 | 89 | @staticmethod 90 | def are_all_orthogonal(vectors: Sequence[np.array]) -> bool: 91 | """ 92 | Check if a set of vectors are all orthogonal to each other. 93 | 94 | Parameters 95 | ---------- 96 | 97 | vectors: sequence of vectors of len(n) 98 | A sequence of numpy arrays representing the vectors 99 | 100 | Returns 101 | ---------- 102 | 103 | all_orthogonal: bool 104 | True if all the vectors are orthogonal to each other, False otherwise 105 | """ 106 | 107 | all_orthogonal = not any([any([abs(vectors[i] @ vectors[j].T) > 1e-10 for j in range(i + 1, len(vectors))]) 108 | for i in range(len(vectors))]) 109 | 110 | return all_orthogonal 111 | 112 | @staticmethod 113 | def are_all_unit_vectors(vectors: Sequence[np.array]) -> bool: 114 | """ 115 | Check if a set of vectors are all of length 1. 116 | 117 | Parameters 118 | ---------- 119 | 120 | vectors: sequence of vectors of len(n) 121 | A sequence of numpy arrays representing the vectors 122 | 123 | Returns 124 | ---------- 125 | 126 | all_orthogonal: bool 127 | True if all the vectors are orthogonal to each other, False otherwise 128 | """ 129 | 130 | all_unit_vectors = all([abs(np.linalg.norm(v) - 1) < 1e-10 for v in vectors]) 131 | 132 | return all_unit_vectors 133 | 134 | 135 | N2_output_variables_names = ['$\\frac{x_1 + x_2}{\\sqrt{2}}$', 136 | '$\\frac{x_2 - x_1}{\\sqrt{2}}$', 137 | '$\\frac{\\dot{x}_1 + \\dot{x}_2}{\\sqrt{2}}$', 138 | '$\\frac{\\dot{x}_2 - \\dot{x}_1}{\\sqrt{2}}$'] 139 | 140 | 141 | def multiprocess_worker_function(N: int, 142 | k: float, 143 | m: float, 144 | q: float, 145 | r: float, 146 | epsilon_x: float, 147 | epsilon_P: float): 148 | x_0 = np.array([10 * i for i in range(1, N + 1)] + [0] * N) 149 | x_T = x_0 * 10 if N == 2 else np.array([(10 * i) ** 3 for i in range(1, N + 1)] + [0] * N) 150 | 151 | output_variables_names = N2_output_variables_names \ 152 | if N == 2 else [f'$q_{i}$' for i in range(1, N + 1)] + ['$\\dot{q}_{' + str(i) + '}$' for i in range(1, N + 1)] 153 | 154 | T_f = 25 155 | masses_with_springs = MassesWithSpringsComparison(N=N, 156 | m=m, 157 | k=k, 158 | q=q, 159 | r=r, 160 | x_0=x_0, 161 | x_T=x_T, 162 | T_f=T_f, 163 | epsilon_x=epsilon_x, 164 | epsilon_P=epsilon_P) 165 | 166 | masses_with_springs(plot_state_spaces=True, 167 | plot_Mx=True, 168 | output_variables_names=output_variables_names, 169 | save_figure=True, 170 | figure_filename=masses_with_springs.figure_filename_generator 171 | ) 172 | 173 | 174 | if __name__ == '__main__': 175 | d = 0.2 176 | N = 8 177 | 178 | Ns = [N] 179 | ks = [10] 180 | ms = [50] 181 | rs = [1] 182 | epsilon_xs = [10e-8] 183 | epsilon_Ps = [10e-8] 184 | qs = [[500, 2000], [500, 250]] if N == 2 else \ 185 | [[500 * (1 + d) ** i for i in range(N)], [500 * (1 - d) ** i for i in range(N)]] 186 | 187 | # N, k, m, r, epsilon_x, epsilon_P = Ns[0], ks[0], ms[0], rs[0], epsilon_xs[0], epsilon_Ps[0] 188 | # q = [500, 2000] if N == 2 else [500 * (1 + d) ** i for i in range(N)] 189 | # x_0 = np.array([10 * i for i in range(1, N + 1)] + [0] * N) 190 | # x_T = x_0 * 10 if N == 2 else np.array([(10 * i) ** 3 for i in range(1, N + 1)] + [0] * N) 191 | # 192 | # masses_with_springs = MassesWithSpringsComparison(N=N, 193 | # m=m, 194 | # k=k, 195 | # q=q, 196 | # r=r, 197 | # x_0=x_0, 198 | # x_T=x_T, 199 | # T_f=25, 200 | # epsilon_x=epsilon_x, 201 | # epsilon_P=epsilon_P) 202 | # 203 | # output_variables_names = N2_output_variables_names \ 204 | # if N == 2 else [f'$q_{i}$' for i in range(1, N + 1)] + ['$\\dot{q}_{' + str(i) + '}$' for i in range(1, N + 1)] 205 | # 206 | # masses_with_springs(plot_state_spaces=True, 207 | # plot_Mx=True, 208 | # output_variables_names=output_variables_names, 209 | # save_figure=True, 210 | # figure_filename=masses_with_springs.figure_filename_generator 211 | # ) 212 | 213 | # 214 | 215 | params = [Ns, ks, ms, qs, rs, epsilon_xs, epsilon_Ps] 216 | PyDiffGameLQRComparison.run_multiprocess(multiprocess_worker_function=multiprocess_worker_function, 217 | values=params) 218 | 219 | -------------------------------------------------------------------------------- /src/PyDiffGame/examples/PVTOL.py: -------------------------------------------------------------------------------- 1 | # pvtol_lqr.m - LQR design for vectored thrust aircraft 2 | # RMM, 14 Jan 03 3 | # 4 | # This file works through an LQR based design problem, using the 5 | # planar vertical takeoff and landing (PVTOLComparison.py) aircraft example from 6 | # Astrom and Murray, Chapter 5. It is intended to demonstrate the 7 | # basic functionality of the python-control package. 8 | # 9 | 10 | import os 11 | import numpy as np 12 | import matplotlib.pyplot as plt # MATLAB-like plotting functions 13 | import control as ct 14 | 15 | # 16 | # System dynamics 17 | # 18 | # These are the dynamics for the PVTOLComparison.py system, written in state space 19 | # form. 20 | # 21 | 22 | # System parameters 23 | m = 4 # mass of aircraft 24 | J = 0.0475 # inertia around pitch axis 25 | r = 0.25 # distance to center of force 26 | g = 9.8 # gravitational constant 27 | c = 0.05 # damping factor (estimated) 28 | 29 | # State space dynamics 30 | xe = [0, 0, 0, 0, 0, 0] # equilibrium point of interest 31 | ue = [0, m * g] # (note these are lists, not matrices) 32 | 33 | # This will involve re-working the subsequent equations as the shapes 34 | # See below. 35 | 36 | A = np.array( 37 | [[0, 0, 0, 1, 0, 0], 38 | [0, 0, 0, 0, 1, 0], 39 | [0, 0, 0, 0, 0, 1], 40 | [0, 0, (-ue[0]*np.sin(xe[2]) - ue[1]*np.cos(xe[2]))/m, -c/m, 0, 0], 41 | [0, 0, (ue[0]*np.cos(xe[2]) - ue[1]*np.sin(xe[2]))/m, 0, -c/m, 0], 42 | [0, 0, 0, 0, 0, 0]] 43 | ) 44 | 45 | # Input matrix 46 | B = np.array( 47 | [[0, 0], [0, 0], [0, 0], 48 | [np.cos(xe[2])/m, -np.sin(xe[2])/m], 49 | [np.sin(xe[2])/m, np.cos(xe[2])/m], 50 | [r/J, 0]] 51 | ) 52 | 53 | # Output matrix 54 | C = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]]) 55 | D = np.array([[0, 0], [0, 0]]) 56 | 57 | # 58 | # Construct inputs and outputs corresponding to steps in xy position 59 | # 60 | # The vectors xd and yd correspond to the states that are the desired 61 | # equilibrium states for the system. The matrices Cx and Cy are the 62 | # corresponding outputs. 63 | # 64 | # The way these vectors are used is to compute the closed loop system 65 | # dynamics as 66 | # 67 | # xdot = Ax + B u => xdot = (A-BK)x + K xd 68 | # u = -K(x - xd) y = Cx 69 | # 70 | # The closed loop dynamics can be simulated using the "step" command, 71 | # with K*xd as the input vector (assumes that the "input" is unit size, 72 | # so that xd corresponds to the desired steady state. 73 | # 74 | 75 | xd = np.array([[1], [0], [0], [0], [0], [0]]) 76 | yd = np.array([[0], [1], [0], [0], [0], [0]]) 77 | 78 | # 79 | # Extract the relevant dynamics for use with SISO library 80 | # 81 | # The current python-control library only supports SISO transfer 82 | # functions, so we have to modify some parts of the original MATLAB 83 | # code to extract out SISO systems. To do this, we define the 'lat' and 84 | # 'alt' index vectors to consist of the states that are are relevant 85 | # to the lateral (x) and vertical (y) dynamics. 86 | # 87 | 88 | # Indices for the parts of the state that we want 89 | lat = (0, 2, 3, 5) 90 | alt = (1, 4) 91 | 92 | # Decoupled dynamics 93 | Ax = A[np.ix_(lat, lat)] 94 | Bx = B[np.ix_(lat, [0])] 95 | Cx = C[np.ix_([0], lat)] 96 | Dx = D[np.ix_([0], [0])] 97 | 98 | Ay = A[np.ix_(alt, alt)] 99 | By = B[np.ix_(alt, [1])] 100 | Cy = C[np.ix_([1], alt)] 101 | Dy = D[np.ix_([1], [1])] 102 | 103 | # Label the plot 104 | plt.clf() 105 | plt.suptitle("LQR controllers for vectored thrust aircraft (pvtol-lqr)") 106 | 107 | # 108 | # LQR design 109 | # 110 | 111 | # Start with a diagonal weighting 112 | Qx1 = np.diag([1, 1, 1, 1, 1, 1]) 113 | Qu1a = np.diag([1, 1]) 114 | K1a, X, E = ct.lqr(A, B, Qx1, Qu1a) 115 | 116 | # Close the loop: xdot = Ax - B K (x-xd) 117 | # 118 | # Note: python-control requires we do this 1 input at a time 119 | # H1a = ss(A-B*K1a, B*K1a*concatenate((xd, yd), axis=1), C, D); 120 | # (T, Y) = step_response(H1a, T=np.linspace(0,10,100)); 121 | # 122 | 123 | # Step response for the first input 124 | H1ax = ct.ss(Ax - Bx @ K1a[np.ix_([0], lat)], 125 | Bx @ K1a[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx) 126 | Tx, Yx = ct.step_response(H1ax, T=np.linspace(0, 10, 100)) 127 | 128 | # Step response for the second input 129 | H1ay = ct.ss(Ay - By @ K1a[np.ix_([1], alt)], 130 | By @ K1a[np.ix_([1], alt)] @ yd[alt, :], Cy, Dy) 131 | Ty, Yy = ct.step_response(H1ay, T=np.linspace(0, 10, 100)) 132 | 133 | plt.subplot(221) 134 | plt.title("Identity weights") 135 | # plt.plot(T, Y[:,1, 1], '-', T, Y[:,2, 2], '--') 136 | plt.plot(Tx.T, Yx.T, '-', Ty.T, Yy.T, '--') 137 | plt.plot([0, 10], [1, 1], 'k-') 138 | 139 | plt.axis([0, 10, -0.1, 1.4]) 140 | plt.ylabel('position') 141 | plt.legend(('x', 'y'), loc='lower right') 142 | 143 | # Look at different input weightings 144 | Qu1a = np.diag([1, 1]) 145 | K1a, X, E = ct.lqr(A, B, Qx1, Qu1a) 146 | H1ax = ct.ss(Ax - Bx @ K1a[np.ix_([0], lat)], 147 | Bx @ K1a[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx) 148 | 149 | Qu1b = (40 ** 2)*np.diag([1, 1]) 150 | K1b, X, E = ct.lqr(A, B, Qx1, Qu1b) 151 | H1bx = ct.ss(Ax - Bx @ K1b[np.ix_([0], lat)], 152 | Bx @ K1b[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx) 153 | 154 | Qu1c = (200 ** 2)*np.diag([1, 1]) 155 | K1c, X, E = ct.lqr(A, B, Qx1, Qu1c) 156 | H1cx = ct.ss(Ax - Bx @ K1c[np.ix_([0], lat)], 157 | Bx @ K1c[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx) 158 | 159 | T1, Y1 = ct.step_response(H1ax, T=np.linspace(0, 10, 100)) 160 | T2, Y2 = ct.step_response(H1bx, T=np.linspace(0, 10, 100)) 161 | T3, Y3 = ct.step_response(H1cx, T=np.linspace(0, 10, 100)) 162 | 163 | plt.subplot(222) 164 | plt.title("Effect of input weights") 165 | plt.plot(T1.T, Y1.T, 'b-') 166 | plt.plot(T2.T, Y2.T, 'b-') 167 | plt.plot(T3.T, Y3.T, 'b-') 168 | plt.plot([0, 10], [1, 1], 'k-') 169 | 170 | plt.axis([0, 10, -0.1, 1.4]) 171 | 172 | # arcarrow([1.3, 0.8], [5, 0.45], -6) 173 | plt.text(5.3, 0.4, r'$\rho$') 174 | 175 | # Output weighting - change Qx to use outputs 176 | Qx2 = C.T @ C 177 | Qu2 = 0.1 * np.diag([1, 1]) 178 | K2, X, E = ct.lqr(A, B, Qx2, Qu2) 179 | 180 | H2x = ct.ss(Ax - Bx @ K2[np.ix_([0], lat)], 181 | Bx @ K2[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx) 182 | H2y = ct.ss(Ay - By @ K2[np.ix_([1], alt)], 183 | By @ K2[np.ix_([1], alt)] @ yd[alt, :], Cy, Dy) 184 | 185 | plt.subplot(223) 186 | plt.title("Output weighting") 187 | T2x, Y2x = ct.step_response(H2x, T=np.linspace(0, 10, 100)) 188 | T2y, Y2y = ct.step_response(H2y, T=np.linspace(0, 10, 100)) 189 | plt.plot(T2x.T, Y2x.T, T2y.T, Y2y.T) 190 | plt.ylabel('position') 191 | plt.xlabel('time') 192 | plt.ylabel('position') 193 | plt.legend(('x', 'y'), loc='lower right') 194 | 195 | # 196 | # Physically motivated weighting 197 | # 198 | # Shoot for 1 cm error in x, 10 cm error in y. Try to keep the angle 199 | # less than 5 degrees in making the adjustments. Penalize side forces 200 | # due to loss in efficiency. 201 | # 202 | 203 | Qx3 = np.diag([100, 10, 2*np.pi/5, 0, 0, 0]) 204 | Qu3 = 0.1*np.diag([1, 10]) 205 | K3, X, E = ct.lqr(A, B, Qx3, Qu3) 206 | 207 | H3x = ct.ss(Ax - Bx @ K3[np.ix_([0], lat)], 208 | Bx @ K3[np.ix_([0], lat)] @ xd[lat, :], Cx, Dx) 209 | H3y = ct.ss(Ay - By @ K3[np.ix_([1], alt)], 210 | By @ K3[np.ix_([1], alt)] @ yd[alt, :], Cy, Dy) 211 | plt.subplot(224) 212 | # step_response(H3x, H3y, 10) 213 | T3x, Y3x = ct.step_response(H3x, T=np.linspace(0, 10, 100)) 214 | T3y, Y3y = ct.step_response(H3y, T=np.linspace(0, 10, 100)) 215 | plt.plot(T3x.T, Y3x.T, T3y.T, Y3y.T) 216 | plt.title("Physically motivated weights") 217 | plt.xlabel('time') 218 | plt.legend(('x', 'y'), loc='lower right') 219 | plt.tight_layout() 220 | 221 | if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: 222 | plt.show() -------------------------------------------------------------------------------- /src/PyDiffGame/examples/PVTOLComparison.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from typing import Optional 3 | 4 | from PyDiffGame.PyDiffGame import PyDiffGame 5 | from PyDiffGame.PyDiffGameLQRComparison import PyDiffGameLQRComparison 6 | from PyDiffGame.Objective import GameObjective, LQRObjective 7 | 8 | 9 | class PVTOLComparison(PyDiffGameLQRComparison): 10 | def __init__(self, 11 | x_0: Optional[np.array] = None, 12 | x_T: Optional[np.array] = None, 13 | T_f: Optional[float] = None, 14 | epsilon_x: Optional[float] = PyDiffGame.epsilon_x_default, 15 | epsilon_P: Optional[float] = PyDiffGame.epsilon_P_default, 16 | L: Optional[int] = PyDiffGame.L_default, 17 | eta: Optional[int] = PyDiffGame.eta_default): 18 | m = 4 # mass of aircraft 19 | J = 0.0475 # inertia around pitch axis 20 | r = 0.25 # distance to center of force 21 | c = 0.05 # damping factor (estimated) 22 | 23 | A_11 = np.zeros((3, 3)) 24 | A_12 = np.eye(3) 25 | A_21 = np.array([[0, 0, -PyDiffGame.g], 26 | [0, 0, 0], 27 | [0, 0, 0]]) 28 | A_22 = np.diag([-c / m, -c / m, 0]) 29 | 30 | A = np.block([[A_11, A_12], 31 | [A_21, A_22]]) 32 | 33 | # Input matrix 34 | B = np.array( 35 | [[0, 0], 36 | [0, 0], 37 | [0, 0], 38 | [1 / m, 0], 39 | [0, 1 / m], 40 | [r / J, 0]] 41 | ) 42 | 43 | psi = 1 44 | 45 | M_x = [1, 0] 46 | M_y = [0, 1] 47 | M_theta = [1, 0] 48 | 49 | Ms = [np.array(M_i).reshape(1, 2) for M_i in [M_x, M_y, M_theta]] 50 | M = np.concatenate(Ms, axis=0) 51 | 52 | self.figure_filename_generator = lambda g: ('LQR_' if g.is_LQR() else '') + f"PVTOL{str(psi).replace('.', '')}" 53 | 54 | Q_x_diag = [psi, 0, 0] 55 | Q_y_diag = [0, 1, 0] 56 | Q_theta_diag = [0, 0, 1] 57 | 58 | r_x = 1 59 | r_y = 1 60 | r_theta = 1 61 | 62 | Qs = [np.diag(Q_i_diag * 2) for Q_i_diag in [Q_x_diag, Q_y_diag, Q_theta_diag]] 63 | Rs = [np.array([r_i]) for r_i in [r_x, r_y, r_theta]] 64 | 65 | variables = ['x', 'y', '\\theta'] 66 | 67 | self.state_variables_names = [f'{v}(t)' for v in variables] + ['\\dot{' + str(v) + '}(t)' for v in variables] 68 | 69 | Q_lqr = sum(Qs) 70 | R_lqr = np.diag([r_x + r_theta, r_y]) 71 | 72 | lqr_objective = [LQRObjective(Q=Q_lqr, 73 | R_ii=R_lqr)] 74 | game_objectives = [GameObjective(Q=Q_i, 75 | R_ii=R_i, 76 | M_i=M_i) for Q_i, R_i, M_i in zip(Qs, Rs, Ms)] 77 | games_objectives = [lqr_objective, 78 | game_objectives] 79 | 80 | args = {'A': A, 81 | 'B': B, 82 | 'x_0': x_0, 83 | 'x_T': x_T, 84 | 'T_f': T_f, 85 | 'state_variables_names': self.state_variables_names, 86 | 'epsilon_x': epsilon_x, 87 | 'epsilon_P': epsilon_P, 88 | 'L': L, 89 | 'eta': eta} 90 | 91 | super().__init__(args=args, 92 | M=M, 93 | games_objectives=games_objectives) 94 | 95 | 96 | if __name__ == '__main__': 97 | epsilon_x = epsilon_P = 10 ** (-8) 98 | 99 | x_0 = np.zeros((6,)) 100 | x_d = y_d = 50 101 | x_T = np.array([x_d, y_d] + [0] * 4) 102 | T_f = 25 103 | 104 | pvtol = PVTOLComparison(x_0=x_0, 105 | x_T=x_T, 106 | T_f=T_f, 107 | epsilon_x=epsilon_x, 108 | epsilon_P=epsilon_P) 109 | pvtol(plot_state_spaces=True, 110 | save_figure=True, 111 | figure_filename=pvtol.figure_filename_generator) 112 | -------------------------------------------------------------------------------- /src/PyDiffGame/examples/QuadRotorControl.py: -------------------------------------------------------------------------------- 1 | # Imports 2 | 3 | import numpy as np 4 | from numpy.linalg import inv 5 | import matplotlib.pyplot as plt 6 | from scipy.integrate import odeint 7 | from tqdm import tqdm 8 | from math import cos, sin 9 | from scipy.optimize import nnls 10 | from numpy import sin, cos, arctan 11 | from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes 12 | from mpl_toolkits.axes_grid1.inset_locator import mark_inset 13 | 14 | from PyDiffGame.ContinuousPyDiffGame import ContinuousPyDiffGame 15 | 16 | # Global Constants 17 | 18 | g = 9.81 19 | sum_theta_init = -3 / g 20 | Ixx = 7.5e-3 21 | Iyy = 7.5e-3 22 | Izz = 1.3e-2 23 | m = 0.65 24 | l = 0.23 25 | Jr = 6e-5 26 | b = 3.13e-5 27 | d = 7.5e-7 28 | 29 | a1 = (Iyy - Izz) / Ixx 30 | a2 = Jr / Ixx 31 | a3 = (Izz - Ixx) / Iyy 32 | a4 = Jr / Iyy 33 | a5 = (Ixx - Iyy) / Izz 34 | 35 | b1 = l / Ixx 36 | b2 = l / Iyy 37 | b3 = 1 / Izz 38 | 39 | v_d_s_0_2 = 0 40 | h_d_s_0_2 = 0 41 | sum_theta = sum_theta_init 42 | sum_theta_2 = 0 43 | 44 | 45 | # Low-Level Control 46 | 47 | def quad_rotor_state_diff_eqn_for_given_pqrT(X, _, p, q, r, T, Plast): 48 | phi, dPhidt, theta, dThetadt, psi, dPsidt, z, dzdt, x, dxdt, y, dydt = X 49 | u_x = cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi) 50 | u_y = cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi) 51 | 52 | K = cos(phi) * cos(theta) / m 53 | U = low_level_angular_rate_controller([dPhidt, dThetadt, dPsidt], p, q, r, T, Plast) 54 | omegas_squared_coeffs = np.array([[b] * 4, 55 | [0, -b, 0, b], 56 | [b, 0, -b, 0], 57 | [-d, d, -d, d] 58 | ]) 59 | u1, u2, u3, u4 = U 60 | omegas_squared = nnls(omegas_squared_coeffs, np.array([u1, u2, u3, u4]))[0] 61 | omegas = np.sqrt(omegas_squared) 62 | 63 | u5 = d * (omegas[0] - omegas[1] + omegas[2] - omegas[3]) 64 | 65 | dPhiddt = dThetadt * (dPsidt * a1 + u5 * a2) + b1 * u2 66 | dThetaddt = dPhidt * (dPsidt * a3 - u5 * a4) + b2 * u3 67 | dPsiddt = dThetadt * dPhidt * a5 + b3 * u4 68 | dzddt = g - K * u1 69 | dxddt = u_x * u1 / m 70 | dyddt = u_y * u1 / m 71 | 72 | return np.array([dPhidt, dPhiddt, dThetadt, dThetaddt, dPsidt, dPsiddt, dzdt, 73 | dzddt, dxdt, dxddt, dydt, dyddt], dtype='float64') 74 | 75 | 76 | def low_level_angular_rate_controller(x, p, q, r, T, Plast): 77 | B1 = np.array([[b1], 78 | [0], 79 | [0]]) 80 | 81 | B2 = np.array([[0], 82 | [b2], 83 | [0]]) 84 | 85 | B3 = np.array([[0], 86 | [0], 87 | [b3]]) 88 | 89 | R1 = np.array([[0.1]]) 90 | R2 = np.array([[0.1]]) 91 | R3 = np.array([[0.1]]) 92 | 93 | B = [B1, B2, B3] 94 | R = [R1, R2, R3] 95 | P_sol = Plast 96 | reduced_X = np.array(x) - np.array([p, q, r]) 97 | reduced_X_tr = reduced_X.T 98 | inv_Rs = [inv(r) for r in R] 99 | B_t = [b.T for b in B] 100 | U_angular = np.array([- r @ b @ p @ reduced_X_tr for r, b, p in zip(inv_Rs, B_t, P_sol)]).reshape(3, ) 101 | u2, u3, u4 = U_angular 102 | U = [T, u2, u3, u4] 103 | 104 | return U 105 | 106 | 107 | def get_P_quad_given_angular_rates(x, P_sol): 108 | A = np.array([[0, (1 / 2) * a1 * x[2], (1 / 2) * a1 * x[1]], 109 | [(1 / 2) * a3 * x[2], 0, (1 / 2) * a3 * x[0]], 110 | [(1 / 2) * a5 * x[1], (1 / 2) * a5 * x[0], 0]]) 111 | 112 | B1 = np.array([[b1], 113 | [0], 114 | [0]]) 115 | 116 | B2 = np.array([[0], 117 | [b2], 118 | [0]]) 119 | 120 | B3 = np.array([[0], 121 | [0], 122 | [b3]]) 123 | 124 | Q1 = np.array([[1000, 0, 0], 125 | [0, 10, 0], 126 | [0, 0, 10]]) 127 | 128 | Q2 = np.array([[10, 0, 0], 129 | [0, 1000, 0], 130 | [0, 0, 10]]) 131 | 132 | Q3 = np.array([[10, 0, 0], 133 | [0, 10, 0], 134 | [0, 0, 1000]]) 135 | 136 | R1 = np.array([[0.1]]) 137 | R2 = np.array([[0.1]]) 138 | R3 = np.array([[0.1]]) 139 | 140 | B = [B1, B2, B3] 141 | R = [R1, R2, R3] 142 | Q = [Q1, Q2, Q3] 143 | game = ContinuousPyDiffGame(A=A, Bs=B, Qs=Q, Rs=R, P_f=P_sol, show_legend=False) 144 | game() 145 | Plast = game.P[-1] 146 | 147 | return Plast 148 | 149 | 150 | # High-Level Control 151 | 152 | def get_mf_numerator(F3, R11, F1, R31, a_y, R12, R32): 153 | return F3 * R11 - F1 * R31 + a_y * (R12 * R31 - R11 * R32) 154 | 155 | 156 | def get_mf_denominator(F2, R11, F1, R21, a_y, R22, R12): 157 | return - F2 * R11 + F1 * R21 + a_y * (R11 * R22 - R12 * R21) 158 | 159 | 160 | def get_mc_numerator(mf_numerator, a_z, R31, R13, R11, R33): 161 | return mf_numerator + a_z * (R13 * R31 - R11 * R33) 162 | 163 | 164 | def get_mc_denominator(mf_denominator, a_z, R11, R23): 165 | return mf_denominator - a_z * R11 * R23 166 | 167 | 168 | def hpf_ode_v_d_s(v_d_s, _, f_a, f_b, P_z_tilda): 169 | return f_a * v_d_s + f_b * P_z_tilda 170 | 171 | 172 | def hpf_ode_h_d_s(h_d_s, _, f_a, f_b, P_y_tilda): 173 | return f_a * h_d_s + f_b * P_y_tilda 174 | 175 | 176 | def calculate_Bs(u_sizes, dividing_matrix, B): 177 | block_matrix = B @ dividing_matrix 178 | Bs = [] 179 | 180 | last = 0 181 | for u_size in u_sizes: 182 | Bs += [block_matrix[:, last:u_size + last]] 183 | last = u_size 184 | 185 | return Bs 186 | 187 | 188 | def wall_punishment(wall_distance, a_y): 189 | return 3 * (10 ** 2) * (wall_distance / a_y) ** 2 190 | 191 | 192 | def get_higher_level_control2(state, st, a_y): 193 | global v_d_s_0_2, h_d_s_0_2, sum_theta, sum_theta_2 194 | # a_y = 1 195 | a_z = -2.5 196 | 197 | x = state[8] 198 | y = state[10] 199 | z = state[6] 200 | phi = state[0] 201 | theta = state[2] 202 | psi = state[4] 203 | 204 | sphi = sin(phi) 205 | cphi = cos(phi) 206 | stheta = sin(theta) 207 | ctheta = cos(theta) 208 | spsi = sin(psi) 209 | cpsi = cos(psi) 210 | 211 | sectheta = 1 / ctheta 212 | tanpsi = spsi / cpsi 213 | 214 | vp_x = sectheta * (sphi * stheta - cphi * tanpsi) 215 | vp_y = sectheta * (- cphi * stheta - sphi * tanpsi) 216 | 217 | R11 = ctheta * cpsi 218 | R21 = cpsi * stheta * sphi - cphi * spsi 219 | R31 = cpsi * stheta * cphi + sphi * spsi 220 | R12 = ctheta * spsi 221 | R22 = spsi * stheta * sphi + cphi * cpsi 222 | R32 = spsi * stheta * cphi - sphi * cpsi 223 | R13 = - stheta 224 | R23 = ctheta * sphi 225 | R33 = ctheta * cphi 226 | r = np.array([[R11, R21, R31], [R12, R22, R32], [R13, R23, R33]]) 227 | 228 | curr_loc = np.array([[x], [y], [z]]) 229 | [F1, F2, F3] = r.T @ curr_loc 230 | 231 | mfr_numerator = get_mf_numerator(F3, R11, F1, R31, a_y, R12, R32) 232 | mfr_denominator = get_mf_denominator(F2, R11, F1, R21, a_y, R22, R12) 233 | mfl_numerator = get_mf_numerator(F3, R11, F1, R31, -a_y, R12, R32) 234 | mfl_denominator = get_mf_denominator(F2, R11, F1, R21, -a_y, R22, R12) 235 | 236 | mfr = mfr_numerator / mfr_denominator 237 | mfl = mfl_numerator / mfl_denominator 238 | 239 | mcr = get_mc_numerator(mfr_numerator, a_z, R31, R13, R11, R33) / get_mc_denominator(mfr_denominator, a_z, R11, R23) 240 | mcl = get_mc_numerator(mfl_numerator, a_z, R31, R13, R11, R33) / get_mc_denominator(mfl_denominator, a_z, R11, R23) 241 | 242 | at_mcl = arctan(mcl) 243 | at_mcr = arctan(mcr) 244 | at_mfl = arctan(mfl) 245 | at_mfr = arctan(mfr) 246 | 247 | p_y_tilda = at_mcl + at_mcr - at_mfl - at_mfr 248 | p_z_tilda = at_mfl - at_mfr + at_mcl - at_mcr 249 | phi_tilda = at_mfl + at_mfr + at_mcl + at_mcr 250 | 251 | f_a = -10 252 | f_b = 8 253 | f_c = -12.5 254 | f_d = 10 255 | 256 | v_d_s = v_d_s_0_2 257 | h_d_s = h_d_s_0_2 258 | v_d = f_c * v_d_s + f_d * p_z_tilda 259 | h_d = f_c * h_d_s + f_d * p_y_tilda 260 | 261 | data_points = 100 262 | t = np.linspace(st, st + 0.1, data_points) 263 | v_d_s = np.mean(odeint(func=hpf_ode_v_d_s, y0=v_d_s_0_2, t=t, args=(f_a, f_b, p_z_tilda))) 264 | h_d_s = np.mean(odeint(func=hpf_ode_h_d_s, y0=h_d_s_0_2, t=t, args=(f_a, f_b, p_y_tilda))) 265 | 266 | v_d_s_0_2 = v_d_s 267 | h_d_s_0_2 = h_d_s 268 | 269 | Q1 = np.array([[1000, 0, 0, 0, 0, 0, 0, 0, 0], 270 | [0, 1000, 0, 0, 0, 0, 0, 0, 0], 271 | [0, 0, 1000, 0, 0, 0, 0, 0, 0], 272 | [0, 0, 0, 0.1, 0, 0, 0, 0, 0], 273 | [0, 0, 0, 0, 0.1, 0, 0, 0, 0], 274 | [0, 0, 0, 0, 0, 10, 0, 0, 0], 275 | [0, 0, 0, 0, 0, 0, 0.05, 0, 0], 276 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 277 | [0, 0, 0, 0, 0, 0, 0, 0, 0]]) 278 | 279 | R1 = np.array([[10, 0, 0, 0], 280 | [0, 10, 0, 0], 281 | [0, 0, 10, 0], 282 | [0, 0, 0, 0.01]]) 283 | 284 | A = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0], 285 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 286 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 287 | [0, 0, 0, 0, 1, 0, 0, 0, 0], 288 | [g, 0, 0, 0, 0, 0, 0, 0, 0], 289 | [0, 0, 0, 0, 0, 0, 1, 0, 0], 290 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 291 | [0, 1, 0, 0, 0, 0, 0, 0, 0], 292 | [0, 1, 0, 0, 0, 0, 0, 0, 0]]) 293 | 294 | B = np.array([[1, 0, 0, 0], 295 | [0, 1, 0, 0], 296 | [0, 0, 1, 0], 297 | [0, 0, 0, 0], 298 | [0, 0, 0, 0], 299 | [0, 0, 0, 0], 300 | [0, 0, 0, -1 / m], 301 | [0, 0, 0, 0], 302 | [0, 0, 0, 0]]) 303 | 304 | dividing_matrix = np.array([[1, 0, 0, 0, 0], 305 | [0, 1, 0, 0, 1], 306 | [0, 0, 1, 0, 0], 307 | [0, 0, 0, 1, 0]]) 308 | 309 | u_sizes = [4, 1] 310 | Ms = [dividing_matrix[:, :u_sizes[0]].T, dividing_matrix[:, u_sizes[0]:u_sizes[0] + u_sizes[1]].T] 311 | Bs = calculate_Bs(u_sizes, dividing_matrix, B) 312 | 313 | R2 = np.array([[10]]) 314 | 315 | max_punishment = wall_punishment(a_y, a_y) 316 | curr_punishment = min(max_punishment, wall_punishment(p_y_tilda[0], a_y)) 317 | 318 | Q_wall_0 = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0], 319 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 320 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 321 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 322 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 323 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 324 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 325 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 326 | [0, 0, 0, 0, 0, 0, 0, 0, curr_punishment]]) 327 | Q_speed_up_0 = np.array([[0, 0, 0, 0, 0, 0, 0, 0, 0], 328 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 329 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 330 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 331 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 332 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 333 | [0, 0, 0, 0, 0, 0, 0, 0, 0], 334 | [0, 0, 0, 0, 0, 0, 0, 1000, 0], 335 | [0, 0, 0, 0, 0, 0, 0, 0, 0]]) 336 | 337 | R = [R1, R2] 338 | if abs(p_y_tilda[0] / a_y) < 0.5: 339 | Q = [0.01 * Q1, 0.01 * Q_speed_up_0] 340 | else: 341 | Q = [0.01 * Q1, 0.01 * Q_wall_0] 342 | P_sol = [0.01 * Q1, 0.01 * Q1] 343 | 344 | game = ContinuousPyDiffGame(A=A, Bs=Bs, Qs=Q, Rs=R, P_f=P_sol, show_legend=False) 345 | game() 346 | Plast = game.P[-1] 347 | N = 2 348 | M = 9 349 | P_size = M ** 2 350 | Plast = [(Plast[i * P_size:(i + 1) * P_size]).reshape(M, M) for i in range(N)] 351 | 352 | inv_Rs = [inv(r) for r in R] 353 | B_t = [b.T for b in Bs] 354 | 355 | U_Agenda1 = - inv_Rs[0] @ B_t[0] @ Plast[0] @ np.array( 356 | [-phi_tilda[0], -vp_y, -vp_x, -p_y_tilda[0], -h_d[0], -p_z_tilda[0], -v_d[0], sum_theta, sum_theta_2]) 357 | U_Agenda2 = - inv_Rs[1] @ B_t[1] @ Plast[1] @ np.array( 358 | [-phi_tilda[0], -vp_y, -vp_x, -p_y_tilda[0], -h_d[0], -p_z_tilda[0], -v_d[0], sum_theta, sum_theta_2]) 359 | 360 | Us = [U_Agenda1, U_Agenda2] 361 | 362 | U_all_Out = dividing_matrix @ np.concatenate(Us).ravel().T 363 | p_r, q_r, r_r, t_r = U_all_Out 364 | 365 | tilda_state = np.array( 366 | [phi_tilda[0], vp_y, vp_x, p_y_tilda[0], h_d[0], p_z_tilda[0], v_d[0], sum_theta, sum_theta_2]) 367 | sum_theta = sum_theta - vp_y * 0.1 368 | sum_theta_2 = sum_theta_2 - vp_y * 0.1 369 | 370 | return p_r, q_r, r_r, t_r + m * g, tilda_state 371 | 372 | 373 | # Simulation 374 | 375 | Q1 = np.array([[1000, 0, 0], 376 | [0, 10, 0], 377 | [0, 0, 10]]) 378 | Q2 = np.array([[10, 0, 0], 379 | [0, 1000, 0], 380 | [0, 0, 10]]) 381 | Q3 = np.array([[10, 0, 0], 382 | [0, 10, 0], 383 | [0, 0, 1000]]) 384 | Q = [Q1, Q2, Q3] 385 | M = 3 386 | P_size = M ** 2 387 | N = 3 388 | tTotal = [0] 389 | tTotal_low = [0] 390 | a_ys = [0.55] 391 | quad_rotor_state_PD_dynamic = {} 392 | quad_rotor_omega_D_dynamic = {} 393 | quad_rotor_state_PD_dynamic_low = {} 394 | tilda_state_dynamic = {} 395 | 396 | for a_y in a_ys: 397 | 398 | T_start = 0 399 | deltaTstate = 0.1 400 | X_rotor_0 = np.array([0.1, 0, 0, 0, 0.1, 0, -1, 0, 0, 0, 0.3, 0]) 401 | omega_rotor_0 = np.array([0, 0, 0]) 402 | quad_rotor_state_PD_dynamic[a_y] = [X_rotor_0] 403 | quad_rotor_omega_D_dynamic[a_y] = [omega_rotor_0] 404 | quad_rotor_state_PD_dynamic_low[a_y] = [X_rotor_0] 405 | tilda_state = np.array([0, 0, 0, 0, 0, 0, 0, sum_theta_init, 0]) 406 | tilda_state_dynamic[a_y] = [tilda_state] 407 | 408 | X_rotor_0_PD = X_rotor_0 409 | Plast = [Q1, Q2, Q3] 410 | v_d_s_0_2 = 0 411 | h_d_s_0_2 = 0 412 | sum_theta = sum_theta_init 413 | sum_theta_2 = 0 414 | 415 | for i in tqdm(range(300)): 416 | # p_r, q_r, r_r, t_r, tilda_state_l = get_higher_level_control(X_rotor_0_PD,T_start, a_y) 417 | p_r2, q_r2, r_r2, t_r2, tilda_state_l2 = get_higher_level_control2(X_rotor_0_PD, T_start, a_y) 418 | tilda_state = [tilda_state_l2] 419 | tilda_state_dynamic[a_y] = np.append(tilda_state_dynamic[a_y], tilda_state, axis=0) 420 | T_end = T_start + deltaTstate 421 | data_points = 100 422 | t = np.linspace(T_start, T_end, data_points) 423 | Plast = get_P_quad_given_angular_rates([X_rotor_0_PD[1], X_rotor_0_PD[3], X_rotor_0_PD[5]], Plast) 424 | Plast = [(Plast[k * P_size:(k + 1) * P_size]).reshape(M, M) for k in range(N)] 425 | quad_rotor_state_PD = odeint(quad_rotor_state_diff_eqn_for_given_pqrT, X_rotor_0_PD, t, 426 | args=(p_r2, q_r2, r_r2, t_r2, Plast)) 427 | omega_rotor_i = np.array([p_r2, q_r2, r_r2]) 428 | X_rotor_0_PD = quad_rotor_state_PD[-1] 429 | T_start = T_end 430 | quad_rotor_state_PD_dynamic[a_y] = np.append(quad_rotor_state_PD_dynamic[a_y], 431 | quad_rotor_state_PD, axis=0) 432 | quad_rotor_omega_D_dynamic[a_y] = np.append(quad_rotor_omega_D_dynamic[a_y], 433 | [omega_rotor_i], axis=0) 434 | quad_rotor_state_PD_dynamic_low[a_y] = np.append(quad_rotor_state_PD_dynamic_low[a_y], 435 | [X_rotor_0_PD], axis=0) 436 | if a_y == a_ys[0]: 437 | tTotal = np.append(tTotal, t) 438 | tTotal_low = np.append(tTotal_low, T_end) 439 | 440 | angles = {'phi': [0, 0], 'theta': [2, 1], 'psi': [4, 2]} 441 | positions = {'x': [8, 7], 'z': [6, 5], 'y': [10, 3]} 442 | velocities = {'y_dot': [11, 4], 'z_dot': [7, 6], 'x_dot': 9} 443 | 444 | plot_vars = {**angles, **positions, **velocities} 445 | 446 | 447 | def plot_var(var): 448 | var_indices = plot_vars[var] 449 | 450 | for a_y in quad_rotor_state_PD_dynamic_low.keys(): 451 | plt.figure(dpi=130) 452 | 453 | plt.title('$a _y = \ $' + str(a_y) + '$ \ [m]$', fontsize=16) 454 | 455 | plt.plot(tTotal_low[1:], 456 | quad_rotor_state_PD_dynamic_low[a_y][1:, var_indices[0] if var != 'x_dot' else var_indices]) 457 | 458 | if var in positions.keys(): 459 | plt.plot(tTotal_low[1:], tilda_state_dynamic[a_y][1:, var_indices[1]]) 460 | else: 461 | plt.plot(tTotal_low[1:], -tilda_state_dynamic[a_y][1:, var_indices[1]]) 462 | 463 | if var in positions.keys(): 464 | plt.legend(['$' + var + ' \\ [m]$', '$\\tilde{' + var + '} \\ [m]$']) 465 | else: 466 | if 'dot' in var: 467 | plt.legend(['$\\dot{' + var[0] + '} \\ \\left[ \\frac{m}{sec} \\right]$', 468 | '$\\tilde{\\dot{' + var[0] + '}} \\ \\left[ \\frac{m}{sec} \\right]$']) 469 | else: 470 | plt.legend(['$\\' + var + ' \\ [rad]$', '$\\tilde{\\' + var + '} \\ [rad]$']) 471 | 472 | plt.xlabel('$t \ [sec]$', fontsize=16) 473 | plt.grid() 474 | plt.show() 475 | 476 | 477 | for curr_a_y, var in quad_rotor_state_PD_dynamic_low.items(): 478 | plt.figure(dpi=300) 479 | plt.plot(tTotal_low[1:], var[1:, 9]) 480 | plt.plot(tTotal_low[1:], abs(tilda_state_dynamic[curr_a_y][1:, 3] / curr_a_y)) 481 | plt.plot(tTotal_low[1:], 0.5 * np.ones(len(tTotal_low[1:]))) 482 | plt.grid() 483 | plt.xlabel('$t \ [sec]$', fontsize=12) 484 | # plt.title('$a_y = ' + str(curr_a_y) + ' \ [m]$', fontsize=12) 485 | # plt.legend(['$\\dot{x} \ [\\frac{m}{s}]$', '$\\frac{\\tilde{y}}{a_y}$']) 486 | plt.legend(['Forward Velocity', 'Wall Distance Measure']) 487 | plt.show() 488 | 489 | for var in ['x', 'y', 'y_dot', 'z', 'z_dot', 'phi', 'theta', 'psi']: 490 | plot_var(var) 491 | 492 | time_ratio1 = len(tTotal_low) / max(tTotal_low) 493 | time_ratio2 = len(tTotal) / max(tTotal) 494 | 495 | for a_y in quad_rotor_omega_D_dynamic.keys(): 496 | fig, ax = plt.subplots(1, dpi=150) 497 | 498 | t01 = int(time_ratio1 * 0.8) 499 | t1 = int(time_ratio1 * 1.1) 500 | t02 = int(time_ratio2 * 0.8) 501 | t2 = int(time_ratio2 * 1) 502 | 503 | x1 = tTotal_low[t01:t1] 504 | y1 = quad_rotor_omega_D_dynamic[a_y][t01:t1, 1] 505 | x2 = tTotal[t02:t2] 506 | y2 = quad_rotor_state_PD_dynamic[a_y][t02:t2, 3] 507 | 508 | ax.step(x1, y1, linewidth=1) 509 | ax.plot(x2, y2, linewidth=1) 510 | plt.xlabel('$t \ [sec]$', fontsize=14) 511 | plt.grid() 512 | 513 | axins = zoomed_inset_axes(ax, 3.5, borderpad=3) 514 | mark_inset(ax, axins, loc1=2, loc2=4, fc="none", ec="0.5") 515 | 516 | xlim = [0.898, 0.905] 517 | ylim = [0.193, 0.205] 518 | 519 | axins.set_xlim(xlim) 520 | axins.set_ylim(ylim) 521 | axins.step(x1, y1, linewidth=1) 522 | axins.plot(x2, y2, linewidth=1) 523 | plt.grid() 524 | 525 | fig.legend(['$\\dot{\\theta}_d \\ \\left[ \\frac{rad}{sec} \\right]$', 526 | '$\\dot{\\theta} \\ \\left[ \\frac{rad}{sec} \\right]$'], 527 | bbox_to_anchor=(0.28, 0.35) if a_y != 0.6 else (0.3, 0.35)) 528 | 529 | plt.show() 530 | 531 | for sol in quad_rotor_state_PD_dynamic_low.values(): 532 | plt.figure(dpi=300) 533 | plt.plot(tTotal_low[0:], sol[0:, 0:6]) 534 | plt.xlabel('$t \ [sec]$', fontsize=14) 535 | plt.legend(['$\\phi[rad]$', '$\\dot{\phi}\\left[ \\frac{rad}{sec} \\right]$', '$\\theta[rad]$', 536 | '$\\dot{\\theta}\\left[ \\frac{rad}{sec} \\right]$', '$\\psi[rad]$', 537 | '$\\dot{\psi}\\left[ \\frac{rad}{sec} \\right]$'], ncol=2, loc='upper right') 538 | plt.grid() 539 | plt.show() 540 | 541 | for sol in quad_rotor_state_PD_dynamic_low.values(): 542 | plt.figure(dpi=300) 543 | plt.plot(tTotal_low[0:], sol[0:, 6:8], tTotal_low[0:], sol[0:, 10:12]) 544 | plt.xlabel('$t \ [sec]$', fontsize=14) 545 | plt.legend( 546 | ['$z[m]$', '$\\dot{z}\\left[ \\frac{m}{sec} \\right]$', '$y[m]$', '$\\dot{y}\\left[ \\frac{m}{sec} \\right]$']) 547 | plt.grid() 548 | plt.show() 549 | -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/2/2-players_large_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/2/2-players_large_1.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/2/2-players_large_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/2/2-players_large_2.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/2/LQR_large_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/2/LQR_large_1.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/2/LQR_large_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/2/LQR_large_2.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/2/two_masses_tikz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/2/two_masses_tikz.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/4/4-players_large_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/4/4-players_large_1.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/4/4-players_large_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/4/4-players_large_2.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/4/LQR_large_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/4/LQR_large_1.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/4/LQR_large_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/4/LQR_large_2.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/8/8-players_large_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/8/8-players_large_1.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/8/8-players_large_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/8/8-players_large_2.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/8/LQR_large_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/8/LQR_large_1.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/8/LQR_large_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/8/LQR_large_2.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/PVTOL/PVTOL1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/PVTOL/PVTOL1.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/PVTOL/PVTOL10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/PVTOL/PVTOL10.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/PVTOL/PVTOL100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/PVTOL/PVTOL100.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/PVTOL/PVTOL1000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/PVTOL/PVTOL1000.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/PVTOL0001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/PVTOL0001.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/PVTOL001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/PVTOL001.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/PVTOL01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/PVTOL01.png -------------------------------------------------------------------------------- /src/PyDiffGame/examples/figures/PVTOL1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/krichelj/PyDiffGame/c0bb4ff9877c9a05a7c201f6e68effc8c451f736/src/PyDiffGame/examples/figures/PVTOL1.png --------------------------------------------------------------------------------