motion-planning / rrt-algorithms

n-dimensional RRT, RRT* (RRT-Star)

rrt

Collection of rrt-based algorithms that scale to n-dimensions:

• rrt
• rrt* (rrt-star)
• rrt* (bidirectional)
• rrt* (bidriectional, lazy shortening)
• rrt connect

Utilizes R-trees to improve performance by avoiding point-wise collision-checking and distance-checking.

This would not have been possible without Steven M. LaValle's excellent Planning Algorithms book, specifically Chapter 5, Section 5: Sampling-Based Motion Planning, Rapidly Exploring Dense Trees.

Requirements

• Python 3+
• NumPy
• Rtree
• Plotly (only needed for plotting)

Usage

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.

Search Space

Assign bounds to Search Space in form: [(x_lower, x_upper), (y_lower, y_upper), ...]

Start and Goal

Points represented by tuples of form: (x, y, ...)

Obstacles

Axis-aligned (hyper)rectangles represented by a tuples of form (x_lower, y_lower, ..., x_upper, y_upper, ...)

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.

Resolution

Assign resolution of edges:

• q: Distance away from existing vertices to probe.
• 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.

Examples

Visualization examples can be found for rrt and rrt* in both 2 and 3 dimensions.

• 2D RRT
• 3D RRT
• 2D RRT*
• 3D RRT*
• 2D Bidirectional RRT*
• 3D Bidirectional RRT*
• 2D Heuristic Bidirectional RRT*
• 3D Heuristic Bidirectional RRT*

Contributing

1. Fork it!
2. Create your feature branch: git checkout -b my-new-feature
3. Commit your changes: git commit -am 'Add some feature'
4. Push to the branch: git push origin my-new-feature
5. Submit a pull request :D

References

1. Steven Michael Lavalle. Planning Algorithms. New York (Ny), Cambridge University Press, 2014, pp. 228–237, lavalle.pl/planning/.
2. LaValle, Steven. "Rapidly-exploring random trees: A new tool for path planning." Research Report 9811 (1998).
3. Kuffner, James J., and Steven M. LaValle. "RRT-connect: An efficient approach to single-query path planning." Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065). Vol. 2. IEEE, 2000.

License

MIT License

rrt-algorithms's Issues

Which article are ideas of RRT* using R-tree from?

Can someone tell me where this idea comes from? Thanks.

Conditional Neighbors

Nearest Neighbors should only return single nearest neighbor until a feasible solution found. Then should use count.

Allow for multiple trees

Implement RRT incorporating an arbitrary number of trees besides trees growing from the start and goal.
The tree structure already allows for multiple trees, but RRT must be modified to allow for extra trees.

OSError: libspatialindex_c.so.5: cannot open shared object file

I'm getting the following error. Any ideas?

Traceback (most recent call last):
  File "examples/rrt/rrt_2d.py", line 5, in <module>
    from src.rrt.rrt import RRT
  File "/home/ferdinand/dev/rrt-algorithms/src/rrt/rrt.py", line 1, in <module>
    from src.rrt.rrt_base import RRTBase
  File "/home/ferdinand/dev/rrt-algorithms/src/rrt/rrt_base.py", line 5, in <module>
    from src.rrt.tree import Tree
  File "/home/ferdinand/dev/rrt-algorithms/src/rrt/tree.py", line 1, in <module>
    from rtree import index
  File "/home/ferdinand/.local/lib/python3.6/site-packages/rtree/__init__.py", line 1, in <module>
    from .index import Rtree
  File "/home/ferdinand/.local/lib/python3.6/site-packages/rtree/index.py", line 5, in <module>
    from . import core
  File "/home/ferdinand/.local/lib/python3.6/site-packages/rtree/core.py", line 127, in <module>
    rt = ctypes.CDLL(lib_name)
  File "/usr/lib/python3.6/ctypes/__init__.py", line 348, in __init__
    self._handle = _dlopen(self._name, mode)
OSError: libspatialindex_c.so.5: cannot open shared object file: No such file or directory

Rewire never occurs due to inaccurate nearby vertices cost calculation.

Hey @SZanlongo. I would like to first thank you for your great work. It was extremely useful in my own C++ RRT library implementation.

One thing I have noticed during my experiments is that the tent_cost is always more than curr_cost. Its only reason is the following line:
path_cost(self.trees[tree].E, x_init, x_near) + segment_cost(x_near, x_new). You are computing all the cost from start when you are searching for nearby vertices. But then in rewire(), you add this cost to tentative cost one more time unnecessarily. That is, you simply double the tentative cost and it becomes impossible for it to be better than current cost. Despite this, your RRT* behaves very different than RRT, because you sort the nearby vertices according to their cost and connect with the minimal one. If you use connect_to_point() of RRT in RRT*, the path you will get will be the exact same of the former one without any rewiring.

I'm preparing to submit a PR for that, that's why I would like to note the problem. Again, many thanks for your effort.

EDIT: After printing costs of the path, I found out that current version outputs a much shorter path. However, rewire function is never called since get_nearby_vertices already determines the optimum nearest node due to different metric for cost. In classical RRT* implementations, that cost is computed with segment_cost(x_near, x_new) only. In terms of efficiency, this is much better, but I would like to know whether this is RRT* or has another name.

For educational purposes, I'm still planning to submit the PR. Perhaps we can discuss in more detail.

Seems hava bug in method lazy_shortening of class RRTStarBidirectionalHeuristic

# update best path
# remove cost of removed edges
pre_best = self.c_best
self.c_best -= sum(dist_between_points(i, j) for i, j in pairwise(self.sigma_best[a:b]))
# add cost of new edge
self.c_best += + dist_between_points(self.sigma_best[a], self.sigma_best[b])
self.sigma_best = self.sigma_best[:min(a, b) + 1] + self.sigma_best[max(a, b):]
after_best = self.c_best
print(self.samples_taken,"samples ","pre_best: ", pre_best, "after_best: ", after_best)

I add two variable to remember the change of c_best, run the example rrt_star_bid_h_2d.py(max_samples = 50000,prc = 0.000001)
the console is

1136 samples pre_best: 147.27631712157728 after_best: 150.50517143061194
1580 samples pre_best: 149.07440848778623 after_best: 154.02473881731657
1591 samples pre_best: 152.70569271151646 after_best: 189.95360817018766
1601 samples pre_best: 149.32839308025953 after_best: 179.4453476769691
1609 samples pre_best: 172.77377293324594 after_best: 157.15884990689494
1627 samples pre_best: 148.31354996340158 after_best: 156.29333137162521
1637 samples pre_best: 149.1706237177986 after_best: 157.16829854933493
1657 samples pre_best: 153.34796461046608 after_best: 157.16829854933496
1666 samples pre_best: 153.3652822500094 after_best: 160.96965032326972
1687 samples pre_best: 155.06727461460594 after_best: 169.25245716730342
1692 samples pre_best: 159.71872887272414 after_best: 170.4275243089187
1696 samples pre_best: 151.96171927099167 after_best: 172.5873966049496
1715 samples pre_best: 147.9352498164779 after_best: 154.97443333935067
1733 samples pre_best: 148.0548974739475 after_best: 173.80074256390108
1743 samples pre_best: 149.32378668988508 after_best: 175.93740885996755
1755 samples pre_best: 153.97781044999525 after_best: 181.29535020507882
1765 samples pre_best: 155.80810660614787 after_best: 170.51843700102742
1769 samples pre_best: 165.69613793043368 after_best: 194.97927005774272
1783 samples pre_best: 151.91759841366624 after_best: 193.3618164841871
1799 samples pre_best: 155.70062223710895 after_best: 163.16083759986492
1807 samples pre_best: 153.56712099976983 after_best: 169.4573163110155
1811 samples pre_best: 169.4573163110155 after_best: 196.4927441024516
1817 samples pre_best: 151.44461546378926 after_best: 175.0240219131137
1822 samples pre_best: 149.1612152734508 after_best: 164.85613888193072
1830 samples pre_best: 153.9478548782316 after_best: 160.52198975701043
1837 samples pre_best: 160.41089651882388 after_best: 168.35113349301423
2236 samples pre_best: 146.62926170454017 after_best: 147.5816293148402
2241 samples pre_best: 147.5816293148402 after_best: 151.61023624966396
2242 samples pre_best: 151.61023624966396 after_best: 181.43622010703797
2251 samples pre_best: 153.70750118031822 after_best: 157.54004003665247
2253 samples pre_best: 157.54004003665247 after_best: 173.14227684364138
2265 samples pre_best: 157.36418886806683 after_best: 164.9897074568459
2269 samples pre_best: 151.27365231989575 after_best: 192.9479331301609
2271 samples pre_best: 179.03234723755835 after_best: 218.14467572605895
2277 samples pre_best: 150.44856477666883 after_best: 156.97356214791188
2279 samples pre_best: 156.97356214791188 after_best: 164.83801818148063
2326 samples pre_best: 149.6099219232762 after_best: 152.5874562397474
2329 samples pre_best: 152.5874562397474 after_best: 160.25403684005317
2333 samples pre_best: 153.45287773460592 after_best: 173.800502332282
2335 samples pre_best: 154.17718899425404 after_best: 171.6316206505236
2338 samples pre_best: 156.06523698537885 after_best: 171.77863115575138
2343 samples pre_best: 151.45986597871934 after_best: 178.91763239481583
2349 samples pre_best: 149.15797474873287 after_best: 159.58070000333905
2363 samples pre_best: 150.0154718759291 after_best: 158.02998062964014
2381 samples pre_best: 149.48485494540273 after_best: 177.11044834512631
2390 samples pre_best: 148.91429948696754 after_best: 155.8776982411878
2392 samples pre_best: 148.9332211525741 after_best: 177.04876948395608
2398 samples pre_best: 148.64593590835386 after_best: 168.47199424897258
2400 samples pre_best: 151.1349936614904 after_best: 159.0976186154945
2414 samples pre_best: 146.35960502533163 after_best: 155.29782622292154
2426 samples pre_best: 150.37464467196168 after_best: 155.7942227186323
2434 samples pre_best: 155.7942227186323 after_best: 168.68684880982022
2441 samples pre_best: 161.68427836558953 after_best: 171.05784697968141
2462 samples pre_best: 153.34056678632658 after_best: 169.65327609241263
2481 samples pre_best: 148.80447276481226 after_best: 168.45850094303756
2483 samples pre_best: 165.63047407729917 after_best: 179.523988133413
2487 samples pre_best: 150.7347010169954 after_best: 154.5681529983975
2509 samples pre_best: 150.48920306172397 after_best: 159.06384380869687
2512 samples pre_best: 159.06384380869687 after_best: 188.34102476107265
2521 samples pre_best: 148.49010659871817 after_best: 173.6172654252876
2530 samples pre_best: 150.50728730026722 after_best: 172.52243182850617
2532 samples pre_best: 150.96133314918342 after_best: 163.27333650603805
2537 samples pre_best: 150.43652531756268 after_best: 158.2451879629066
2554 samples pre_best: 149.87327040803999 after_best: 171.53825345780712
2557 samples pre_best: 151.06725426003794 after_best: 183.82313642937237
2561 samples pre_best: 153.951208335046 after_best: 164.0427601623763
2564 samples pre_best: 153.08933898574304 after_best: 177.95379691685451
2566 samples pre_best: 172.40827627791012 after_best: 180.22835519065126
2568 samples pre_best: 149.86725589376144 after_best: 180.32719821066428
2580 samples pre_best: 150.0030732580511 after_best: 158.0030711255912
2581 samples pre_best: 158.0030711255912 after_best: 185.17292871463349
2583 samples pre_best: 149.36097389846253 after_best: 167.694582078274
2586 samples pre_best: 154.2788957293173 after_best: 170.06069615064087
2595 samples pre_best: 150.56956518196557 after_best: 157.09456255320862
2604 samples pre_best: 149.69724066197475 after_best: 165.08631184523537
2607 samples pre_best: 154.07481982880125 after_best: 166.29742055517153
2615 samples pre_best: 147.37582381850086 after_best: 165.31709364168796
2623 samples pre_best: 150.11558067197655 after_best: 158.07202869108107
2625 samples pre_best: 152.56589213641718 after_best: 175.31563415470316
2629 samples pre_best: 149.87620185906923 after_best: 149.87705787991501
2652 samples pre_best: 149.87705787991501 after_best: 160.1865682242192
2675 samples pre_best: 147.57654441702468 after_best: 182.97273703360898
2677 samples pre_best: 149.74642256792947 after_best: 160.85757727280804
2690 samples pre_best: 149.03731687964074 after_best: 156.14409697415604
2695 samples pre_best: 152.71344491143924 after_best: 173.46244316056683
2702 samples pre_best: 161.63372931567147 after_best: 173.60477336036297
2709 samples pre_best: 148.55702642327205 after_best: 169.3085830810782
2725 samples pre_best: 148.2630011567049 after_best: 152.69770135960783
2735 samples pre_best: 148.18791139146055 after_best: 154.30061538386755
2772 samples pre_best: 147.7942484395685 after_best: 149.84915605788547
2788 samples pre_best: 149.84915605788547 after_best: 179.05605538348172
...

seems the method does not shortening the current best path?

Animate tree growth

Path Shortening / Smoothing

Final path should be shortened or smoothed using something like the Ramer-Douglas-Peucker Algorithm, while also respecting obstacles.

collision_free should use an all-numpy implementation

Safety Distance From Obstacle

Hello, thanks for this work, that's a good :)

But I am wondering how can I modify the safe distance to be with obstacles?

How can I find your paper here?

How can I find your paper here?

The length of extending step

Hello,

I would like to know how to adjust the length of extending step when the RRT is executing.
I saw you wrote this line in the readme, but I can't find the parameter in programs.

q: Distance away from existing vertices to probe.

Hope you can give me some tips, thanks!

Dynamic parameter adjustment

Allow for parameters to automatically adjust when a solution is not found.

Help with this errror

michael@michael:~/dev/rrt-algorithms/examples/rrt$ python3 rrt_2d.py
Traceback (most recent call last):
File "rrt_2d.py", line 5, in
from src.rrt.rrt import RRT
ModuleNotFoundError: No module named 'src'

Hello,
Can someone please help me with this error? i'm running this on ubuntu 18.04.

Thanks

Multithread where possible

step size in rrt-connect

Great work! I spot is a general issue in the use of step size in steer function. Maybe your other RRT implementations also suffer from the same issue. Please let me know if the following observations are wrong.

In your code, steer always takes a fixed step size which is fine till q_new gets very close to the target.

Let's think about the case of rrt-connect. The second tree is repeatedly getting closer to a target vertex q_first in the first tree,
Eventually it reaches a point where distance between q_second and q_first is below edge length q. If I understand your code correctly, in such a case the next steer will move beyond q_first to a position q_last_steer. This has two consequences:

1. q_second and q_first are sampled within boundaries. But q_last_steer may not be. I suspect this is the cause of the out-of-bound errors reported by other users (your shouldn't need to define bound_point function).
2. The two trees at this moment should be very likely to connect since they have two nodes that are less than q-distance apart. But sadly, q_last_steer will not yield REACH unless you increase the threshold 1e-2 to the scale of edge length q.

This explains why rrt-connect does not exhibit superior efficiency against other planners. I suggest a quick fix by simply modifying the step size used in steer to be min(q, norm(q_second-q_first)) .

unable to connect to the goal

Hi,i get this when i run the code.Has someone meet the same question?
Checking if can connect to goal at 9 samples
Could not connect to goal
Checking if can connect to goal at 38 samples
Can connect to goal

nearest_vertices should take advantage of rtree

Setting Edge Lengths as Float Type

Firstly, I want to express my gratitude for your open-source code—it helps me a lot : )

In my current project, I find the need for edge lengths to be represented as floating-point numbers. This specifically relates to the variable Q in examples/rrt_3d.py. However, this requirement exposes an issue in the file rrt-algorithms/rrt/rrt.py.

while True:
    for q in self.Q:  # iterating over different edge lengths until a solution is found or a timeout occurs
        for i in range(int(q[1])):  # iterating over the number of edges of the given length to add
            x_new, x_nearest = self.new_and_near(0, q)

            if x_new is None:
                continue

            # connecting the shortest valid edge
            self.connect_to_point(0, x_nearest, x_new)

            solution = self.check_solution()
            if solution[0]:
                return solution[1]

The issue arises because Q is a numpy array, and q[1] is automatically interpreted as a float, causing a problem when passing it to the range() function. To resolve this, I performed an integer type conversion onq[1] using range(int(q[1])).

I'm curious if this is a minor oversight in the code that could be addressed.

Best regards

Unpredictable Obstacles

Hi , i want to add triangle Obstacles, not rectangle , i don't know how to change the code. I look forward to hearing from you. Thakns!

Forgotten c_best update in lazy_shortening of rrt_star_bid_h

Hi there,

I was producing metrics with these algorithms and figured out this problem which results in an infinite loop in Heuristicized Bidirectional RRT*. Whenever it shortens the path, the best cost should be adapted accordingly after the line:
self.sigma_best = self.sigma_best[:min(a, b) + 1] + self.sigma_best[max(a, b):]

So we need to simply iterate over the sigma_best and compute new cost after the line above. I'm planning to submit a minor PR for that in this evening.

The choice of heuristic function in RRT Star

Thank you for your excellent work! It has been incredibly helpful to me.

While reviewing your code, I encountered some confusion regarding the design of the "Heuristic function" within the following functions in rrt_star.py:

pythonCopy codedef connect_shortest_valid(self, tree, x_new, L_near):
    """
    Connect to the nearest vertex that has an unobstructed path
    :param tree: int, tree being added to
    :param x_new: tuple, vertex being added
    :param L_near: list of nearby vertices
    """
    # Check nearby vertices for total cost and connect shortest valid edge
    for c_near, x_near in L_near:
        if c_near + cost_to_go(x_near, self.x_goal) < self.c_best and self.connect_to_point(tree, x_near, x_new):
            break

I perceive the expression "c_near + cost_to_go(x_near, self.x_goal)" as a heuristic function. However, I'm not quite sure why this particular heuristic function was chosen.

Additionally, when running the RRT star algorithm alone, the value of self.c_best remains inf. Consequently, the function above consistently chooses the first collision-free nearby vertex as the parent node of the new vertex.

RRT* entering loop

Caused by a loop in P, so path is the point itself

Preserve work when modifying obstacles

If an obstacle is added, update the graph as needed, rather than having to re-create the entire Search Space.
This can be done by checking every single vertex in the tree (or trees), to see if it resides within the newly-added obstacle, and removing the vertex and corresponding edges as needed.

When removing an obstacle, no modification of the graph should be necessary.

in the rrt_start.py there two comments confused me

in the method get_nearby_vertices
:return: list of nearby vertices and their costs, sorted in ascending order by cost
but it real raturn sorted in ascending order by x_near's first coordinate

in the method current_rewire_count

if no rewire count specified, set rewire count to be all vertices

but the code after it is
if self.rewire_count is not None:
return self.trees[tree].V_count
does it means if rewire count specified, always set rewire count to be all vertices?

Collision-checking should use a Van-der-Corput-like approach

Branch-and-bound

Efficiently keep track of vertex weights, and update them when a parent is modified. Probabilistically prune the trees when a solution has already been found and the edges cannot possibly beat the solution. #

Bias Sampling towards Goal

Allow for sampling to be biased towards the goal as time moves on.
Example: sample from a normal distribution biased towards the goal, where the bias increases over time as

bias = t / (t + r)

where r is a constant that tunes the steepness of the curve.