├── .gitignore ├── .pre-commit-config.yaml ├── CONTRIBUTING.rst ├── LICENSE ├── README.md ├── animations └── .gitignore ├── control ├── dcbf_controller.py └── dcbf_optimizer.py ├── environment.yml ├── figures └── .gitignore ├── models ├── dubin_car.py ├── dubin_car_test.py ├── geometry_utils.py ├── geometry_utils_test.py ├── kinematic_car.py └── kinematic_car_test.py ├── planning ├── path_generator │ ├── astar.py │ ├── astar_test.py │ ├── opt_planner.py │ ├── opt_planner_test.py │ ├── optimization_path_generator.py │ └── search_path_generator.py └── trajectory_generator │ ├── constant_speed_generator.py │ └── constant_speed_generator_test.py ├── pyproject.toml ├── requirements.txt └── sim ├── logger.py └── simulation.py /.gitignore: -------------------------------------------------------------------------------- 1 | ### Vscode 2 | .vscode/* 3 | !.vscode/settings.json 4 | !.vscode/tasks.json 5 | !.vscode/launch.json 6 | !.vscode/extensions.json 7 | *.code-workspace 8 | 9 | # Local History for Visual Studio Code 10 | .history/ 11 | 12 | ### PYTHON 13 | # Byte-compiled / optimized / DLL files 14 | __pycache__/ 15 | *.py[cod] 16 | *$py.class 17 | 18 | # C extensions 19 | *.so 20 | 21 | # Distribution / packaging 22 | .Python 23 | build/ 24 | develop-eggs/ 25 | dist/ 26 | downloads/ 27 | eggs/ 28 | .eggs/ 29 | lib/ 30 | lib64/ 31 | parts/ 32 | sdist/ 33 | var/ 34 | wheels/ 35 | share/python-wheels/ 36 | *.egg-info/ 37 | .installed.cfg 38 | *.egg 39 | MANIFEST 40 | 41 | # PyInstaller 42 | # Usually these files are written by a python script from a template 43 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 44 | *.manifest 45 | *.spec 46 | 47 | # Installer logs 48 | pip-log.txt 49 | pip-delete-this-directory.txt 50 | 51 | # Unit test / coverage reports 52 | htmlcov/ 53 | .tox/ 54 | .nox/ 55 | .coverage 56 | .coverage.* 57 | .cache 58 | nosetests.xml 59 | coverage.xml 60 | *.cover 61 | *.py,cover 62 | .hypothesis/ 63 | .pytest_cache/ 64 | cover/ 65 | 66 | # Translations 67 | *.mo 68 | *.pot 69 | 70 | # Django stuff: 71 | *.log 72 | local_settings.py 73 | db.sqlite3 74 | db.sqlite3-journal 75 | 76 | # Flask stuff: 77 | instance/ 78 | .webassets-cache 79 | 80 | # Scrapy stuff: 81 | .scrapy 82 | 83 | # Sphinx documentation 84 | docs/_build/ 85 | 86 | # PyBuilder 87 | .pybuilder/ 88 | target/ 89 | 90 | # Jupyter Notebook 91 | .ipynb_checkpoints 92 | 93 | # IPython 94 | profile_default/ 95 | ipython_config.py 96 | 97 | # pyenv 98 | # For a library or package, you might want to ignore these files since the code is 99 | # intended to run in multiple environments; otherwise, check them in: 100 | # .python-version 101 | 102 | # pipenv 103 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 104 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 105 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 106 | # install all needed dependencies. 107 | #Pipfile.lock 108 | 109 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 110 | __pypackages__/ 111 | 112 | # Celery stuff 113 | celerybeat-schedule 114 | celerybeat.pid 115 | 116 | # SageMath parsed files 117 | *.sage.py 118 | 119 | # Environments 120 | .env 121 | .venv 122 | env/ 123 | venv/ 124 | ENV/ 125 | env.bak/ 126 | venv.bak/ 127 | 128 | # Spyder project settings 129 | .spyderproject 130 | .spyproject 131 | 132 | # Rope project settings 133 | .ropeproject 134 | 135 | # mkdocs documentation 136 | /site 137 | 138 | # mypy 139 | .mypy_cache/ 140 | .dmypy.json 141 | dmypy.json 142 | 143 | # Pyre type checker 144 | .pyre/ 145 | 146 | # pytype static type analyzer 147 | .pytype/ 148 | 149 | # Cython debug symbols 150 | cython_debug/ -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/psf/black 3 | rev: 21.7b0 4 | hooks: 5 | - id: black 6 | language_version: python3.6 7 | 8 | - repo: https://github.com/pycqa/isort 9 | rev: 5.8.0 10 | hooks: 11 | - id: isort -------------------------------------------------------------------------------- /CONTRIBUTING.rst: -------------------------------------------------------------------------------- 1 | Ready to contribute? Here's how to set up for local development. 2 | 3 | 1. Fork the repo on Github. 4 | 2. Clone your fork locally. 5 | 3. Create the environment with:: 6 | 7 | $ conda env create -f environment.yml 8 | 9 | 4. Create a branch for local development:: 10 | 11 | $ git checkout -b name-of-your-bugfix-or-feature 12 | 13 | Now you can make your changes locally. 14 | 15 | 5. When you're done making changes, ensure that your code is formatted using black and isort:: 16 | 17 | $ black . 18 | $ isort . 19 | 20 | you can add pre-commit hooks for both isort and black to make all formatting easier:: 21 | 22 | $ pip install pre-commit 23 | 24 | 6. Commit your changes and push your branch to GitHub and submit a pull request through the GitHub website. -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # cbf 2 | This repo serves as a toolkit for testing different algorithm for control barrier functions. 3 | 4 | **Status:** This repository is still under development, expecting new features/papers and a complete tutorial to explain it. Feel free to raise questions/suggestions through GitHub Issues, if you want to use the current version of this repository. Please watch and star for subscribing further updates which will be related to our latest preprints and published papers. 5 | 6 | ### Citing 7 | If you find this repository useful in your work, please consider citing following work: 8 | 9 | ``` 10 | @inproceedings{thirugnanam2022safety, 11 | title={Safety-Critical Control and Planning for Obstacle Avoidance between Polytopes with Control Barrier Functions}, 12 | author={Thirugnanam, Akshay and Zeng, Jun and Sreenath, Koushil}, 13 | booktitle={2022 IEEE International Conference on Robotics and Automation (ICRA)}, 14 | year={2022} 15 | } 16 | ``` 17 | 18 | ### Environments 19 | * Create your environment via `conda env create -f environment.yml`. The default conda environment name is `cbf`, and you could also choose that name with your own preferences by editing the .yml file. 20 | 21 | ### Maze navigation with duality-based obstacle avoidance 22 | This represents the implementation of the following paper: 23 | * A. Thirugnanam, J. Zeng, K. Sreenath. "Safety-Critical Control and Planning for Obstacle Avoidance between Polytopes with Control Barrier Functions." *2022 IEEE International Conference on Robotics and Automation (ICRA)*. [[arXiv]](https://arxiv.org/abs/2109.12313) [[Video]](https://youtu.be/2hKlihdERog) 24 | 25 | Run `python models/kinematic_car_test.py`. This simulates maze navigation (two maze setups) with duality-based obstacle avoidance (four robot shapes including rectangle, pentagon, triangle and l-shape) in the discrete-time domain. The animations and snapshots can be found in folder `animations` and `figures`. An example animation video can be generated as follows, 26 | 27 | https://user-images.githubusercontent.com/27001847/147999361-faf3557a-3c87-48ab-aa3a-8830b3d565d5.mp4 28 | 29 | ### Contributors 30 | Jun Zeng, Akshay Thirugnanam. 31 | -------------------------------------------------------------------------------- /animations/.gitignore: -------------------------------------------------------------------------------- 1 | *.gif 2 | *.mp4 -------------------------------------------------------------------------------- /control/dcbf_controller.py: -------------------------------------------------------------------------------- 1 | from control.dcbf_optimizer import NmpcDbcfOptimizer, NmpcDcbfOptimizerParam 2 | 3 | 4 | class NmpcDcbfController: 5 | # TODO: Refactor this class to inheritate from a general optimizer 6 | def __init__(self, dynamics=None, opt_param=None): 7 | # TODO: Rename self._param into self._opt_param to avoid ambiguity 8 | self._param = opt_param 9 | self._optimizer = NmpcDbcfOptimizer({}, {}, dynamics.forward_dynamics_opt(0.1)) 10 | 11 | def generate_control_input(self, system, global_path, local_trajectory, obstacles): 12 | self._optimizer.setup(self._param, system, local_trajectory, obstacles) 13 | self._opt_sol = self._optimizer.solve_nlp() 14 | return self._opt_sol.value(self._optimizer.variables["u"][:, 0]) 15 | 16 | def logging(self, logger): 17 | logger._xtrajs.append(self._opt_sol.value(self._optimizer.variables["x"]).T) 18 | logger._utrajs.append(self._opt_sol.value(self._optimizer.variables["u"]).T) 19 | -------------------------------------------------------------------------------- /control/dcbf_optimizer.py: -------------------------------------------------------------------------------- 1 | import datetime 2 | 3 | import casadi as ca 4 | import numpy as np 5 | 6 | from models.geometry_utils import * 7 | 8 | 9 | class NmpcDcbfOptimizerParam: 10 | def __init__(self): 11 | self.horizon = 11 12 | self.horizon_dcbf = 6 13 | self.mat_Q = np.diag([100.0, 100.0, 1.0, 1.0]) 14 | self.mat_R = np.diag([0.0, 0.0]) 15 | self.mat_Rold = np.diag([1.0, 1.0]) * 0.0 16 | self.mat_dR = np.diag([1.0, 1.0]) * 0.0 17 | self.gamma = 0.8 18 | self.pomega = 10.0 19 | self.margin_dist = 0.00 20 | self.terminal_weight = 10.0 21 | 22 | 23 | class NmpcDbcfOptimizer: 24 | def __init__(self, variables: dict, costs: dict, dynamics_opt): 25 | self.opti = None 26 | self.variables = variables 27 | self.costs = costs 28 | self.dynamics_opt = dynamics_opt 29 | self.solver_times = [] 30 | 31 | def set_state(self, state): 32 | self.state = state 33 | 34 | def initialize_variables(self, param): 35 | self.variables["x"] = self.opti.variable(4, param.horizon + 1) 36 | self.variables["u"] = self.opti.variable(2, param.horizon) 37 | 38 | def add_initial_condition_constraint(self): 39 | self.opti.subject_to(self.variables["x"][:, 0] == self.state._x) 40 | 41 | def add_input_constraint(self, param): 42 | # TODO: wrap params 43 | amin, amax = -0.5, 0.5 44 | omegamin, omegamax = -0.5, 0.5 45 | for i in range(param.horizon): 46 | # input constraints 47 | self.opti.subject_to(self.variables["u"][0, i] <= amax) 48 | self.opti.subject_to(amin <= self.variables["u"][0, i]) 49 | self.opti.subject_to(self.variables["u"][1, i] <= omegamax) 50 | self.opti.subject_to(omegamin <= self.variables["u"][1, i]) 51 | 52 | def add_input_derivative_constraint(self, param): 53 | # TODO: Remove this hardcoded function with timestep 54 | jerk_min, jerk_max = -1.0, 1.0 55 | omegadot_min, omegadot_max = -0.5, 0.5 56 | for i in range(param.horizon - 1): 57 | # input constraints 58 | self.opti.subject_to(self.variables["u"][0, i + 1] - self.variables["u"][0, i] <= jerk_max) 59 | self.opti.subject_to(self.variables["u"][0, i + 1] - self.variables["u"][0, i] >= jerk_min) 60 | self.opti.subject_to(self.variables["u"][1, i + 1] - self.variables["u"][1, i] <= omegadot_max) 61 | self.opti.subject_to(self.variables["u"][1, i + 1] - self.variables["u"][1, i] >= omegadot_min) 62 | self.opti.subject_to(self.variables["u"][0, 0] - self.state._u[0] <= jerk_max) 63 | self.opti.subject_to(self.variables["u"][0, 0] - self.state._u[0] >= jerk_min) 64 | self.opti.subject_to(self.variables["u"][1, 0] - self.state._u[1] <= omegadot_max) 65 | self.opti.subject_to(self.variables["u"][1, 0] - self.state._u[1] >= omegadot_min) 66 | 67 | def add_dynamics_constraint(self, param): 68 | for i in range(param.horizon): 69 | self.opti.subject_to( 70 | self.variables["x"][:, i + 1] == self.dynamics_opt(self.variables["x"][:, i], self.variables["u"][:, i]) 71 | ) 72 | 73 | def add_reference_trajectory_tracking_cost(self, param, reference_trajectory): 74 | self.costs["reference_trajectory_tracking"] = 0 75 | for i in range(param.horizon - 1): 76 | x_diff = self.variables["x"][:, i] - reference_trajectory[i, :] 77 | self.costs["reference_trajectory_tracking"] += ca.mtimes(x_diff.T, ca.mtimes(param.mat_Q, x_diff)) 78 | x_diff = self.variables["x"][:, -1] - reference_trajectory[-1, :] 79 | self.costs["reference_trajectory_tracking"] += param.terminal_weight * ca.mtimes( 80 | x_diff.T, ca.mtimes(param.mat_Q, x_diff) 81 | ) 82 | 83 | def add_input_stage_cost(self, param): 84 | self.costs["input_stage"] = 0 85 | for i in range(param.horizon): 86 | self.costs["input_stage"] += ca.mtimes( 87 | self.variables["u"][:, i].T, ca.mtimes(param.mat_R, self.variables["u"][:, i]) 88 | ) 89 | 90 | def add_prev_input_cost(self, param): 91 | self.costs["prev_input"] = 0 92 | self.costs["prev_input"] += ca.mtimes( 93 | (self.variables["u"][:, 0] - self.state._u).T, 94 | ca.mtimes(param.mat_Rold, (self.variables["u"][:, 0] - self.state._u)), 95 | ) 96 | 97 | def add_input_smoothness_cost(self, param): 98 | self.costs["input_smoothness"] = 0 99 | for i in range(param.horizon - 1): 100 | self.costs["input_smoothness"] += ca.mtimes( 101 | (self.variables["u"][:, i + 1] - self.variables["u"][:, i]).T, 102 | ca.mtimes(param.mat_dR, (self.variables["u"][:, i + 1] - self.variables["u"][:, i])), 103 | ) 104 | 105 | def add_point_to_convex_constraint(self, param, obs_geo, safe_dist): 106 | # get current value of cbf 107 | mat_A, vec_b = obs_geo.get_convex_rep() 108 | cbf_curr, lamb_curr = get_dist_point_to_region(self.state._x[0:2], mat_A, vec_b) 109 | # filter obstacle if it's still far away 110 | if cbf_curr > safe_dist: 111 | return 112 | # duality-cbf constraints 113 | lamb = self.opti.variable(mat_A.shape[0], param.horizon_dcbf) 114 | omega = self.opti.variable(param.horizon_dcbf, 1) 115 | for i in range(param.horizon_dcbf): 116 | self.opti.subject_to(lamb[:, i] >= 0) 117 | self.opti.subject_to( 118 | ca.mtimes((ca.mtimes(mat_A, self.variables["x"][0:2, i + 1]) - vec_b).T, lamb[:, i]) 119 | >= omega[i] * param.gamma ** (i + 1) * (cbf_curr - param.margin_dist) + param.margin_dist 120 | ) 121 | temp = ca.mtimes(mat_A.T, lamb[:, i]) 122 | self.opti.subject_to(ca.mtimes(temp.T, temp) <= 1) 123 | self.opti.subject_to(omega[i] >= 0) 124 | self.costs["decay_rate_relaxing"] += param.pomega * (omega[i] - 1) ** 2 125 | # warm start 126 | self.opti.set_initial(lamb[:, i], lamb_curr) 127 | self.opti.set_initial(omega[i], 0.1) 128 | 129 | def add_convex_to_convex_constraint(self, param, robot_geo, obs_geo, safe_dist): 130 | mat_A, vec_b = obs_geo.get_convex_rep() 131 | robot_G, robot_g = robot_geo.get_convex_rep() 132 | # get current value of cbf 133 | cbf_curr, lamb_curr, mu_curr = get_dist_region_to_region( 134 | mat_A, 135 | vec_b, 136 | np.dot(robot_G, self.state.rotation().T), 137 | np.dot(np.dot(robot_G, self.state.rotation().T), self.state.translation()) + robot_g, 138 | ) 139 | # filter obstacle if it's still far away 140 | if cbf_curr > safe_dist: 141 | return 142 | # duality-cbf constraints 143 | lamb = self.opti.variable(mat_A.shape[0], param.horizon_dcbf) 144 | mu = self.opti.variable(robot_G.shape[0], param.horizon_dcbf) 145 | omega = self.opti.variable(param.horizon, 1) 146 | for i in range(param.horizon_dcbf): 147 | robot_R = ca.hcat( 148 | [ 149 | ca.vcat( 150 | [ 151 | ca.cos(self.variables["x"][3, i + 1]), 152 | ca.sin(self.variables["x"][3, i + 1]), 153 | ] 154 | ), 155 | ca.vcat( 156 | [ 157 | -ca.sin(self.variables["x"][3, i + 1]), 158 | ca.cos(self.variables["x"][3, i + 1]), 159 | ] 160 | ), 161 | ] 162 | ) 163 | robot_T = self.variables["x"][0:2, i + 1] 164 | self.opti.subject_to(lamb[:, i] >= 0) 165 | self.opti.subject_to(mu[:, i] >= 0) 166 | self.opti.subject_to( 167 | -ca.mtimes(robot_g.T, mu[:, i]) + ca.mtimes((ca.mtimes(mat_A, robot_T) - vec_b).T, lamb[:, i]) 168 | >= omega[i] * param.gamma ** (i + 1) * (cbf_curr - param.margin_dist) + param.margin_dist 169 | ) 170 | self.opti.subject_to( 171 | ca.mtimes(robot_G.T, mu[:, i]) + ca.mtimes(ca.mtimes(robot_R.T, mat_A.T), lamb[:, i]) == 0 172 | ) 173 | temp = ca.mtimes(mat_A.T, lamb[:, i]) 174 | self.opti.subject_to(ca.mtimes(temp.T, temp) <= 1) 175 | self.opti.subject_to(omega[i] >= 0) 176 | self.costs["decay_rate_relaxing"] += param.pomega * (omega[i] - 1) ** 2 177 | # warm start 178 | self.opti.set_initial(lamb[:, i], lamb_curr) 179 | self.opti.set_initial(mu[:, i], mu_curr) 180 | self.opti.set_initial(omega[i], 0.1) 181 | 182 | def add_obstacle_avoidance_constraint(self, param, system, obstacles_geo): 183 | self.costs["decay_rate_relaxing"] = 0 184 | # TODO: wrap params 185 | # TODO: move safe dist inside attribute `system` 186 | safe_dist = system._dynamics.safe_dist(system._state._x, 0.1, -1.0, 1.0, param.margin_dist) 187 | robot_components = system._geometry.equiv_rep() 188 | for obs_geo in obstacles_geo: 189 | for robot_comp in robot_components: 190 | # TODO: need to add case for `add_point_convex_constraint()` 191 | if isinstance(robot_comp, ConvexRegion2D): 192 | self.add_convex_to_convex_constraint(param, robot_comp, obs_geo, safe_dist) 193 | else: 194 | raise NotImplementedError() 195 | 196 | def add_warm_start(self, param, system): 197 | # TODO: wrap params 198 | x_ws, u_ws = system._dynamics.nominal_safe_controller(self.state._x, 0.1, -1.0, 1.0) 199 | for i in range(param.horizon): 200 | self.opti.set_initial(self.variables["x"][:, i + 1], x_ws) 201 | self.opti.set_initial(self.variables["u"][:, i], u_ws) 202 | 203 | def setup(self, param, system, reference_trajectory, obstacles): 204 | self.set_state(system._state) 205 | self.opti = ca.Opti() 206 | self.initialize_variables(param) 207 | self.add_initial_condition_constraint() 208 | self.add_input_constraint(param) 209 | # self.add_input_derivative_constraint(param) 210 | self.add_dynamics_constraint(param) 211 | self.add_reference_trajectory_tracking_cost(param, reference_trajectory) 212 | self.add_input_stage_cost(param) 213 | self.add_prev_input_cost(param) 214 | self.add_input_smoothness_cost(param) 215 | self.add_obstacle_avoidance_constraint(param, system, obstacles) 216 | self.add_warm_start(param, system) 217 | 218 | def solve_nlp(self): 219 | cost = 0 220 | for cost_name in self.costs: 221 | cost += self.costs[cost_name] 222 | self.opti.minimize(cost) 223 | option = {"verbose": False, "ipopt.print_level": 0, "print_time": 0} 224 | start_timer = datetime.datetime.now() 225 | self.opti.solver("ipopt", option) 226 | opt_sol = self.opti.solve() 227 | end_timer = datetime.datetime.now() 228 | delta_timer = end_timer - start_timer 229 | self.solver_times.append(delta_timer.total_seconds()) 230 | print("solver time: ", delta_timer.total_seconds()) 231 | return opt_sol 232 | -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: cbf 2 | channels: 3 | - defaults 4 | dependencies: 5 | - python=3.6 6 | - pip: 7 | - -r ./requirements.txt -------------------------------------------------------------------------------- /figures/.gitignore: -------------------------------------------------------------------------------- 1 | *.eps 2 | *.png -------------------------------------------------------------------------------- /models/dubin_car.py: -------------------------------------------------------------------------------- 1 | import datetime 2 | 3 | import matplotlib.patches as patches 4 | 5 | from models.geometry_utils import * 6 | from sim.simulation import * 7 | 8 | 9 | class DubinCarDynamics: 10 | @staticmethod 11 | def forward_dynamics(x, u, timestep): 12 | """Return updated state in a form of `np.ndnumpy`""" 13 | x_next = np.ndarray(shape=(4,), dtype=float) 14 | x_next[0] = x[0] + x[2] * math.cos(x[3]) * timestep 15 | x_next[1] = x[1] + x[2] * math.sin(x[3]) * timestep 16 | x_next[2] = x[2] + u[0] * timestep 17 | x_next[3] = x[3] + u[1] * timestep 18 | return x_next 19 | 20 | @staticmethod 21 | def forward_dynamics_opt(timestep): 22 | """Return updated state in a form of `ca.SX`""" 23 | x_symbol = ca.SX.sym("x", 4) 24 | u_symbol = ca.SX.sym("u", 2) 25 | x_symbol_next = x_symbol[0] + x_symbol[2] * ca.cos(x_symbol[3]) * timestep 26 | y_symbol_next = x_symbol[1] + x_symbol[2] * ca.sin(x_symbol[3]) * timestep 27 | v_symbol_next = x_symbol[2] + u_symbol[0] * timestep 28 | theta_symbol_next = x_symbol[3] + u_symbol[1] * timestep 29 | state_symbol_next = ca.vertcat(x_symbol_next, y_symbol_next, v_symbol_next, theta_symbol_next) 30 | return ca.Function("dubin_car_dynamics", [x_symbol, u_symbol], [state_symbol_next]) 31 | 32 | @staticmethod 33 | def nominal_safe_controller(x, timestep, amin, amax): 34 | """Return updated state using nominal safe controller in a form of `np.ndnumpy`""" 35 | u_nom = np.zeros(shape=(2,)) 36 | u_nom[0] = np.clip(-x[2] / timestep, amin, amax) 37 | return DubinCarDynamics.forward_dynamics(x, u_nom, timestep), u_nom 38 | 39 | @staticmethod 40 | def safe_dist(x, timestep, amin, amax, dist_margin): 41 | """Return a safe distance outside which to ignore obstacles""" 42 | # TODO: wrap params 43 | safe_ratio = 1.25 44 | brake_min_dist = (abs(x[2]) + amax * timestep) ** 2 / (2 * amax) + dist_margin 45 | return safe_ratio * brake_min_dist + abs(x[2]) * timestep + 0.5 * amax * timestep ** 2 46 | 47 | 48 | class DubinCarGeometry: 49 | def __init__(self, length, width): 50 | self._length = length 51 | self._width = width 52 | self._region = RectangleRegion(-length / 2, length / 2, -width / 2, width / 2) 53 | 54 | def equiv_rep(self): 55 | return [self._region] 56 | 57 | def get_plot_patch(self, state): 58 | length, width = self._length, self._width 59 | x, y, theta = state[0], state[1], state[3] 60 | vertices = np.array( 61 | [ 62 | [ 63 | x + length / 2 * np.cos(theta) - width / 2 * np.sin(theta), 64 | y + length / 2 * np.sin(theta) + width / 2 * np.cos(theta), 65 | ], 66 | [ 67 | x + length / 2 * np.cos(theta) + width / 2 * np.sin(theta), 68 | y + length / 2 * np.sin(theta) - width / 2 * np.cos(theta), 69 | ], 70 | [ 71 | x - length / 2 * np.cos(theta) + width / 2 * np.sin(theta), 72 | y - length / 2 * np.sin(theta) - width / 2 * np.cos(theta), 73 | ], 74 | [ 75 | x - length / 2 * np.cos(theta) - width / 2 * np.sin(theta), 76 | y - length / 2 * np.sin(theta) + width / 2 * np.cos(theta), 77 | ], 78 | ] 79 | ) 80 | return patches.Polygon(vertices, alpha=1.0, closed=True, fc="None", ec="tab:brown") 81 | 82 | 83 | class DubinCarStates: 84 | def __init__(self, x, u=np.array([0.0, 0.0])): 85 | self._x = x 86 | self._u = u 87 | 88 | def translation(self): 89 | return np.array([[self._x[0]], [self._x[1]]]) 90 | 91 | def rotation(self): 92 | return np.array( 93 | [ 94 | [math.cos(self._x[3]), -math.sin(self._x[3])], 95 | [math.sin(self._x[3]), math.cos(self._x[3])], 96 | ] 97 | ) 98 | 99 | 100 | class DubinCarSystem(System): 101 | def get_state(self): 102 | return self._state._x 103 | 104 | def update(self, unew): 105 | xnew = self._dynamics.forward_dynamics(self.get_state(), unew, 0.1) 106 | self._state._x = xnew 107 | self._time += 0.1 108 | 109 | def logging(self, logger): 110 | logger._xs.append(self._state._x) 111 | logger._us.append(self._state._u) 112 | -------------------------------------------------------------------------------- /models/dubin_car_test.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import matplotlib.patches as patches 4 | import matplotlib.pyplot as plt 5 | from matplotlib import animation 6 | 7 | from control.dcbf_controller import NmpcDcbfController 8 | from control.dcbf_optimizer import NmpcDcbfOptimizerParam 9 | from models.geometry_utils import * 10 | from planning.path_generator.search_path_generator import ( 11 | AstarLoSPathGenerator, 12 | AstarPathGenerator, 13 | ThetaStarPathGenerator, 14 | ) 15 | from planning.trajectory_generator.constant_speed_generator import ( 16 | ConstantSpeedTrajectoryGenerator, 17 | ) 18 | from sim.simulation import SingleAgentSimulation 19 | 20 | 21 | def plot_world(simulation): 22 | # TODO: make this plotting function general applicable to different systems 23 | fig, ax = plt.subplots() 24 | plt.axis("equal") 25 | global_paths = simulation._robot._global_planner_logger._paths 26 | global_path = global_paths[0] 27 | ax.plot(global_path[:, 0], global_path[:, 1], "bo--", linewidth=1, markersize=4) 28 | closedloop_traj = np.vstack(simulation._robot._system_logger._xs) 29 | ax.plot(closedloop_traj[:, 0], closedloop_traj[:, 1], "k-", linewidth=2, markersize=4) 30 | for obs in simulation._obstacles: 31 | obs_patch = obs.get_plot_patch() 32 | ax.add_patch(obs_patch) 33 | plt.savefig("figures/world.eps", format="eps", dpi=1000, pad_inches=0) 34 | plt.savefig("figures/world.png", format="png", dpi=1000, pad_inches=0) 35 | 36 | 37 | def animate_world(simulation): 38 | # TODO: make this plotting function general applicable to different systems 39 | fig, ax = plt.subplots() 40 | plt.axis("equal") 41 | global_paths = simulation._robot._global_planner_logger._paths 42 | global_path = global_paths[0] 43 | ax.plot(global_path[:, 0], global_path[:, 1], "bo--", linewidth=1, markersize=4) 44 | 45 | local_paths = simulation._robot._local_planner_logger._trajs 46 | local_path = local_paths[0] 47 | (reference_traj_line,) = ax.plot(local_path[:, 0], local_path[:, 1]) 48 | 49 | optimized_trajs = simulation._robot._controller_logger._xtrajs 50 | optimized_traj = optimized_trajs[0] 51 | (optimized_traj_line,) = ax.plot(optimized_traj[:, 0], optimized_traj[:, 1]) 52 | 53 | closedloop_traj = np.vstack(simulation._robot._system_logger._xs) 54 | ax.plot(closedloop_traj[:, 0], closedloop_traj[:, 1], "k-", linewidth=2, markersize=4) 55 | for obs in simulation._obstacles: 56 | obs_patch = obs.get_plot_patch() 57 | ax.add_patch(obs_patch) 58 | 59 | robot_patch = patches.Polygon(np.zeros((1, 2)), alpha=1.0, closed=True, fc="None", ec="tab:brown") 60 | ax.add_patch(robot_patch) 61 | 62 | def update(index): 63 | local_path = local_paths[index] 64 | reference_traj_line.set_data(local_path[:, 0], local_path[:, 1]) 65 | optimized_traj = optimized_trajs[index] 66 | optimized_traj_line.set_data(optimized_traj[:, 0], optimized_traj[:, 1]) 67 | polygon_patch_next = simulation._robot._system._geometry.get_plot_patch(closedloop_traj[index, :]) 68 | robot_patch.set_xy(polygon_patch_next.get_xy()) 69 | 70 | anim = animation.FuncAnimation(fig, update, frames=len(closedloop_traj), interval=1000 * 0.1) 71 | anim.save("animations/world.mp4", dpi=300, writer=animation.writers["ffmpeg"](fps=60)) 72 | 73 | 74 | def dubin_car_system_test(): 75 | sys = DubinCarSystem( 76 | state=DubinCarStates(x=np.array([0.0, 0.2, 0.0, 0.0])), 77 | geometry=DubinCarGeometry(0.08, 0.14), 78 | dynamics=DubinCarDynamics(), 79 | ) 80 | sys.update(np.array([0.0, 0.0])) 81 | print(sys.get_state()) 82 | 83 | 84 | def dubin_car_planner_test(): 85 | start_pos, goal_pos, grid, obstacles = create_env("s_path") 86 | sys = DubinCarSystem( 87 | state=DubinCarStates(x=np.block([start_pos[:2], np.array([0.0, start_pos[2]])])), 88 | geometry=DubinCarGeometry(0.08, 0.14), 89 | dynamics=DubinCarDynamics(), 90 | ) 91 | # Reduce margin (radius) for tighter corners 92 | global_path_margin = 0.05 93 | global_planner = AstarPathGenerator(grid, quad=False, margin=global_path_margin) 94 | global_path = global_planner.generate_path(sys, obstacles, goal_pos) 95 | local_planner = ConstantSpeedTrajectoryGenerator() 96 | local_trajectory = local_planner.generate_trajectory(sys, global_path) 97 | print(local_trajectory) 98 | 99 | 100 | def dubin_car_controller_test(): 101 | start_pos, goal_pos, grid, obstacles = create_env("s_path") 102 | sys = DubinCarSystem( 103 | state=DubinCarStates(x=np.block([start_pos[:2], np.array([0.0, start_pos[2]])])), 104 | geometry=DubinCarGeometry(0.08, 0.14), 105 | dynamics=DubinCarDynamics(), 106 | ) 107 | # Reduce margin (radius) for tighter corners 108 | global_path_margin = 0.05 109 | global_planner = AstarPathGenerator(grid, quad=False, margin=global_path_margin) 110 | global_path = global_planner.generate_path(sys, obstacles, goal_pos) 111 | local_planner = ConstantSpeedTrajectoryGenerator() 112 | local_trajectory = local_planner.generate_trajectory(sys, global_path) 113 | controller = NmpcDcbfController(dynamics=DubinCarDynamics(), opt_param=NmpcDcbfOptimizerParam()) 114 | action = controller.generate_control_input(sys, global_path, local_trajectory, obstacles) 115 | 116 | 117 | def dubin_car_simulation_test(): 118 | start_pos, goal_pos, grid, obstacles = create_env("maze") 119 | robot = Robot( 120 | DubinCarSystem( 121 | state=DubinCarStates(x=np.block([start_pos[:2], np.array([0.0, start_pos[2]])])), 122 | geometry=DubinCarGeometry(0.14, 0.08), 123 | dynamics=DubinCarDynamics(), 124 | ) 125 | ) 126 | # Reduce margin (radius) for tighter corners 127 | global_path_margin = 0.07 128 | robot.set_global_planner(ThetaStarPathGenerator(grid, quad=False, margin=global_path_margin)) 129 | robot.set_local_planner(ConstantSpeedTrajectoryGenerator()) 130 | robot.set_controller(NmpcDcbfController(dynamics=DubinCarDynamics(), opt_param=NmpcDcbfOptimizerParam())) 131 | sim = SingleAgentSimulation(robot, obstacles, goal_pos) 132 | sim.run_navigation(40.0) 133 | plot_world(sim) 134 | animate_world(sim) 135 | 136 | 137 | def create_env(env_type): 138 | if env_type == "s_path": 139 | s = 1.0 # scale of environment 140 | start = np.array([0.0 * s, 0.2 * s, 0.0]) 141 | goal = np.array([1.0 * s, 0.8 * s]) 142 | bounds = ((-0.2 * s, 0.0 * s), (1.2 * s, 1.2 * s)) 143 | cell_size = 0.05 * s 144 | grid = (bounds, cell_size) 145 | obstacles = [] 146 | obstacles.append(RectangleRegion(0.0 * s, 1.0 * s, 0.9 * s, 1.0 * s)) 147 | obstacles.append(RectangleRegion(0.0 * s, 0.4 * s, 0.4 * s, 1.0 * s)) 148 | obstacles.append(RectangleRegion(0.6 * s, 1.0 * s, 0.0 * s, 0.7 * s)) 149 | return start, goal, grid, obstacles 150 | elif env_type == "maze": 151 | s = 0.2 # scale of environment 152 | start = np.array([0.5 * s, 5.5 * s, -math.pi / 2.0]) 153 | goal = np.array([12.5 * s, 0.5 * s]) 154 | bounds = ((0.0 * s, 0.0 * s), (13.0 * s, 6.0 * s)) 155 | cell_size = 0.25 * s 156 | grid = (bounds, cell_size) 157 | obstacles = [] 158 | obstacles.append(RectangleRegion(0.0 * s, 3.0 * s, 0.0 * s, 3.0 * s)) 159 | obstacles.append(RectangleRegion(1.0 * s, 2.0 * s, 4.0 * s, 6.0 * s)) 160 | obstacles.append(RectangleRegion(2.0 * s, 6.0 * s, 5.0 * s, 6.0 * s)) 161 | obstacles.append(RectangleRegion(6.0 * s, 7.0 * s, 4.0 * s, 6.0 * s)) 162 | obstacles.append(RectangleRegion(4.0 * s, 5.0 * s, 0.0 * s, 4.0 * s)) 163 | obstacles.append(RectangleRegion(5.0 * s, 7.0 * s, 2.0 * s, 3.0 * s)) 164 | obstacles.append(RectangleRegion(6.0 * s, 9.0 * s, 1.0 * s, 2.0 * s)) 165 | obstacles.append(RectangleRegion(8.0 * s, 9.0 * s, 2.0 * s, 4.0 * s)) 166 | obstacles.append(RectangleRegion(9.0 * s, 12.0 * s, 3.0 * s, 4.0 * s)) 167 | obstacles.append(RectangleRegion(11.0 * s, 12.0 * s, 4.0 * s, 5.0 * s)) 168 | obstacles.append(RectangleRegion(8.0 * s, 10.0 * s, 5.0 * s, 6.0 * s)) 169 | obstacles.append(RectangleRegion(10.0 * s, 11.0 * s, 0.0 * s, 2.0 * s)) 170 | obstacles.append(RectangleRegion(12.0 * s, 13.0 * s, 1.0 * s, 2.0 * s)) 171 | obstacles.append(RectangleRegion(0.0 * s, 13.0 * s, 6.0 * s, 7.0 * s)) 172 | obstacles.append(RectangleRegion(-1.0 * s, 0.0 * s, 0.0 * s, 6.0 * s)) 173 | obstacles.append(RectangleRegion(0.0 * s, 13.0 * s, -1.0 * s, 0.0 * s)) 174 | obstacles.append(RectangleRegion(13.0 * s, 14.0 * s, 0.0 * s, 6.0 * s)) 175 | return start, goal, grid, obstacles 176 | 177 | 178 | if __name__ == "__main__": 179 | dubin_car_system_test() 180 | dubin_car_planner_test() 181 | dubin_car_controller_test() 182 | dubin_car_simulation_test() 183 | -------------------------------------------------------------------------------- /models/geometry_utils.py: -------------------------------------------------------------------------------- 1 | from abc import ABCMeta 2 | 3 | import casadi as ca 4 | import matplotlib.patches as patches 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | import polytope as pt 8 | 9 | 10 | class ConvexRegion2D: 11 | __metaclass__ = ABCMeta 12 | 13 | def get_convex_rep(self): 14 | raise NotImplementedError() 15 | 16 | def get_plot_patch(self): 17 | raise NotImplementedError() 18 | 19 | 20 | class RectangleRegion(ConvexRegion2D): 21 | """[Rectangle shape]""" 22 | 23 | def __init__(self, left, right, down, up): 24 | self.left = left 25 | self.right = right 26 | self.down = down 27 | self.up = up 28 | 29 | def get_convex_rep(self): 30 | mat_A = np.array([[-1, 0], [0, -1], [1, 0], [0, 1]]) 31 | vec_b = np.array([[-self.left], [-self.down], [self.right], [self.up]]) 32 | return mat_A, vec_b 33 | 34 | def get_plot_patch(self): 35 | return patches.Rectangle( 36 | (self.left, self.down), 37 | self.right - self.left, 38 | self.up - self.down, 39 | linewidth=1, 40 | edgecolor="k", 41 | facecolor="r", 42 | ) 43 | 44 | 45 | class PolytopeRegion(ConvexRegion2D): 46 | """[Genral polytope shape]""" 47 | 48 | def __init__(self, mat_A, vec_b): 49 | self.mat_A = mat_A 50 | self.vec_b = vec_b 51 | self.points = pt.extreme(pt.Polytope(mat_A, vec_b)) 52 | 53 | @classmethod 54 | def convex_hull(self, points): 55 | """Convex hull of N points in d dimensions as Nxd numpy array""" 56 | P = pt.reduce(pt.qhull(points)) 57 | return PolytopeRegion(P.A, P.b) 58 | 59 | def get_convex_rep(self): 60 | # TODO: Move this change into constructor instead of API here 61 | return self.mat_A, self.vec_b.reshape(self.vec_b.shape[0], -1) 62 | 63 | def get_plot_patch(self): 64 | return patches.Polygon(self.points, closed=True, linewidth=1, edgecolor="k", facecolor="r") 65 | 66 | 67 | def get_dist_point_to_region(point, mat_A, vec_b): 68 | """Return distance between a point and a convex region""" 69 | opti = ca.Opti() 70 | # variables and cost 71 | point_in_region = opti.variable(mat_A.shape[-1], 1) 72 | cost = 0 73 | # constraints 74 | constraint = ca.mtimes(mat_A, point_in_region) <= vec_b 75 | opti.subject_to(constraint) 76 | dist_vec = point - point_in_region 77 | cost += ca.mtimes(dist_vec.T, dist_vec) 78 | # solve optimization 79 | opti.minimize(cost) 80 | option = {"verbose": False, "ipopt.print_level": 0, "print_time": 0} 81 | opti.solver("ipopt", option) 82 | opt_sol = opti.solve() 83 | # minimum distance & dual variables 84 | dist = opt_sol.value(ca.norm_2(dist_vec)) 85 | if dist > 0: 86 | lamb = opt_sol.value(opti.dual(constraint)) / (2 * dist) 87 | else: 88 | lamb = np.zeros(shape=(mat_A.shape[0],)) 89 | return dist, lamb 90 | 91 | 92 | def get_dist_region_to_region(mat_A1, vec_b1, mat_A2, vec_b2): 93 | opti = ca.Opti() 94 | # variables and cost 95 | point1 = opti.variable(mat_A1.shape[-1], 1) 96 | point2 = opti.variable(mat_A2.shape[-1], 1) 97 | cost = 0 98 | # constraints 99 | constraint1 = ca.mtimes(mat_A1, point1) <= vec_b1 100 | constraint2 = ca.mtimes(mat_A2, point2) <= vec_b2 101 | opti.subject_to(constraint1) 102 | opti.subject_to(constraint2) 103 | dist_vec = point1 - point2 104 | cost += ca.mtimes(dist_vec.T, dist_vec) 105 | # solve optimization 106 | opti.minimize(cost) 107 | option = {"verbose": False, "ipopt.print_level": 0, "print_time": 0} 108 | opti.solver("ipopt", option) 109 | opt_sol = opti.solve() 110 | # minimum distance & dual variables 111 | dist = opt_sol.value(ca.norm_2(dist_vec)) 112 | if dist > 0: 113 | lamb = opt_sol.value(opti.dual(constraint1)) / (2 * dist) 114 | mu = opt_sol.value(opti.dual(constraint2)) / (2 * dist) 115 | else: 116 | lamb = np.zeros(shape=(mat_A1.shape[0],)) 117 | mu = np.zeros(shape=(mat_A2.shape[0],)) 118 | return dist, lamb, mu 119 | -------------------------------------------------------------------------------- /models/geometry_utils_test.py: -------------------------------------------------------------------------------- 1 | from models.geometry_utils import * 2 | 3 | # # PolytopeRegion test 4 | obs_1 = RectangleRegion(0.0, 1.0, 0.90, 1.0) 5 | A, b = obs_1.get_convex_rep() 6 | obs_2 = PolytopeRegion(A, b) 7 | obs_3 = PolytopeRegion.convex_hull(obs_2.points) 8 | print(obs_3.mat_A) 9 | print(obs_3.vec_b) 10 | print(all([all(obs_3.points[:, i] == obs_2.points[:, i]) for i in range(2)])) 11 | obs_4 = PolytopeRegion.convex_hull(np.array([[3, 0], [4, 1], [5, 3], [3.5, 5], [1, 2]])) 12 | print(obs_4.mat_A) 13 | print(obs_4.vec_b) 14 | fig, ax = plt.subplots() 15 | patch_2 = obs_2.get_plot_patch() 16 | patch_4 = obs_4.get_plot_patch() 17 | ax.add_patch(patch_2) 18 | ax.add_patch(patch_4) 19 | plt.xlim([-5, 10]) 20 | plt.ylim([-5, 10]) 21 | plt.show() 22 | 23 | 24 | # # dual variable test in distance functions 25 | # point_to_region 26 | obs_1 = RectangleRegion(0.0, 1.0, 0.90, 1.0) 27 | A, b = obs_1.get_convex_rep() 28 | point = np.array([[0.0], [0.0]]) 29 | dist_1, dual_1 = get_dist_point_to_region(point, A, b) 30 | print(A) 31 | print(dist_1) 32 | print(dual_1) 33 | # before normalization of dual in distance function 34 | # print(np.sqrt(-0.25*dual_1@A@A.T@dual_1 + dual_1@(A@point - b))) 35 | # after normalization of dual in distance function 36 | print(dual_1 @ (A @ point - b)) 37 | print(np.linalg.norm(dual_1 @ A), "\n") 38 | 39 | # region_to_region 40 | obs_2 = RectangleRegion(0.0, 1.0, -0.90, 0.0) 41 | C, d = obs_2.get_convex_rep() 42 | dist_2, dual_2, dual_3 = get_dist_region_to_region(A, b, C, d) 43 | print(C) 44 | print(dist_2) 45 | print(dual_2, " ", dual_3) 46 | # before normalization of dual in distance function 47 | # print(np.sqrt(-0.25*dual_2@A@A.T@dual_2 - dual_2@b - dual_3@d)) 48 | # after normalization of dual in distance function 49 | print(-dual_2 @ b - dual_3 @ d) 50 | print(np.linalg.norm(dual_2 @ A), " ", np.linalg.norm(dual_3 @ C), "\n") 51 | -------------------------------------------------------------------------------- /models/kinematic_car.py: -------------------------------------------------------------------------------- 1 | import datetime 2 | 3 | import matplotlib.patches as patches 4 | 5 | from models.geometry_utils import * 6 | from sim.simulation import * 7 | 8 | 9 | class KinematicCarDynamics: 10 | @staticmethod 11 | def forward_dynamics(x, u, timestep): 12 | """Return updated state in a form of `np.ndnumpy`""" 13 | l = 0.1 14 | x_next = np.ndarray(shape=(4,), dtype=float) 15 | x_next[0] = x[0] + x[2] * math.cos(x[3]) * timestep 16 | x_next[1] = x[1] + x[2] * math.sin(x[3]) * timestep 17 | x_next[2] = x[2] + u[0] * timestep 18 | x_next[3] = x[3] + x[2] * math.tan(u[1]) / l * timestep 19 | return x_next 20 | 21 | @staticmethod 22 | def forward_dynamics_opt(timestep): 23 | """Return updated state in a form of `ca.SX`""" 24 | l = 0.1 25 | x_symbol = ca.SX.sym("x", 4) 26 | u_symbol = ca.SX.sym("u", 2) 27 | x_symbol_next = x_symbol[0] + x_symbol[2] * ca.cos(x_symbol[3]) * timestep 28 | y_symbol_next = x_symbol[1] + x_symbol[2] * ca.sin(x_symbol[3]) * timestep 29 | v_symbol_next = x_symbol[2] + u_symbol[0] * timestep 30 | theta_symbol_next = x_symbol[3] + x_symbol[2] * ca.tan(u_symbol[1]) / l * timestep 31 | state_symbol_next = ca.vertcat(x_symbol_next, y_symbol_next, v_symbol_next, theta_symbol_next) 32 | return ca.Function("dubin_car_dynamics", [x_symbol, u_symbol], [state_symbol_next]) 33 | 34 | @staticmethod 35 | def nominal_safe_controller(x, timestep, amin, amax): 36 | """Return updated state using nominal safe controller in a form of `np.ndnumpy`""" 37 | u_nom = np.zeros(shape=(2,)) 38 | u_nom[0] = np.clip(-x[2] / timestep, amin, amax) 39 | return KinematicCarDynamics.forward_dynamics(x, u_nom, timestep), u_nom 40 | 41 | @staticmethod 42 | def safe_dist(x, timestep, amin, amax, dist_margin): 43 | """Return a safe distance outside which to ignore obstacles""" 44 | # TODO: wrap params 45 | safe_ratio = 1.25 46 | brake_min_dist = (abs(x[2]) + amax * timestep) ** 2 / (2 * amax) + dist_margin 47 | return safe_ratio * brake_min_dist + abs(x[2]) * timestep + 0.5 * amax * timestep ** 2 48 | 49 | 50 | class KinematicCarStates: 51 | def __init__(self, x, u=np.array([0.0, 0.0])): 52 | self._x = x 53 | self._u = u 54 | 55 | def translation(self): 56 | return np.array([[self._x[0]], [self._x[1]]]) 57 | 58 | def rotation(self): 59 | return np.array( 60 | [ 61 | [math.cos(self._x[3]), -math.sin(self._x[3])], 62 | [math.sin(self._x[3]), math.cos(self._x[3])], 63 | ] 64 | ) 65 | 66 | 67 | class KinematicCarRectangleGeometry: 68 | def __init__(self, length, width, rear_dist): 69 | self._length = length 70 | self._width = width 71 | self._rear_dist = rear_dist 72 | self._region = RectangleRegion((-length + rear_dist) / 2, (length + rear_dist) / 2, -width / 2, width / 2) 73 | 74 | def equiv_rep(self): 75 | return [self._region] 76 | 77 | def get_plot_patch(self, state, i=0, alpha=0.5): 78 | length, width, rear_dist = self._length, self._width, self._rear_dist 79 | x, y, theta = state[0], state[1], state[3] 80 | xc = x + (rear_dist / 2) * math.cos(theta) 81 | yc = y + (rear_dist / 2) * math.sin(theta) 82 | vertices = np.array( 83 | [ 84 | [ 85 | xc + length / 2 * np.cos(theta) - width / 2 * np.sin(theta), 86 | yc + length / 2 * np.sin(theta) + width / 2 * np.cos(theta), 87 | ], 88 | [ 89 | xc + length / 2 * np.cos(theta) + width / 2 * np.sin(theta), 90 | yc + length / 2 * np.sin(theta) - width / 2 * np.cos(theta), 91 | ], 92 | [ 93 | xc - length / 2 * np.cos(theta) + width / 2 * np.sin(theta), 94 | yc - length / 2 * np.sin(theta) - width / 2 * np.cos(theta), 95 | ], 96 | [ 97 | xc - length / 2 * np.cos(theta) - width / 2 * np.sin(theta), 98 | yc - length / 2 * np.sin(theta) + width / 2 * np.cos(theta), 99 | ], 100 | ] 101 | ) 102 | return patches.Polygon(vertices, alpha=alpha, closed=True, fc="tab:brown", ec="None", linewidth=0.5) 103 | 104 | 105 | class KinematicCarMultipleGeometry: 106 | def __init__(self): 107 | self._num_geometry = 0 108 | self._geometries = [] 109 | self._regions = [] 110 | 111 | def equiv_rep(self): 112 | return self._regions 113 | 114 | def add_geometry(self, geometry): 115 | self._geometries.append(geometry) 116 | self._regions.append(geometry._region) 117 | self._num_geometry += 1 118 | 119 | def get_plot_patch(self, state, region_idx, alpha=0.5): 120 | return self._geometries[region_idx].get_plot_patch(state, alpha) 121 | 122 | 123 | class KinematicCarTriangleGeometry: 124 | def __init__(self, vertex_points): 125 | self._vertex_points = vertex_points 126 | self._region = PolytopeRegion.convex_hull(vertex_points) 127 | 128 | def equiv_rep(self): 129 | return [self._region] 130 | 131 | def get_plot_patch(self, state, i=0, alpha=0.5): 132 | x, y, theta = state[0], state[1], state[3] 133 | mat_rotation = np.array([[math.cos(theta), -math.sin(theta)], [math.sin(theta), math.cos(theta)]]) 134 | vertices_points_curr = (mat_rotation @ self._vertex_points.T).T + np.array([x, y]) 135 | return patches.Polygon(vertices_points_curr, alpha=alpha, closed=True, fc="tab:brown", ec="None", linewidth=0.5) 136 | 137 | 138 | class KinematicCarPentagonGeometry: 139 | def __init__(self, vertex_points): 140 | self._vertex_points = vertex_points 141 | self._region = PolytopeRegion.convex_hull(vertex_points) 142 | 143 | def equiv_rep(self): 144 | return [self._region] 145 | 146 | def get_plot_patch(self, state, i=0, alpha=0.5): 147 | x, y, theta = state[0], state[1], state[3] 148 | mat_rotation = np.array([[math.cos(theta), -math.sin(theta)], [math.sin(theta), math.cos(theta)]]) 149 | vertices_points_curr = (mat_rotation @ self._vertex_points.T).T + np.array([x, y]) 150 | return patches.Polygon(vertices_points_curr, alpha=alpha, closed=True, fc="tab:brown", ec="None", linewidth=0.5) 151 | 152 | 153 | class KinematicCarSystem(System): 154 | def get_state(self): 155 | return self._state._x 156 | 157 | def update(self, unew): 158 | xnew = self._dynamics.forward_dynamics(self.get_state(), unew, 0.1) 159 | self._state._x = xnew 160 | self._time += 0.1 161 | 162 | def logging(self, logger): 163 | logger._xs.append(self._state._x) 164 | logger._us.append(self._state._u) 165 | -------------------------------------------------------------------------------- /models/kinematic_car_test.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import matplotlib.patches as patches 4 | import matplotlib.pyplot as plt 5 | from matplotlib import animation 6 | import matplotlib as mpl 7 | import statistics as st 8 | 9 | from control.dcbf_optimizer import NmpcDcbfOptimizerParam 10 | from control.dcbf_controller import NmpcDcbfController 11 | from models.dubin_car import ( 12 | DubinCarDynamics, 13 | DubinCarGeometry, 14 | DubinCarStates, 15 | DubinCarSystem, 16 | ) 17 | from models.geometry_utils import * 18 | from models.kinematic_car import ( 19 | KinematicCarDynamics, 20 | KinematicCarRectangleGeometry, 21 | KinematicCarMultipleGeometry, 22 | KinematicCarTriangleGeometry, 23 | KinematicCarPentagonGeometry, 24 | KinematicCarStates, 25 | KinematicCarSystem, 26 | ) 27 | from planning.path_generator.search_path_generator import ( 28 | AstarLoSPathGenerator, 29 | AstarPathGenerator, 30 | ThetaStarPathGenerator, 31 | ) 32 | from planning.trajectory_generator.constant_speed_generator import ( 33 | ConstantSpeedTrajectoryGenerator, 34 | ) 35 | from sim.simulation import Robot, SingleAgentSimulation 36 | 37 | 38 | def plot_world(simulation, snapshot_indexes, figure_name="world", local_traj_indexes=[], maze_type=None): 39 | # TODO: make this plotting function general applicable to different systems 40 | if maze_type == "maze": 41 | fig, ax = plt.subplots(figsize=(8.3, 5.0)) 42 | elif maze_type == "oblique_maze": 43 | fig, ax = plt.subplots(figsize=(6.7, 5.0)) 44 | # degrees_rot = 90 45 | # transform = mpl.transforms.Affine2D().rotate_deg(degrees_rot) + ax.transData 46 | # extract data 47 | global_paths = simulation._robot._global_planner_logger._paths 48 | global_path = global_paths[0] 49 | closedloop_traj = np.vstack(simulation._robot._system_logger._xs) 50 | local_paths = simulation._robot._local_planner_logger._trajs 51 | optimized_trajs = simulation._robot._controller_logger._xtrajs 52 | # plot robot 53 | for index in snapshot_indexes: 54 | for i in range(simulation._robot._system._geometry._num_geometry): 55 | polygon_patch = simulation._robot._system._geometry.get_plot_patch(closedloop_traj[index, :], i, 0.25) 56 | # polygon_patch.set_transform(transform) 57 | ax.add_patch(polygon_patch) 58 | # plot global reference 59 | ax.plot(global_path[:, 0], global_path[:, 1], "o--", color="grey", linewidth=1.5, markersize=2) 60 | # plot closed loop trajectory 61 | # ax.plot(closedloop_traj[:, 0], closedloop_traj[:, 1], "k-", linewidth=1, markersize=4, transform=transform) 62 | # plot obstacles 63 | for obs in simulation._obstacles: 64 | obs_patch = obs.get_plot_patch() 65 | # obs_patch.set_transform(transform) 66 | ax.add_patch(obs_patch) 67 | # plot local reference and local optimized trajectories 68 | for index in local_traj_indexes: 69 | local_path = local_paths[index] 70 | ax.plot(local_path[:, 0], local_path[:, 1], "-", color="blue", linewidth=3, markersize=4) 71 | optimized_traj = optimized_trajs[index] 72 | ax.plot( 73 | optimized_traj[:, 0], 74 | optimized_traj[:, 1], 75 | "-", 76 | color="gold", 77 | linewidth=3, 78 | markersize=4, 79 | ) 80 | # set figure properties 81 | plt.tight_layout() 82 | # save figure 83 | plt.savefig("figures/" + figure_name + ".eps", format="eps", dpi=500, pad_inches=0) 84 | plt.savefig("figures/" + figure_name + ".png", format="png", dpi=500, pad_inches=0) 85 | 86 | 87 | def animate_world(simulation, animation_name="world", maze_type=None): 88 | # TODO: make this plotting function general applicable to different systems 89 | if maze_type == "maze": 90 | fig, ax = plt.subplots(figsize=(8.3, 5.0)) 91 | elif maze_type == "oblique_maze": 92 | fig, ax = plt.subplots(figsize=(6.7, 5.0)) 93 | global_paths = simulation._robot._global_planner_logger._paths 94 | global_path = global_paths[0] 95 | ax.plot(global_path[:, 0], global_path[:, 1], "bo--", linewidth=1.5, markersize=4) 96 | 97 | local_paths = simulation._robot._local_planner_logger._trajs 98 | local_path = local_paths[0] 99 | (reference_traj_line,) = ax.plot(local_path[:, 0], local_path[:, 1], "-", color="blue", linewidth=3, markersize=4) 100 | 101 | optimized_trajs = simulation._robot._controller_logger._xtrajs 102 | optimized_traj = optimized_trajs[0] 103 | (optimized_traj_line,) = ax.plot( 104 | optimized_traj[:, 0], 105 | optimized_traj[:, 1], 106 | "-", 107 | color="gold", 108 | linewidth=3, 109 | markersize=4, 110 | ) 111 | 112 | closedloop_traj = np.vstack(simulation._robot._system_logger._xs) 113 | for obs in simulation._obstacles: 114 | obs_patch = obs.get_plot_patch() 115 | ax.add_patch(obs_patch) 116 | 117 | robot_patch = [] 118 | for i in range(simulation._robot._system._geometry._num_geometry): 119 | robot_patch.append(patches.Polygon(np.zeros((1, 2)), alpha=1.0, closed=True, fc="None", ec="tab:brown")) 120 | ax.add_patch(robot_patch[i]) 121 | plt.subplots_adjust(left=0.05, bottom=0.05, right=0.95, top=0.95) 122 | plt.tight_layout() 123 | 124 | def update(index): 125 | local_path = local_paths[index] 126 | reference_traj_line.set_data(local_path[:, 0], local_path[:, 1]) 127 | optimized_traj = optimized_trajs[index] 128 | optimized_traj_line.set_data(optimized_traj[:, 0], optimized_traj[:, 1]) 129 | # plt.xlabel(str(index)) 130 | for i in range(simulation._robot._system._geometry._num_geometry): 131 | polygon_patch_next = simulation._robot._system._geometry.get_plot_patch(closedloop_traj[index, :], i) 132 | robot_patch[i].set_xy(polygon_patch_next.get_xy()) 133 | if index == len(closedloop_traj) - 1: 134 | ax.plot(closedloop_traj[:, 0], closedloop_traj[:, 1], "k-", linewidth=3, markersize=4) 135 | 136 | anim = animation.FuncAnimation(fig, update, frames=len(closedloop_traj), interval=1000 * 0.1) 137 | anim.save("animations/" + animation_name + ".mp4", dpi=300, writer=animation.writers["ffmpeg"](fps=10)) 138 | 139 | 140 | def kinematic_car_triangle_simulation_test(): 141 | start_pos, goal_pos, grid, obstacles = create_env("maze") 142 | robot = Robot( 143 | KinematicCarSystem( 144 | state=KinematicCarStates(x=np.block([start_pos[:2], np.array([0.0, start_pos[2]])])), 145 | geometry=KinematicCarTriangleGeometry(np.array([[0.14, 0.00], [-0.03, 0.05], [-0.03, -0.05]])), 146 | dynamics=KinematicCarDynamics(), 147 | ) 148 | ) 149 | global_path_margin = 0.07 150 | robot.set_global_planner(ThetaStarPathGenerator(grid, quad=False, margin=global_path_margin)) 151 | robot.set_local_planner(ConstantSpeedTrajectoryGenerator()) 152 | robot.set_controller(NmpcDcbfController(dynamics=KinematicCarDynamics(), opt_param=NmpcDcbfOptimizerParam)) 153 | sim = SingleAgentSimulation(robot, obstacles, goal_pos) 154 | sim.run_navigation(20.0) 155 | plot_world(sim, np.arange(0, 200, 5), figure_name="triangle") 156 | animate_world(sim, animation_name="triangle") 157 | 158 | 159 | def kinematic_car_pentagon_simulation_test(): 160 | start_pos, goal_pos, grid, obstacles = create_env("maze") 161 | robot = Robot( 162 | KinematicCarSystem( 163 | state=KinematicCarStates(x=np.block([start_pos[:2], np.array([0.0, start_pos[2]])])), 164 | geometry=KinematicCarPentagonGeometry( 165 | np.array([[0.15, 0.00], [0.03, 0.05], [-0.01, 0.02], [-0.01, -0.02], [0.03, -0.05]]) 166 | ), 167 | dynamics=KinematicCarDynamics(), 168 | ) 169 | ) 170 | global_path_margin = 0.06 171 | robot.set_global_planner(ThetaStarPathGenerator(grid, quad=False, margin=global_path_margin)) 172 | robot.set_local_planner(ConstantSpeedTrajectoryGenerator()) 173 | robot.set_controller(NmpcDcbfController(dynamics=KinematicCarDynamics(), opt_param=NmpcDcbfOptimizerParam())) 174 | sim = SingleAgentSimulation(robot, obstacles, goal_pos) 175 | sim.run_navigation(20.0) 176 | plot_world(sim, np.arange(0, 200, 5), figure_name="pentagon") 177 | animate_world(sim, animation_name="pentagon") 178 | 179 | 180 | def kinematic_car_all_shapes_simulation_test(maze_type, robot_shape): 181 | start_pos, goal_pos, grid, obstacles = create_env(maze_type) 182 | geometry_regions = KinematicCarMultipleGeometry() 183 | 184 | if robot_shape == "rectangle": 185 | geometry_regions.add_geometry(KinematicCarRectangleGeometry(0.15, 0.06, 0.1)) 186 | if maze_type == "maze": 187 | robot_indexes = [0, 17, 24, 33, 39, 48, 58, 66, 76, 86, 92, 102, 112, 129] 188 | traj_indexes = [0, 24, 33, 48, 66, 92, 112] 189 | elif maze_type == "oblique_maze": 190 | robot_indexes = [1, 9, 22, 26, 34, 41, 47, 56, 64, 70, 79, 88, 93, 124, 131, 143, 164] 191 | traj_indexes = [1, 19, 47, 70, 93] 192 | if robot_shape == "pentagon": 193 | geometry_regions.add_geometry( 194 | KinematicCarTriangleGeometry( 195 | np.array([[0.15, 0.00], [0.03, 0.05], [-0.01, 0.02], [-0.01, -0.02], [0.03, -0.05]]) 196 | ) 197 | ) 198 | if maze_type == "maze": 199 | robot_indexes = [2, 15, 33, 43, 50, 61, 69, 81, 99, 111, 125, 130, 142, 164] 200 | traj_indexes = [2, 33, 50, 69, 99, 125, 164] 201 | elif maze_type == "oblique_maze": 202 | robot_indexes = [0, 11, 23, 41, 53, 62, 69, 78, 83, 92, 110, 113, 138, 151, 164, 185] 203 | traj_indexes = [0, 23, 62, 78, 92, 110, 151] 204 | if robot_shape == "triangle": 205 | geometry_regions.add_geometry( 206 | KinematicCarTriangleGeometry(0.75 * np.array([[0.14, 0.00], [-0.03, 0.05], [-0.03, -0.05]])) 207 | ) 208 | if maze_type == "maze": 209 | robot_indexes = [0, 10, 18, 27, 35, 40, 46, 51, 61, 72, 86, 91, 102, 113, 199] 210 | traj_indexes = [0, 18, 35, 46, 61, 86, 102] 211 | elif maze_type == "oblique_maze": 212 | robot_indexes = [0, 11, 17, 26, 32, 43, 50, 58, 74, 78, 110, 123, 168, 182, 199] 213 | traj_indexes = [0, 17, 32, 50, 74, 168] 214 | if robot_shape == "lshape": 215 | geometry_regions.add_geometry( 216 | KinematicCarTriangleGeometry(0.4 * np.array([[0, 0.1], [0.02, 0.08], [-0.2, -0.1], [-0.22, -0.08]])) 217 | ) 218 | geometry_regions.add_geometry( 219 | KinematicCarTriangleGeometry(0.4 * np.array([[0, 0.1], [-0.02, 0.08], [0.2, -0.1], [0.22, -0.08]])) 220 | ) 221 | if maze_type == "maze": 222 | robot_indexes = [3, 13, 27, 36, 46, 56, 65, 74, 85, 98, 114, 121, 224, 299] 223 | traj_indexes = [3, 27, 46, 74, 114, 224] 224 | elif maze_type == "oblique_maze": 225 | robot_indexes = [0, 12, 21, 32, 40, 49, 59, 67, 81, 94, 122, 129, 141, 177] 226 | traj_indexes = [0, 21, 40, 59, 81, 129] 227 | robot = Robot( 228 | KinematicCarSystem( 229 | state=KinematicCarStates(x=np.block([start_pos[:2], np.array([0.0, start_pos[2]])])), 230 | geometry=geometry_regions, 231 | dynamics=KinematicCarDynamics(), 232 | ) 233 | ) 234 | global_path_margin = 0.05 235 | robot.set_global_planner(AstarLoSPathGenerator(grid, quad=False, margin=global_path_margin)) 236 | robot.set_local_planner(ConstantSpeedTrajectoryGenerator()) 237 | opt_param = NmpcDcbfOptimizerParam() 238 | # TODO: Wrap these parameters 239 | if robot_shape == "rectangle" or robot_shape == "lshape": 240 | opt_param.terminal_weight = 10.0 241 | elif robot_shape == "pentagon": 242 | if maze_type == 1: 243 | opt_param.terminal_weight = 2.0 244 | elif maze_type == 2: 245 | opt_param.terminal_weight = 5.0 246 | elif robot_shape == "triangle": 247 | opt_param.terminal_weight = 2.0 248 | robot.set_controller(NmpcDcbfController(dynamics=KinematicCarDynamics(), opt_param=opt_param)) 249 | sim = SingleAgentSimulation(robot, obstacles, goal_pos) 250 | sim.run_navigation(30.0) 251 | print("median: ", st.median(robot._controller._optimizer.solver_times)) 252 | print("std: ", st.stdev(robot._controller._optimizer.solver_times)) 253 | print("min: ", min(robot._controller._optimizer.solver_times)) 254 | print("max: ", max(robot._controller._optimizer.solver_times)) 255 | print("Simulation finished.") 256 | name = robot_shape + "_" + maze_type 257 | plot_world(sim, robot_indexes, figure_name=name, local_traj_indexes=traj_indexes, maze_type=maze_type) 258 | animate_world(sim, animation_name=name, maze_type=maze_type) 259 | 260 | 261 | def create_env(env_type): 262 | if env_type == "s_path": 263 | s = 1.0 # scale of environment 264 | start = np.array([0.0 * s, 0.2 * s, 0.0]) 265 | goal = np.array([1.0 * s, 0.8 * s]) 266 | bounds = ((-0.2 * s, 0.0 * s), (1.2 * s, 1.2 * s)) 267 | cell_size = 0.05 * s 268 | grid = (bounds, cell_size) 269 | obstacles = [] 270 | obstacles.append(RectangleRegion(0.0 * s, 1.0 * s, 0.9 * s, 1.0 * s)) 271 | obstacles.append(RectangleRegion(0.0 * s, 0.4 * s, 0.4 * s, 1.0 * s)) 272 | obstacles.append(RectangleRegion(0.6 * s, 1.0 * s, 0.0 * s, 0.7 * s)) 273 | return start, goal, grid, obstacles 274 | elif env_type == "maze": 275 | s = 0.15 # scale of environment 276 | start = np.array([0.5 * s, 5.5 * s, -math.pi / 2.0]) 277 | goal = np.array([12.5 * s, 0.5 * s]) 278 | bounds = ((0.0 * s, 0.0 * s), (13.0 * s, 6.0 * s)) 279 | cell_size = 0.25 * s 280 | grid = (bounds, cell_size) 281 | obstacles = [] 282 | obstacles.append(RectangleRegion(0.0 * s, 3.0 * s, 0.0 * s, 3.0 * s)) 283 | obstacles.append(RectangleRegion(1.0 * s, 2.0 * s, 4.0 * s, 6.0 * s)) 284 | obstacles.append(RectangleRegion(2.0 * s, 6.0 * s, 5.0 * s, 6.0 * s)) 285 | obstacles.append(RectangleRegion(6.0 * s, 7.0 * s, 4.0 * s, 6.0 * s)) 286 | obstacles.append(RectangleRegion(4.0 * s, 5.0 * s, 0.0 * s, 4.0 * s)) 287 | obstacles.append(RectangleRegion(5.0 * s, 7.0 * s, 2.0 * s, 3.0 * s)) 288 | obstacles.append(RectangleRegion(6.0 * s, 9.0 * s, 1.0 * s, 2.0 * s)) 289 | obstacles.append(RectangleRegion(8.0 * s, 9.0 * s, 2.0 * s, 4.0 * s)) 290 | obstacles.append(RectangleRegion(9.0 * s, 12.0 * s, 3.0 * s, 4.0 * s)) 291 | obstacles.append(RectangleRegion(11.0 * s, 12.0 * s, 4.0 * s, 5.0 * s)) 292 | obstacles.append(RectangleRegion(8.0 * s, 10.0 * s, 5.0 * s, 6.0 * s)) 293 | obstacles.append(RectangleRegion(10.0 * s, 11.0 * s, 0.0 * s, 2.0 * s)) 294 | obstacles.append(RectangleRegion(12.0 * s, 13.0 * s, 1.0 * s, 2.0 * s)) 295 | obstacles.append(RectangleRegion(0.0 * s, 13.0 * s, 6.0 * s, 7.0 * s)) 296 | obstacles.append(RectangleRegion(-1.0 * s, 0.0 * s, -1.0 * s, 7.0 * s)) 297 | obstacles.append(RectangleRegion(0.0 * s, 13.0 * s, -1.0 * s, 0.0 * s)) 298 | obstacles.append(RectangleRegion(13.0 * s, 14.0 * s, -1.0 * s, 7.0 * s)) 299 | return start, goal, grid, obstacles 300 | elif env_type == "oblique_maze": 301 | s = 0.15 # scale of environemtn 302 | start = np.array([1.0 * s, 1.5 * s, 0.0]) 303 | goal = np.array([8.5 * s, 6.5 * s]) 304 | bounds = ((0.0 * s, 1.0 * s), (10.0 * s, 7.0 * s)) 305 | cell_size = 0.2 * s 306 | grid = (bounds, cell_size) 307 | obstacles = [] 308 | # TODO: Overload the constructor of RectangleRegion() 309 | obstacles.append(RectangleRegion(-1.0 * s, 0.0 * s, 0.0 * s, 8.0 * s)) 310 | obstacles.append(RectangleRegion(0.0 * s, 10.0 * s, 0.0 * s, 1.0 * s)) 311 | obstacles.append(RectangleRegion(0.0 * s, 8.0 * s, 7.0 * s, 8.0 * s)) 312 | obstacles.append(RectangleRegion(10.0 * s, 11.0 * s, 0.0 * s, 8.0 * s)) 313 | obstacles.append( 314 | PolytopeRegion.convex_hull(s * np.array([[0.0, 2.0], [1.25, 3.875], [2.875, 3.125], [2.5, 2.25]])) 315 | ) 316 | obstacles.append(PolytopeRegion.convex_hull(s * np.array([[1, 4.75], [0.0, 5.0], [0.875, 7], [1.875, 6.375]]))) 317 | obstacles.append( 318 | PolytopeRegion.convex_hull(s * np.array([[2.75, 1], [4.2, 3.25], [5.125, 3.75], [6.625, 2.5], [6.5, 1.0]])) 319 | ) 320 | obstacles.append(PolytopeRegion.convex_hull(s * np.array([[6.0, 7.0], [6, 6], [6.5, 7.0]]))) 321 | obstacles.append( 322 | PolytopeRegion.convex_hull( 323 | s * np.array([[2.375, 4.875], [2.875, 5.875], [4.5, 5.875], [4.75, 4], [3.375, 4]]) 324 | ) 325 | ) 326 | obstacles.append(PolytopeRegion.convex_hull(s * np.array([[6.75, 1.0], [7.25, 2.375], [8.5, 2.0], [8.5, 1.0]]))) 327 | obstacles.append(PolytopeRegion.convex_hull(s * np.array([[8.625, 1.0], [10.0, 2.5], [10.0, 1.0]]))) 328 | obstacles.append(PolytopeRegion.convex_hull(s * np.array([[10.0, 2.875], [9.5, 5.75], [10.0, 5.875]]))) 329 | obstacles.append( 330 | PolytopeRegion.convex_hull( 331 | s * np.array([[8.875, 3.125], [8.0, 5.5], [6.875, 6.375], [5.875, 5.875], [6.25, 4.375], [7.125, 3.5]]) 332 | ) 333 | ) 334 | return start, goal, grid, obstacles 335 | 336 | 337 | if __name__ == "__main__": 338 | # kinematic_car_triangle_simulation_test() 339 | # kinematic_car_pentagon_simulation_test() 340 | maze_types = ["maze", "oblique_maze"] 341 | robot_shapes = ["triangle", "rectangle", "pentagon", "lshape"] 342 | for maze_type in maze_types: 343 | for robot_shape in robot_shapes: 344 | kinematic_car_all_shapes_simulation_test(maze_type, robot_shape) 345 | -------------------------------------------------------------------------------- /planning/path_generator/astar.py: -------------------------------------------------------------------------------- 1 | import heapq as hq 2 | import math 3 | 4 | import numpy as np 5 | 6 | from models.geometry_utils import * 7 | 8 | 9 | # TODO: Generalize to 3D? 10 | class Node: 11 | def __init__(self, pos, parent=None, g_cost=math.inf, f_cost=math.inf): 12 | self.pos = pos 13 | self.parent = parent 14 | self.g_cost = g_cost 15 | self.f_cost = f_cost 16 | 17 | def __eq__(self, other): 18 | return all(self.pos == other.pos) 19 | 20 | def __le__(self, other): 21 | if self.pos[0] == other.pos[0]: 22 | return self.pos[1] <= other.pos[1] 23 | else: 24 | return self.pos[0] <= other.pos[0] 25 | 26 | def __lt__(self, other): 27 | if self.pos[0] == other.pos[0]: 28 | return self.pos[1] < other.pos[1] 29 | else: 30 | return self.pos[0] < other.pos[0] 31 | 32 | 33 | # TODO: Generalize to 3D 34 | class GridMap: 35 | # cell_size > 0; don't make cell_size too small 36 | def __init__(self, bounds=((0.0, 0.0), (10.0, 10.0)), cell_size=0.1, quad=True): 37 | self.bounds = bounds 38 | self.cell_size = cell_size 39 | self.quad = quad 40 | self.Nx = math.ceil((bounds[1][0] - bounds[0][0]) / cell_size) 41 | self.Ny = math.ceil((bounds[1][1] - bounds[0][1]) / cell_size) 42 | 43 | pos = lambda i, j: np.array([bounds[0][0] + (i + 0.5) * cell_size, bounds[0][1] + (j + 0.5) * cell_size]) 44 | self.grid = [[Node(pos(i, j)) for j in range(self.Ny)] for i in range(self.Nx)] 45 | 46 | # pos should be within bounds 47 | def set_node(self, pos, parent, g_cost, f_cost): 48 | i_x = math.floor((pos[0] - self.bounds[0][0]) / self.cell_size) 49 | i_y = math.floor((pos[1] - self.bounds[0][1]) / self.cell_size) 50 | self.grid[i_x][i_y].parent = parent 51 | self.grid[i_x][i_y].g_cost = g_cost 52 | self.grid[i_x][i_y].f_cost = f_cost 53 | return self.grid[i_x][i_y] 54 | 55 | # pos should be within bounds 56 | def get_node(self, pos): 57 | i_x = math.floor((pos[0] - self.bounds[0][0]) / self.cell_size) 58 | i_y = math.floor((pos[1] - self.bounds[0][1]) / self.cell_size) 59 | return self.grid[i_x][i_y] 60 | 61 | def get_neighbours(self, node): 62 | i_x = math.floor((node.pos[0] - self.bounds[0][0]) / self.cell_size) 63 | i_y = math.floor((node.pos[1] - self.bounds[0][1]) / self.cell_size) 64 | neighbours = [] 65 | for i in range(i_x - 1, i_x + 2): 66 | for j in range(i_y - 1, i_y + 2): 67 | if i == i_x and j == i_y: 68 | continue 69 | if self.quad: 70 | if 0 <= i <= self.Nx - 1 and 0 <= j <= self.Ny - 1 and abs(i - i_x) + abs(j - i_y) <= 1: 71 | neighbours.append(self.grid[i][j]) 72 | else: 73 | if 0 <= i <= self.Nx - 1 and 0 <= j <= self.Ny - 1: 74 | neighbours.append(self.grid[i][j]) 75 | return neighbours 76 | 77 | 78 | class GraphSearch: 79 | def __init__(self, graph, obstacles, margin): 80 | self.graph = graph 81 | self.obstacles = obstacles 82 | self.margin = margin 83 | 84 | def a_star(self, start_pos, goal_pos): 85 | h_cost = lambda pos: np.linalg.norm(goal_pos - pos) 86 | edge_cost = lambda n1, n2: np.linalg.norm(n1.pos - n2.pos) 87 | 88 | openSet = [] 89 | start = self.graph.set_node(start_pos, None, 0.0, h_cost(start_pos)) 90 | goal = self.graph.get_node(goal_pos) 91 | 92 | hq.heappush(openSet, (start.f_cost, start)) 93 | 94 | while len(openSet) > 0: 95 | current = openSet[0][1] 96 | if current == goal: 97 | return self.reconstruct_path(current) 98 | 99 | hq.heappop(openSet) 100 | 101 | for n in self.graph.get_neighbours(current): 102 | if self.check_collision(n.pos): 103 | continue 104 | g_score = current.g_cost + edge_cost(current, n) 105 | if g_score < n.g_cost: 106 | n_ = self.graph.set_node(n.pos, current, g_score, g_score + h_cost(n.pos)) 107 | if not n in (x[1] for x in openSet): 108 | hq.heappush(openSet, (n_.f_cost, n_)) 109 | return [] 110 | 111 | def theta_star(self, start_pos, goal_pos): 112 | h_cost = lambda pos: np.linalg.norm(goal_pos - pos) 113 | edge_cost = lambda n1, n2: np.linalg.norm(n1.pos - n2.pos) 114 | 115 | openSet = [] 116 | start = self.graph.set_node(start_pos, None, 0.0, h_cost(start_pos)) 117 | goal = self.graph.get_node(goal_pos) 118 | 119 | hq.heappush(openSet, (start.f_cost, start)) 120 | 121 | while len(openSet) > 0: 122 | current = openSet[0][1] 123 | if current == goal: 124 | return self.reconstruct_path(current) 125 | 126 | hq.heappop(openSet) 127 | 128 | for n in self.graph.get_neighbours(current): 129 | if self.check_collision(n.pos): 130 | continue 131 | if (not current.parent is None) and self.line_of_sight(current.parent, n): 132 | g_score = current.parent.g_cost + edge_cost(current.parent, n) 133 | if g_score < n.g_cost: 134 | n_ = self.graph.set_node(n.pos, current.parent, g_score, g_score + h_cost(n.pos)) 135 | # delete n from min-heap 136 | for i in range(len(openSet)): 137 | if openSet[i][1] == n: 138 | openSet[i] = openSet[-1] 139 | openSet.pop() 140 | if i < len(openSet): 141 | hq._siftup(openSet, i) 142 | hq._siftdown(openSet, 0, i) 143 | break 144 | hq.heappush(openSet, (n_.f_cost, n_)) 145 | else: 146 | g_score = current.g_cost + edge_cost(current, n) 147 | if g_score < n.g_cost: 148 | n_ = self.graph.set_node(n.pos, current, g_score, g_score + h_cost(n.pos)) 149 | # delete n from min-heap 150 | for i in range(len(openSet)): 151 | if openSet[i][1] == n: 152 | openSet[i] = openSet[-1] 153 | openSet.pop() 154 | if i < len(openSet): 155 | hq._siftup(openSet, i) 156 | hq._siftdown(openSet, 0, i) 157 | break 158 | hq.heappush(openSet, (n_.f_cost, n_)) 159 | return [] 160 | 161 | # TODO: optimize 162 | def line_of_sight(self, n1, n2): 163 | e = self.graph.cell_size 164 | div = np.linalg.norm(n2.pos - n1.pos) / e 165 | for i in range(1, math.floor(div) + 1): 166 | if self.check_collision((n2.pos * i + n1.pos * (div - i)) / div): 167 | return False 168 | return True 169 | 170 | def check_collision(self, pos): 171 | for o in self.obstacles: 172 | A, b = o.get_convex_rep() 173 | b = b.reshape((len(b),)) 174 | if all(A @ pos - b - self.margin * np.linalg.norm(A, axis=1) <= 0): 175 | return True 176 | return False 177 | 178 | def reconstruct_path(self, node): 179 | path = [node] 180 | while not node.parent is None: 181 | node = node.parent 182 | path.append(node) 183 | return [path[len(path) - i - 1] for i in range(len(path))] 184 | 185 | def reduce_path(self, path): 186 | red_path = [] 187 | if len(path) > 1: 188 | for i in range(1, len(path)): 189 | if (not path[i].parent.parent is None) and self.line_of_sight(path[i], path[i].parent.parent): 190 | path[i].parent = path[i].parent.parent 191 | else: 192 | red_path.append(path[i].parent) 193 | red_path.append(path[-1]) 194 | return red_path 195 | -------------------------------------------------------------------------------- /planning/path_generator/astar_test.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import matplotlib.pyplot as plt 4 | 5 | from planning.path_generator.astar import * 6 | 7 | # # Node test 8 | pos = np.array([1.0, 1.0]) 9 | node_1 = Node(pos=pos) 10 | node_2 = Node(pos=2 * pos, parent=node_1, f_cost=1.0) 11 | print(node_1.parent) 12 | print(node_2.parent.pos) 13 | print(node_2.g_cost) 14 | print(node_1 == node_2, "\n") 15 | 16 | 17 | # # GridMap test 18 | grid_1 = GridMap() 19 | grid_2 = GridMap(bounds=((1.0, 1.0), (2.0, 1.3)), cell_size=0.1, quad=False) 20 | print(grid_1.Nx, " ", grid_1.Ny) 21 | print(grid_2.Nx, " ", grid_2.Ny) 22 | print(len(grid_2.grid[0])) 23 | grid_2_pos = [[grid_2.grid[i][j].pos for i in range(grid_2.Nx)] for j in range(grid_2.Ny)] 24 | print(grid_2_pos) 25 | grid_2_node = grid_2.set_node( 26 | grid_2_pos[0][0], parent=grid_2.grid[grid_2.Nx - 1][grid_2.Ny - 1], g_cost=1.0, f_cost=1.0 27 | ) 28 | print(grid_2.get_node(grid_2_pos[0][0]) == grid_2_node) 29 | print([n.pos for n in grid_1.get_neighbours(grid_1.grid[0][0])]) 30 | print([n.pos for n in grid_1.get_neighbours(grid_1.grid[grid_1.Nx // 2][grid_1.Ny // 2])]) 31 | print([n.pos for n in grid_1.get_neighbours(grid_1.grid[grid_1.Nx - 1][grid_1.Ny - 1])]) 32 | print([n.pos for n in grid_2.get_neighbours(grid_2.grid[0][0])]) 33 | print([n.pos for n in grid_2.get_neighbours(grid_2.grid[grid_2.Nx // 2][grid_2.Ny // 2])]) 34 | print([n.pos for n in grid_2.get_neighbours(grid_2.grid[grid_2.Nx - 1][grid_2.Ny - 1])], "\n") 35 | 36 | 37 | # # GraphSearch test 38 | obstacles = [] 39 | mat_A = np.array([[2.0, 0.0], [0.0, 2.0]]) 40 | vec_b = np.array([10.0, 10.0]) 41 | obstacles.append(PolytopeRegion(mat_A, vec_b)) 42 | graph = GraphSearch(graph=grid_1, obstacles=obstacles, margin=1.0) 43 | print(graph.check_collision(grid_1.grid[0][0].pos)) 44 | print(graph.check_collision(grid_1.grid[grid_1.Nx - 1][grid_1.Ny - 1].pos)) 45 | start_pos = np.array([7.5, 0.0]) 46 | goal_pos = np.array([0.0, 7.5]) 47 | 48 | # astar test with quad directional neighbours 49 | path = graph.a_star(start_pos, goal_pos) 50 | path_arr = np.array([p.pos for p in path]) 51 | plt.plot(path_arr[:, 0], path_arr[:, 1]) 52 | plt.show() 53 | 54 | # astar test with octo directional neighbours 55 | grid_1 = GridMap(quad=False) 56 | graph = GraphSearch(graph=grid_1, obstacles=obstacles, margin=1.0) 57 | path = graph.a_star(start_pos, goal_pos) 58 | path_arr = np.array([p.pos for p in path]) 59 | plt.plot(path_arr[:, 0], path_arr[:, 1]) 60 | plt.show() 61 | 62 | # reduce_path test 63 | reduced_path = graph.reduce_path(path) 64 | reduced_path_arr = np.array([p.pos for p in reduced_path]) 65 | plt.plot(reduced_path_arr[:, 0], reduced_path_arr[:, 1]) 66 | plt.show() 67 | 68 | # theta_star test 69 | grid_1 = GridMap(quad=False) 70 | graph = GraphSearch(graph=grid_1, obstacles=obstacles, margin=1.0) 71 | 72 | path = graph.theta_star(start_pos, goal_pos) 73 | path_arr = np.array([p.pos for p in path]) 74 | plt.plot(path_arr[:, 0], path_arr[:, 1]) 75 | plt.show() 76 | 77 | 78 | # # maze environment test 79 | def plot_map(obstacles, path_arr): 80 | fig, ax = plt.subplots() 81 | for o in obstacles: 82 | patch = o.get_plot_patch() 83 | ax.add_patch(patch) 84 | ax.plot(path_arr[:, 0], path_arr[:, 1]) 85 | plt.show() 86 | 87 | 88 | s = 0.2 # scale of environment 89 | start = np.array([0.5 * s, 5.5 * s]) 90 | goal = np.array([12.5 * s, 0.5 * s]) 91 | bounds = ((0.0 * s, 0.0 * s), (13.0 * s, 6.0 * s)) 92 | cell_size = 0.25 * s 93 | obstacles = [] 94 | obstacles.append(RectangleRegion(0.0 * s, 3.0 * s, 0.0 * s, 3.0 * s)) 95 | obstacles.append(RectangleRegion(1.0 * s, 2.0 * s, 4.0 * s, 6.0 * s)) 96 | obstacles.append(RectangleRegion(2.0 * s, 6.0 * s, 5.0 * s, 6.0 * s)) 97 | obstacles.append(RectangleRegion(6.0 * s, 7.0 * s, 4.0 * s, 6.0 * s)) 98 | obstacles.append(RectangleRegion(4.0 * s, 5.0 * s, 0.0 * s, 4.0 * s)) 99 | obstacles.append(RectangleRegion(5.0 * s, 7.0 * s, 2.0 * s, 3.0 * s)) 100 | obstacles.append(RectangleRegion(6.0 * s, 9.0 * s, 1.0 * s, 2.0 * s)) 101 | obstacles.append(RectangleRegion(8.0 * s, 9.0 * s, 2.0 * s, 4.0 * s)) 102 | obstacles.append(RectangleRegion(9.0 * s, 12.0 * s, 3.0 * s, 4.0 * s)) 103 | obstacles.append(RectangleRegion(11.0 * s, 12.0 * s, 4.0 * s, 5.0 * s)) 104 | obstacles.append(RectangleRegion(8.0 * s, 10.0 * s, 5.0 * s, 6.0 * s)) 105 | obstacles.append(RectangleRegion(10.0 * s, 11.0 * s, 0.0 * s, 2.0 * s)) 106 | obstacles.append(RectangleRegion(12.0 * s, 13.0 * s, 1.0 * s, 2.0 * s)) 107 | obstacles.append(RectangleRegion(0.0 * s, 13.0 * s, 6.0 * s, 7.0 * s)) 108 | obstacles.append(RectangleRegion(-1.0 * s, 0.0 * s, 0.0 * s, 6.0 * s)) 109 | obstacles.append(RectangleRegion(0.0 * s, 13.0 * s, -1.0 * s, 0.0 * s)) 110 | obstacles.append(RectangleRegion(13.0 * s, 14.0 * s, 0.0 * s, 6.0 * s)) 111 | 112 | # astar test with quad directional neighbours 113 | grid = GridMap(bounds=bounds, cell_size=cell_size, quad=True) 114 | graph = GraphSearch(graph=grid, obstacles=obstacles, margin=0.05) 115 | print(graph.check_collision(start)) 116 | path = graph.a_star(start, goal) 117 | path_arr = np.array([p.pos for p in path]) 118 | plot_map(obstacles, path_arr) 119 | 120 | # astar test with octo directional neighbours 121 | grid = GridMap(bounds=bounds, cell_size=cell_size, quad=False) 122 | graph = GraphSearch(graph=grid, obstacles=obstacles, margin=0.05) 123 | path = graph.a_star(start, goal) 124 | path_arr = np.array([p.pos for p in path]) 125 | plot_map(obstacles, path_arr) 126 | 127 | # reduce_path test 128 | reduced_path = graph.reduce_path(path) 129 | reduced_path_arr = np.array([p.pos for p in reduced_path]) 130 | plot_map(obstacles, reduced_path_arr) 131 | 132 | # theta_star test 133 | grid = GridMap(bounds=bounds, cell_size=cell_size, quad=False) 134 | graph = GraphSearch(graph=grid, obstacles=obstacles, margin=0.05) 135 | path = graph.theta_star(start, goal) 136 | path_arr = np.array([p.pos for p in path]) 137 | plot_map(obstacles, path_arr) 138 | print(path_arr) 139 | -------------------------------------------------------------------------------- /planning/path_generator/opt_planner.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import casadi as ca 4 | import numpy as np 5 | 6 | from models.geometry_utils import * 7 | 8 | 9 | class OptimizationPlanner(object): 10 | def __init__(self, obstacles, margin): 11 | self.opt = ca.Opti() 12 | self.obstacles = obstacles 13 | self.margin = margin 14 | self.guess = None 15 | 16 | def optimize(self, start_pos, goal_pos, horizon): 17 | p = self.opt.variable(2, horizon + 2) 18 | gamma = self.opt.variable(horizon + 1, 1) 19 | 20 | # set initial and final positions: 21 | self.opt.subject_to(p[:, 0] == start_pos) 22 | self.opt.subject_to(p[:, -1] == goal_pos) 23 | 24 | # set cost 25 | cost = 0.0 26 | for t in range(1, horizon + 2): 27 | # x_diff = p[:,t] - goal_pos 28 | # cost += ca.mtimes(x_diff.T, x_diff) 29 | x_diff = p[:, t] - p[:, t - 1] 30 | cost += ca.mtimes(x_diff.T, x_diff) 31 | # cost += ca.norm_2(x_diff) 32 | 33 | # set obstacle avoidance constraints 34 | for o in self.obstacles: 35 | mat_A, vec_b = o.get_convex_rep() 36 | mu = self.opt.variable(horizon + 1, mat_A.shape[0]) 37 | for t in range(horizon + 1): 38 | norm = ca.mtimes(mu[t, :], mat_A) 39 | # separation constraint 40 | self.opt.subject_to( 41 | -ca.mtimes(norm, norm.T) / 4 + ca.mtimes(norm, p[:, t + 1]) - ca.mtimes(mu[t, :], vec_b) - gamma[t] 42 | >= self.margin ** 2 43 | ) 44 | # selection constraint 45 | self.opt.subject_to(gamma[t] + ca.mtimes(norm, (p[:, t] - p[:, t + 1])) >= 0) 46 | # non-negativity constraints 47 | self.opt.subject_to(gamma[t] >= 0) 48 | self.opt.subject_to(mu[t, :] >= 0) 49 | # set mu initia; guess 50 | if self.guess is None: 51 | self.opt.set_initial(mu, 0.1 * np.ones((horizon + 1, mat_A.shape[0]))) 52 | else: 53 | self.opt.set_initial(mu, self.guess.mu) 54 | 55 | # set initial guess 56 | if self.guess is None: 57 | for t in range(horizon + 1): 58 | self.opt.set_initial(p[:, t], start_pos + (goal_pos - start_pos) * t / (horizon + 1)) 59 | self.opt.set_initial(gamma[t], 0.0) 60 | self.opt.set_initial(p[:, -1], goal_pos) 61 | else: 62 | self.opt.set_initial(p, self.guess.p) 63 | self.opt.set_initial(gamma, self.guess.gamma) 64 | 65 | # solve opt 66 | self.opt.minimize(cost) 67 | options_p = {"verbose": False, "ipopt.print_level": 0, "print_time": 0} 68 | options_s = {"max_iter": 10000} 69 | self.opt.solver("ipopt", options_p, options_s) 70 | self.sol = self.opt.solve() 71 | 72 | return self.sol.value(p) 73 | 74 | def warm_start(self, guess): 75 | self.guess = guess 76 | -------------------------------------------------------------------------------- /planning/path_generator/opt_planner_test.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | 3 | from planning.path_generator.opt_planner import * 4 | 5 | # # optimization_planner test 6 | obstacles = [] 7 | mat_A = np.array([[2.0, 0.0], [0.0, 2.0]]) 8 | vec_b = np.array([10.0, 10.0]) 9 | obstacles.append(PolytopeRegion(mat_A, vec_b)) 10 | start_pos = np.array([7.5, 0.0]) 11 | goal_pos = np.array([0.0, 7.5]) 12 | optim = OptimizationPlanner(obstacles, margin=1.0) 13 | path = optim.optimize(start_pos, goal_pos, 10) 14 | print(path) 15 | plt.plot(path[0, :], path[1, :]) 16 | plt.show() 17 | 18 | 19 | # # maze environment test 20 | s = 0.2 # scale of environment 21 | start = np.array([0.5 * s, 5.5 * s]) 22 | goal = np.array([12.5 * s, 0.5 * s]) 23 | bounds = ((0.0 * s, 0.0 * s), (13.0 * s, 6.0 * s)) 24 | cell_size = 0.25 * s 25 | obstacles = [] 26 | obstacles.append(RectangleRegion(0.0 * s, 3.0 * s, 0.0 * s, 3.0 * s)) 27 | obstacles.append(RectangleRegion(1.0 * s, 2.0 * s, 4.0 * s, 6.0 * s)) 28 | obstacles.append(RectangleRegion(2.0 * s, 6.0 * s, 5.0 * s, 6.0 * s)) 29 | obstacles.append(RectangleRegion(6.0 * s, 7.0 * s, 4.0 * s, 6.0 * s)) 30 | obstacles.append(RectangleRegion(4.0 * s, 5.0 * s, 0.0 * s, 4.0 * s)) 31 | obstacles.append(RectangleRegion(5.0 * s, 7.0 * s, 2.0 * s, 3.0 * s)) 32 | obstacles.append(RectangleRegion(6.0 * s, 9.0 * s, 1.0 * s, 2.0 * s)) 33 | obstacles.append(RectangleRegion(8.0 * s, 9.0 * s, 2.0 * s, 4.0 * s)) 34 | obstacles.append(RectangleRegion(9.0 * s, 12.0 * s, 3.0 * s, 4.0 * s)) 35 | obstacles.append(RectangleRegion(11.0 * s, 12.0 * s, 4.0 * s, 5.0 * s)) 36 | obstacles.append(RectangleRegion(8.0 * s, 10.0 * s, 5.0 * s, 6.0 * s)) 37 | obstacles.append(RectangleRegion(10.0 * s, 11.0 * s, 0.0 * s, 2.0 * s)) 38 | obstacles.append(RectangleRegion(12.0 * s, 13.0 * s, 1.0 * s, 2.0 * s)) 39 | obstacles.append(RectangleRegion(0.0 * s, 13.0 * s, 6.0 * s, 7.0 * s)) 40 | obstacles.append(RectangleRegion(-1.0 * s, 0.0 * s, 0.0 * s, 6.0 * s)) 41 | obstacles.append(RectangleRegion(0.0 * s, 13.0 * s, -1.0 * s, 0.0 * s)) 42 | obstacles.append(RectangleRegion(13.0 * s, 14.0 * s, 0.0 * s, 6.0 * s)) 43 | 44 | optim = OptimizationPlanner(obstacles, margin=0.05) 45 | try: 46 | path = optim.optimize(start, goal, 40) 47 | print(path) 48 | fig, ax = plt.subplots() 49 | for o in obstacles: 50 | patch = o.get_plot_patch() 51 | ax.add_patch(patch) 52 | ax.plot(path_arr[0, :], path_arr[1, :]) 53 | plt.show() 54 | except: 55 | print("Global path not found using optimization") 56 | -------------------------------------------------------------------------------- /planning/path_generator/optimization_path_generator.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | import numpy as np 4 | 5 | from planning.path_generator.opt_planner import * 6 | 7 | 8 | class OptPathGenerator: 9 | def __init__(self, margin=0.05, horizon=10): 10 | self._global_path = None 11 | self._margin = margin 12 | self._horizon = horizon 13 | 14 | def generate_path(self, sys, obstacles, goal_pos): 15 | optim = optimization_planner(obstacles, margin=self._margin) 16 | try: 17 | path = optim.optimize(sys.get_state()[:2], goal_pos, self._horizon) 18 | except: 19 | print("Global path not found using optimization") 20 | self._global_path = path.T 21 | if False: 22 | plot_global_map(self._global_path, obstacles) 23 | return self._global_path 24 | 25 | def logging(self, logger): 26 | logger._paths.append(self._global_path) 27 | -------------------------------------------------------------------------------- /planning/path_generator/search_path_generator.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | import numpy as np 4 | 5 | from planning.path_generator.astar import * 6 | 7 | 8 | def plot_global_map(path, obstacles): 9 | fig, ax = plt.subplots() 10 | for o in obstacles: 11 | patch = o.get_plot_patch() 12 | ax.add_patch(patch) 13 | ax.plot(path[:, 0], path[:, 1]) 14 | plt.xlim([-1 * 0.15, 11 * 0.15]) 15 | plt.ylim([0 * 0.15, 8 * 0.15]) 16 | plt.show() 17 | 18 | 19 | class AstarPathGenerator: 20 | def __init__(self, grid, quad, margin): 21 | self._global_path = None 22 | self._grid = GridMap(bounds=grid[0], cell_size=grid[1], quad=quad) 23 | self._margin = margin 24 | 25 | def generate_path(self, sys, obstacles, goal_pos): 26 | graph = GraphSearch(graph=self._grid, obstacles=obstacles, margin=self._margin) 27 | path = graph.a_star(sys.get_state()[:2], goal_pos) 28 | self._global_path = np.array([p.pos for p in path]) 29 | print(self._global_path) 30 | if self._global_path == []: 31 | print("Global Path not found.") 32 | sys.exit(1) 33 | if True: 34 | plot_global_map(self._global_path, obstacles) 35 | return self._global_path 36 | 37 | def logging(self, logger): 38 | logger._paths.append(self._global_path) 39 | 40 | 41 | class AstarLoSPathGenerator: 42 | def __init__(self, grid, quad, margin): 43 | self._global_path = None 44 | self._grid = GridMap(bounds=grid[0], cell_size=grid[1], quad=quad) 45 | self._margin = margin 46 | 47 | def generate_path(self, sys, obstacles, goal_pos): 48 | graph = GraphSearch(graph=self._grid, obstacles=obstacles, margin=self._margin) 49 | path = graph.a_star(sys.get_state()[:2], goal_pos) 50 | path = graph.reduce_path(path) 51 | self._global_path = np.array([p.pos for p in path]) 52 | print(self._global_path) 53 | if self._global_path == []: 54 | print("Global Path not found.") 55 | sys.exit(1) 56 | if False: 57 | plot_global_map(self._global_path, obstacles) 58 | return self._global_path 59 | 60 | def logging(self, logger): 61 | logger._paths.append(self._global_path) 62 | 63 | 64 | class ThetaStarPathGenerator: 65 | def __init__(self, grid, quad, margin): 66 | self._global_path = None 67 | self._grid = GridMap(bounds=grid[0], cell_size=grid[1], quad=False) 68 | self._margin = margin 69 | 70 | def generate_path(self, sys, obstacles, goal_pos): 71 | graph = GraphSearch(graph=self._grid, obstacles=obstacles, margin=self._margin) 72 | path = graph.theta_star(sys.get_state()[:2], goal_pos) 73 | self._global_path = np.array([p.pos for p in path]) 74 | print(self._global_path) 75 | if self._global_path == []: 76 | print("Global Path not found.") 77 | sys.exit(1) 78 | if True: 79 | plot_global_map(self._global_path, obstacles) 80 | return self._global_path 81 | 82 | def logging(self, logger): 83 | logger._paths.append(self._global_path) 84 | -------------------------------------------------------------------------------- /planning/trajectory_generator/constant_speed_generator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class ConstantSpeedTrajectoryGenerator: 5 | # TODO: Refactor this class to make it light-weight. 6 | # TODO: Create base class for this local planner 7 | def __init__(self): 8 | # TODO: wrap params 9 | self._global_path_index = 0 10 | # TODO: number of waypoints shall equal to length of global path 11 | self._num_waypoint = None 12 | # local path 13 | self._reference_speed = 0.2 14 | self._num_horizon = 11 15 | self._local_path_timestep = 0.1 16 | self._local_trajectory = None 17 | self._proj_dist_buffer = 0.05 18 | 19 | def generate_trajectory(self, system, global_path): 20 | # TODO: move initialization of _num_waypoint and _global_path to constructor 21 | if self._num_waypoint is None: 22 | self._global_path = global_path 23 | self._num_waypoint = global_path.shape[0] 24 | pos = system._state._x[0:2] 25 | # TODO: pass _global_path as a reference 26 | return self.generate_trajectory_internal(pos, self._global_path) 27 | 28 | def generate_trajectory_internal(self, pos, global_path): 29 | local_index = self._global_path_index 30 | trunc_path = np.vstack([global_path[local_index:, :], global_path[-1, :]]) 31 | curv_vec = trunc_path[1:, :] - trunc_path[:-1, :] 32 | curv_length = np.linalg.norm(curv_vec, axis=1) 33 | 34 | if curv_length[0] == 0.0: 35 | curv_direct = np.zeros((2,)) 36 | else: 37 | curv_direct = curv_vec[0, :] / curv_length[0] 38 | proj_dist = np.dot(pos - trunc_path[0, :], curv_direct) 39 | 40 | if proj_dist >= curv_length[0] - self._proj_dist_buffer and local_index < self._num_waypoint - 1: 41 | self._global_path_index += 1 42 | return self.generate_trajectory_internal(pos, global_path) 43 | 44 | # TODO: make the if statement optional 45 | if proj_dist <= 0.0: 46 | proj_dist = 0.0 47 | 48 | t_c = (proj_dist + self._proj_dist_buffer) / self._reference_speed 49 | t_s = t_c + self._local_path_timestep * np.linspace(0, self._num_horizon - 1, self._num_horizon) 50 | 51 | curv_time = np.cumsum(np.hstack([0.0, curv_length / self._reference_speed])) 52 | curv_time[-1] += ( 53 | t_c + 2 * self._local_path_timestep * self._num_horizon + self._proj_dist_buffer / self._reference_speed 54 | ) 55 | 56 | path_idx = np.searchsorted(curv_time, t_s, side="right") - 1 57 | path = np.vstack( 58 | [ 59 | np.interp(t_s, curv_time, trunc_path[:, 0]), 60 | np.interp(t_s, curv_time, trunc_path[:, 1]), 61 | ] 62 | ).T 63 | path_vel = self._reference_speed * np.ones((self._num_horizon, 1)) 64 | path_head = np.arctan2(curv_vec[path_idx, 1], curv_vec[path_idx, 0]).reshape(self._num_horizon, 1) 65 | self._local_trajectory = np.hstack([path, path_vel, path_head]) 66 | return self._local_trajectory 67 | 68 | def logging(self, logger): 69 | logger._trajs.append(self._local_trajectory) 70 | -------------------------------------------------------------------------------- /planning/trajectory_generator/constant_speed_generator_test.py: -------------------------------------------------------------------------------- 1 | from planning.trajectory_generator.constant_speed_generator import * 2 | 3 | 4 | class State: 5 | def __init__(self, x): 6 | self._x = x 7 | 8 | 9 | class System: 10 | def __init__(self, state): 11 | self._state = state 12 | 13 | 14 | # # local trajectory generation test 15 | global_path = np.array([[0.0, 0.2], [0.5, 0.2], [0.5, 0.8], [1.0, 0.8]]) 16 | 17 | # single and repeated waypoints, enpoint test 18 | sys_1 = System(State(np.array([1.0, 0.0]))) 19 | sys_2 = System(State(np.array([2.0, 0.0]))) 20 | traj_generator_1 = ConstantSpeedTrajectoryGenerator() 21 | traj_generator_2 = ConstantSpeedTrajectoryGenerator() 22 | path_1 = traj_generator_1.generate_trajectory(sys_1, np.array([[1.0, 0.0]])) 23 | path_2 = traj_generator_2.generate_trajectory(sys_2, np.array([[2.1, 0.0], [2.1, 0.0]])) 24 | print(path_1) 25 | print(path_2) 26 | 27 | # function iteration test 28 | traj_generator_3 = ConstantSpeedTrajectoryGenerator() 29 | path_3 = traj_generator_3.generate_trajectory( 30 | sys_1, np.array([[1.12, 0.0], [1.14, 0.015], [1.18, 0.015], [1.22, 0.015], [1.26, 0.015]]) 31 | ) 32 | print(path_3) 33 | 34 | # general path 35 | traj_generator_4 = ConstantSpeedTrajectoryGenerator() 36 | path_4 = traj_generator_4.generate_trajectory(sys_1, global_path) 37 | print(path_4) 38 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.isort] 2 | multi_line_output = 3 3 | include_trailing_comma = true 4 | force_grid_wrap = 0 5 | use_parentheses = true 6 | line_length = 88 7 | combine_as_imports = true 8 | 9 | [tool.black] 10 | line-length = 120 11 | target-version = ['py36', 'py37', 'py38'] -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pre-commit 2 | isort 3 | casadi 4 | matplotlib 5 | numpy 6 | sympy 7 | polytope -------------------------------------------------------------------------------- /sim/logger.py: -------------------------------------------------------------------------------- 1 | class SystemLogger: 2 | def __init__(self): 3 | self._xs = [] 4 | self._us = [] 5 | 6 | 7 | class ControllerLogger: 8 | def __init__(self): 9 | self._xtrajs = [] 10 | self._utrajs = [] 11 | 12 | 13 | class LocalPlannerLogger: 14 | def __init__(self): 15 | self._trajs = [] 16 | 17 | 18 | class GlobalPlannerLogger: 19 | def __init__(self): 20 | self._paths = [] 21 | -------------------------------------------------------------------------------- /sim/simulation.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import casadi as ca 4 | import numpy as np 5 | 6 | from models.geometry_utils import RectangleRegion 7 | from sim.logger import ( 8 | ControllerLogger, 9 | GlobalPlannerLogger, 10 | LocalPlannerLogger, 11 | SystemLogger, 12 | ) 13 | 14 | 15 | class System: 16 | def __init__(self, time=0.0, state=None, geometry=None, dynamics=None): 17 | self._time = time 18 | self._state = state 19 | self._geometry = geometry 20 | self._dynamics = dynamics 21 | 22 | 23 | class Robot: 24 | def __init__(self, system): 25 | self._system = system 26 | self._system_logger = SystemLogger() 27 | 28 | def set_global_planner(self, global_planner): 29 | self._global_planner = global_planner 30 | self._global_planner_logger = GlobalPlannerLogger() 31 | 32 | def set_local_planner(self, local_planner): 33 | self._local_planner = local_planner 34 | self._local_planner_logger = LocalPlannerLogger() 35 | 36 | def set_controller(self, controller): 37 | self._controller = controller 38 | self._controller_logger = ControllerLogger() 39 | 40 | def run_global_planner(self, sys, obstacles, goal_pos): 41 | # TODO: global path shall be generated with `system` and `obstacles`. 42 | self._global_path = self._global_planner.generate_path(sys, obstacles, goal_pos) 43 | self._global_planner.logging(self._global_planner_logger) 44 | 45 | def run_local_planner(self): 46 | # TODO: local path shall be generated with `obstacles`. 47 | self._local_trajectory = self._local_planner.generate_trajectory(self._system, self._global_path) 48 | self._local_planner.logging(self._local_planner_logger) 49 | 50 | def run_controller(self, obstacles): 51 | self._control_action = self._controller.generate_control_input( 52 | self._system, self._global_path, self._local_trajectory, obstacles 53 | ) 54 | self._controller.logging(self._controller_logger) 55 | 56 | def run_system(self): 57 | self._system.update(self._control_action) 58 | self._system.logging(self._system_logger) 59 | 60 | 61 | class SingleAgentSimulation: 62 | def __init__(self, robot, obstacles, goal_position): 63 | self._robot = robot 64 | self._obstacles = obstacles 65 | self._goal_position = goal_position 66 | 67 | def run_navigation(self, navigation_time): 68 | self._robot.run_global_planner(self._robot._system, self._obstacles, self._goal_position) 69 | while self._robot._system._time < navigation_time: 70 | self._robot.run_local_planner() 71 | self._robot.run_controller(self._obstacles) 72 | self._robot.run_system() 73 | --------------------------------------------------------------------------------