├── .gitignore ├── .vscode └── launch.json ├── LICENSE ├── README.md ├── examples ├── rrt │ ├── rrt_2d.py │ ├── rrt_2d_with_random_obstacles.py │ ├── rrt_3d.py │ └── rrt_3d_with_random_obstacles.py ├── rrt_bid │ ├── rrt_star_bid_2d.py │ └── rrt_star_bid_3d.py ├── rrt_bid_h │ ├── rrt_star_bid_h_2d.py │ ├── rrt_star_bid_h_3d.py │ └── rrt_star_bid_h_3d_with_random_obstacles.py ├── rrt_connect │ ├── rrt_connect_2d.py │ ├── rrt_connect_2d_with_random_obstacles.py │ ├── rrt_connect_3d.py │ └── rrt_connect_3d_with_random_obstacles.py └── rrt_star │ ├── rrt_star_2d.py │ ├── rrt_star_2d_with_random_obstacles.py │ ├── rrt_star_3d.py │ └── rrt_star_3d_with_random_obstacles.py ├── rrt_algorithms ├── __init__.py ├── rrt │ ├── heuristics.py │ ├── rrt.py │ ├── rrt_base.py │ ├── rrt_connect.py │ ├── rrt_star.py │ ├── rrt_star_bid.py │ ├── rrt_star_bid_h.py │ └── tree.py ├── search_space │ ├── __init__.py │ └── search_space.py └── utilities │ ├── __init__.py │ ├── geometry.py │ ├── obstacle_generation.py │ └── plotting.py └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.gitignore.io/api/osx,linux,python,pycharm,windows 3 | 4 | ### Linux ### 5 | *~ 6 | 7 | # temporary files which can be created if a process still has a handle open of a deleted file 8 | .fuse_hidden* 9 | 10 | # KDE directory preferences 11 | .directory 12 | 13 | # Linux trash folder which might appear on any partition or disk 14 | .Trash-* 15 | 16 | # .nfs files are created when an open file is removed but is still being accessed 17 | .nfs* 18 | 19 | ### OSX ### 20 | *.DS_Store 21 | .AppleDouble 22 | .LSOverride 23 | 24 | # Icon must end with two \r 25 | Icon 26 | 27 | # Thumbnails 28 | ._* 29 | 30 | # Files that might appear in the root of a volume 31 | .DocumentRevisions-V100 32 | .fseventsd 33 | .Spotlight-V100 34 | .TemporaryItems 35 | .Trashes 36 | .VolumeIcon.icns 37 | .com.apple.timemachine.donotpresent 38 | 39 | # Directories potentially created on remote AFP share 40 | .AppleDB 41 | .AppleDesktop 42 | Network Trash Folder 43 | Temporary Items 44 | .apdisk 45 | 46 | ### PyCharm ### 47 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm 48 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 49 | 50 | # User-specific stuff: 51 | .idea/**/workspace.xml 52 | .idea/**/tasks.xml 53 | .idea/dictionaries 54 | 55 | # Sensitive or high-churn files: 56 | .idea/**/dataSources/ 57 | .idea/**/dataSources.ids 58 | .idea/**/dataSources.xml 59 | .idea/**/dataSources.local.xml 60 | .idea/**/sqlDataSources.xml 61 | .idea/**/dynamic.xml 62 | .idea/**/uiDesigner.xml 63 | 64 | # Gradle: 65 | .idea/**/gradle.xml 66 | .idea/**/libraries 67 | 68 | # CMake 69 | cmake-build-debug/ 70 | 71 | # Mongo Explorer plugin: 72 | .idea/**/mongoSettings.xml 73 | 74 | ## File-based project format: 75 | *.iws 76 | 77 | ## Plugin-specific files: 78 | 79 | # IntelliJ 80 | /out/ 81 | 82 | # mpeltonen/sbt-idea plugin 83 | .idea_modules/ 84 | 85 | # JIRA plugin 86 | atlassian-ide-plugin.xml 87 | 88 | # Cursive Clojure plugin 89 | .idea/replstate.xml 90 | 91 | # Crashlytics plugin (for Android Studio and IntelliJ) 92 | com_crashlytics_export_strings.xml 93 | crashlytics.properties 94 | crashlytics-build.properties 95 | fabric.properties 96 | 97 | ### PyCharm Patch ### 98 | # Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721 99 | 100 | # *.iml 101 | # modules.xml 102 | # .idea/misc.xml 103 | # *.ipr 104 | 105 | # Sonarlint plugin 106 | .idea/sonarlint 107 | 108 | ### Python ### 109 | # Byte-compiled / optimized / DLL files 110 | __pycache__/ 111 | *.py[cod] 112 | *$py.class 113 | 114 | # C extensions 115 | *.so 116 | 117 | # Distribution / packaging 118 | .Python 119 | env/ 120 | build/ 121 | develop-eggs/ 122 | dist/ 123 | downloads/ 124 | eggs/ 125 | .eggs/ 126 | lib/ 127 | lib64/ 128 | parts/ 129 | sdist/ 130 | var/ 131 | wheels/ 132 | *.egg-info/ 133 | .installed.cfg 134 | *.egg 135 | 136 | # PyInstaller 137 | # Usually these files are written by a python script from a template 138 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 139 | *.manifest 140 | *.spec 141 | 142 | # Installer logs 143 | pip-log.txt 144 | pip-delete-this-directory.txt 145 | 146 | # Unit test / coverage reports 147 | htmlcov/ 148 | .tox/ 149 | .coverage 150 | .coverage.* 151 | .cache 152 | nosetests.xml 153 | coverage.xml 154 | *,cover 155 | .hypothesis/ 156 | 157 | # Translations 158 | *.mo 159 | *.pot 160 | 161 | # Django stuff: 162 | *.log 163 | local_settings.py 164 | 165 | # Flask stuff: 166 | instance/ 167 | .webassets-cache 168 | 169 | # Scrapy stuff: 170 | .scrapy 171 | 172 | # Sphinx documentation 173 | docs/_build/ 174 | 175 | # PyBuilder 176 | target/ 177 | 178 | # Jupyter Notebook 179 | .ipynb_checkpoints 180 | 181 | # pyenv 182 | .python-version 183 | 184 | # celery beat schedule file 185 | celerybeat-schedule 186 | 187 | # SageMath parsed files 188 | *.sage.py 189 | 190 | # dotenv 191 | .env 192 | 193 | # virtualenv 194 | .venv 195 | venv/ 196 | ENV/ 197 | 198 | # Spyder project settings 199 | .spyderproject 200 | .spyproject 201 | 202 | # Rope project settings 203 | .ropeproject 204 | 205 | # mkdocs documentation 206 | /site 207 | 208 | ### Windows ### 209 | # Windows thumbnail cache files 210 | Thumbs.db 211 | ehthumbs.db 212 | ehthumbs_vista.db 213 | 214 | # Folder config file 215 | Desktop.ini 216 | 217 | # Recycle Bin used on file shares 218 | $RECYCLE.BIN/ 219 | 220 | # Windows Installer files 221 | *.cab 222 | *.msi 223 | *.msm 224 | *.msp 225 | 226 | # Windows shortcuts 227 | *.lnk 228 | 229 | # End of https://www.gitignore.io/api/osx,linux,python,pycharm,windows 230 | 231 | # Output 232 | output 233 | 234 | \.idea/ 235 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Python: Current File", 5 | "type": "python", 6 | "request": "launch", 7 | "program": "${file}", 8 | "console": "integratedTerminal", 9 | } 10 | ] 11 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 motion-planning 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rrt 2 | Collection of rrt-based algorithms that scale to n-dimensions: 3 | - rrt 4 | - rrt* (rrt-star) 5 | - rrt* (bidirectional) 6 | - rrt* (bidriectional, lazy shortening) 7 | - rrt connect 8 | 9 | Utilizes [R-trees](https://en.wikipedia.org/wiki/R-tree) to improve performance by avoiding point-wise collision-checking and distance-checking. 10 | 11 | This would not have been possible without Steven M. LaValle's excellent [Planning Algorithms](https://lavalle.pl/planning/) book, specifically [Chapter 5, Section 5: Sampling-Based Motion Planning, Rapidly Exploring Dense Trees](https://lavalle.pl/planning/node230.html). 12 | 13 | ## Requirements 14 | - [Python 3+](https://www.python.org/downloads/) 15 | - [NumPy](http://www.numpy.org/) 16 | - [Rtree](https://pypi.python.org/pypi/Rtree/) 17 | - [Plotly](https://plot.ly/python/getting-started/) (only needed for plotting) 18 | 19 | ## Usage 20 | Define an n-dimensional Search Space, and n-dimensional obstacles within that space. Assign start and goal locations as well as the number of iterations to expand the tree before testing for connectivity with the goal, and the max number of overall iterations. 21 | 22 | ### Search Space 23 | Assign bounds to Search Space in form: `[(x_lower, x_upper), (y_lower, y_upper), ...]` 24 | 25 | ### Start and Goal 26 | Points represented by tuples of form: `(x, y, ...)` 27 | 28 | ### Obstacles 29 | Axis-aligned (hyper)rectangles represented by a tuples of form `(x_lower, y_lower, ..., x_upper, y_upper, ...)` 30 | 31 | Non-axis aligned (hyper)rectangles or other obstacle representations should also work, provided that `collision_free` and `obstacle_free` are updated to work with the new obstacles. 32 | 33 | ### Resolution 34 | Assign resolution of edges: 35 | - `q`: Distance away from existing vertices to probe. 36 | - `r`: Discretization length to use for edges when sampling along them to check for collisions. Higher numbers run faster, but may lead to undetected collisions. 37 | 38 | ### Examples 39 | Visualization examples can be found for rrt and rrt* in both 2 and 3 dimensions. 40 | - [2D RRT](https://plot.ly/~szanlongo/79/plot/) 41 | - [3D RRT](https://plot.ly/~szanlongo/81/plot/) 42 | - [2D RRT*](https://plot.ly/~szanlongo/83/plot/) 43 | - [3D RRT*](https://plot.ly/~szanlongo/89/plot/) 44 | - [2D Bidirectional RRT*](https://plot.ly/~szanlongo/85/plot/) 45 | - [3D Bidirectional RRT*](https://plot.ly/~szanlongo/87/plot/) 46 | - [2D Heuristic Bidirectional RRT*](https://plot.ly/~szanlongo/91/plot/) 47 | - [3D Heuristic Bidirectional RRT*](https://plot.ly/~szanlongo/93/plot/) 48 | 49 | ## Contributing 50 | 51 | 1. Fork it! 52 | 2. Create your feature branch: `git checkout -b my-new-feature` 53 | 3. Commit your changes: `git commit -am 'Add some feature'` 54 | 4. Push to the branch: `git push origin my-new-feature` 55 | 5. Submit a pull request :D 56 | 57 | ## References 58 | 59 | 1. Steven Michael Lavalle. [Planning Algorithms.](https://lavalle.pl/planning/) New York (Ny), Cambridge University Press, 2014, pp. 228–237, lavalle.pl/planning/. 60 | 2. LaValle, Steven. "[Rapidly-exploring random trees: A new tool for path planning.](https://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf)" Research Report 9811 (1998). 61 | 3. Kuffner, James J., and Steven M. LaValle. "[RRT-connect: An efficient approach to single-query path planning.](https://www.cs.cmu.edu/afs/cs/academic/class/15494-s14/readings/kuffner_icra2000.pdf)" Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065). Vol. 2. IEEE, 2000. 62 | 63 | ## License 64 | 65 | [MIT License](https://github.com/motion-planning/rrt-algorithms/blob/master/LICENSE) 66 | -------------------------------------------------------------------------------- /examples/rrt/rrt_2d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt import RRT 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), 12 | (60, 20, 80, 40), (60, 60, 80, 80)]) 13 | x_init = (0, 0) # starting location 14 | x_goal = (100, 100) # goal location 15 | 16 | q = 8 # length of tree edges 17 | r = 1 # length of smallest edge to check for intersection with obstacles 18 | max_samples = 1024 # max number of samples to take before timing out 19 | prc = 0.1 # probability of checking for a connection to goal 20 | 21 | # create search space 22 | X = SearchSpace(X_dimensions, Obstacles) 23 | 24 | # create rrt_search 25 | rrt = RRT(X, q, x_init, x_goal, max_samples, r, prc) 26 | path = rrt.rrt_search() 27 | 28 | # plot 29 | plot = Plot("rrt_2d") 30 | plot.plot_tree(X, rrt.trees) 31 | if path is not None: 32 | plot.plot_path(X, path) 33 | plot.plot_obstacles(X, Obstacles) 34 | plot.plot_start(X, x_init) 35 | plot.plot_goal(X, x_goal) 36 | plot.draw(auto_open=True) 37 | -------------------------------------------------------------------------------- /examples/rrt/rrt_2d_with_random_obstacles.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt import RRT 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles 8 | from rrt_algorithms.utilities.plotting import Plot 9 | 10 | X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space 11 | x_init = (0, 0) # starting location 12 | x_goal = (100, 100) # goal location 13 | 14 | q = 8 # length of tree edges 15 | r = 1 # length of smallest edge to check for intersection with obstacles 16 | max_samples = 1024 # max number of samples to take before timing out 17 | prc = 0.1 # probability of checking for a connection to goal 18 | 19 | # create search space 20 | X = SearchSpace(X_dimensions) 21 | n = 50 22 | Obstacles = generate_random_obstacles(X, x_init, x_goal, n) 23 | # create rrt_search 24 | rrt = RRT(X, q, x_init, x_goal, max_samples, r, prc) 25 | path = rrt.rrt_search() 26 | 27 | # plot 28 | plot = Plot("rrt_2d_with_random_obstacles") 29 | plot.plot_tree(X, rrt.trees) 30 | if path is not None: 31 | plot.plot_path(X, path) 32 | plot.plot_obstacles(X, Obstacles) 33 | plot.plot_start(X, x_init) 34 | plot.plot_goal(X, x_goal) 35 | plot.draw(auto_open=True) 36 | -------------------------------------------------------------------------------- /examples/rrt/rrt_3d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt import RRT 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array( 12 | [(20, 20, 20, 40, 40, 40), (20, 20, 60, 40, 40, 80), (20, 60, 20, 40, 80, 40), (60, 60, 20, 80, 80, 40), 13 | (60, 20, 20, 80, 40, 40), (60, 20, 60, 80, 40, 80), (20, 60, 60, 40, 80, 80), (60, 60, 60, 80, 80, 80)]) 14 | x_init = (0, 0, 0) # starting location 15 | x_goal = (100, 100, 100) # goal location 16 | 17 | q = 8 # length of tree edges 18 | r = 1 # length of smallest edge to check for intersection with obstacles 19 | max_samples = 1024 # max number of samples to take before timing out 20 | prc = 0.1 # probability of checking for a connection to goal 21 | 22 | # create Search Space 23 | X = SearchSpace(X_dimensions, Obstacles) 24 | 25 | # create rrt_search 26 | rrt = RRT(X, q, x_init, x_goal, max_samples, r, prc) 27 | path = rrt.rrt_search() 28 | 29 | # plot 30 | plot = Plot("rrt_3d") 31 | plot.plot_tree(X, rrt.trees) 32 | if path is not None: 33 | plot.plot_path(X, path) 34 | plot.plot_obstacles(X, Obstacles) 35 | plot.plot_start(X, x_init) 36 | plot.plot_goal(X, x_goal) 37 | plot.draw(auto_open=True) 38 | -------------------------------------------------------------------------------- /examples/rrt/rrt_3d_with_random_obstacles.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt import RRT 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles 8 | from rrt_algorithms.utilities.plotting import Plot 9 | 10 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 11 | x_init = (0, 0, 0) # starting location 12 | x_goal = (100, 100, 100) # goal location 13 | 14 | q = 8 # length of tree edges 15 | r = 1 # length of smallest edge to check for intersection with obstacles 16 | max_samples = 1024 # max number of samples to take before timing out 17 | prc = 0.1 # probability of checking for a connection to goal 18 | 19 | # create Search Space 20 | X = SearchSpace(X_dimensions) 21 | n = 50 22 | Obstacles = generate_random_obstacles(X, x_init, x_goal, n) 23 | # create rrt_search 24 | rrt = RRT(X, q, x_init, x_goal, max_samples, r, prc) 25 | path = rrt.rrt_search() 26 | 27 | # plot 28 | plot = Plot("rrt_3d_with_random_obstacles") 29 | plot.plot_tree(X, rrt.trees) 30 | if path is not None: 31 | plot.plot_path(X, path) 32 | plot.plot_obstacles(X, Obstacles) 33 | plot.plot_start(X, x_init) 34 | plot.plot_goal(X, x_goal) 35 | plot.draw(auto_open=True) 36 | -------------------------------------------------------------------------------- /examples/rrt_bid/rrt_star_bid_2d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star_bid import RRTStarBidirectional 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), 12 | (60, 20, 80, 40), (60, 60, 80, 80)]) 13 | x_init = (0, 0) # starting location 14 | x_goal = (100, 100) # goal location 15 | 16 | q = 8 # length of tree edges 17 | r = 1 # length of smallest edge to check for intersection with obstacles 18 | max_samples = 1024 # max number of samples to take before timing out 19 | rewire_count = 32 # optional, number of nearby branches to rewire 20 | prc = 0.1 # probability of checking for a connection to goal 21 | 22 | # create Search Space 23 | X = SearchSpace(X_dimensions, Obstacles) 24 | 25 | # create rrt_search 26 | rrt = RRTStarBidirectional( 27 | X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 28 | path = rrt.rrt_star_bidirectional() 29 | 30 | # plot 31 | plot = Plot("rrt_star_bid_2d") 32 | plot.plot_tree(X, rrt.trees) 33 | if path is not None: 34 | plot.plot_path(X, path) 35 | plot.plot_obstacles(X, Obstacles) 36 | plot.plot_start(X, x_init) 37 | plot.plot_goal(X, x_goal) 38 | plot.draw(auto_open=True) 39 | -------------------------------------------------------------------------------- /examples/rrt_bid/rrt_star_bid_3d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star_bid import RRTStarBidirectional 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array( 12 | [(20, 20, 20, 40, 40, 40), (20, 20, 60, 40, 40, 80), (20, 60, 20, 40, 80, 40), (60, 60, 20, 80, 80, 40), 13 | (60, 20, 20, 80, 40, 40), (60, 20, 60, 80, 40, 80), (20, 60, 60, 40, 80, 80), (60, 60, 60, 80, 80, 80)]) 14 | x_init = (0, 0, 0) # starting location 15 | x_goal = (100, 100, 100) # goal location 16 | 17 | q = 8 # length of tree edges 18 | r = 1 # length of smallest edge to check for intersection with obstacles 19 | max_samples = 1024 # max number of samples to take before timing out 20 | rewire_count = 32 # optional, number of nearby branches to rewire 21 | prc = 0.01 # probability of checking for a connection to goal 22 | 23 | # create Search Space 24 | X = SearchSpace(X_dimensions, Obstacles) 25 | 26 | # create rrt_search 27 | rrt = RRTStarBidirectional( 28 | X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 29 | path = rrt.rrt_star_bidirectional() 30 | 31 | # plot 32 | plot = Plot("rrt_star_bid_3d") 33 | plot.plot_tree(X, rrt.trees) 34 | if path is not None: 35 | plot.plot_path(X, path) 36 | plot.plot_obstacles(X, Obstacles) 37 | plot.plot_start(X, x_init) 38 | plot.plot_goal(X, x_goal) 39 | plot.draw(auto_open=True) 40 | -------------------------------------------------------------------------------- /examples/rrt_bid_h/rrt_star_bid_h_2d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star_bid_h import RRTStarBidirectionalHeuristic 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), 12 | (60, 20, 80, 40), (60, 60, 80, 80)]) 13 | x_init = (0, 0) # starting location 14 | x_goal = (100, 100) # goal location 15 | 16 | q = 8 # length of tree edges 17 | r = 1 # length of smallest edge to check for intersection with obstacles 18 | max_samples = 1024 # max number of samples to take before timing out 19 | rewire_count = 32 # optional, number of nearby branches to rewire 20 | prc = 0.01 # probability of checking for a connection to goal 21 | 22 | # create Search Space 23 | X = SearchSpace(X_dimensions, Obstacles) 24 | 25 | # create rrt_search 26 | rrt = RRTStarBidirectionalHeuristic(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 27 | path = rrt.rrt_star_bid_h() 28 | 29 | # plot 30 | plot = Plot("rrt_star_bid_h_2d") 31 | plot.plot_tree(X, rrt.trees) 32 | if path is not None: 33 | plot.plot_path(X, path) 34 | plot.plot_obstacles(X, Obstacles) 35 | plot.plot_start(X, x_init) 36 | plot.plot_goal(X, x_goal) 37 | plot.draw(auto_open=True) 38 | -------------------------------------------------------------------------------- /examples/rrt_bid_h/rrt_star_bid_h_3d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star_bid_h import RRTStarBidirectionalHeuristic 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array( 12 | [(20, 20, 20, 40, 40, 40), (20, 20, 60, 40, 40, 80), (20, 60, 20, 40, 80, 40), (60, 60, 20, 80, 80, 40), 13 | (60, 20, 20, 80, 40, 40), (60, 20, 60, 80, 40, 80), (20, 60, 60, 40, 80, 80), (60, 60, 60, 80, 80, 80)]) 14 | x_init = (0, 0, 0) # starting location 15 | x_goal = (100, 100, 100) # goal location 16 | 17 | q = 8 # length of tree edges 18 | r = 1 # length of smallest edge to check for intersection with obstacles 19 | max_samples = 1024 # max number of samples to take before timing out 20 | rewire_count = 32 # optional, number of nearby branches to rewire 21 | prc = 0.01 # probability of checking for a connection to goal 22 | 23 | # create Search Space 24 | X = SearchSpace(X_dimensions, Obstacles) 25 | 26 | # create rrt_search 27 | rrt = RRTStarBidirectionalHeuristic(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 28 | path = rrt.rrt_star_bid_h() 29 | 30 | # plot 31 | plot = Plot("rrt_star_bid_h_3d") 32 | plot.plot_tree(X, rrt.trees) 33 | if path is not None: 34 | plot.plot_path(X, path) 35 | plot.plot_obstacles(X, Obstacles) 36 | plot.plot_start(X, x_init) 37 | plot.plot_goal(X, x_goal) 38 | plot.draw(auto_open=True) 39 | -------------------------------------------------------------------------------- /examples/rrt_bid_h/rrt_star_bid_h_3d_with_random_obstacles.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star_bid_h import RRTStarBidirectionalHeuristic 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles 8 | from rrt_algorithms.utilities.plotting import Plot 9 | 10 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 11 | # obstacles 12 | x_init = (0, 0, 0) # starting location 13 | x_goal = (100, 100, 100) # goal location 14 | 15 | q = 8 # length of tree edges 16 | r = 1 # length of smallest edge to check for intersection with obstacles 17 | max_samples = 1024 # max number of samples to take before timing out 18 | rewire_count = 32 # optional, number of nearby branches to rewire 19 | prc = 0.01 # probability of checking for a connection to goal 20 | 21 | # create Search Space 22 | X = SearchSpace(X_dimensions) 23 | n = 50 24 | Obstacles = generate_random_obstacles(X, x_init, x_goal, n) 25 | 26 | # create rrt_search 27 | rrt = RRTStarBidirectionalHeuristic(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 28 | path = rrt.rrt_star_bid_h() 29 | 30 | # plot 31 | plot = Plot("rrt_star_bid_h_3d_with_random_obstacles") 32 | plot.plot_tree(X, rrt.trees) 33 | if path is not None: 34 | plot.plot_path(X, path) 35 | plot.plot_obstacles(X, Obstacles) 36 | plot.plot_start(X, x_init) 37 | plot.plot_goal(X, x_goal) 38 | plot.draw(auto_open=True) 39 | -------------------------------------------------------------------------------- /examples/rrt_connect/rrt_connect_2d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_connect import RRTConnect 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), 12 | (60, 20, 80, 40), (60, 60, 80, 80)]) 13 | x_init = (0, 0) # starting location 14 | x_goal = (100, 100) # goal location 15 | 16 | q = 2 # length of tree edges 17 | r = 0.5 # length of smallest edge to check for intersection with obstacles 18 | max_samples = 1024 # max number of samples to take before timing out 19 | prc = 0.1 # probability of checking for a connection to goal 20 | 21 | # create search space 22 | X = SearchSpace(X_dimensions, Obstacles) 23 | 24 | # create rrt_search 25 | rrt_connect = RRTConnect(X, q, x_init, x_goal, max_samples, r, prc) 26 | path = rrt_connect.rrt_connect() 27 | # plot 28 | plot = Plot("rrt_connect2d") 29 | plot.plot_tree(X, rrt_connect.trees) 30 | if path is not None: 31 | plot.plot_path(X, path) 32 | plot.plot_obstacles(X, Obstacles) 33 | plot.plot_start(X, x_init) 34 | plot.plot_goal(X, x_goal) 35 | plot.draw(auto_open=True) 36 | -------------------------------------------------------------------------------- /examples/rrt_connect/rrt_connect_2d_with_random_obstacles.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_connect import RRTConnect 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles 8 | from rrt_algorithms.utilities.plotting import Plot 9 | 10 | X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space 11 | x_init = (0, 0) # starting location 12 | x_goal = (100, 100) # goal location 13 | 14 | q = 2 # length of tree edges 15 | r = 0.5 # length of smallest edge to check for intersection with obstacles 16 | max_samples = 2048 # max number of samples to take before timing out 17 | prc = 0.1 # probability of checking for a connection to goal 18 | 19 | # create search space 20 | X = SearchSpace(X_dimensions) 21 | n = 50 22 | Obstacles = generate_random_obstacles(X, x_init, x_goal, n) 23 | 24 | # create rrt_search 25 | rrt_connect = RRTConnect(X, q, x_init, x_goal, max_samples, r, prc) 26 | path = rrt_connect.rrt_connect() 27 | # plot 28 | plot = Plot("rrt_connect_2d_with_random_obstacles") 29 | plot.plot_tree(X, rrt_connect.trees) 30 | if path is not None: 31 | plot.plot_path(X, path) 32 | plot.plot_obstacles(X, Obstacles) 33 | plot.plot_start(X, x_init) 34 | plot.plot_goal(X, x_goal) 35 | plot.draw(auto_open=True) 36 | -------------------------------------------------------------------------------- /examples/rrt_connect/rrt_connect_3d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_connect import RRTConnect 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array( 12 | [(20, 20, 20, 40, 40, 40), (20, 20, 60, 40, 40, 80), (20, 60, 20, 40, 80, 40), (60, 60, 20, 80, 80, 40), 13 | (60, 20, 20, 80, 40, 40), (60, 20, 60, 80, 40, 80), (20, 60, 60, 40, 80, 80), (60, 60, 60, 80, 80, 80)]) 14 | x_init = (0, 0, 0) # starting location 15 | x_goal = (100, 100, 100) # goal location 16 | 17 | q = 2 # length of tree edges 18 | r = 0.5 # length of smallest edge to check for intersection with obstacles 19 | max_samples = 1024 # max number of samples to take before timing out 20 | prc = 0.1 # probability of checking for a connection to goal 21 | 22 | # create search space 23 | X = SearchSpace(X_dimensions, Obstacles) 24 | 25 | # create rrt_search 26 | rrt_connect = RRTConnect(X, q, x_init, x_goal, max_samples, r, prc) 27 | path = rrt_connect.rrt_connect() 28 | # plot 29 | plot = Plot("rrt_connect_3d") 30 | plot.plot_tree(X, rrt_connect.trees) 31 | if path is not None: 32 | plot.plot_path(X, path) 33 | plot.plot_obstacles(X, Obstacles) 34 | plot.plot_start(X, x_init) 35 | plot.plot_goal(X, x_goal) 36 | plot.draw(auto_open=True) 37 | -------------------------------------------------------------------------------- /examples/rrt_connect/rrt_connect_3d_with_random_obstacles.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_connect import RRTConnect 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles 8 | from rrt_algorithms.utilities.plotting import Plot 9 | 10 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 11 | # obstacles 12 | x_init = (0, 0, 0) # starting location 13 | x_goal = (100, 100, 100) # goal location 14 | 15 | q = 2 # length of tree edges 16 | r = 0.5 # length of smallest edge to check for intersection with obstacles 17 | max_samples = 1024 # max number of samples to take before timing out 18 | prc = 0.1 # probability of checking for a connection to goal 19 | 20 | # create search space 21 | X = SearchSpace(X_dimensions) 22 | n = 50 23 | Obstacles = generate_random_obstacles(X, x_init, x_goal, n) 24 | 25 | # create rrt_search 26 | rrt_connect = RRTConnect(X, q, x_init, x_goal, max_samples, r, prc) 27 | path = rrt_connect.rrt_connect() 28 | # plot 29 | plot = Plot("rrt_connect_3d_with_random_obstacles") 30 | plot.plot_tree(X, rrt_connect.trees) 31 | if path is not None: 32 | plot.plot_path(X, path) 33 | plot.plot_obstacles(X, Obstacles) 34 | plot.plot_start(X, x_init) 35 | plot.plot_goal(X, x_goal) 36 | plot.draw(auto_open=True) 37 | -------------------------------------------------------------------------------- /examples/rrt_star/rrt_star_2d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star import RRTStar 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array([(20, 20, 40, 40), (20, 60, 40, 80), 12 | (60, 20, 80, 40), (60, 60, 80, 80)]) 13 | x_init = (0, 0) # starting location 14 | x_goal = (100, 100) # goal location 15 | 16 | q = 8 # length of tree edges 17 | r = 1 # length of smallest edge to check for intersection with obstacles 18 | max_samples = 1024 # max number of samples to take before timing out 19 | rewire_count = 32 # optional, number of nearby branches to rewire 20 | prc = 0.1 # probability of checking for a connection to goal 21 | 22 | # create Search Space 23 | X = SearchSpace(X_dimensions, Obstacles) 24 | 25 | # create rrt_search 26 | rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 27 | path = rrt.rrt_star() 28 | 29 | # plot 30 | plot = Plot("rrt_star_2d") 31 | plot.plot_tree(X, rrt.trees) 32 | if path is not None: 33 | plot.plot_path(X, path) 34 | plot.plot_obstacles(X, Obstacles) 35 | plot.plot_start(X, x_init) 36 | plot.plot_goal(X, x_goal) 37 | plot.draw(auto_open=True) 38 | -------------------------------------------------------------------------------- /examples/rrt_star/rrt_star_2d_with_random_obstacles.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star import RRTStar 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles 8 | from rrt_algorithms.utilities.plotting import Plot 9 | 10 | X_dimensions = np.array([(0, 100), (0, 100)]) # dimensions of Search Space 11 | x_init = (0, 0) # starting location 12 | x_goal = (100, 100) # goal location 13 | 14 | q = 8 # length of tree edges 15 | r = 1 # length of smallest edge to check for intersection with obstacles 16 | max_samples = 1024 # max number of samples to take before timing out 17 | rewire_count = 32 # optional, number of nearby branches to rewire 18 | prc = 0.1 # probability of checking for a connection to goal 19 | 20 | # create Search Space 21 | X = SearchSpace(X_dimensions) 22 | n = 50 23 | Obstacles = generate_random_obstacles(X, x_init, x_goal, n) 24 | # create rrt_search 25 | rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 26 | path = rrt.rrt_star() 27 | 28 | # plot 29 | plot = Plot("rrt_star_2d_with_random_obstacles") 30 | plot.plot_tree(X, rrt.trees) 31 | if path is not None: 32 | plot.plot_path(X, path) 33 | plot.plot_obstacles(X, Obstacles) 34 | plot.plot_start(X, x_init) 35 | plot.plot_goal(X, x_goal) 36 | plot.draw(auto_open=True) 37 | -------------------------------------------------------------------------------- /examples/rrt_star/rrt_star_3d.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star import RRTStar 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.plotting import Plot 8 | 9 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 10 | # obstacles 11 | Obstacles = np.array( 12 | [(20, 20, 20, 40, 40, 40), (20, 20, 60, 40, 40, 80), (20, 60, 20, 40, 80, 40), (60, 60, 20, 80, 80, 40), 13 | (60, 20, 20, 80, 40, 40), (60, 20, 60, 80, 40, 80), (20, 60, 60, 40, 80, 80), (60, 60, 60, 80, 80, 80)]) 14 | x_init = (0, 0, 0) # starting location 15 | x_goal = (100, 100, 100) # goal location 16 | 17 | q = 8 # length of tree edges 18 | r = 1 # length of smallest edge to check for intersection with obstacles 19 | max_samples = 1024 # max number of samples to take before timing out 20 | rewire_count = 32 # optional, number of nearby branches to rewire 21 | prc = 0.1 # probability of checking for a connection to goal 22 | 23 | # create Search Space 24 | X = SearchSpace(X_dimensions, Obstacles) 25 | 26 | # create rrt_search 27 | rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 28 | path = rrt.rrt_star() 29 | 30 | # plot 31 | plot = Plot("rrt_star_3d") 32 | plot.plot_tree(X, rrt.trees) 33 | if path is not None: 34 | plot.plot_path(X, path) 35 | plot.plot_obstacles(X, Obstacles) 36 | plot.plot_start(X, x_init) 37 | plot.plot_goal(X, x_goal) 38 | plot.draw(auto_open=True) 39 | -------------------------------------------------------------------------------- /examples/rrt_star/rrt_star_3d_with_random_obstacles.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_star import RRTStar 6 | from rrt_algorithms.search_space.search_space import SearchSpace 7 | from rrt_algorithms.utilities.obstacle_generation import generate_random_obstacles 8 | from rrt_algorithms.utilities.plotting import Plot 9 | 10 | X_dimensions = np.array([(0, 100), (0, 100), (0, 100)]) # dimensions of Search Space 11 | x_init = (0, 0, 0) # starting location 12 | x_goal = (100, 100, 100) # goal location 13 | 14 | q = 8 # length of tree edges 15 | r = 1 # length of smallest edge to check for intersection with obstacles 16 | max_samples = 1024 # max number of samples to take before timing out 17 | rewire_count = 32 # optional, number of nearby branches to rewire 18 | prc = 0.1 # probability of checking for a connection to goal 19 | 20 | # create Search Space 21 | X = SearchSpace(X_dimensions) 22 | n = 50 23 | Obstacles = generate_random_obstacles(X, x_init, x_goal, n) 24 | # create rrt_search 25 | rrt = RRTStar(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 26 | path = rrt.rrt_star() 27 | 28 | # plot 29 | plot = Plot("rrt_star_3d_with_random_obstacles") 30 | plot.plot_tree(X, rrt.trees) 31 | if path is not None: 32 | plot.plot_path(X, path) 33 | plot.plot_obstacles(X, Obstacles) 34 | 35 | plot.plot_start(X, x_init) 36 | plot.plot_goal(X, x_goal) 37 | plot.draw(auto_open=True) 38 | -------------------------------------------------------------------------------- /rrt_algorithms/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/motion-planning/rrt-algorithms/e51d95ee489a225220d6ae2a764c4111f6ba7d85/rrt_algorithms/__init__.py -------------------------------------------------------------------------------- /rrt_algorithms/rrt/heuristics.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | 4 | from rrt_algorithms.utilities.geometry import dist_between_points 5 | 6 | 7 | def cost_to_go(a: tuple, b: tuple) -> float: 8 | """ 9 | :param a: current location 10 | :param b: next location 11 | :return: estimated segment_cost-to-go from a to b 12 | """ 13 | return dist_between_points(a, b) 14 | 15 | 16 | def path_cost(E, a, b): 17 | """ 18 | Cost of the unique path from x_init to x 19 | :param E: edges, in form of E[child] = parent 20 | :param a: initial location 21 | :param b: goal location 22 | :return: segment_cost of unique path from x_init to x 23 | """ 24 | cost = 0 25 | while not b == a: 26 | p = E[b] 27 | cost += dist_between_points(b, p) 28 | b = p 29 | 30 | return cost 31 | 32 | 33 | def segment_cost(a, b): 34 | """ 35 | Cost function of the line between x_near and x_new 36 | :param a: start of line 37 | :param b: end of line 38 | :return: segment_cost function between a and b 39 | """ 40 | return dist_between_points(a, b) 41 | -------------------------------------------------------------------------------- /rrt_algorithms/rrt/rrt.py: -------------------------------------------------------------------------------- 1 | from rrt_algorithms.rrt.rrt_base import RRTBase 2 | 3 | 4 | class RRT(RRTBase): 5 | def __init__(self, X, q, x_init, x_goal, max_samples, r, prc=0.01): 6 | """ 7 | Template RRT planner 8 | :param X: Search Space 9 | :param q: list of lengths of edges added to tree 10 | :param x_init: tuple, initial location 11 | :param x_goal: tuple, goal location 12 | :param max_samples: max number of samples to take 13 | :param r: resolution of points to sample along edge when checking for collisions 14 | :param prc: probability of checking whether there is a solution 15 | """ 16 | super().__init__(X, q, x_init, x_goal, max_samples, r, prc) 17 | 18 | def rrt_search(self): 19 | """ 20 | Create and return a Rapidly-exploring Random Tree, keeps expanding until can connect to goal 21 | https://en.wikipedia.org/wiki/Rapidly-exploring_random_tree 22 | :return: list representation of path, dict representing edges of tree in form E[child] = parent 23 | """ 24 | self.add_vertex(0, self.x_init) 25 | self.add_edge(0, self.x_init, None) 26 | 27 | while True: 28 | x_new, x_nearest = self.new_and_near(0, self.q) 29 | 30 | if x_new is None: 31 | continue 32 | 33 | # connect shortest valid edge 34 | self.connect_to_point(0, x_nearest, x_new) 35 | 36 | solution = self.check_solution() 37 | if solution[0]: 38 | return solution[1] 39 | -------------------------------------------------------------------------------- /rrt_algorithms/rrt/rrt_base.py: -------------------------------------------------------------------------------- 1 | import random 2 | 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.tree import Tree 6 | from rrt_algorithms.utilities.geometry import steer 7 | 8 | 9 | class RRTBase(object): 10 | def __init__(self, X, q, x_init, x_goal, max_samples, r, prc=0.01): 11 | """ 12 | Template RRT planner 13 | :param X: Search Space 14 | :param q: length of edges added to tree 15 | :param x_init: tuple, initial location 16 | :param x_goal: tuple, goal location 17 | :param max_samples: max number of samples to take 18 | :param r: resolution of points to sample along edge when checking for collisions 19 | :param prc: probability of checking whether there is a solution 20 | """ 21 | self.X = X 22 | self.samples_taken = 0 23 | self.max_samples = max_samples 24 | self.q = q 25 | self.r = r 26 | self.prc = prc 27 | self.x_init = x_init 28 | self.x_goal = x_goal 29 | self.trees = [] # list of all trees 30 | self.add_tree() # add initial tree 31 | 32 | def add_tree(self): 33 | """ 34 | Create an empty tree and add to trees 35 | """ 36 | self.trees.append(Tree(self.X)) 37 | 38 | def add_vertex(self, tree, v): 39 | """ 40 | Add vertex to corresponding tree 41 | :param tree: int, tree to which to add vertex 42 | :param v: tuple, vertex to add 43 | """ 44 | self.trees[tree].V.insert(0, v + v, v) 45 | self.trees[tree].V_count += 1 # increment number of vertices in tree 46 | self.samples_taken += 1 # increment number of samples taken 47 | 48 | def add_edge(self, tree, child, parent): 49 | """ 50 | Add edge to corresponding tree 51 | :param tree: int, tree to which to add vertex 52 | :param child: tuple, child vertex 53 | :param parent: tuple, parent vertex 54 | """ 55 | self.trees[tree].E[child] = parent 56 | 57 | def nearby(self, tree, x, n): 58 | """ 59 | Return nearby vertices 60 | :param tree: int, tree being searched 61 | :param x: tuple, vertex around which searching 62 | :param n: int, max number of neighbors to return 63 | :return: list of nearby vertices 64 | """ 65 | return self.trees[tree].V.nearest(x, num_results=n, objects="raw") 66 | 67 | def get_nearest(self, tree, x): 68 | """ 69 | Return vertex nearest to x 70 | :param tree: int, tree being searched 71 | :param x: tuple, vertex around which searching 72 | :return: tuple, nearest vertex to x 73 | """ 74 | return next(self.nearby(tree, x, 1)) 75 | 76 | def new_and_near(self, tree, q): 77 | """ 78 | Return a new steered vertex and the vertex in tree that is nearest 79 | :param tree: int, tree being searched 80 | :param q: length of edge when steering 81 | :return: vertex, new steered vertex, vertex, nearest vertex in tree to new vertex 82 | """ 83 | x_rand = self.X.sample_free() 84 | x_nearest = self.get_nearest(tree, x_rand) 85 | x_new = self.bound_point(steer(x_nearest, x_rand, q)) 86 | # check if new point is in X_free and not already in V 87 | if not self.trees[0].V.count(x_new) == 0 or not self.X.obstacle_free(x_new): 88 | return None, None 89 | self.samples_taken += 1 90 | return x_new, x_nearest 91 | 92 | def connect_to_point(self, tree, x_a, x_b): 93 | """ 94 | Connect vertex x_a in tree to vertex x_b 95 | :param tree: int, tree to which to add edge 96 | :param x_a: tuple, vertex 97 | :param x_b: tuple, vertex 98 | :return: bool, True if able to add edge, False if prohibited by an obstacle 99 | """ 100 | if self.trees[tree].V.count(x_b) == 0 and self.X.collision_free(x_a, x_b, self.r): 101 | self.add_vertex(tree, x_b) 102 | self.add_edge(tree, x_b, x_a) 103 | return True 104 | return False 105 | 106 | def can_connect_to_goal(self, tree): 107 | """ 108 | Check if the goal can be connected to the graph 109 | :param tree: rtree of all Vertices 110 | :return: True if can be added, False otherwise 111 | """ 112 | x_nearest = self.get_nearest(tree, self.x_goal) 113 | if self.x_goal in self.trees[tree].E and x_nearest in self.trees[tree].E[self.x_goal]: 114 | # tree is already connected to goal using nearest vertex 115 | return True 116 | # check if obstacle-free 117 | if self.X.collision_free(x_nearest, self.x_goal, self.r): 118 | return True 119 | return False 120 | 121 | def get_path(self): 122 | """ 123 | Return path through tree from start to goal 124 | :return: path if possible, None otherwise 125 | """ 126 | if self.can_connect_to_goal(0): 127 | print("Can connect to goal") 128 | self.connect_to_goal(0) 129 | return self.reconstruct_path(0, self.x_init, self.x_goal) 130 | print("Could not connect to goal") 131 | return None 132 | 133 | def connect_to_goal(self, tree): 134 | """ 135 | Connect x_goal to graph 136 | (does not check if this should be possible, for that use: can_connect_to_goal) 137 | :param tree: rtree of all Vertices 138 | """ 139 | x_nearest = self.get_nearest(tree, self.x_goal) 140 | self.trees[tree].E[self.x_goal] = x_nearest 141 | 142 | def reconstruct_path(self, tree, x_init, x_goal): 143 | """ 144 | Reconstruct path from start to goal 145 | :param tree: int, tree in which to find path 146 | :param x_init: tuple, starting vertex 147 | :param x_goal: tuple, ending vertex 148 | :return: sequence of vertices from start to goal 149 | """ 150 | path = [x_goal] 151 | current = x_goal 152 | if x_init == x_goal: 153 | return path 154 | while not self.trees[tree].E[current] == x_init: 155 | path.append(self.trees[tree].E[current]) 156 | current = self.trees[tree].E[current] 157 | path.append(x_init) 158 | path.reverse() 159 | return path 160 | 161 | def check_solution(self): 162 | # probabilistically check if solution found 163 | if self.prc and random.random() < self.prc: 164 | print("Checking if can connect to goal at", str(self.samples_taken), "samples") 165 | path = self.get_path() 166 | if path is not None: 167 | return True, path 168 | # check if can connect to goal after generating max_samples 169 | if self.samples_taken >= self.max_samples: 170 | return True, self.get_path() 171 | return False, None 172 | 173 | def bound_point(self, point): 174 | # if point is out-of-bounds, set to bound 175 | point = np.maximum(point, self.X.dimension_lengths[:, 0]) 176 | point = np.minimum(point, self.X.dimension_lengths[:, 1]) 177 | return tuple(point) 178 | -------------------------------------------------------------------------------- /rrt_algorithms/rrt/rrt_connect.py: -------------------------------------------------------------------------------- 1 | import enum 2 | 3 | import numpy as np 4 | 5 | from rrt_algorithms.rrt.rrt_base import RRTBase 6 | from rrt_algorithms.utilities.geometry import steer 7 | 8 | 9 | class Status(enum.Enum): 10 | FAILED = 1 11 | TRAPPED = 2 12 | ADVANCED = 3 13 | REACHED = 4 14 | 15 | 16 | class RRTConnect(RRTBase): 17 | def __init__(self, X, q, x_init, x_goal, max_samples, r, prc=0.01): 18 | """ 19 | Template RRTConnect planner 20 | :param X: Search Space 21 | :param Q: list of lengths of edges added to tree 22 | :param x_init: tuple, initial location 23 | :param x_goal: tuple, goal location 24 | :param max_samples: max number of samples to take 25 | :param r: resolution of points to sample along edge when checking for collisions 26 | :param prc: probability of checking whether there is a solution 27 | """ 28 | super().__init__(X, q, x_init, x_goal, max_samples, r, prc) 29 | self.swapped = False 30 | 31 | def swap_trees(self): 32 | """ 33 | Swap trees only 34 | """ 35 | # swap trees 36 | self.trees[0], self.trees[1] = self.trees[1], self.trees[0] 37 | self.swapped = not self.swapped 38 | 39 | def unswap(self): 40 | """ 41 | Check if trees have been swapped and unswap 42 | """ 43 | if self.swapped: 44 | self.swap_trees() 45 | 46 | def extend(self, tree, x_rand): 47 | x_nearest = self.get_nearest(tree, x_rand) 48 | x_new = steer(x_nearest, x_rand, self.q) 49 | if self.connect_to_point(tree, x_nearest, x_new): 50 | if np.abs(np.sum(np.array(x_new) - np.array(x_rand))) < 1e-2: 51 | return x_new, Status.REACHED 52 | return x_new, Status.ADVANCED 53 | return x_new, Status.TRAPPED 54 | 55 | def connect(self, tree, x): 56 | S = Status.ADVANCED 57 | while S == Status.ADVANCED: 58 | x_new, S = self.extend(tree, x) 59 | return x_new, S 60 | 61 | def rrt_connect(self): 62 | """ 63 | RRTConnect 64 | :return: set of Vertices; Edges in form: vertex: [neighbor_1, neighbor_2, ...] 65 | """ 66 | self.add_vertex(0, self.x_init) 67 | self.add_edge(0, self.x_init, None) 68 | self.add_tree() 69 | self.add_vertex(1, self.x_goal) 70 | self.add_edge(1, self.x_goal, None) 71 | 72 | while self.samples_taken < self.max_samples: 73 | x_rand = self.X.sample_free() 74 | x_new, status = self.extend(0, x_rand) 75 | if status != Status.TRAPPED: 76 | x_new, connect_status = self.connect(1, x_new) 77 | if connect_status == Status.REACHED: 78 | self.unswap() 79 | first_part = self.reconstruct_path(0, self.x_init, self.get_nearest(0, x_new)) 80 | second_part = self.reconstruct_path(1, self.x_goal, self.get_nearest(1, x_new)) 81 | second_part.reverse() 82 | return first_part + second_part 83 | self.swap_trees() 84 | self.samples_taken += 1 85 | -------------------------------------------------------------------------------- /rrt_algorithms/rrt/rrt_star.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | from operator import itemgetter 4 | 5 | from rrt_algorithms.rrt.heuristics import cost_to_go 6 | from rrt_algorithms.rrt.heuristics import segment_cost, path_cost 7 | from rrt_algorithms.rrt.rrt import RRT 8 | 9 | 10 | class RRTStar(RRT): 11 | def __init__(self, X, q, x_init, x_goal, max_samples, r, prc=0.01, rewire_count=None): 12 | """ 13 | RRT* Search 14 | :param X: Search Space 15 | :param Q: list of lengths of edges added to tree 16 | :param x_init: tuple, initial location 17 | :param x_goal: tuple, goal location 18 | :param max_samples: max number of samples to take 19 | :param r: resolution of points to sample along edge when checking for collisions 20 | :param prc: probability of checking whether there is a solution 21 | :param rewire_count: number of nearby vertices to rewire 22 | """ 23 | super().__init__(X, q, x_init, x_goal, max_samples, r, prc) 24 | self.rewire_count = rewire_count if rewire_count is not None else 0 25 | 26 | def get_nearby_vertices(self, tree, x_init, x_new): 27 | """ 28 | Get nearby vertices to new vertex and their associated path costs from the root of tree 29 | as if new vertex is connected to each one separately. 30 | 31 | :param tree: tree in which to search 32 | :param x_init: starting vertex used to calculate path cost 33 | :param x_new: vertex around which to find nearby vertices 34 | :return: list of nearby vertices and their costs, sorted in ascending order by cost 35 | """ 36 | X_near = self.nearby(tree, x_new, self.current_rewire_count(tree)) 37 | L_near = [(path_cost(self.trees[tree].E, x_init, x_near) + segment_cost(x_near, x_new), x_near) for 38 | x_near in X_near] 39 | # noinspection PyTypeChecker 40 | L_near.sort(key=itemgetter(0)) 41 | 42 | return L_near 43 | 44 | def rewire(self, tree, x_new, L_near): 45 | """ 46 | Rewire tree to shorten edges if possible 47 | Only rewires vertices according to rewire count 48 | :param tree: int, tree to rewire 49 | :param x_new: tuple, newly added vertex 50 | :param L_near: list of nearby vertices used to rewire 51 | :return: 52 | """ 53 | for _, x_near in L_near: 54 | curr_cost = path_cost(self.trees[tree].E, self.x_init, x_near) 55 | tent_cost = path_cost( 56 | self.trees[tree].E, self.x_init, x_new) + segment_cost(x_new, x_near) 57 | if tent_cost < curr_cost and self.X.collision_free(x_near, x_new, self.r): 58 | self.trees[tree].E[x_near] = x_new 59 | 60 | def connect_shortest_valid(self, tree, x_new, L_near): 61 | """ 62 | Connect to nearest vertex that has an unobstructed path 63 | :param tree: int, tree being added to 64 | :param x_new: tuple, vertex being added 65 | :param L_near: list of nearby vertices 66 | """ 67 | # check nearby vertices for total cost and connect shortest valid edge 68 | for _, x_near in L_near: 69 | if self.connect_to_point(tree, x_near, x_new): 70 | break 71 | 72 | def current_rewire_count(self, tree): 73 | """ 74 | Return rewire count 75 | :param tree: tree being rewired 76 | :return: rewire count 77 | """ 78 | # if no rewire count specified, set rewire count to be all vertices 79 | if self.rewire_count is None: 80 | return self.trees[tree].V_count 81 | 82 | # max valid rewire count 83 | return min(self.trees[tree].V_count, self.rewire_count) 84 | 85 | def rrt_star(self): 86 | """ 87 | Based on algorithm found in: Incremental Sampling-based Algorithms for Optimal Motion Planning 88 | http://roboticsproceedings.org/rss06/p34.pdf 89 | :return: set of Vertices; Edges in form: vertex: [neighbor_1, neighbor_2, ...] 90 | """ 91 | self.add_vertex(0, self.x_init) 92 | self.add_edge(0, self.x_init, None) 93 | 94 | while True: 95 | x_new, x_nearest = self.new_and_near(0, self.q) 96 | if x_new is None: 97 | continue 98 | 99 | # get nearby vertices and cost-to-come 100 | L_near = self.get_nearby_vertices(0, self.x_init, x_new) 101 | 102 | # check nearby vertices for total cost and connect shortest valid edge 103 | self.connect_shortest_valid(0, x_new, L_near) 104 | 105 | if x_new in self.trees[0].E: 106 | # rewire tree 107 | self.rewire(0, x_new, L_near) 108 | 109 | solution = self.check_solution() 110 | if solution[0]: 111 | return solution[1] 112 | -------------------------------------------------------------------------------- /rrt_algorithms/rrt/rrt_star_bid.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | 4 | import random 5 | 6 | from rrt_algorithms.rrt.heuristics import path_cost 7 | from rrt_algorithms.rrt.rrt_star import RRTStar 8 | 9 | 10 | class RRTStarBidirectional(RRTStar): 11 | def __init__(self, X, q, x_init, x_goal, max_samples, r, prc=0.01, rewire_count=None): 12 | """ 13 | Bidirectional RRT* Search 14 | :param X: Search Space 15 | :param Q: list of lengths of edges added to tree 16 | :param x_init: tuple, initial location 17 | :param x_goal: tuple, goal location 18 | :param max_samples: max number of samples to take 19 | :param r: resolution of points to sample along edge when checking for collisions 20 | :param prc: probability of checking whether there is a solution 21 | :param rewire_count: number of nearby vertices to rewire 22 | """ 23 | super().__init__(X, q, x_init, x_goal, max_samples, r, prc, rewire_count) 24 | self.sigma_best = None # best solution thus far 25 | self.c_best = float('inf') # length of best solution thus far 26 | self.swapped = False 27 | 28 | def connect_trees(self, a, b, x_new, L_near): 29 | """ 30 | Check nearby vertices for total cost and connect shortest valid edge if possible 31 | This results in both trees being connected 32 | :param a: first tree to connect 33 | :param b: second tree to connect 34 | :param x_new: new vertex to add 35 | :param L_near: nearby vertices 36 | """ 37 | for c_near, x_near in L_near: 38 | c_tent = c_near + path_cost(self.trees[a].E, self.x_init, x_new) 39 | if c_tent < self.c_best and self.X.collision_free(x_near, x_new, self.r): 40 | self.trees[b].V_count += 1 41 | self.trees[b].E[x_new] = x_near 42 | self.c_best = c_tent 43 | sigma_a = self.reconstruct_path(a, self.x_init, x_new) 44 | sigma_b = self.reconstruct_path(b, self.x_goal, x_new) 45 | del sigma_b[-1] 46 | sigma_b.reverse() 47 | self.sigma_best = sigma_a + sigma_b 48 | 49 | break 50 | 51 | def swap_trees(self): 52 | """ 53 | Swap trees and start/goal 54 | """ 55 | # swap trees 56 | self.trees[0], self.trees[1] = self.trees[1], self.trees[0] 57 | # swap start/goal 58 | self.x_init, self.x_goal = self.x_goal, self.x_init 59 | self.swapped = not self.swapped 60 | 61 | def unswap(self): 62 | """ 63 | Check if trees have been swapped and unswap 64 | Reverse path if needed to correspond with swapped trees 65 | """ 66 | if self.swapped: 67 | self.swap_trees() 68 | 69 | if self.sigma_best is not None and self.sigma_best[0] is not self.x_init: 70 | self.sigma_best.reverse() 71 | 72 | def rrt_star_bidirectional(self): 73 | """ 74 | Bidirectional RRT* 75 | :return: set of Vertices; Edges in form: vertex: [neighbor_1, neighbor_2, ...] 76 | """ 77 | # tree a 78 | self.add_vertex(0, self.x_init) 79 | self.add_edge(0, self.x_init, None) 80 | 81 | # tree b 82 | self.add_tree() 83 | self.add_vertex(1, self.x_goal) 84 | self.add_edge(1, self.x_goal, None) 85 | 86 | while True: 87 | x_new, _ = self.new_and_near(0, self.q) 88 | if x_new is None: 89 | continue 90 | 91 | # get nearby vertices and cost-to-come 92 | L_near = self.get_nearby_vertices(0, self.x_init, x_new) 93 | 94 | # check nearby vertices for total cost and connect shortest valid edge 95 | self.connect_shortest_valid(0, x_new, L_near) 96 | 97 | if x_new in self.trees[0].E: 98 | # rewire tree 99 | self.rewire(0, x_new, L_near) 100 | 101 | # nearby vertices from opposite tree and cost-to-come 102 | L_near = self.get_nearby_vertices(1, self.x_goal, x_new) 103 | 104 | self.connect_trees(0, 1, x_new, L_near) 105 | 106 | if self.prc and random.random() < self.prc: # probabilistically check if solution found 107 | print("Checking if can connect to goal at", 108 | str(self.samples_taken), "samples") 109 | if self.sigma_best is not None: 110 | print("Can connect to goal") 111 | self.unswap() 112 | 113 | return self.sigma_best 114 | 115 | if self.samples_taken >= self.max_samples: 116 | self.unswap() 117 | 118 | if self.sigma_best is not None: 119 | print("Can connect to goal") 120 | 121 | return self.sigma_best 122 | else: 123 | print("Could not connect to goal") 124 | 125 | return self.sigma_best 126 | 127 | self.swap_trees() 128 | -------------------------------------------------------------------------------- /rrt_algorithms/rrt/rrt_star_bid_h.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | import random 4 | 5 | from rrt_algorithms.rrt.rrt_star_bid import RRTStarBidirectional 6 | from rrt_algorithms.utilities.geometry import dist_between_points, pairwise 7 | 8 | 9 | class RRTStarBidirectionalHeuristic(RRTStarBidirectional): 10 | def __init__(self, X, q, x_init, x_goal, max_samples, r, prc=0.01, 11 | rewire_count: int = None, conditional_rewire: bool = False): 12 | """ 13 | Bidirectional RRT* Search 14 | :param X: Search Space 15 | :param Q: list of lengths of edges added to tree 16 | :param x_init: tuple, initial location 17 | :param x_goal: tuple, goal location 18 | :param max_samples: max number of samples to take 19 | :param r: resolution of points to sample along edge when checking for collisions 20 | :param prc: probability of checking whether there is a solution 21 | :param rewire_count: number of nearby vertices to rewire 22 | :param conditional_rewire: if True, set rewire count to 1 until solution found, 23 | then set to specified rewire count (ensure runtime complexity guarantees) 24 | """ 25 | super().__init__(X, q, x_init, x_goal, max_samples, r, prc, 26 | 1 if conditional_rewire else rewire_count) 27 | self.original_rewire_count = rewire_count 28 | 29 | def rrt_star_bid_h(self): 30 | """ 31 | Bidirectional RRT* using added heuristics 32 | :return: set of Vertices; Edges in form: vertex: [neighbor_1, neighbor_2, ...] 33 | """ 34 | # tree a 35 | self.add_vertex(0, self.x_init) 36 | self.add_edge(0, self.x_init, None) 37 | 38 | # tree b 39 | self.add_tree() 40 | self.add_vertex(1, self.x_goal) 41 | self.add_edge(1, self.x_goal, None) 42 | 43 | while True: 44 | x_new, x_nearest = self.new_and_near(0, self.q) 45 | if x_new is None: 46 | continue 47 | 48 | # get nearby vertices and cost-to-come 49 | L_near = self.get_nearby_vertices(0, self.x_init, x_new) 50 | 51 | # check nearby vertices for total cost and connect shortest valid edge 52 | self.connect_shortest_valid(0, x_new, L_near) 53 | 54 | if x_new in self.trees[0].E: 55 | # rewire tree 56 | self.rewire(0, x_new, L_near) 57 | 58 | # nearby vertices from opposite tree and cost-to-come 59 | L_near = self.get_nearby_vertices(1, self.x_goal, x_new) 60 | 61 | self.connect_trees(0, 1, x_new, L_near) 62 | self.rewire_count = self.original_rewire_count 63 | 64 | self.lazy_shortening() 65 | 66 | if self.prc and random.random() < self.prc: # probabilistically check if solution found 67 | print("Checking if can connect to goal at", str(self.samples_taken), "samples") 68 | if self.sigma_best is not None: 69 | print("Can connect to goal") 70 | self.unswap() 71 | 72 | return self.sigma_best 73 | 74 | if self.samples_taken >= self.max_samples: 75 | self.unswap() 76 | 77 | if self.sigma_best is not None: 78 | print("Can connect to goal") 79 | 80 | return self.sigma_best 81 | else: 82 | print("Could not connect to goal") 83 | 84 | return self.sigma_best 85 | 86 | self.swap_trees() 87 | 88 | def lazy_shortening(self): 89 | """ 90 | Lazily attempt to shorten current best path 91 | """ 92 | if self.sigma_best is not None and len(self.sigma_best) > 2: 93 | a, b = 0, 0 94 | while not abs(a - b) > 1: 95 | a, b = random.sample(range(0, len(self.sigma_best)), 2) 96 | 97 | a, b = min(a, b), max(a, b) 98 | v_a, v_b = tuple(self.sigma_best[a]), tuple(self.sigma_best[b]) 99 | 100 | if self.X.collision_free(v_a, v_b, self.r): 101 | # create new edge connecting vertices 102 | if v_a in self.trees[0].E and v_b in self.reconstruct_path(0, self.x_init, v_a): 103 | self.trees[0].E[v_a] = v_b 104 | elif v_a in self.trees[1].E and v_b in self.reconstruct_path(1, self.x_goal, v_a): 105 | self.trees[1].E[v_a] = v_b 106 | elif v_b in self.trees[0].E and v_a in self.reconstruct_path(0, self.x_init, v_b): 107 | self.trees[0].E[v_b] = v_a 108 | elif v_b in self.trees[1].E and v_a in self.reconstruct_path(1, self.x_goal, v_b): 109 | self.trees[1].E[v_b] = v_a 110 | elif v_a in self.trees[0].E: 111 | self.trees[0].E[v_b] = v_a 112 | else: 113 | self.trees[1].E[v_b] = v_a 114 | 115 | # update best path 116 | # remove cost of removed edges 117 | self.c_best -= sum(dist_between_points(i, j) 118 | for i, j in pairwise(self.sigma_best[a:b + 1])) 119 | # add cost of new edge 120 | self.c_best += dist_between_points( 121 | self.sigma_best[a], self.sigma_best[b]) 122 | self.sigma_best = self.sigma_best[:a + 1] + self.sigma_best[b:] 123 | -------------------------------------------------------------------------------- /rrt_algorithms/rrt/tree.py: -------------------------------------------------------------------------------- 1 | from rtree import index 2 | 3 | 4 | class Tree(object): 5 | def __init__(self, X): 6 | """ 7 | Tree representation 8 | :param X: Search Space 9 | """ 10 | p = index.Property() 11 | p.dimension = X.dimensions 12 | # vertices in an rtree 13 | self.V = index.Index(interleaved=True, properties=p) 14 | self.V_count = 0 15 | self.E = {} # edges in form E[child] = parent 16 | -------------------------------------------------------------------------------- /rrt_algorithms/search_space/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/motion-planning/rrt-algorithms/e51d95ee489a225220d6ae2a764c4111f6ba7d85/rrt_algorithms/search_space/__init__.py -------------------------------------------------------------------------------- /rrt_algorithms/search_space/search_space.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | 4 | import numpy as np 5 | from rtree import index 6 | 7 | from rrt_algorithms.utilities.geometry import es_points_along_line 8 | from rrt_algorithms.utilities.obstacle_generation import obstacle_generator 9 | 10 | 11 | class SearchSpace(object): 12 | def __init__(self, dimension_lengths, O=None): 13 | """ 14 | Initialize Search Space 15 | :param dimension_lengths: range of each dimension 16 | :param O: list of obstacles 17 | """ 18 | # sanity check 19 | if len(dimension_lengths) < 2: 20 | raise Exception("Must have at least 2 dimensions") 21 | self.dimensions = len(dimension_lengths) # number of dimensions 22 | # sanity checks 23 | if any(len(i) != 2 for i in dimension_lengths): 24 | raise Exception("Dimensions can only have a start and end") 25 | if any(i[0] >= i[1] for i in dimension_lengths): 26 | raise Exception("Dimension start must be less than dimension end") 27 | self.dimension_lengths = dimension_lengths # length of each dimension 28 | p = index.Property() 29 | p.dimension = self.dimensions 30 | if O is None: 31 | self.obs = index.Index(interleaved=True, properties=p) 32 | else: 33 | # r-tree representation of obstacles 34 | # sanity check 35 | if any(len(o) / 2 != len(dimension_lengths) for o in O): 36 | raise Exception("Obstacle has incorrect dimension definition") 37 | if any(o[i] >= o[int(i + len(o) / 2)] for o in O for i in range(int(len(o) / 2))): 38 | raise Exception("Obstacle start must be less than obstacle end") 39 | self.obs = index.Index(obstacle_generator(O), interleaved=True, properties=p) 40 | 41 | def obstacle_free(self, x): 42 | """ 43 | Check if a location resides inside of an obstacle 44 | :param x: location to check 45 | :return: True if not inside an obstacle, False otherwise 46 | """ 47 | return self.obs.count(x) == 0 48 | 49 | def sample_free(self): 50 | """ 51 | Sample a location within X_free 52 | :return: random location within X_free 53 | """ 54 | while True: # sample until not inside of an obstacle 55 | x = self.sample() 56 | if self.obstacle_free(x): 57 | return x 58 | 59 | def collision_free(self, start, end, r): 60 | """ 61 | Check if a line segment intersects an obstacle 62 | :param start: starting point of line 63 | :param end: ending point of line 64 | :param r: resolution of points to sample along edge when checking for collisions 65 | :return: True if line segment does not intersect an obstacle, False otherwise 66 | """ 67 | points = es_points_along_line(start, end, r) 68 | coll_free = all(map(self.obstacle_free, points)) 69 | return coll_free 70 | 71 | def sample(self): 72 | """ 73 | Return a random location within X 74 | :return: random location within X (not necessarily X_free) 75 | """ 76 | x = np.random.uniform(self.dimension_lengths[:, 0], self.dimension_lengths[:, 1]) 77 | return tuple(x) 78 | -------------------------------------------------------------------------------- /rrt_algorithms/utilities/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/motion-planning/rrt-algorithms/e51d95ee489a225220d6ae2a764c4111f6ba7d85/rrt_algorithms/utilities/__init__.py -------------------------------------------------------------------------------- /rrt_algorithms/utilities/geometry.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | 4 | from itertools import tee 5 | 6 | import numpy as np 7 | 8 | 9 | def dist_between_points(a, b): 10 | """ 11 | Return the Euclidean distance between two points 12 | :param a: first point 13 | :param b: second point 14 | :return: Euclidean distance between a and b 15 | """ 16 | distance = np.linalg.norm(np.array(b) - np.array(a)) 17 | return distance 18 | 19 | 20 | def pairwise(iterable): 21 | """ 22 | Pairwise iteration over iterable 23 | :param iterable: iterable 24 | :return: s -> (s0,s1), (s1,s2), (s2, s3), ... 25 | """ 26 | a, b = tee(iterable) 27 | next(b, None) 28 | return zip(a, b) 29 | 30 | 31 | def es_points_along_line(start, end, r): 32 | """ 33 | Equally-spaced points along a line defined by start, end, with resolution r 34 | :param start: starting point 35 | :param end: ending point 36 | :param r: maximum distance between points 37 | :return: yields points along line from start to end, separated by distance r 38 | """ 39 | d = dist_between_points(start, end) 40 | n_points = int(np.ceil(d / r)) 41 | if n_points > 1: 42 | step = d / (n_points - 1) 43 | for i in range(n_points): 44 | next_point = steer(start, end, i * step) 45 | yield next_point 46 | 47 | 48 | def steer(start, goal, d): 49 | """ 50 | Return a point in the direction of the goal, that is distance away from start 51 | :param start: start location 52 | :param goal: goal location 53 | :param d: distance away from start 54 | :return: point in the direction of the goal, distance away from start 55 | """ 56 | start, end = np.array(start), np.array(goal) 57 | v = end - start 58 | u = v / (np.sqrt(np.sum(v ** 2))) 59 | d = min(d, np.linalg.norm(goal - start)) 60 | steered_point = start + u * d 61 | return tuple(steered_point) 62 | -------------------------------------------------------------------------------- /rrt_algorithms/utilities/obstacle_generation.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | import random 4 | import uuid 5 | 6 | import numpy as np 7 | 8 | from typing import TYPE_CHECKING 9 | 10 | if TYPE_CHECKING: 11 | from rrt_algorithms.search_space.search_space import SearchSpace 12 | 13 | 14 | 15 | def generate_random_obstacles(X: SearchSpace, start, end, n): 16 | """ 17 | Generates n random obstacles without disrupting world connectivity. 18 | It also respects start and end points so that they don't lie inside of an obstacle. 19 | """ 20 | # Note: Current implementation only supports hyperrectangles. 21 | i = 0 22 | obstacles = [] 23 | while i < n: 24 | center = np.empty(len(X.dimension_lengths), float) 25 | scollision = True 26 | fcollision = True 27 | edge_lengths = [] 28 | for j in range(X.dimensions): 29 | # None of the sides of a hyperrectangle can be higher than 0.1 of the total span 30 | # in that particular X.dimensions 31 | max_edge_length = (X.dimension_lengths[j][1] - X.dimension_lengths[j][0]) / 10.0 32 | # None of the sides of a hyperrectangle can be higher than 0.01 of the total span 33 | # in that particular X.dimensions 34 | min_edge_length = (X.dimension_lengths[j][1] - X.dimension_lengths[j][0]) / 100.0 35 | edge_length = random.uniform(min_edge_length, max_edge_length) 36 | center[j] = random.uniform(X.dimension_lengths[j][0] + edge_length, 37 | X.dimension_lengths[j][1] - edge_length) 38 | edge_lengths.append(edge_length) 39 | 40 | if abs(start[j] - center[j]) > edge_length: 41 | scollision = False 42 | if abs(end[j] - center[j]) > edge_length: 43 | fcollision = False 44 | 45 | # Check if any part of the obstacle is inside of another obstacle. 46 | min_corner = np.empty(X.dimensions, float) 47 | max_corner = np.empty(X.dimensions, float) 48 | for j in range(X.dimensions): 49 | min_corner[j] = center[j] - edge_lengths[j] 50 | max_corner[j] = center[j] + edge_lengths[j] 51 | obstacle = np.append(min_corner, max_corner) 52 | # Check newly generated obstacle intersects any former ones. Also respect start and end points 53 | if len(list(X.obs.intersection(obstacle))) > 0 or scollision or fcollision: 54 | continue 55 | i += 1 56 | obstacles.append(obstacle) 57 | X.obs.insert(uuid.uuid4().int, tuple(obstacle), tuple(obstacle)) 58 | 59 | return obstacles 60 | 61 | 62 | def obstacle_generator(obstacles): 63 | """ 64 | Add obstacles to r-tree 65 | :param obstacles: list of obstacles 66 | """ 67 | for obstacle in obstacles: 68 | yield (uuid.uuid4().int, obstacle, obstacle) 69 | -------------------------------------------------------------------------------- /rrt_algorithms/utilities/plotting.py: -------------------------------------------------------------------------------- 1 | # This file is subject to the terms and conditions defined in 2 | # file 'LICENSE', which is part of this source code package. 3 | from pathlib import Path 4 | 5 | import plotly as py 6 | from plotly import graph_objs as go 7 | 8 | colors = ['darkblue', 'teal'] 9 | 10 | 11 | class Plot(object): 12 | def __init__(self, filename): 13 | """ 14 | Create a plot 15 | :param filename: filename 16 | """ 17 | self.filename = Path(__file__).parent / "../../output/visualizations/" / f"{filename}.html" 18 | if not self.filename.parent.exists(): 19 | self.filename.parent.mkdir(parents=True, exist_ok=True) 20 | self.filename = str(self.filename) 21 | self.data = [] 22 | self.layout = {'title': 'Plot', 23 | 'showlegend': False 24 | } 25 | 26 | self.fig = {'data': self.data, 27 | 'layout': self.layout} 28 | 29 | def plot_tree(self, X, trees): 30 | """ 31 | Plot tree 32 | :param X: Search Space 33 | :param trees: list of trees 34 | """ 35 | if X.dimensions == 2: # plot in 2D 36 | self.plot_tree_2d(trees) 37 | elif X.dimensions == 3: # plot in 3D 38 | self.plot_tree_3d(trees) 39 | else: # can't plot in higher dimensions 40 | print("Cannot plot in > 3 dimensions") 41 | 42 | def plot_tree_2d(self, trees): 43 | """ 44 | Plot 2D trees 45 | :param trees: trees to plot 46 | """ 47 | for i, tree in enumerate(trees): 48 | for start, end in tree.E.items(): 49 | if end is not None: 50 | trace = go.Scatter( 51 | x=[start[0], end[0]], 52 | y=[start[1], end[1]], 53 | line=dict( 54 | color=colors[i] 55 | ), 56 | mode="lines" 57 | ) 58 | self.data.append(trace) 59 | 60 | def plot_tree_3d(self, trees): 61 | """ 62 | Plot 3D trees 63 | :param trees: trees to plot 64 | """ 65 | for i, tree in enumerate(trees): 66 | for start, end in tree.E.items(): 67 | if end is not None: 68 | trace = go.Scatter3d( 69 | x=[start[0], end[0]], 70 | y=[start[1], end[1]], 71 | z=[start[2], end[2]], 72 | line=dict( 73 | color=colors[i] 74 | ), 75 | mode="lines" 76 | ) 77 | self.data.append(trace) 78 | 79 | def plot_obstacles(self, X, O): 80 | """ 81 | Plot obstacles 82 | :param X: Search Space 83 | :param O: list of obstacles 84 | """ 85 | if X.dimensions == 2: # plot in 2D 86 | self.layout['shapes'] = [] 87 | for O_i in O: 88 | # noinspection PyUnresolvedReferences 89 | self.layout['shapes'].append( 90 | { 91 | 'type': 'rect', 92 | 'x0': O_i[0], 93 | 'y0': O_i[1], 94 | 'x1': O_i[2], 95 | 'y1': O_i[3], 96 | 'line': { 97 | 'color': 'purple', 98 | 'width': 4, 99 | }, 100 | 'fillcolor': 'purple', 101 | 'opacity': 0.70 102 | }, 103 | ) 104 | elif X.dimensions == 3: # plot in 3D 105 | for O_i in O: 106 | obs = go.Mesh3d( 107 | x=[O_i[0], O_i[0], O_i[3], O_i[3], O_i[0], O_i[0], O_i[3], O_i[3]], 108 | y=[O_i[1], O_i[4], O_i[4], O_i[1], O_i[1], O_i[4], O_i[4], O_i[1]], 109 | z=[O_i[2], O_i[2], O_i[2], O_i[2], O_i[5], O_i[5], O_i[5], O_i[5]], 110 | i=[7, 0, 0, 0, 4, 4, 6, 6, 4, 0, 3, 2], 111 | j=[3, 4, 1, 2, 5, 6, 5, 2, 0, 1, 6, 3], 112 | k=[0, 7, 2, 3, 6, 7, 1, 1, 5, 5, 7, 6], 113 | color='purple', 114 | opacity=0.70 115 | ) 116 | self.data.append(obs) 117 | else: # can't plot in higher dimensions 118 | print("Cannot plot in > 3 dimensions") 119 | 120 | def plot_path(self, X, path): 121 | """ 122 | Plot path through Search Space 123 | :param X: Search Space 124 | :param path: path through space given as a sequence of points 125 | """ 126 | if X.dimensions == 2: # plot in 2D 127 | x, y = [], [] 128 | for i in path: 129 | x.append(i[0]) 130 | y.append(i[1]) 131 | trace = go.Scatter( 132 | x=x, 133 | y=y, 134 | line=dict( 135 | color="red", 136 | width=4 137 | ), 138 | mode="lines" 139 | ) 140 | 141 | self.data.append(trace) 142 | elif X.dimensions == 3: # plot in 3D 143 | x, y, z = [], [], [] 144 | for i in path: 145 | x.append(i[0]) 146 | y.append(i[1]) 147 | z.append(i[2]) 148 | trace = go.Scatter3d( 149 | x=x, 150 | y=y, 151 | z=z, 152 | line=dict( 153 | color="red", 154 | width=4 155 | ), 156 | mode="lines" 157 | ) 158 | 159 | self.data.append(trace) 160 | else: # can't plot in higher dimensions 161 | print("Cannot plot in > 3 dimensions") 162 | 163 | def plot_start(self, X, x_init): 164 | """ 165 | Plot starting point 166 | :param X: Search Space 167 | :param x_init: starting location 168 | """ 169 | if X.dimensions == 2: # plot in 2D 170 | trace = go.Scatter( 171 | x=[x_init[0]], 172 | y=[x_init[1]], 173 | line=dict( 174 | color="orange", 175 | width=10 176 | ), 177 | mode="markers" 178 | ) 179 | 180 | self.data.append(trace) 181 | elif X.dimensions == 3: # plot in 3D 182 | trace = go.Scatter3d( 183 | x=[x_init[0]], 184 | y=[x_init[1]], 185 | z=[x_init[2]], 186 | line=dict( 187 | color="orange", 188 | width=10 189 | ), 190 | mode="markers" 191 | ) 192 | 193 | self.data.append(trace) 194 | else: # can't plot in higher dimensions 195 | print("Cannot plot in > 3 dimensions") 196 | 197 | def plot_goal(self, X, x_goal): 198 | """ 199 | Plot goal point 200 | :param X: Search Space 201 | :param x_goal: goal location 202 | """ 203 | if X.dimensions == 2: # plot in 2D 204 | trace = go.Scatter( 205 | x=[x_goal[0]], 206 | y=[x_goal[1]], 207 | line=dict( 208 | color="green", 209 | width=10 210 | ), 211 | mode="markers" 212 | ) 213 | 214 | self.data.append(trace) 215 | elif X.dimensions == 3: # plot in 3D 216 | trace = go.Scatter3d( 217 | x=[x_goal[0]], 218 | y=[x_goal[1]], 219 | z=[x_goal[2]], 220 | line=dict( 221 | color="green", 222 | width=10 223 | ), 224 | mode="markers" 225 | ) 226 | 227 | self.data.append(trace) 228 | else: # can't plot in higher dimensions 229 | print("Cannot plot in > 3 dimensions") 230 | 231 | def draw(self, auto_open=True): 232 | """ 233 | Render the plot to a file 234 | """ 235 | py.offline.plot(self.fig, filename=self.filename, auto_open=auto_open) 236 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | 3 | with open("README.md", "r") as fh: 4 | long_description = fh.read() 5 | 6 | setuptools.setup( 7 | name="rrt-algorithms", 8 | version="0.0.1", 9 | author="SZanlongo", 10 | author_email="", 11 | description="Collection of rrt-based algorithms that scale to n-dimensions", 12 | long_description=long_description, 13 | long_description_content_type="text/markdown", 14 | url="https://github.com/motion-planning/rrt-algorithms", 15 | packages=setuptools.find_packages(), 16 | classifiers=[ 17 | "Programming Language :: Python :: 3", 18 | "Operating System :: OS Independent", 19 | ], 20 | include_package_data=True, 21 | python_requires='>=3.10', 22 | install_requires=['rtree==1.2.0', 23 | 'numpy>=1.25', 24 | 'plotly', 25 | 'scipy>=1.11'], 26 | ) 27 | --------------------------------------------------------------------------------