├── .github └── workflows │ ├── deploy.yml │ └── test.yml ├── .gitignore ├── LICENSE ├── README.md ├── conda_recipe └── meta.yaml ├── examples ├── maze.ipynb ├── maze.py ├── random_boxes.ipynb ├── small_2d.ipynb ├── uav.ipynb └── uav_visualization.py ├── fastpathplanning ├── __init__.py ├── bezier.py ├── boxes.py ├── fastpathplanning.py ├── graph.py ├── mixed_integer.py ├── polygonal.py └── smooth.py ├── pyproject.toml ├── requirements.txt ├── setup.py └── tests └── test_placeholder.py /.github/workflows/deploy.yml: -------------------------------------------------------------------------------- 1 | name: Deploy Main 2 | on: 3 | release: 4 | types: [published] 5 | jobs: 6 | deploy-pypi: 7 | runs-on: ubuntu-latest 8 | env: 9 | PYPI_USERNAME: ${{ secrets.PYPI_USERNAME }} 10 | PYPI_PASSWORD: ${{ secrets.PYPI_PASSWORD }} 11 | steps: 12 | - name: Checkout Repository 13 | uses: actions/checkout@v3 14 | 15 | - name: Install Python 16 | uses: actions/setup-python@v3 17 | 18 | - name: Install Twine 19 | run: sudo pip install twine wheel 20 | 21 | - name: Create the distribution 22 | run: | 23 | git fetch --prune --unshallow --tags 24 | sudo python setup.py sdist bdist_wheel 25 | - name: Push to PyPI 26 | run: sudo twine upload -u $PYPI_USERNAME -p $PYPI_PASSWORD dist/* 27 | 28 | deploy-conda: 29 | runs-on: ubuntu-latest 30 | # sets default shell to remove need for source to run the conda shell 31 | defaults: 32 | run: 33 | shell: bash -l {0} 34 | steps: 35 | - name: Checkout Repository 36 | uses: actions/checkout@v3 37 | 38 | - name: Install Python 39 | uses: actions/setup-python@v3 40 | 41 | - name: Install Miniconda 42 | uses: conda-incubator/setup-miniconda@v2 43 | with: 44 | auto-activate-base: true 45 | activate-environment: "" 46 | miniconda-version: "latest" 47 | 48 | - name: Install the Conda Dependencies 49 | run: | 50 | conda config --set always_yes yes --set auto_update_conda false 51 | conda update conda 52 | conda install conda-build 53 | 54 | # echo yes before login to prevent anaconda bug breaking automation 55 | # git tags MUST be fetched otherwise output will be blank 56 | # bash variables cannot be used in github actions, must use actions specific syntax and methods 57 | - name: Build the Anaconda Package 58 | id: condabuild 59 | env: 60 | ANACONDA_USERNAME: ${{ secrets.ANACONDA_CLOUD_USERNAME }} 61 | ANACONDA_PASSWORD: ${{ secrets.ANACONDA_CLOUD_PASSWORD }} 62 | run: | 63 | conda install anaconda-client 64 | conda config --set anaconda_upload no 65 | echo yes | anaconda login --username $ANACONDA_USERNAME --password $ANACONDA_PASSWORD 66 | git fetch --prune --unshallow --tags 67 | VERSION_FROM_GIT_TAG=$(git tag --list "v*[0-9]" --sort=version:refname | tail -1 | cut -c 2-) conda build . -c stanfordcvxgrp -c conda-forge --numpy 1.22.2 68 | echo "GIT_VERSION=$(git tag --list 'v*[0-9]' --sort=version:refname | tail -1 | cut -c 2-)" >> $GITHUB_ENV 69 | - name: Upload the Anaconda Package 70 | id: condaload 71 | env: 72 | ANACONDA_USERNAME: ${{ secrets.ANACONDA_CLOUD_USERNAME }} 73 | ANACONDA_PASSWORD: ${{ secrets.ANACONDA_CLOUD_PASSWORD }} 74 | run: | 75 | anaconda upload -u stanfordcvxgrp /usr/share/miniconda3/conda-bld/noarch/fastpathplanning-${{ env.GIT_VERSION }}-*.tar.bz2 76 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: Main Test 2 | on: 3 | pull_request: 4 | push: 5 | branches: 6 | - main 7 | jobs: 8 | run-tests: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: Checkout Repository 12 | uses: actions/checkout@v3 13 | 14 | - name: Install Python 15 | uses: actions/setup-python@v3 16 | 17 | - name: Install Dependencies 18 | run: | 19 | git fetch --prune --unshallow --tags 20 | sudo $pythonLocation/bin/python3 -m pip install -r requirements.txt 21 | sudo $pythonLocation/bin/python3 -m pip install setuptools --upgrade 22 | sudo $pythonLocation/bin/python3 -m pip install pytest 23 | sudo $pythonLocation/bin/python3 -m pip install -e . 24 | 25 | - name: Run Unit Tests 26 | run: | 27 | sudo $pythonLocation/bin/python3 -m pytest --import-mode=append tests/ 28 | 29 | test-build-pypi: 30 | needs: run-tests 31 | runs-on: ubuntu-latest 32 | steps: 33 | - name: Checkout Repository 34 | uses: actions/checkout@v3 35 | 36 | - name: Install Python 37 | uses: actions/setup-python@v3 38 | 39 | - name: Install Twine 40 | run: sudo pip install twine wheel 41 | 42 | - name: Create the distribution 43 | run: | 44 | git fetch --prune --unshallow --tags 45 | sudo python setup.py sdist bdist_wheel 46 | test-build-conda: 47 | needs: run-tests 48 | runs-on: ubuntu-latest 49 | # sets default shell to remove need for source to run the conda shell 50 | defaults: 51 | run: 52 | shell: bash -l {0} 53 | steps: 54 | - name: Checkout Repository 55 | uses: actions/checkout@v3 56 | 57 | - name: Install Python 58 | uses: actions/setup-python@v3 59 | 60 | - name: Install Miniconda 61 | uses: conda-incubator/setup-miniconda@v2 62 | with: 63 | auto-activate-base: true 64 | activate-environment: "" 65 | miniconda-version: "latest" 66 | 67 | - name: Install the Conda Dependencies 68 | run: | 69 | conda config --set always_yes yes --set auto_update_conda false 70 | conda update conda 71 | conda install conda-build 72 | 73 | # There needs to be an initial git tag to use, otherwise will fail 74 | # git tags MUST be fetched otherwise output will be blank 75 | # bash variables cannot be used in github actions, must use actions specific syntax and methods 76 | - name: Build the Anaconda Package 77 | id: condabuild 78 | run: | 79 | conda install anaconda-client 80 | conda config --set anaconda_upload no 81 | git fetch --prune --unshallow --tags 82 | VERSION_FROM_GIT_TAG=$(git tag --list "v*[0-9]" --sort=version:refname | tail -1 | cut -c 2-) conda build . -c stanfordcvxgrp -c conda-forge --numpy 1.22.2 83 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | # Mac. 132 | .DS_Store -------------------------------------------------------------------------------- /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 | # fastpathplanning 2 | 3 | [![Main Test](https://github.com/cvxgrp/fastpathplanning/actions/workflows/test.yml/badge.svg?branch=main)](https://github.com/cvxgrp/fastpathplanning/actions/workflows/test.yml) 4 | 5 | Library companion to the paper "[Fast Path PlanningThrough Large Collections of 6 | Safe Boxes](https://web.stanford.edu/~boyd/papers/pdf/fpp.pdf)" by Tobia Marcucci, Parth Nobel, Russ Tedrake, and Stephen Boyd. 7 | 8 | `fastpathplanning` has the following dependencies: 9 | - `numpy` 10 | - `cvxpy>=1.3` 11 | - `networkx` 12 | - `scipy` 13 | - `clarabel` 14 | 15 | These dependencies are optional: 16 | - `matplotlib` (for plotting) 17 | - `meshcat` (for 3D visualizations) 18 | -------------------------------------------------------------------------------- /conda_recipe/meta.yaml: -------------------------------------------------------------------------------- 1 | {% set name = "fastpathplanning" %} 2 | 3 | package: 4 | name: "{{ name|lower }}" 5 | version: {{ environ.get('VERSION_FROM_GIT_TAG') }} 6 | 7 | source: 8 | git_url: https://github.com/cvxgrp/fastpathplanning 9 | 10 | build: 11 | noarch: python 12 | number: 0 13 | script: "{{ PYTHON }} -m pip install . --ignore-installed -vv " 14 | 15 | requirements: 16 | host: 17 | - pip 18 | - python >=3.8 19 | - numpy >=1.17.5 20 | - scipy 21 | - networkx 22 | - cvxpy 23 | 24 | run: 25 | - pip 26 | - python >=3.8 27 | - numpy >=1.17.5 28 | - scipy 29 | - networkx 30 | - cvxpy 31 | 32 | about: 33 | home: https://github.com/cvxgrp/fastpathplanning 34 | license: APACHEv2 35 | license_family: Apache 36 | license_file: 37 | summary: An algorithm for path planning in sequences of safe boxes. 38 | doc_url: 39 | dev_url: 40 | 41 | extra: 42 | recipe-maintainers: 43 | - Parth Nobel 44 | - Tobia Marcucci 45 | - Stanford-CVX-group -------------------------------------------------------------------------------- /examples/maze.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "ffbeecf3", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import numpy as np\n", 11 | "import fastpathplanning as fpp" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "edda7f7f", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "from maze import Maze\n", 22 | "\n", 23 | "# side of the maze\n", 24 | "side = 20\n", 25 | "\n", 26 | "# number of walls that are eliminated\n", 27 | "# (if zero the maze has only one path)\n", 28 | "knock_downs = 10\n", 29 | "\n", 30 | "# construct maze\n", 31 | "maze = Maze(side, side)\n", 32 | "maze.make_maze()\n", 33 | "maze.knock_down_walls(knock_downs)" 34 | ] 35 | }, 36 | { 37 | "cell_type": "code", 38 | "execution_count": null, 39 | "id": "66feabd2", 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "# nominal values for box bounds\n", 44 | "from itertools import product\n", 45 | "L = np.array(list(product(range(side), range(side))), dtype=float)\n", 46 | "U = L + 1\n", 47 | "\n", 48 | "# shrink boxes to eliminate overlap\n", 49 | "# if two cells are separated by a wall\n", 50 | "eps = .01\n", 51 | "for i in range(side):\n", 52 | " for j in range(side):\n", 53 | " cell = maze.get_cell(i, j)\n", 54 | " k = i * side + j\n", 55 | " for direction in maze.directions:\n", 56 | " if cell.walls[direction]:\n", 57 | " if direction == 'W':\n", 58 | " L[k, 0] += eps\n", 59 | " elif direction == 'S':\n", 60 | " L[k, 1] += eps\n", 61 | " elif direction == 'E':\n", 62 | " U[k, 0] -= eps\n", 63 | " elif direction == 'N':\n", 64 | " U[k, 1] -= eps\n", 65 | "\n", 66 | "# compute safe set\n", 67 | "S = fpp.SafeSet(L, U, verbose=True)" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": null, 73 | "id": "e4cef784", 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "# online path planning\n", 78 | "p_init = np.zeros(2) + 0.5 # initial point\n", 79 | "p_term = np.ones(2) * side - 0.5 # terminal point\n", 80 | "T = side * 10 # final time\n", 81 | "alpha = [0, 0, 1] # cost weights\n", 82 | "p = fpp.plan(S, p_init, p_term, T, alpha, verbose=True)" 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "id": "45ae503e", 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "import matplotlib.pyplot as plt\n", 93 | "\n", 94 | "plt.figure()\n", 95 | "maze.plot() # plot maze walls\n", 96 | "p.plot2d() # plot smooth path" 97 | ] 98 | }, 99 | { 100 | "cell_type": "code", 101 | "execution_count": null, 102 | "id": "dc607204", 103 | "metadata": {}, 104 | "outputs": [], 105 | "source": [] 106 | } 107 | ], 108 | "metadata": { 109 | "kernelspec": { 110 | "display_name": "Python 3 (ipykernel)", 111 | "language": "python", 112 | "name": "python3" 113 | }, 114 | "language_info": { 115 | "codemirror_mode": { 116 | "name": "ipython", 117 | "version": 3 118 | }, 119 | "file_extension": ".py", 120 | "mimetype": "text/x-python", 121 | "name": "python", 122 | "nbconvert_exporter": "python", 123 | "pygments_lexer": "ipython3", 124 | "version": "3.11.6" 125 | } 126 | }, 127 | "nbformat": 4, 128 | "nbformat_minor": 5 129 | } 130 | -------------------------------------------------------------------------------- /examples/maze.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import random as rd 3 | 4 | 5 | class Cell: 6 | 7 | def __init__(self, x, y): 8 | self.x = x 9 | self.y = y 10 | self.walls = {"N": True, "S": True, "E": True, "W": True} 11 | 12 | def has_all_walls(self): 13 | return all(self.walls.values()) 14 | 15 | def knock_down_wall(self, wall): 16 | self.walls[wall] = False 17 | 18 | 19 | class Maze: 20 | directions = {"W": (-1, 0), "E": (1, 0), "S": (0, -1), "N": (0, 1)} 21 | 22 | def __init__(self, nx, ny): 23 | self.nx = nx 24 | self.ny = ny 25 | self.cells = [[Cell(x, y) for y in range(ny)] for x in range(nx)] 26 | 27 | def get_cell(self, x, y): 28 | return self.cells[x][y] 29 | 30 | def plot(self): 31 | plt.gca().axis('off') 32 | plt.plot([0, self.nx - 1], [self.ny, self.ny], c='k') 33 | plt.plot([self.nx, self.nx], [0, self.ny], c='k') 34 | for x in range(self.nx): 35 | for y in range(self.ny): 36 | if self.get_cell(x, y).walls['S'] and (x != 0 or y != 0): 37 | plt.plot([x, x + 1], [y, y], c='k') 38 | if self.get_cell(x, y).walls['W']: 39 | plt.plot([x, x], [y, y + 1], c='k') 40 | 41 | def unexplored_neighbors(self, cell): 42 | neighbours = [] 43 | for direction, (dx, dy) in self.directions.items(): 44 | x2 = cell.x + dx 45 | y2 = cell.y + dy 46 | if (0 <= x2 < self.nx) and (0 <= y2 < self.ny): 47 | neighbour = self.get_cell(x2, y2) 48 | if neighbour.has_all_walls(): 49 | neighbours.append((direction, neighbour)) 50 | return neighbours 51 | 52 | def make_maze(self, seed=0): 53 | rd.seed(seed) 54 | n = self.nx * self.ny 55 | cell_stack = [self.get_cell(0, 0)] 56 | while len(cell_stack) > 0: 57 | neighbours = self.unexplored_neighbors(cell_stack[-1]) 58 | if not neighbours: 59 | cell_stack.pop() 60 | else: 61 | direction, next_cell = rd.choice(neighbours) 62 | self.knock_down_wall(cell_stack[-1], direction) 63 | cell_stack.append(next_cell) 64 | 65 | def knock_down_wall(self, cell, wall): 66 | cell.knock_down_wall(wall) 67 | dx, dy = self.directions[wall] 68 | neighbor = self.get_cell(cell.x + dx, cell.y + dy) 69 | neighbor_wall = {'N': 'S', 'S': 'N', 'E': 'W', 'W': 'E'}[wall] 70 | neighbor.knock_down_wall(neighbor_wall) 71 | 72 | def knock_down_walls(self, n, seed=0): 73 | rd.seed(seed) 74 | knock_downs = 0 75 | while knock_downs < n: 76 | x = rd.randint(1, self.nx - 2) 77 | y = rd.randint(1, self.ny - 2) 78 | cell = self.get_cell(x, y) 79 | walls = [wall for wall, has_wall in cell.walls.items() if has_wall] 80 | if len(walls) > 0: 81 | wall = rd.choice(walls) 82 | self.knock_down_wall(cell, wall) 83 | knock_downs += 1 84 | -------------------------------------------------------------------------------- /examples/random_boxes.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "1491aa5a", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import numpy as np\n", 11 | "import fastpathplanning as fpp" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "ec7e1b1f", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "from itertools import product\n", 22 | "\n", 23 | "# parameters\n", 24 | "P = 20 # side of the grid (square root of number of boxes)\n", 25 | "sides = [2, .5] # sides of the boxes\n", 26 | "\n", 27 | "# generate boxes\n", 28 | "np.random.seed(0)\n", 29 | "L = np.zeros((P ** 2, 2))\n", 30 | "U = np.zeros((P ** 2, 2))\n", 31 | "for k, c in enumerate(product(range(P), range(P))):\n", 32 | " np.random.shuffle(sides)\n", 33 | " diag = np.multiply(np.random.rand(2), sides)\n", 34 | " L[k] = c - diag\n", 35 | " U[k] = c + diag\n", 36 | " \n", 37 | "# safe set\n", 38 | "S = fpp.SafeSet(L, U, verbose=True)" 39 | ] 40 | }, 41 | { 42 | "cell_type": "code", 43 | "execution_count": null, 44 | "id": "d6bceb12", 45 | "metadata": {}, 46 | "outputs": [], 47 | "source": [ 48 | "# online path planning\n", 49 | "p_init = np.zeros(2) # initial point\n", 50 | "p_term = np.ones(2) * (P - 1) # terminal point\n", 51 | "T = P # final time\n", 52 | "alpha = [0, 1, 1] # cost weights\n", 53 | "p = fpp.plan(S, p_init, p_term, T, alpha, verbose=True)" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": null, 59 | "id": "58e19315", 60 | "metadata": {}, 61 | "outputs": [], 62 | "source": [ 63 | "import matplotlib.pyplot as plt\n", 64 | "\n", 65 | "# plot result\n", 66 | "plt.figure()\n", 67 | "S.plot2d(alpha=.5) # plot safe set\n", 68 | "p.plot2d() # plot smooth path" 69 | ] 70 | }, 71 | { 72 | "cell_type": "code", 73 | "execution_count": null, 74 | "id": "3983ded5", 75 | "metadata": {}, 76 | "outputs": [], 77 | "source": [] 78 | } 79 | ], 80 | "metadata": { 81 | "kernelspec": { 82 | "display_name": "Python 3 (ipykernel)", 83 | "language": "python", 84 | "name": "python3" 85 | }, 86 | "language_info": { 87 | "codemirror_mode": { 88 | "name": "ipython", 89 | "version": 3 90 | }, 91 | "file_extension": ".py", 92 | "mimetype": "text/x-python", 93 | "name": "python", 94 | "nbconvert_exporter": "python", 95 | "pygments_lexer": "ipython3", 96 | "version": "3.11.6" 97 | } 98 | }, 99 | "nbformat": 4, 100 | "nbformat_minor": 5 101 | } 102 | -------------------------------------------------------------------------------- /examples/small_2d.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "1491aa5a", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import numpy as np\n", 11 | "import fastpathplanning as fpp" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "a72a19da", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "# offline preprocessing\n", 22 | "L = np.array([\n", 23 | " [ 4, 6.25],\n", 24 | " [ 2.5, 5.5],\n", 25 | " [ 4, 1],\n", 26 | " [ 1.5, .25],\n", 27 | " [ .75, 2.25],\n", 28 | " [-.25, .5],\n", 29 | " [ 0, 2],\n", 30 | " [-.25, 4.75],\n", 31 | " [ 5.2, 0]\n", 32 | "]) # lower bounds of the safe boxes\n", 33 | "U = np.array([\n", 34 | " [5.75, 7.25],\n", 35 | " [4.75, 7.5],\n", 36 | " [ 5, 6],\n", 37 | " [ 3, 7],\n", 38 | " [ 2.5, 3],\n", 39 | " [ 4.5, 1.5],\n", 40 | " [ 1, 6.25],\n", 41 | " [3.75, 6],\n", 42 | " [ 6, 7]\n", 43 | "])# upper bounds of the safe boxes\n", 44 | "S = fpp.SafeSet(L, U, verbose=True)" 45 | ] 46 | }, 47 | { 48 | "cell_type": "code", 49 | "execution_count": null, 50 | "id": "d6bceb12", 51 | "metadata": {}, 52 | "outputs": [], 53 | "source": [ 54 | "# online path planning\n", 55 | "p_init = np.array([.25, 1]) # initial point\n", 56 | "p_term = np.array([5.6, .5]) # terminal point\n", 57 | "T = 10 # final time\n", 58 | "global_minimum = 0.5198965312777519\n", 59 | "alpha = [0, 0, 1/global_minimum] # cost weights\n", 60 | "p = fpp.plan(S, p_init, p_term, T, alpha, verbose=True)" 61 | ] 62 | }, 63 | { 64 | "cell_type": "code", 65 | "execution_count": null, 66 | "id": "4ff570b3", 67 | "metadata": {}, 68 | "outputs": [], 69 | "source": [ 70 | "# evaluate solution\n", 71 | "t = 0.5 # sample time\n", 72 | "print(p(t))" 73 | ] 74 | }, 75 | { 76 | "cell_type": "code", 77 | "execution_count": null, 78 | "id": "58e19315", 79 | "metadata": { 80 | "scrolled": false 81 | }, 82 | "outputs": [], 83 | "source": [ 84 | "import matplotlib.pyplot as plt\n", 85 | "\n", 86 | "# plot result\n", 87 | "plt.figure()\n", 88 | "S.plot2d(alpha=.5) # plot safe set\n", 89 | "p.plot2d() # plot smooth path" 90 | ] 91 | } 92 | ], 93 | "metadata": { 94 | "kernelspec": { 95 | "display_name": "Python 3 (ipykernel)", 96 | "language": "python", 97 | "name": "python3" 98 | }, 99 | "language_info": { 100 | "codemirror_mode": { 101 | "name": "ipython", 102 | "version": 3 103 | }, 104 | "file_extension": ".py", 105 | "mimetype": "text/x-python", 106 | "name": "python", 107 | "nbconvert_exporter": "python", 108 | "pygments_lexer": "ipython3", 109 | "version": "3.11.6" 110 | } 111 | }, 112 | "nbformat": 4, 113 | "nbformat_minor": 5 114 | } 115 | -------------------------------------------------------------------------------- /examples/uav.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "16d0af52", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import numpy as np\n", 11 | "import fastpathplanning as fpp\n", 12 | "from uav_visualization import Village\n", 13 | "\n", 14 | "# initializes 3d environment (run this notebook to the end, and\n", 15 | "# then go to the link below to see the animation of the uav flight)\n", 16 | "village = Village()" 17 | ] 18 | }, 19 | { 20 | "cell_type": "code", 21 | "execution_count": null, 22 | "id": "eaa59f07", 23 | "metadata": {}, 24 | "outputs": [], 25 | "source": [ 26 | "# numerical parameters\n", 27 | "village_height = 5 \n", 28 | "village_side = 14\n", 29 | "building_every = 5\n", 30 | "assert (village_side + 1) % building_every == 0\n", 31 | "p_init = np.zeros(3) # initial point\n", 32 | "p_term = np.array([village_side, village_side, 0]) # terminal point\n", 33 | "r_uav = .1 # uav radius" 34 | ] 35 | }, 36 | { 37 | "cell_type": "code", 38 | "execution_count": null, 39 | "id": "fa220992", 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "# helper functions for construction of village\n", 44 | "\n", 45 | "def direction():\n", 46 | " I = np.eye(2)\n", 47 | " directions = np.vstack((I, -I))\n", 48 | " return directions[np.random.randint(0, 4)]\n", 49 | "\n", 50 | "def walk(m):\n", 51 | " d1 = direction()\n", 52 | " starts = [np.zeros(2)]\n", 53 | " ends = [d1]\n", 54 | " blocks = [np.zeros(2), d1]\n", 55 | " for i in range(m):\n", 56 | " d2 = direction()\n", 57 | " blocks.append(blocks[-1] + d2)\n", 58 | " if all(d2 == d1):\n", 59 | " ends[-1] += d1\n", 60 | " else:\n", 61 | " starts.append(ends[-1])\n", 62 | " ends.append(starts[-1] + d2)\n", 63 | " d1 = d2\n", 64 | " return np.array(starts), np.array(ends), np.array(blocks)\n", 65 | "\n", 66 | "def building(village, i, j, m):\n", 67 | " starts, ends, blocks = walk(m)\n", 68 | " offset = np.array([i, j]) + .5\n", 69 | " starts += offset\n", 70 | " ends += offset\n", 71 | " blocks += [i, j]\n", 72 | " for k, (s, e) in enumerate(zip(starts, ends)):\n", 73 | " c = (s + e) / 2\n", 74 | " d = np.abs(e - s) + 1\n", 75 | " r = list(.5 * d - r_uav) + [village_height / 2]\n", 76 | " n = list(d) + [village_height]\n", 77 | " village.building(f'building_{i}_{j}_{k}', c, r, n)\n", 78 | " return blocks\n", 79 | " \n", 80 | "def outer_box(i, j, x, y, r, zmin, zmax):\n", 81 | " L = [[i, j, zmin],\n", 82 | " [x + r, j, zmin],\n", 83 | " [i, y + r, zmin],\n", 84 | " [i, j, zmin]]\n", 85 | " U = [[x - r, j + 1, zmax],\n", 86 | " [i + 1, j + 1, zmax],\n", 87 | " [i + 1, j + 1, zmax],\n", 88 | " [i + 1, y - r, zmax]]\n", 89 | " return L, U\n", 90 | " \n", 91 | "def tree(village, i, j):\n", 92 | " name = f'tree_{i}_{j}'\n", 93 | " r = .5 - r_uav # Radius of foliage.\n", 94 | " r_trunk = .1\n", 95 | " low = [i + .5, j + .5, 1]\n", 96 | " high = [i + .5, j + .5, village_height - .5]\n", 97 | " c = np.random.uniform(low, high)\n", 98 | " village.tree(name, c, r, r_trunk)\n", 99 | " L, U = outer_box(i, j, c[0], c[1], r_trunk + r_uav, 0, c[2] - .5) # Trunk.\n", 100 | " L.append([i, j, c[2] + .5]) # Above the tree.\n", 101 | " U.append([i + 1, j + 1, village_height]) # Above the tree.\n", 102 | " return L, U\n", 103 | " \n", 104 | "def bush(village, i, j):\n", 105 | " name = f'bush_{i}_{j}'\n", 106 | " r = np.random.uniform(low=.1, high=.35) # Radius.\n", 107 | " h = 4 * r # Height.\n", 108 | " c = np.array([i + .5, j + .5])\n", 109 | " village.bush(name, c, r, h)\n", 110 | " L, U = outer_box(i, j, c[0], c[1], r + r_uav, 0, h + r_uav) # Bush.\n", 111 | " L.append([i, j, h + r_uav]) # Above the bush.\n", 112 | " U.append([i + 1, j + 1, village_height]) # Above the bush. \n", 113 | " return L, U" 114 | ] 115 | }, 116 | { 117 | "cell_type": "code", 118 | "execution_count": null, 119 | "id": "4821014a", 120 | "metadata": {}, 121 | "outputs": [], 122 | "source": [ 123 | "village.delete()\n", 124 | "np.random.seed(1)\n", 125 | "\n", 126 | "# village ground\n", 127 | "c_ground = (.7, 1, .7) # color\n", 128 | "ground = village.ground(village_side, c_ground)\n", 129 | "\n", 130 | "# start and ending platforms\n", 131 | "r_platform = .15 # radius\n", 132 | "c_platform = (1, 1, 1) # color\n", 133 | "start_platform = village.platform('start_platform', *p_init[:2], r_platform, c_platform)\n", 134 | "goal_platform = village.platform('goal_platform', *p_term[:2], r_platform, c_platform)\n", 135 | "\n", 136 | "# buildings\n", 137 | "blocks = []\n", 138 | "for i in range(village_side):\n", 139 | " for j in range(village_side):\n", 140 | " if (i + 1) % building_every == 0 and (j + 1) % building_every == 0:\n", 141 | " blocks.append(building(village, i, j, 4))\n", 142 | "blocks = [tuple(b) for b in np.vstack(blocks)]\n", 143 | " \n", 144 | "# free-space boxes\n", 145 | "L = []\n", 146 | "U = []\n", 147 | "for i in range(village_side):\n", 148 | " for j in range(village_side):\n", 149 | " if (i, j) not in blocks:\n", 150 | " if np.random.choice([0, 1]) == 0:\n", 151 | " Lij, Uij = tree(village, i, j)\n", 152 | " else:\n", 153 | " Lij, Uij = bush(village, i, j)\n", 154 | " L += Lij\n", 155 | " U += Uij" 156 | ] 157 | }, 158 | { 159 | "cell_type": "code", 160 | "execution_count": null, 161 | "id": "e96a2790", 162 | "metadata": {}, 163 | "outputs": [], 164 | "source": [ 165 | "# offline preprocessing\n", 166 | "S = fpp.SafeSet(L, U, verbose=True, solver='CLARABEL')\n", 167 | "\n", 168 | "# this will take a while if using Clarabel as a solver\n", 169 | "# for the optimization of the representative points\n", 170 | "# (for better performance consider using Mosek\n", 171 | "# by passing solver='MOSEK')" 172 | ] 173 | }, 174 | { 175 | "cell_type": "code", 176 | "execution_count": null, 177 | "id": "04fd7c2a", 178 | "metadata": {}, 179 | "outputs": [], 180 | "source": [ 181 | "# function that computes the initial time steps\n", 182 | "# given the polygonal curve C and the traveral time T\n", 183 | "# assumes that: i) the central segments are traveled at\n", 184 | "# constant speed, ii) the first and the last at\n", 185 | "# constant 6th derivatives, iii) the bondary conditions\n", 186 | "# are met, iv) the curve is cont. diff. 4 times\n", 187 | "def init_times(C, T):\n", 188 | " c = 1.75\n", 189 | " norms = np.linalg.norm(C[1:] - C[:-1], axis=1)\n", 190 | " d = sum(norms[1:-1]) + c * (norms[0] + norms[-1])\n", 191 | " trav_times = T * norms / d\n", 192 | " trav_times[0] *= c\n", 193 | " trav_times[-1] *= c\n", 194 | " return trav_times" 195 | ] 196 | }, 197 | { 198 | "cell_type": "code", 199 | "execution_count": null, 200 | "id": "172cecef", 201 | "metadata": {}, 202 | "outputs": [], 203 | "source": [ 204 | "# online path planning\n", 205 | "\n", 206 | "T = village_side + 1 # traversal time\n", 207 | "alpha = [0, 0, 0, 1] # cost weights\n", 208 | "der_init = {1: np.zeros(3), 2: np.zeros(3), 3: np.zeros(3)} # initial conditions for derivatives\n", 209 | "der_term = der_init # final conditions for derivatives\n", 210 | "p = fpp.plan(S, p_init, p_term, T, alpha, der_init, der_term,\n", 211 | " init_times=init_times, verbose=True)" 212 | ] 213 | }, 214 | { 215 | "cell_type": "code", 216 | "execution_count": null, 217 | "id": "73e7c77c", 218 | "metadata": {}, 219 | "outputs": [], 220 | "source": [ 221 | "def differential_flatness(p, pdd, yaw=np.pi/4):\n", 222 | " g = np.array([0, 0, 9.81])\n", 223 | " xC = np.array([np.cos(yaw), np.sin(yaw), 0])\n", 224 | " T = np.eye(4)\n", 225 | " T[:3, 3] = p\n", 226 | " z = g + pdd\n", 227 | " z /= np.linalg.norm(z)\n", 228 | " T[:3, 2] = z\n", 229 | " y = np.cross(z, xC)\n", 230 | " y /= np.linalg.norm(y)\n", 231 | " T[:3, 1] = y\n", 232 | " x = np.cross(y, z)\n", 233 | " T[:3, 0] = x\n", 234 | " return T" 235 | ] 236 | }, 237 | { 238 | "cell_type": "code", 239 | "execution_count": null, 240 | "id": "7b7fd2b0", 241 | "metadata": {}, 242 | "outputs": [], 243 | "source": [ 244 | "from meshcat.animation import Animation, convert_frames_to_video\n", 245 | "from meshcat.transformations import translation_matrix\n", 246 | "\n", 247 | "# on-board animation of the flight\n", 248 | "anim = Animation()\n", 249 | "uav = village.uav('uav')\n", 250 | "pdd = p.derivative().derivative()\n", 251 | "t_samples = np.linspace(0, p.duration, 1000)\n", 252 | "p_samples = [p(ti) for ti in t_samples]\n", 253 | "pdd_samples = [pdd(ti) for ti in t_samples]\n", 254 | "village.trajectory('traj', p_samples)\n", 255 | "\n", 256 | "fps = 30\n", 257 | "for ti, pi, pddi in zip(t_samples, p_samples, pdd_samples):\n", 258 | " with anim.at_frame(village, ti * fps) as frame:\n", 259 | " tranform = differential_flatness(pi, pddi)\n", 260 | " tranform_inv = np.linalg.inv(tranform)\n", 261 | " frame.set_transform(tranform_inv)\n", 262 | " frame['uav'].set_transform(tranform)\n", 263 | " \n", 264 | "# constructs animation\n", 265 | "village.set_animation(anim, play=False)\n", 266 | "\n", 267 | "# now click the meshcat link in the top cell to see the animation\n", 268 | "# in the top right of the meshcat window click:\n", 269 | "# \"Open Controls\" -> \"Animations\" -> \"default\" -> \"play\"" 270 | ] 271 | }, 272 | { 273 | "cell_type": "code", 274 | "execution_count": null, 275 | "id": "7e31728e", 276 | "metadata": {}, 277 | "outputs": [], 278 | "source": [] 279 | } 280 | ], 281 | "metadata": { 282 | "kernelspec": { 283 | "display_name": "Python 3 (ipykernel)", 284 | "language": "python", 285 | "name": "python3" 286 | }, 287 | "language_info": { 288 | "codemirror_mode": { 289 | "name": "ipython", 290 | "version": 3 291 | }, 292 | "file_extension": ".py", 293 | "mimetype": "text/x-python", 294 | "name": "python", 295 | "nbconvert_exporter": "python", 296 | "pygments_lexer": "ipython3", 297 | "version": "3.11.6" 298 | } 299 | }, 300 | "nbformat": 4, 301 | "nbformat_minor": 5 302 | } 303 | -------------------------------------------------------------------------------- /examples/uav_visualization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from meshcat import Visualizer 4 | from meshcat.geometry import Box, Sphere, Cylinder, MeshLambertMaterial 5 | from meshcat.transformations import translation_matrix, rotation_matrix 6 | from matplotlib.colors import to_hex as _to_hex 7 | 8 | 9 | to_hex = lambda rgb: '0x' + _to_hex(rgb)[1:] 10 | 11 | 12 | class EnvironmentVisualizer(Visualizer): 13 | 14 | def __init__(self): 15 | 16 | super().__init__() 17 | self['/Background'].set_property('visible', False) 18 | 19 | def cube(vis, name, c, r, color=(1,0,0), opacity=1): 20 | 21 | return _cube(vis, name, c, r, color, opacity) 22 | 23 | def box(self, name, l, u, color=(1,0,0), opacity=1): 24 | 25 | return _box(self, name, l, u, color, opacity) 26 | 27 | def sphere(self, name, c, r, color=(1,0,0), opacity=1): 28 | 29 | return _sphere(self, name, c, r, color, opacity) 30 | 31 | def cylinder(self, name, c1, c2, r, color=(1,0,0), opacity=1): 32 | 33 | return _cylinder(self, name, c1, c2, r, color, opacity) 34 | 35 | def capsule(self, name, c1, c2, r, color=(1,0,0), opacity=1): 36 | 37 | return _capsule(self, name, c1, c2, r, color, opacity) 38 | 39 | def trajectory(self, name, traj, r=.005, color=(0,0,1)): 40 | 41 | return _trajectory(self, name, traj, r, color) 42 | 43 | def _cube(vis, name, c, r, color, opacity): 44 | 45 | c = np.array(c, dtype=float) 46 | color = to_hex(color) 47 | material = MeshLambertMaterial(color=color, opacity=opacity) 48 | cube = vis[name] 49 | cube.set_object(Box(2 * r * np.ones(3)), material) 50 | 51 | cube.set_transform(translation_matrix(c)) 52 | 53 | return cube 54 | 55 | def _box(vis, name, l, u, color, opacity): 56 | 57 | l = np.array(l, dtype=float) 58 | u = np.array(u, dtype=float) 59 | color = to_hex(color) 60 | material = MeshLambertMaterial(color=color, opacity=opacity) 61 | box = vis[name] 62 | box.set_object(Box(u - l), material) 63 | 64 | c = (u - l) / 2 65 | box.set_transform(translation_matrix(l + c)) 66 | 67 | return box 68 | 69 | def _sphere(vis, name, c, r, color, opacity): 70 | 71 | c = np.array(c, dtype=float) 72 | color = to_hex(color) 73 | material = MeshLambertMaterial(color=color, opacity=opacity) 74 | sphere = vis[name] 75 | sphere.set_object(Sphere(r), material) 76 | 77 | sphere.set_transform(translation_matrix(c)) 78 | 79 | return sphere 80 | 81 | def _cylinder(vis, name, c1, c2, r, color, opacity): 82 | 83 | c1 = np.array(c1, dtype=float) 84 | c2 = np.array(c2, dtype=float) 85 | l = np.linalg.norm(c1 - c2) 86 | color = to_hex(color) 87 | material = MeshLambertMaterial(color=color, opacity=opacity) 88 | cylinder = vis[name] 89 | cylinder.set_object(Cylinder(l, r), material) 90 | 91 | c = (c2 + c1) / 2 92 | d = (c2 - c1) / l 93 | angle = np.arccos(d[1]) 94 | if np.isclose(angle % np.pi, 0): 95 | ax = np.array([1, 0, 0]) 96 | else: 97 | ax = np.array([d[2], 0, -d[0]]) 98 | R = rotation_matrix(angle, ax) 99 | T = translation_matrix(c) 100 | cylinder.set_transform(T.dot(R)) 101 | 102 | return cylinder 103 | 104 | def _capsule(vis, name, c1, c2, r, color, opacity): 105 | 106 | cylinder = _cylinder(vis, name + '_cylinder', c1, c2, r, color, opacity) 107 | sphere1 = _sphere(vis, name + '_sphere1', c1, r, color, opacity) 108 | sphere2 = _sphere(vis, name + '_sphere2', c2, r, color, opacity) 109 | 110 | return cylinder, sphere1, sphere2 111 | 112 | def _trajectory(vis, name, traj, r, color): 113 | 114 | capsules = [] 115 | for i, (x, y) in enumerate(zip(traj[:-1], traj[1:])): 116 | capsules.append(vis.capsule(name + f'_{i}', x, y, r, color)) 117 | 118 | return capsules 119 | 120 | 121 | class Village(EnvironmentVisualizer): 122 | 123 | def __init__(self): 124 | 125 | super().__init__() 126 | self['/Grid'].set_property('visible', False) 127 | 128 | def ground(self, side, color=(1, 1, 1)): 129 | 130 | l = [-.5, -.5, -.02] 131 | u = [side + .5, side + .5, -.01] 132 | ground = self.box('ground', l, u, color) 133 | 134 | return ground 135 | 136 | def platform(self, name, x, y, r, color=(1, 1, 1)): 137 | 138 | c1 = np.array([x, y, -.01]) 139 | c2 = np.array([x, y, -.005]) 140 | platform = self.cylinder(name, c1, c2, r, color) 141 | 142 | return platform 143 | 144 | def bush(self, name, c, r, h): 145 | 146 | c = np.array(c, dtype=float) 147 | 148 | l = [c[0] - r, c[1] - r, 0] 149 | u = [c[0] + r, c[1] + r, h] 150 | color = (0, .5, 0) 151 | bush = self.box(name, l, u, color) 152 | 153 | return bush 154 | 155 | def tree(self, name, c, r, r_trunk): 156 | 157 | c = np.array(c, dtype=float) 158 | 159 | l = [c[0] - r_trunk, c[1] - r_trunk, 0] 160 | u = [c[0] + r_trunk, c[1] + r_trunk, c[2] - r] 161 | color = (.7, .35, 0) 162 | trunk = self.box(name + '_trunk', l, u, color) 163 | 164 | color = (.2, .8, .2) 165 | foliage = self.cube(name + '_foliage', c, r, color) 166 | 167 | return trunk, foliage 168 | 169 | def building(self, name, c, r, n): 170 | 171 | c = np.array(c, dtype=float) 172 | r = np.array(r, dtype=float) 173 | n = np.array(n, dtype=int) 174 | 175 | h = 2 * r[2] 176 | l = [c[0] - r[0], c[1] - r[1], 0] 177 | u = [c[0] + r[0], c[1] + r[1], h] 178 | color = (.8, .8, .8) 179 | body = self.box(name + '_body', l, u, color) 180 | 181 | roof_ratio = 1 / 50 182 | l[2] = h 183 | u[2] = h * (1 + roof_ratio) 184 | color = (.7, .0, .0) 185 | roof = self.box(name + '_roof', l, u, color) 186 | 187 | windows = [] 188 | eps = 1e-2 189 | wcolor = (.3, .6, 1) 190 | fcolor = (0, 0, 0) 191 | d = 2 * r / n # Distance between windows in all directions. 192 | wr = d / 3.5 # Window radius in all directions. 193 | f = wr / 5 # Frame size in all directions. 194 | 195 | dw = np.array([wr[0], r[1] + 2 * eps, wr[2]]) 196 | df = [f[0], - eps, f[2]] 197 | for i in range(n[0]): 198 | for j in range(n[2]): 199 | cij = np.array([c[0] - r[0] + d[0] * (i + .5), c[1], d[2] * (j + .5)]) 200 | lij = cij - dw 201 | uij = cij + dw 202 | windows.append(self.box(name + f'_window_x_{i}_{j}', lij, uij, wcolor)) 203 | lij -= df 204 | uij += df 205 | windows.append(self.box(name + f'_frame_x_{i}_{j}', lij, uij, fcolor)) 206 | 207 | dw = np.array([r[0] + 2 * eps, wr[1], wr[2]]) 208 | df = [- eps, f[1], f[2]] 209 | for i in range(n[1]): 210 | for j in range(n[2]): 211 | cij = np.array([c[0], c[1] - r[1] + d[1] * (i + .5), d[2] * (j + .5)]) 212 | lij = cij - dw 213 | uij = cij + dw 214 | windows.append(self.box(name + f'_window_y_{i}_{j}', lij, uij, wcolor)) 215 | 216 | lij -= df 217 | uij += df 218 | windows.append(self.box(name + f'_frame_y_{i}_{j}', lij, uij, fcolor)) 219 | 220 | return body, roof, windows 221 | 222 | def uav(self, name='uav', c=[0, 0, 0]): 223 | 224 | c = np.array(c, dtype=float) 225 | size = np.array([.03, .03, .007]) 226 | 227 | l = c - size 228 | u = c + size 229 | color = (1, 0, 0) 230 | opacity = 1 231 | uav = _box(self, name, l, u, color, opacity) 232 | 233 | r = .025 234 | d1 = np.array([0, 0, .02]) 235 | d2 = np.array([0, 0, .025]) 236 | color_prop = (.5, .5, .5) 237 | for i, b in enumerate([[1, 1, 0], [1, -1, 0], [-1, -1, 0], [-1, 1, 0]]): 238 | ci = .05 * np.array(b) 239 | di = .025 * np.array(b) 240 | propi = _cylinder(uav, f'prop{i}', ci + d1, ci + d2, r, color_prop, opacity) 241 | beami = _cylinder(uav, f'beam{i}', di, ci + d1, r/5, color, opacity) 242 | 243 | return uav 244 | -------------------------------------------------------------------------------- /fastpathplanning/__init__.py: -------------------------------------------------------------------------------- 1 | from fastpathplanning.fastpathplanning import SafeSet, plan 2 | from fastpathplanning.mixed_integer import mixed_integer 3 | -------------------------------------------------------------------------------- /fastpathplanning/bezier.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | from time import time 4 | from itertools import accumulate 5 | from bisect import bisect 6 | from scipy.special import binom 7 | 8 | class BezierCurve: 9 | 10 | def __init__(self, points, a=0, b=1): 11 | 12 | assert b > a 13 | 14 | self.points = points 15 | self.M = points.shape[0] - 1 16 | self.d = points.shape[1] 17 | self.a = a 18 | self.b = b 19 | self.duration = b - a 20 | 21 | def __call__(self, t): 22 | 23 | c = np.array([self.berstein(t, n) for n in range(self.M + 1)]) 24 | return c.T.dot(self.points) 25 | 26 | def berstein(self, t, n): 27 | 28 | c1 = binom(self.M, n) 29 | c2 = (t - self.a) / self.duration 30 | c3 = (self.b - t) / self.duration 31 | value = c1 * c2 ** n * c3 ** (self.M - n) 32 | 33 | return value 34 | 35 | def start_point(self): 36 | 37 | return self.points[0] 38 | 39 | def end_point(self): 40 | 41 | return self.points[-1] 42 | 43 | def derivative(self): 44 | 45 | points = (self.points[1:] - self.points[:-1]) * (self.M / self.duration) 46 | 47 | return BezierCurve(points, self.a, self.b) 48 | 49 | def l2_squared(self): 50 | 51 | A = l2_matrix(self.M, self.d) 52 | p = self.points.flatten() 53 | 54 | return p.dot(A.dot(p)) * self.duration 55 | 56 | def plot2d(self, samples=51, **kwargs): 57 | 58 | import matplotlib.pyplot as plt 59 | 60 | options = {'c':'b'} 61 | options.update(kwargs) 62 | t = np.linspace(self.a, self.b, samples) 63 | plt.plot(*self(t).T, **options) 64 | 65 | def scatter2d(self, **kwargs): 66 | 67 | import matplotlib.pyplot as plt 68 | 69 | options = {'fc':'orange', 'ec':'k', 'zorder':3} 70 | options.update(kwargs) 71 | plt.scatter(*self.points.T, **options) 72 | 73 | def plot_2dpolygon(self, **kwargs): 74 | 75 | import matplotlib.pyplot as plt 76 | from matplotlib.patches import Polygon 77 | from scipy.spatial import ConvexHull 78 | 79 | options = {'fc':'lightcoral'} 80 | options.update(kwargs) 81 | hull = ConvexHull(self.points) 82 | ordered_points = hull.points[hull.vertices] 83 | poly = Polygon(ordered_points, **options) 84 | plt.gca().add_patch(poly) 85 | 86 | def l2_matrix(M, d): 87 | 88 | A = np.zeros((M + 1, M + 1)) 89 | for m in range(M + 1): 90 | for n in range(M + 1): 91 | A[m, n] = binom(M, m) * binom(M, n) / binom(2 * M, m + n) 92 | A /= (2 * M + 1) 93 | A_kron = np.kron(A, np.eye(d)) 94 | 95 | return A_kron 96 | 97 | class CompositeBezierCurve: 98 | 99 | def __init__(self, beziers): 100 | 101 | for bez1, bez2 in zip(beziers[:-1], beziers[1:]): 102 | assert bez1.b == bez2.a 103 | assert bez1.d == bez2.d 104 | 105 | self.beziers = beziers 106 | self.N = len(self.beziers) 107 | self.d = beziers[0].d 108 | self.a = beziers[0].a 109 | self.b = beziers[-1].b 110 | self.duration = self.b - self.a 111 | self.transition_times = [self.a] + [bez.b for bez in beziers] 112 | 113 | def find_segment(self, t): 114 | 115 | return min(bisect(self.transition_times, t) - 1, self.N - 1) 116 | 117 | def __call__(self, t): 118 | 119 | i = self.find_segment(t) 120 | 121 | return self.beziers[i](t) 122 | 123 | def start_point(self): 124 | 125 | return self.beziers[0].start_point() 126 | 127 | def end_point(self): 128 | 129 | return self.beziers[-1].end_point() 130 | 131 | def derivative(self): 132 | 133 | return CompositeBezierCurve([b.derivative() for b in self.beziers]) 134 | 135 | def l2_squared(self): 136 | 137 | return sum(bez.l2_squared() for bez in self.beziers) 138 | 139 | def plot2d(self, **kwargs): 140 | 141 | for bez in self.beziers: 142 | bez.plot2d(**kwargs) 143 | 144 | def scatter2d(self, **kwargs): 145 | 146 | for bez in self.beziers: 147 | bez.scatter2d(**kwargs) 148 | 149 | def plot_2dpolygon(self, **kwargs): 150 | 151 | for bez in self.beziers: 152 | bez.plot_2dpolygon(**kwargs) 153 | -------------------------------------------------------------------------------- /fastpathplanning/boxes.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from copy import copy 3 | from itertools import product 4 | from bisect import bisect_left, bisect_right 5 | from fastpathplanning.graph import LineGraph 6 | 7 | class Box: 8 | 9 | def __init__(self, l, u): 10 | 11 | self.d = len(l) 12 | self.l = np.array(l) 13 | self.u = np.array(u) 14 | assert all(self.u >= self.l) 15 | self.c = (self.l + self.u) / 2 16 | 17 | def intersects(self, box): 18 | 19 | l = np.maximum(box.l, self.l) 20 | u = np.minimum(box.u, self.u) 21 | 22 | return all(u >= l) 23 | 24 | def intersect(self, box): 25 | 26 | l = np.maximum(box.l, self.l) 27 | u = np.minimum(box.u, self.u) 28 | 29 | return Box(l, u) 30 | 31 | def contains(self, x, tol=0): 32 | 33 | return all(x - self.l >= - tol) and all(x - self.u <= tol) 34 | 35 | def active_set(self, x, tol=1e-9): 36 | 37 | # Assumes that the box contains x. 38 | L = x <= self.l + tol 39 | U = x >= self.u - tol 40 | 41 | return L, U 42 | 43 | def plot2d(self, label=None, **kwargs): 44 | 45 | import matplotlib.pyplot as plt 46 | from matplotlib.patches import Rectangle 47 | assert self.d == 2 48 | options = {'fc':'lightcyan', 'ec':'k'} 49 | options.update(kwargs) 50 | diag = self.u - self.l 51 | rect = Rectangle(self.l, *diag, **options) 52 | plt.gca().add_patch(rect) 53 | 54 | if label is not None: 55 | plt.text(*self.c, label, va='center', ha='center') 56 | 57 | def plot3d(self, vis, name, color=(1,0,0), opacity=1): 58 | 59 | assert self.d == 3 60 | vis.box(name, self.l, self.u, color, opacity) 61 | 62 | 63 | class BoxCollection: 64 | def __init__(self, boxes, verbose=True, tol=0): 65 | 66 | self.boxes = boxes 67 | self.n = len(boxes) 68 | self.d = boxes[0].d 69 | 70 | if verbose: 71 | print(f'Computing interesctions of {self.n} boxes in {self.d} dimensions') 72 | 73 | self.ls = np.vstack([box.l for box in boxes]).T 74 | self.us = np.vstack([box.u for box in boxes]).T 75 | self.orders = np.empty((self.d, 2, self.n), dtype=int) 76 | self.orders_inv = np.empty((self.d, 2, self.n), dtype=int) 77 | 78 | def sort_coordinates_construct_forward_backwards(vals, order, order_inv): 79 | order[:] = np.argsort(vals) 80 | order_inv[order] = np.arange(self.n) 81 | vals[:] = vals[order] 82 | 83 | for i in range(self.d): 84 | sort_coordinates_construct_forward_backwards( 85 | self.ls[i], self.orders[i, 0], self.orders_inv[i, 0]) 86 | sort_coordinates_construct_forward_backwards( 87 | self.us[i], self.orders[i, 1], self.orders_inv[i, 1]) 88 | 89 | self.inters = self._iintersections(0, tol) 90 | for i in range(1, self.d): 91 | for k, box_indices in self._iintersections(i, tol).items(): 92 | self.inters[k] = self.inters[k] & box_indices 93 | 94 | if verbose: 95 | n_inters = int(sum(len(v) for v in self.inters.values()) / 2) 96 | print(f'...found {n_inters} intersections') 97 | 98 | def contain(self, x, tol=0, subset=...): 99 | 100 | x = np.array(x) 101 | box_indices = self._icontain(0, x[0], tol, subset) 102 | for i in range(1, self.d): 103 | box_indices = box_indices & self._icontain(i, x[i], tol, subset) 104 | 105 | return box_indices 106 | 107 | def _ilcontain(self, i, xi, tol=0, subset=..., first=0): 108 | '''Returns the indices of the boxes whose lower value in the ith 109 | dimension is smaller than or equal to the given value xi.''' 110 | 111 | if subset != Ellipsis: 112 | subset = sorted(self.orders_inv[i, 0, subset]) # How to avoid sorting here? 113 | 114 | last = bisect_right(self.ls[i, subset], xi + tol, lo=first) 115 | 116 | return set(self.orders[i, 0, subset][first:last]) 117 | 118 | def _iucontain(self, i, xi, tol=0, subset=...): 119 | '''Returns the indices of the boxes whose upper value in the ith 120 | dimension is greater than or equal to the given value xi.''' 121 | 122 | if subset != Ellipsis: 123 | subset = sorted(self.orders_inv[i, 1, subset]) # How to avoid sorting here? 124 | 125 | first = bisect_left(self.us[i, subset], xi - tol) 126 | 127 | return set(self.orders[i, 1, subset][first:]) 128 | 129 | def _icontain(self, i, xi, tol=0, subset=...): 130 | '''Returns the indices of the boxes whose projection onto the ith axis 131 | contains to the given value xi.''' 132 | 133 | box_indices_l = self._ilcontain(i, xi, tol, subset) 134 | box_indices_u = self._iucontain(i, xi, tol, subset) 135 | 136 | return box_indices_l & box_indices_u 137 | 138 | def _iintersections(self, i, tol=0): 139 | 140 | inters = {k: set() for k in range(self.n)} 141 | for l, k in enumerate(self.orders[i, 0]): 142 | xi = self.boxes[k].u[i] 143 | for m in self._ilcontain(i, xi, tol, first=l+1): 144 | inters[k].add(m) 145 | inters[m].add(k) 146 | 147 | return inters 148 | 149 | def line_graph(self, verbose=True, solver='CLARABEL'): 150 | 151 | return LineGraph(self, verbose, solver) 152 | 153 | def plot2d(self, subset=None, label=None, frame_ratio=50, **kwargs): 154 | import matplotlib.pyplot as plt 155 | from matplotlib.patches import Rectangle 156 | 157 | plot_box = np.array([plt.gca().get_xlim(), plt.gca().get_ylim()]).T 158 | for i, box in enumerate(self.boxes): 159 | if subset is None or i in subset: 160 | label_i = label if label is None else str(i) 161 | box.plot2d(label=label_i, **kwargs) 162 | plot_box[0] = np.minimum(plot_box[0], box.l) 163 | plot_box[1] = np.maximum(plot_box[1], box.u) 164 | 165 | frame = min(plot_box[1] - plot_box[0]) / frame_ratio 166 | plot_box[0] -= frame 167 | plot_box[1] += frame 168 | plt.xlim(plot_box[:, 0]) 169 | plt.ylim(plot_box[:, 1]) 170 | 171 | def plot3d(self, vis, name, color=(1,0,0), opacity=1, subset=None): 172 | 173 | for i, box in enumerate(self.boxes): 174 | if subset is None or i in subset: 175 | box.plot3d(vis, name + '_' + str(i), color, opacity) 176 | 177 | @staticmethod 178 | def generate_grid(d, n, sides, seed=0): 179 | 180 | np.random.seed(seed) 181 | sides = copy(sides) 182 | boxes = [] 183 | for c in product(*[range(n) for _ in range(d)]): 184 | np.random.shuffle(sides) 185 | diag = np.multiply(np.random.rand(d), sides) 186 | boxes.append(Box(c - diag, c + diag)) 187 | 188 | return BoxCollection(boxes) 189 | -------------------------------------------------------------------------------- /fastpathplanning/fastpathplanning.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from time import time 3 | from fastpathplanning.boxes import Box, BoxCollection 4 | from fastpathplanning.polygonal import iterative_planner 5 | from fastpathplanning.smooth import seq_conv_prog 6 | 7 | class SafeSet: 8 | 9 | def __init__(self, L, U, verbose=True, solver='CLARABEL'): 10 | tic = time() 11 | 12 | if verbose: 13 | print(f'Preprocessing phase:') 14 | 15 | L = np.array(L) 16 | U = np.array(U) 17 | assert L.shape == U.shape 18 | boxes = [Box(l, u) for l, u in zip(L, U)] 19 | self.B = BoxCollection(boxes, verbose) 20 | self.G = self.B.line_graph(verbose, solver) 21 | 22 | self.runtime = time() - tic 23 | self.cvxpy_time = self.G.cvxpy_time 24 | 25 | if verbose: 26 | print('Preprocessing terminated in {:.1e}s'.format(self.runtime)) 27 | print('CVXPY time was {:.1e}s'.format(self.cvxpy_time)) 28 | 29 | def plot2d(self, **kwargs): 30 | 31 | self.B.plot2d(**kwargs) 32 | 33 | def plan(S, p_init, p_term, T, alpha, der_init={}, der_term={}, verbose=True, 34 | init_times=None): 35 | 36 | tic = time() 37 | if verbose: 38 | print('Polygonal phase:') 39 | 40 | discrete_planner, runtime = S.G.shortest_path(p_term) 41 | box_seq, length, runtime = discrete_planner(p_init) 42 | if box_seq is None: 43 | print('Infeasible problem, initial and terminal points are disconnected.') 44 | return 45 | box_seq, traj, length, cvxpy_time = iterative_planner(S.B, p_init, p_term, box_seq, verbose) 46 | 47 | polygonal_time = time() - tic 48 | if verbose: 49 | print('Polygonal phase terminated in {:.1e}s'.format(polygonal_time)) 50 | print('CVXPY time was {:.1e}s'.format(cvxpy_time)) 51 | print('\nSmooth phase:') 52 | tic = time() 53 | 54 | # Fix box sequence. 55 | L = np.array([S.B.boxes[i].l for i in box_seq]) 56 | U = np.array([S.B.boxes[i].u for i in box_seq]) 57 | 58 | # Boundary conditions. 59 | initial = {0: p_init} | der_init 60 | final = {0: p_term} | der_term 61 | 62 | # Initialize transition times. 63 | if init_times is not None: 64 | trav_times = init_times(traj, T) 65 | else: 66 | trav_times = np.linalg.norm(traj[1:] - traj[:-1], axis=1) 67 | trav_times *= T / sum(trav_times) 68 | 69 | alpha = {i + 1: ai for i, ai in enumerate(alpha)} 70 | path, cvxpy_time = seq_conv_prog(L, U, trav_times, alpha, initial, final, verbose) 71 | 72 | smooth_time = time() - tic 73 | if verbose: 74 | print('Smooth phase terminated in {:.1e}s'.format(smooth_time)) 75 | print('CVXPY time was {:.1e}s'.format(cvxpy_time)) 76 | 77 | return path 78 | -------------------------------------------------------------------------------- /fastpathplanning/graph.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy as sp 3 | import cvxpy as cp 4 | import networkx as nx 5 | from time import time 6 | from itertools import product 7 | 8 | class LineGraph(nx.Graph): 9 | 10 | def __init__(self, B, verbose=True, solver='CLARABEL', *args, **kwargs): 11 | 12 | if verbose: 13 | print('Computing line graph') 14 | 15 | # Initialize and store boxes. 16 | super().__init__(*args, **kwargs) 17 | self.B = B 18 | self.solver = solver 19 | 20 | # Compute line graph using networkx. 21 | inters_graph = nx.Graph() 22 | inters_graph.add_nodes_from(B.inters.keys()) 23 | for k, k_inters in B.inters.items(): 24 | k_inters_unique = [l for l in k_inters if l > k] 25 | inters_graph.add_edges_from(product([k], k_inters_unique)) 26 | line_graph = nx.line_graph(inters_graph) 27 | self.add_nodes_from(line_graph.nodes) 28 | self.add_edges_from(line_graph.edges) 29 | self.v2i = {v: i for i, v in enumerate(self.nodes)} 30 | self.i2v = {i: v for i, v in enumerate(self.nodes)} 31 | 32 | if verbose: 33 | print(f'...line graph has {self.number_of_nodes()} vertices ' \ 34 | f'and {self.number_of_edges()} edges') 35 | 36 | if verbose: 37 | print('Optimizing representative points') 38 | 39 | # Pair each vertex with the corresponding box intersection. 40 | for v in self.nodes: 41 | boxk = B.boxes[v[0]] 42 | boxl = B.boxes[v[1]] 43 | self.nodes[v]['box'] = boxk.intersect(boxl) 44 | 45 | # Place representative point in each box intersection. 46 | self.optimize_points() 47 | 48 | # Assign fixed length to each edge of the line graph. 49 | for e in self.edges: 50 | pu = self.nodes[e[0]]['point'] 51 | pv = self.nodes[e[1]]['point'] 52 | self.edges[e]['weight'] = np.linalg.norm(pv - pu) 53 | 54 | # Store adjacency matrix for scipy's shortest-path algorithms. 55 | self.adj_mat = nx.to_scipy_sparse_array(self) 56 | 57 | if verbose: 58 | print('...done') 59 | 60 | def optimize_points(self): 61 | tic = time() 62 | 63 | x = cp.Variable((self.number_of_nodes(), self.B.d)) 64 | x.value = np.array([self.nodes[v]['box'].c for v in self.nodes]) 65 | 66 | l = np.vstack([self.nodes[v]['box'].l for v in self.nodes]) 67 | u = np.vstack([self.nodes[v]['box'].u for v in self.nodes]) 68 | constraints = [x >= l, x <= u] 69 | 70 | A = nx.incidence_matrix(self, oriented=True) 71 | y = A.T.dot(x) 72 | cost = cp.sum(cp.norm(y, 2, axis=1)) 73 | 74 | prob = cp.Problem(cp.Minimize(cost), constraints) 75 | prob.solve(solver=self.solver) 76 | 77 | for i, v in enumerate(self.nodes): 78 | self.nodes[v]['point'] = x[i].value 79 | 80 | self.cvxpy_time = time() - tic - prob.solver_stats.solve_time 81 | 82 | @staticmethod 83 | def node(k, l): 84 | 85 | return (k, l) if k < l else (l, k) 86 | 87 | def shortest_path(self, goal): 88 | 89 | tic = time() 90 | 91 | rows = [] 92 | data = [] 93 | for k in self.B.contain(goal): 94 | for l in self.B.inters[k]: 95 | v = self.node(k, l) 96 | i = self.v2i[v] 97 | rows.append(i) 98 | pv = self.nodes[v]['point'] 99 | data.append(np.linalg.norm(goal - pv)) 100 | cols = [0] * len(rows) 101 | shape = (len(self.nodes), 1) 102 | adj_col = sp.sparse.csr_matrix((data, (rows, cols)), shape) 103 | adj_mat = sp.sparse.bmat([[self.adj_mat, adj_col], [adj_col.T, None]]) 104 | 105 | dist, succ = sp.sparse.csgraph.dijkstra( 106 | csgraph=adj_mat, 107 | directed=False, 108 | return_predecessors=True, 109 | indices=-1 110 | ) 111 | 112 | planner = lambda start: self._planner_all_to_one(start, dist, succ) 113 | 114 | return planner, time() - tic 115 | 116 | def _planner_all_to_one(self, start, dist, succ): 117 | 118 | tic = time() 119 | 120 | length = np.inf 121 | for k in self.B.contain(start): 122 | for l in self.B.inters[k]: 123 | v = self.node(k, l) 124 | i = self.v2i[v] 125 | dist_vg = dist[i] 126 | if np.isinf(dist_vg): 127 | return None, None, time() - tic 128 | dist_sv = np.linalg.norm(self.nodes[v]['point'] - start) 129 | dist_sg = dist_sv + dist_vg 130 | if dist_sg < length: 131 | length = dist_sg 132 | first_box = k 133 | first_vertex = i 134 | 135 | box_sequence = self._succ_to_box_sequence(succ, first_box, first_vertex) 136 | 137 | return box_sequence, length, time() - tic 138 | 139 | def _succ_to_box_sequence(self, succ, first_box, first_vertex): 140 | 141 | box_sequence = [first_box] 142 | i = first_vertex 143 | while succ[i] >= 0: 144 | v = self.i2v[i] 145 | j = succ[i] 146 | if succ[j] >= 0: 147 | w = self.i2v[j] 148 | while box_sequence[-1] in w: 149 | i = j 150 | v = w 151 | j = succ[i] 152 | w = self.i2v[j] 153 | if v[0] == box_sequence[-1]: 154 | box_sequence.append(v[1]) 155 | else: 156 | box_sequence.append(v[0]) 157 | i = succ[i] 158 | 159 | return box_sequence 160 | 161 | def plot(self, **kwargs): 162 | 163 | plot_graph(self, **kwargs) 164 | 165 | def plot_graph(G, **kwargs): 166 | 167 | import matplotlib.pyplot as plt 168 | 169 | options = {'marker':'o', 'markerfacecolor':'w', 'markeredgecolor':'k', 'c':'k'} 170 | options.update(kwargs) 171 | 172 | if 'label' in options: 173 | plt.plot([np.nan], [np.nan], **options) 174 | options.pop('label') 175 | 176 | for e in G.edges: 177 | start = G.nodes[e[0]]['point'] 178 | stop = G.nodes[e[1]]['point'] 179 | P = np.array([start, stop]) 180 | plt.plot(*P.T, **options) 181 | -------------------------------------------------------------------------------- /fastpathplanning/mixed_integer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | from fastpathplanning.bezier import BezierCurve, CompositeBezierCurve, l2_matrix 4 | 5 | def mixed_integer(L, U, alpha, p_init, p_term, T, N, der_init={}, der_term={}, 6 | n_points=None, **kwargs): 7 | 8 | initial = {0: p_init} | der_init 9 | final = {0: p_term} | der_term 10 | alpha = {i + 1: ai for i, ai in enumerate(alpha)} 11 | 12 | # Problem size. 13 | K, d = L.shape 14 | D = max(alpha) 15 | assert max(initial) <= D 16 | assert max(final) <= D 17 | if n_points is None: 18 | n_points = (D + 1) * 2 19 | 20 | # Control points of the curves and their derivatives. 21 | points = {} 22 | for j in range(N): 23 | points[j] = {} 24 | for i in range(D + 1): 25 | points[j][i] = cp.Variable((n_points - i, d)) 26 | 27 | # Binary variables. 28 | constraints = [] 29 | binaries = cp.Variable((N, K), boolean=True) 30 | for j in range(N): 31 | constraints.append(sum(binaries[j]) == 1) 32 | 33 | # Boundary conditions. 34 | for i, value in initial.items(): 35 | constraints.append(points[0][i][0] == value) 36 | for i, value in final.items(): 37 | constraints.append(points[N-1][i][-1] == value) 38 | 39 | # Box containment. 40 | for j in range(N): 41 | lj = cp.sum([np.array([L[k]] * n_points) * binaries[j, k] for k in range(K)]) 42 | uj = cp.sum([np.array([U[k]] * n_points) * binaries[j, k] for k in range(K)]) 43 | constraints.append(points[j][0] >= lj) 44 | constraints.append(points[j][0] <= uj) 45 | 46 | # Continuity and differentiability. 47 | for j in range(N - 1): 48 | for i in range(D + 1): 49 | constraints.append(points[j][i][-1] == points[j+1][i][0]) 50 | 51 | # Bezier dynamics. 52 | for j in range(N): 53 | for i in range(D): 54 | h = n_points - i - 1 55 | ci = T / N / h 56 | constraints.append(points[j][i][1:] - points[j][i][:-1] == ci * points[j][i+1]) 57 | 58 | # Cost function. 59 | cost = 0 60 | for i, ai in alpha.items(): 61 | if ai > 0: 62 | A = l2_matrix(n_points - i - 1, d) 63 | for j in range(N): 64 | p = cp.vec(points[j][i], order='C') 65 | cost += ai * cp.quad_form(p, A) * T / N 66 | 67 | # Solve problem. 68 | prob = cp.Problem(cp.Minimize(cost), constraints) 69 | prob.solve(**kwargs) 70 | 71 | # Reconstruct trajectory. 72 | beziers = [] 73 | for j in range(N): 74 | a = j * T / N 75 | b = (j + 1) * T / N 76 | beziers.append(BezierCurve(points[j][0].value, a, b)) 77 | path = CompositeBezierCurve(beziers) 78 | 79 | return path, prob.value, prob.solver_stats.solve_time 80 | -------------------------------------------------------------------------------- /fastpathplanning/polygonal.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | from time import time 4 | from itertools import accumulate 5 | from bisect import bisect 6 | from scipy.special import binom 7 | 8 | class Log: 9 | 10 | def __init__(self): 11 | print(self.write('Iter.') + \ 12 | self.write('Length') + \ 13 | self.write('N boxes')) 14 | self.size = 36 15 | print('-' * self.size) 16 | 17 | def write(self, s, size=10): 18 | s = str(s) 19 | s0 = list('|' + ' ' * size + '|') 20 | s0[2:2 + len(s)] = s 21 | return ''.join(s0) 22 | 23 | def update(self, i, length, n_boxes): 24 | print(self.write(i) + \ 25 | self.write('{:.2e}'.format(length)) + \ 26 | self.write(n_boxes)) 27 | 28 | def terminate(self, n_iters, length): 29 | print('-' * self.size) 30 | print(f'Polygonal phase terminated in {n_iters} iterations') 31 | print('Final length is {:.3e}'.format(length)) 32 | 33 | def solve_min_distance(B, box_seq, start, goal): 34 | tic = time() 35 | 36 | x = cp.Variable((len(box_seq) + 1, B.d)) 37 | 38 | boxes = [B.boxes[i] for i in box_seq] 39 | l1 = np.array([b.l for b in boxes[:-1]]) 40 | u1 = np.array([b.u for b in boxes[:-1]]) 41 | l2 = np.array([b.l for b in boxes[1:]]) 42 | u2 = np.array([b.u for b in boxes[1:]]) 43 | l = np.maximum(l1, l2) 44 | u = np.minimum(u1, u2) 45 | 46 | cost = cp.sum(cp.norm(x[1:] - x[:-1], 2, axis=1)) 47 | constr = [x[0] == start, x[1:-1] >= l, x[1:-1] <= u, x[-1] == goal] 48 | 49 | prob = cp.Problem(cp.Minimize(cost), constr) 50 | prob.solve(solver='CLARABEL') 51 | 52 | length = prob.value 53 | traj = x.value 54 | cvxpy_time = (time() - tic) - prob.solver_stats.solve_time 55 | 56 | return traj, length, cvxpy_time 57 | 58 | 59 | def iterative_planner(B, start, goal, box_seq, verbose=True, tol=1e-5, **kwargs): 60 | 61 | if verbose: 62 | log = Log() 63 | 64 | box_seq = np.array(box_seq) 65 | cvxpy_time = 0 66 | n_iters = 0 67 | while True: 68 | n_iters += 1 69 | 70 | box_seq = jump_box_repetitions(box_seq) 71 | traj, length, cvxpy_time_i = solve_min_distance(B, box_seq, start, goal, **kwargs) 72 | cvxpy_time += cvxpy_time_i 73 | 74 | if verbose: 75 | log.update(n_iters, length, len(box_seq)) 76 | 77 | box_seq, traj = merge_overlaps(box_seq, traj, tol) 78 | 79 | kinks = find_kinks(traj, tol) 80 | 81 | insert_k = [] 82 | insert_i = [] 83 | for k in kinks: 84 | 85 | i1 = box_seq[k - 1] 86 | i2 = box_seq[k] 87 | B1 = B.boxes[i1] 88 | B2 = B.boxes[i2] 89 | cached_finf = 0 90 | 91 | subset = list(B.inters[i1] & B.inters[i2]) 92 | for i in B.contain(traj[k], tol, subset): 93 | B3 = B.boxes[i] 94 | B13 = B1.intersect(B3) 95 | B23 = B2.intersect(B3) 96 | f = dual_box_insertion(*traj[k-1:k+2], B13, B23, tol) 97 | f2 = np.linalg.norm(f) 98 | finf = np.linalg.norm(f, ord=np.inf) 99 | 100 | if f2 > 1 + tol and finf > cached_finf + tol: 101 | cached_i = i 102 | cached_finf = finf 103 | 104 | if cached_finf > 0: 105 | insert_k.append(k) 106 | insert_i.append(cached_i) 107 | 108 | if len(insert_k) > 0: 109 | box_seq = np.insert(box_seq, insert_k, insert_i) 110 | else: 111 | if verbose: 112 | log.terminate(n_iters, length) 113 | return list(box_seq), traj, length, cvxpy_time 114 | 115 | def merge_overlaps(box_seq, traj, tol): 116 | 117 | keep = list(np.linalg.norm(traj[:-1] - traj[1:], axis=1) > tol) 118 | box_seq = box_seq[keep] 119 | traj = traj[keep + [True]] 120 | 121 | return box_seq, traj 122 | 123 | 124 | def find_kinks(traj, tol): 125 | ''' 126 | Detects the indices of the points where the trajectory bends. To do so, it 127 | uses the triangle inequality (division free): 128 | 129 | |traj[i] - traj[i-1]| + |traj[i+1] - traj[i]| > |traj[i+1] - traj[i-1]| 130 | 131 | implies that traj[i] is a kink. 132 | ''' 133 | 134 | xy = np.linalg.norm(traj[1:-1] - traj[:-2], axis=1) 135 | yz = np.linalg.norm(traj[2:] - traj[1:-1], axis=1) 136 | xz = np.linalg.norm(traj[2:] - traj[:-2], axis=1) 137 | 138 | return np.where(xy + yz - xz > tol)[0] + 1 139 | 140 | def dual_box_insertion(a, x, b, B13, B23, tol=1e-9): 141 | ''' 142 | Given a point x that solves 143 | 144 | minimize |x - a| + |x - b| 145 | subject to x in B1 cap B2, 146 | 147 | checks if x1 = x2 = x is also optimal for the following problem: 148 | 149 | minimize |x1 - a| + |x1 - x2| + |x2 - b| 150 | subject to x1 in B1 cap B3 151 | x2 in B2 cap B3. 152 | 153 | The arguments B13 and B23 denote B1 cap B3 and B2 cap B3, respectively. 154 | Assumes that neither a nor b is equal to x. 155 | 156 | Returns the dual variable f associated with the additional constraint 157 | x1 = x2 needed for the check. The multiplier f gives the rate of change of 158 | the cost as the points x1 and x2 are moved away from each other, i.e., 159 | f = d length / d (x2 - x1). If the two-norm of f is greater than one then 160 | inserting the box B3 decreases the cost. 161 | ''' 162 | 163 | lam1 = a - x 164 | lam2 = b - x 165 | lam1 /= np.linalg.norm(lam1) 166 | lam2 /= np.linalg.norm(lam2) 167 | 168 | L1, U1 = B13.active_set(x, tol) 169 | L2, U2 = B23.active_set(x, tol) 170 | 171 | fmin1 = - lam1 172 | fmax1 = - lam1 173 | fmin2 = lam2.copy() 174 | fmax2 = lam2.copy() 175 | 176 | fmin1[L1] = - np.inf 177 | fmax1[U1] = np.inf 178 | fmin2[U2] = - np.inf 179 | fmax2[L2] = np.inf 180 | 181 | fmin = np.maximum(fmin1, fmin2) 182 | fmax = np.minimum(fmax1, fmax2) 183 | f = np.clip(0, fmin, fmax) 184 | 185 | return f 186 | 187 | 188 | def jump_box_repetitions(box_seq): 189 | 190 | i = 0 191 | keep = [] 192 | while True: 193 | keep.append(i) 194 | b = box_seq[i] 195 | for j, c in enumerate(box_seq[i:][::-1]): 196 | if c == b: 197 | break 198 | i = len(box_seq) - j 199 | if i >= len(box_seq): 200 | break 201 | 202 | return box_seq[keep] 203 | -------------------------------------------------------------------------------- /fastpathplanning/smooth.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cvxpy as cp 3 | from time import time 4 | from itertools import accumulate 5 | from bisect import bisect 6 | from scipy.special import binom 7 | from fastpathplanning.bezier import BezierCurve, CompositeBezierCurve, l2_matrix 8 | 9 | class Log: 10 | 11 | def __init__(self): 12 | print(self.write('Iter.') + \ 13 | self.write('Cost') + \ 14 | self.write('% Decr.') + \ 15 | self.write('Kappa') + \ 16 | self.write('Accept')) 17 | self.size = 60 18 | print('-' * self.size) 19 | 20 | def write(self, s, size=10): 21 | s = str(s) 22 | s0 = list('|' + ' ' * size + '|') 23 | s0[2:2 + len(s)] = s 24 | return ''.join(s0) 25 | 26 | def update(self, i, cost, decr, kappa, accept): 27 | if not np.isnan(decr): 28 | decr = - round(decr * 100) 29 | print(self.write(i) + \ 30 | self.write('{:.2e}'.format(cost)) + \ 31 | self.write(f'{decr}') + \ 32 | self.write('{:.1e}'.format(kappa)) + \ 33 | self.write(accept)) 34 | 35 | def terminate(self, cost, perc_cost, cost_tol): 36 | print('-' * self.size) 37 | if perc_cost < cost_tol: 38 | print('Expected cost decrease is {:.1e}'.format(perc_cost)) 39 | print('...smaller than tolerance {:.1e}'.format(cost_tol)) 40 | print('Final cost is {:.3e}'.format(cost)) 41 | 42 | def seq_conv_prog(L, U, trav_times, alpha, initial, final, verbose=False): 43 | 44 | # Check inputs. 45 | assert L.shape == U.shape 46 | assert max(initial) < max(alpha) 47 | assert max(final) < max(alpha) 48 | 49 | # Algorithm parameters. 50 | kappa = 1 51 | omega = 3 52 | cost_tol = 1e-2 53 | 54 | # Initialize optimization problems. 55 | T = sum(trav_times) 56 | projection_step, cvxpy_time_proj = projection_problem(L, U, alpha, initial, final) 57 | tangent_step, cvxpy_time_tan = tangent_problem(L, U, alpha, initial, final, T) 58 | cvxpy_time = cvxpy_time_proj + cvxpy_time_tan 59 | 60 | # Solve initial Bezier problem. 61 | best_trav_times = trav_times 62 | best_path, best_cost, best_points, cvxpy_time_proj = projection_step(best_trav_times) 63 | cvxpy_time += cvxpy_time_proj 64 | n_iters = 1 65 | if verbose: 66 | log = Log() 67 | log.update(n_iters, best_cost, np.nan, np.nan, True) 68 | 69 | # Iterate retiming and Bezier. 70 | convergence = False 71 | perc_cost = np.inf 72 | while not convergence: 73 | 74 | # Tangent step. 75 | new_trav_times, tangent_cost, kappa_max, cvxpy_time_tan = tangent_step(kappa, best_trav_times, best_points) 76 | perc_cost = (best_cost - tangent_cost) / best_cost 77 | if perc_cost < cost_tol: 78 | convergence = True 79 | 80 | # Projection step. 81 | new_path, new_cost, new_points, cvxpy_time_proj = projection_step(new_trav_times) 82 | cvxpy_time += cvxpy_time_tan + cvxpy_time_proj 83 | n_iters += 1 84 | 85 | # If tangent step improved the trajectory. 86 | decr = new_cost - best_cost 87 | accept = decr < 0 88 | if verbose: 89 | log.update(n_iters, new_cost, decr / best_cost, kappa, accept) 90 | if accept: 91 | best_trav_times = new_trav_times 92 | best_path = new_path 93 | best_cost = new_cost 94 | best_points = new_points 95 | kappa = kappa_max / omega 96 | 97 | if verbose: 98 | log.terminate(best_cost, perc_cost, cost_tol) 99 | 100 | return best_path, cvxpy_time 101 | 102 | def control_points(dimension, D, n_boxes, n_points, type=cp.Variable): 103 | 104 | points = [] 105 | for j in range(n_boxes): 106 | points_j = [] 107 | for i in range(D + 1): 108 | points_j.append(type((n_points - i, dimension))) 109 | points.append(points_j) 110 | 111 | return points 112 | 113 | def boundary_conditions(points, initial, final): 114 | 115 | constraints = [] 116 | for i, value in initial.items(): 117 | constraints.append(points[0][i][0] == value) 118 | for i, value in final.items(): 119 | constraints.append(points[-1][i][-1] == value) 120 | 121 | return constraints 122 | 123 | def box_containment(points, L, U): 124 | 125 | constraints = [] 126 | for j, points_j in enumerate(points): 127 | n_points = points_j[0].shape[0] 128 | Lj = np.array([L[j]] * n_points) 129 | Uj = np.array([U[j]] * n_points) 130 | constraints.append(points_j[0] >= Lj) 131 | constraints.append(points_j[0] <= Uj) 132 | 133 | return constraints 134 | 135 | def differentiability(points): 136 | 137 | constraints = [] 138 | for points_jp1, points_j in zip(points[1:], points[:-1]): 139 | for points_jp1_i, points_j_i in zip(points_jp1, points_j): 140 | constraints.append(points_j_i[-1] == points_jp1_i[0]) 141 | 142 | return constraints 143 | 144 | def get_path(points, trav_times): 145 | 146 | beziers = [] 147 | a = 0 148 | for j, points_j in enumerate(points): 149 | b = a + trav_times[j] 150 | beziers.append(BezierCurve(points_j[0].value, a, b)) 151 | a = b 152 | 153 | return CompositeBezierCurve(beziers) 154 | 155 | def get_points(points): 156 | 157 | opt_points = [] 158 | for points_j in points: 159 | opt_points_j = [] 160 | for points_ji in points_j: 161 | opt_points_j.append(points_ji.value) 162 | opt_points.append(opt_points_j) 163 | 164 | return opt_points 165 | 166 | def projection_problem(L, U, alpha, initial, final): 167 | 168 | # Start clock. 169 | tic = time() 170 | 171 | # Problem size. 172 | n_boxes, d = L.shape 173 | D = max(alpha) 174 | n_points = (D + 1) * 2 175 | 176 | # Variables. 177 | points = control_points(d, D, n_boxes, n_points) 178 | 179 | # Parameters. 180 | trav_times = cp.Parameter(n_boxes, pos=True) 181 | 182 | # Constraints. 183 | constraints = boundary_conditions(points, initial, final) 184 | constraints += box_containment(points, L, U) 185 | constraints += differentiability(points) 186 | 187 | # Bezier dynamics. 188 | for j in range(n_boxes): 189 | for i in range(D): 190 | ci = trav_times[j] / (n_points - i - 1) 191 | constraints.append(points[j][i][1:] - points[j][i][:-1] == ci * points[j][i + 1]) 192 | 193 | # Cost function. 194 | cost = 0 195 | for i, ai in alpha.items(): 196 | if ai > 0: 197 | A = ai * l2_matrix(n_points - i - 1, d) 198 | for j in range(n_boxes): 199 | p = cp.vec(points[j][i], order='C') 200 | cost += trav_times[j] * cp.quad_form(p, A) 201 | 202 | # Construct problem. 203 | prob = cp.Problem(cp.Minimize(cost), constraints) 204 | assert prob.is_dcp(dpp=True) 205 | 206 | def projection_step(trav_times_val): 207 | 208 | # Solve problem. 209 | tic = time() 210 | trav_times.value = trav_times_val 211 | prob.solve(solver='CLARABEL', ignore_dpp=True) 212 | path = get_path(points, trav_times_val) 213 | opt_points = get_points(points) 214 | cvxpy_time = time() - tic - prob.solver_stats.solve_time 215 | 216 | return path, prob.value, opt_points, cvxpy_time 217 | 218 | cvxpy_time = time() - tic 219 | 220 | return projection_step, cvxpy_time 221 | 222 | def tangent_problem(L, U, alpha, initial, final, T): 223 | 224 | # Start clock. 225 | tic = time() 226 | 227 | # Problem size. 228 | n_boxes, d = L.shape 229 | D = max(alpha) 230 | n_points = (D + 1) * 2 231 | 232 | # Variables. 233 | points = control_points(d, D, n_boxes, n_points) 234 | 235 | # Parameters. 236 | trust_region = cp.Parameter(pos=True) 237 | trust_region_inv = cp.Parameter(pos=True) 238 | nom_trav_times = cp.Parameter(n_boxes, pos=True) 239 | nom_points = control_points(d, D, n_boxes, n_points, cp.Parameter) 240 | nom_points_scaled = control_points(d, D, n_boxes, n_points, cp.Parameter) 241 | 242 | # Constraints. 243 | constraints = boundary_conditions(points, initial, final) 244 | constraints += box_containment(points, L, U) 245 | constraints += differentiability(points) 246 | 247 | # Traversal times. 248 | trav_times = cp.Variable(n_boxes) 249 | constraints.append(sum(trav_times) == T) 250 | constraints.append(trav_times * trust_region >= nom_trav_times) 251 | constraints.append(trav_times * trust_region_inv <= nom_trav_times) 252 | 253 | # Bezier dynamics. 254 | for j in range(n_boxes): 255 | for i in range(D): 256 | ci = 1 / (n_points - i - 1) 257 | lin = trav_times[j] * nom_points[j][i + 1] \ 258 | + nom_trav_times[j] * points[j][i + 1] \ 259 | - nom_points_scaled[j][i + 1] 260 | point_diff = points[j][i][1:] - points[j][i][:-1] 261 | constraints.append(point_diff == ci * lin) 262 | 263 | # Cost function. 264 | cost = 0 265 | for i, ai in alpha.items(): 266 | if ai > 0: 267 | A = ai * l2_matrix(n_points - i - 1, d) 268 | A_chol = np.linalg.cholesky(A).T 269 | for j in range(n_boxes): 270 | point_diff = points[j][i - 1][1:] - points[j][i - 1][:-1] 271 | p = cp.vec(point_diff, order='C') * (n_points - i) 272 | cost += cp.quad_over_lin(A_chol @ p, trav_times[j]) 273 | 274 | # Construct problem. 275 | prob = cp.Problem(cp.Minimize(cost), constraints) 276 | assert prob.is_dcp(dpp=True) 277 | 278 | def tangent_step(kappa, trav_times_val, points_val): 279 | 280 | # Start clock. 281 | tic = time() 282 | 283 | # Solve problem. 284 | trust_region.value = 1 + kappa 285 | trust_region_inv.value = 1 / trust_region.value 286 | nom_trav_times.value = trav_times_val 287 | for j in range(n_boxes): 288 | for i in range(D + 1): 289 | nom_points[j][i].value = points_val[j][i] 290 | nom_points_scaled[j][i].value = points_val[j][i] * trav_times_val[j] 291 | prob.solve(solver='CLARABEL', ignore_dpp=True) 292 | 293 | # Trust region update. 294 | opt_trav_times = trav_times.value 295 | ratios = np.divide(opt_trav_times, trav_times_val) 296 | kappa_max = max(max(ratios), 1 / min(ratios)) - 1 297 | cvxpy_time = time() - tic - prob.solver_stats.solve_time 298 | 299 | return opt_trav_times, prob.value, kappa_max, cvxpy_time 300 | 301 | cvxpy_time = time() - tic 302 | 303 | return tangent_step, cvxpy_time 304 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools >= 18.0", "wheel"] 3 | build-backend = "setuptools.build_meta" -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy>=1.17.5 2 | cvxpy>=1.3 3 | networkx 4 | scipy 5 | clarabel 6 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import subprocess 3 | from setuptools import setup, find_packages 4 | 5 | with open("README.md", "r") as fh: 6 | long_description = fh.read() 7 | 8 | # get all the git tags from the cmd line that follow our versioning pattern 9 | git_tags = subprocess.Popen( 10 | ["git", "tag", "--list", "v*[0-9]", "--sort=version:refname"], 11 | stdout=subprocess.PIPE, 12 | ) 13 | tags = git_tags.stdout.read() 14 | git_tags.stdout.close() 15 | tags = tags.decode("utf-8").split("\n") 16 | tags.sort() 17 | 18 | # PEP 440 won't accept the v in front, so here we remove it, strip the new line and decode the byte stream 19 | VERSION_FROM_GIT_TAG = tags[-1][1:] 20 | 21 | setup( 22 | name="fastpathplanning", 23 | version=VERSION_FROM_GIT_TAG, # Required 24 | setup_requires=["setuptools>=18.0"], 25 | packages=find_packages(exclude=["notebooks"]), # Required 26 | install_requires=[ 27 | "numpy >= 1.17.5", 28 | "scipy", 29 | "cvxpy", 30 | "networkx", 31 | ], 32 | description="An algorithm for path planning in sequences of safe boxes", 33 | long_description=long_description, 34 | long_description_content_type="text/markdown", 35 | url="https://github.com/cvxgrp/fpp.py", 36 | classifiers=[ 37 | "Programming Language :: Python :: 3", 38 | ], 39 | authors=[ "Tobia Marcucci", "Parth Nobel"], 40 | author_emails=["tobiam@mit.edu", "ptnobel@stanford.edu"], 41 | ) 42 | -------------------------------------------------------------------------------- /tests/test_placeholder.py: -------------------------------------------------------------------------------- 1 | # Rename the file as desired for your testing structure/type, 2 | # or add new files following the format below. 3 | # E.G. test_.py 4 | # This contains an example test that always passes. 5 | # When ready, you can add proper tests for your algorithm in this folder. 6 | 7 | def test_always_passes(): 8 | assert 1 + 1 == 2 9 | --------------------------------------------------------------------------------