├── .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 |
3 |
4 |
5 | [](https://opensource.org/licenses/MIT) [](https://github.com/krichelj/PyDiffGame/actions/workflows/python-publish.yml) [](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 |
281 |
282 |
283 |
284 |
285 |
286 |
287 |
288 |
289 |
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 |
3 |
4 |
5 | [](https://github.com/krichelj/PyDiffGame/actions/workflows/pages/pages-build-deployment)
6 | [](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 |
271 |
272 |
273 |
274 |
275 |
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
--------------------------------------------------------------------------------