zhm-real / pathplanning

Common used path planning algorithms with animations.

Overview

This repository implements some common path planning algorithms used in robotics, including Search-based algorithms and Sampling-based algorithms. We designed animation for each algorithm to display the running process. The related papers are listed in Papers.

Directory Structure

.
└── Search-based Planning
    ├── Breadth-First Searching (BFS)
    ├── Depth-First Searching (DFS)
    ├── Best-First Searching
    ├── Dijkstra's
    ├── A*
    ├── Bidirectional A*
    ├── Anytime Repairing A*
    ├── Learning Real-time A* (LRTA*)
    ├── Real-time Adaptive A* (RTAA*)
    ├── Lifelong Planning A* (LPA*)
    ├── Dynamic A* (D*)
    ├── D* Lite
    └── Anytime D*
└── Sampling-based Planning
    ├── RRT
    ├── RRT-Connect
    ├── Extended-RRT
    ├── Dynamic-RRT
    ├── RRT*
    ├── Informed RRT*
    ├── RRT* Smart
    ├── Anytime RRT*
    ├── Closed-Loop RRT*
    ├── Spline-RRT*
    ├── Fast Marching Trees (FMT*)
    └── Batch Informed Trees (BIT*)
└── Papers

Animations - Search-Based

Best-First & Dijkstra

A* and A* Variants

Animation - Sampling-Based

RRT & Variants

Papers

Search-base Planning

• A*: A Formal Basis for the heuristic Determination of Minimum Cost Paths
• Learning Real-Time A*: Learning in Real-Time Search: A Unifying Framework
• Real-Time Adaptive A*: Real-Time Adaptive A*
• Lifelong Planning A*: Lifelong Planning A*
• Anytime Repairing A*: ARA*: Anytime A* with Provable Bounds on Sub-Optimality
• D*: Optimal and Efficient Path Planning for Partially-Known Environments
• D* Lite: D* Lite
• Field D*: Field D*: An Interpolation-based Path Planner and Replanner
• Anytime D*: Anytime Dynamic A*: An Anytime, Replanning Algorithm
• Focussed D*: The Focussed D* Algorithm for Real-Time Replanning
• Potential Field, [PPT]: Real-Time Obstacle Avoidance for Manipulators and Mobile Robots
• Hybrid A*: Practical Search Techniques in Path Planning for Autonomous Driving

Sampling-based Planning

• RRT: Rapidly-Exploring Random Trees: A New Tool for Path Planning
• RRT-Connect: RRT-Connect: An Efficient Approach to Single-Query Path Planning
• Extended-RRT: Real-Time Randomized Path Planning for Robot Navigation
• Dynamic-RRT: Replanning with RRTs
• RRT*: Sampling-based algorithms for optimal motion planning
• Anytime-RRT*: Anytime Motion Planning using the RRT*
• Closed-loop RRT* (CL-RRT*): Real-time Motion Planning with Applications to Autonomous Urban Driving
• Spline-RRT*: Optimal path planning based on spline-RRT* for fixed-wing UAVs operating in three-dimensional environments
• LQR-RRT*: Optimal Sampling-Based Motion Planning with Automatically Derived Extension Heuristics
• RRT#: Use of Relaxation Methods in Sampling-Based Algorithms for Optimal Motion Planning
• RRT*-Smart: Rapid convergence implementation of RRT* towards optimal solution
• Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal heuristic
• Fast Marching Trees (FMT*): a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions
• Motion Planning using Lower Bounds (MPLB): Asymptotically-optimal Motion Planning using lower bounds on cost
• Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs
• Advanced Batch Informed Trees (ABIT*): Sampling-Based Planning with Advanced Graph-Search Techniques ((ICRA) 2020)
• Adaptively Informed Trees (AIT*): Fast Asymptotically Optimal Path Planning through Adaptive Heuristics ((ICRA) 2020)

pathplanning's Issues

D* algorithm

Hello, after I run the D* algorithm, the mouse click on the picture does not produce the effect of generating obstacles. I want to achieve the dynamic picture effect you showed, what should I do？For example, if I click on a certain area in the picture, an obstacle will be created, and the path will also change as the obstacle appears.

Hello author

May I ask if there is more detailed code explanation for A* ? I got some do not understand as a beginner.

Any chance to add jps?

tool name

which tool is required to run these codes

start problem

from Search_2D import plotting, env，显示错误import“Search_2D" could not be resolved

rrt algorith mistake

In line 54 there is a minor mistake.
There is must be
if dist <= self.step_len and not self.utils.is_collision(node_new, self.s_goal):
instead
if dist <= self.step_len:
because otherwise, if there is obstacle in a dist, than the last node will be connected trought it.

Hello, how to find the actual path length of rrt_star algorithm

Hello, how to find the actual path length of rrt_star algorithm

information

Hello, do you have any information about these algorithms?

I have a question of "rrt_star.py" in rrt_2D.

def find_near_neighbor(self, node_new):
n = len(self.vertex) + 1
r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and
not self.utils.is_collision(node_new, self.vertex[ind])]
return dist_table_index

Is this n plus 1 in the code correct?

2D A*算法

为什么在一些场景（简单）中会出现死循环和parent没找到坐标的情况

There is a problem in astar algorithm.

Hi zhm-real:
In astart, you build a priority queue.However, it will have repetition point when update the f value of a point.
I would like to know that how to avoid this situation or something i miss that you have already avioded this situation.`

how to solve？

use python3.8
The following prompt appears when running arastar.py
ModuleNotFoundError: No module named 'Search_based_Planning'
thank you

Informed RRT* 2D mistake

some high-level questions

Hello,

Nice work to combine several robot motion planners and open source a package!

I have some questions:

• What is the main difference between this package and the planners available in the moveit framework?
• How would you advise people to use this?
• Is this part of a greater project you are working on?
• Did you or are you planning to use it on real-robots?
• What are the current limitations of this framework?

Thanks a lot for your advice and thoughts!
Best,
Bryan

how to run

hello how to run it？

demand

Could you have a matlab version, thank you very much.

I have a question of "rrt_star.py" in rrt_2D.

rrt_star.py :
this part of code

def find_near_neighbor(self, node_new):
        n = len(self.vertex) + 1
        r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
        dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
        dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and
                            not self.utils.is_collision(node_new, self.vertex[ind])]
        return dist_table_index

r is the search radius to search for potential best parent nodes，but I don‘t think it should use min() function. I guess the function of the code about r is that as the vertex increases, the radius of the search for the parent node also increases. But it should use max() function. Maybe there is something I misunderstood, but using max() works better works more in principle..

using min():

using max():

Is it my understanding of your code is wrong, I hope to get your help.

hello

if np.random.random() > goal_sample_rate:
return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))

    return self.s_goal

why np.random.random() > goal_sample_rate:？？？and if not why return self.s_goal？？？

Can you include an overview paper for two different of path planning?

It's nice to see the algorithms implemented. Can you also include an overview paper to describe the basic ideas and differences of the algorithms?

3D.py errror

facing issues when trying to change the search space

I tried to change the search space dimensions and obstacles but it is not being implemented when i execute the code. The same search space and obstacles set by you appears instead. Please help as I need to test the algorithms to suit them to my need.

Install Sampling_based_Planning

How do you install Sampling based Planning as a library?

use a_star ,change the goal point ,but can't get the route

![road](https://user-images.githubusercontent.com/42332058/157599499-7cd79207-4150-4f6c-991e-4600a74487d
hello,when i use a star,change the goal point,but it can't get the result,can u give me some sdvice ,i will be appreciated

Path planning

use a_star change the goal point ,but can't get the result route

hello,when i use a_star,change the goal point,but it can't get the result,can u give me some sdvice ,i will be appreciated

.

There is some problem in LPAstar

In some case, there would be no path extrated right!

Adding Jump Point Search

Can you please implement JPS(Jump point search) under Path Finding Algorithms?

https://dl.acm.org/doi/10.5555/2900423.2900600

How about using poetry to manage dependencies?

There is a bug in rrt_star. The final path crosses with obstacles!

I have a problem when run rrt_connect3D.py

I try run the Sampling_based_Planning\rrt_3D\FMT_star3D.py, Sampling_based_Planning\rrt_3D\rrt_connect3D.py. Then, I got this.

Is this normal?

An exception occurred when drawing with bfs in "Search_2D"

I downloaded the entire zip and unzipped it in PyCharm. When I tried bfs for testing, everything went well until plot.animation(path,visited,"Breadth-first Search (BFS)")

Then I created a breakpoint and found the statement that stops the program. It is plt.plot(self.xI[0], self.xI[1], "bs") in def plot_grid(self, name) : In Search_based_Planning/Search_2D/plotting.py.

There may be something I didn't realize, but can you give a solution or some help? thanks : )

my environment is as follows:

1. Python 3.8.12 with acaconda
2. matplotlib 3.4.3
3. numpy 1.18.5

By the way, queue.py is best renamed to something like diy_queue.py, and queue.py will cause problems AttributeError: module'queue' has no attribute'queue'

when run 3D moudle: ValueError: operands could not be broadcast together with shapes (25,2,3) (0,)

when i run "Search_3D", the screenshot shows what happed.

GIF related

how to draw these gif?

Using smbclient to load PyTorch model weights from a server, but I encountered some issues.

I tried using smbclient to load PyTorch model weights from a server, but I encountered some issues.

file_path = r"\\x.x.x.x\home\pytorch-models\resnet18-f37072fd.pth"
with smbclient.open_file(file_path, mode='rb', buffering=0) as fd:
    data = torch.load(fd, map_location=torch.device('cpu'))

The error

Exception has occurred: RuntimeError
PytorchStreamReader failed reading file data/140219036705280: file read failed

Is there any way I can load model weights?

gifs

I am wonderring how do u make the gifs~

Need help with DYNAMIC_RRT_2D

I'm doing some stuff that dynamic rrt 2D will fit perfectly. But I'm having a really hard time understand the code. I know the basics of RRT and RRT* but the code still confusing.

1. Why separate Node and Edge ? You can just create a Node with a parent node and a child node right ? And in Edge class parent and child is a Node ?
2. How do you represent the entire map ? Using some kind of grid ? And can you show me the code of that grid ?
3. What's the difference between generate_random_node and generate_random_node_replanning ?
4. What does the is_collision_obs_add function do ?
5. What is self.utils.delta value ?
6. What do extract_path and extract_waypoint function do ?
7. In the DynamicRrt class what is
   - goal_sample_rate
   - waypoint_sample_rate
   - iter_max
   - vertex
   - vertex_old
   - vertex_new
   - x_range
   - y_range
   - path waypoint
   Thank you

path length

Hello, where can I see the length of this path in the code?

self.motions

self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1),
(1, 0), (1, -1), (0, -1), (-1, -1)]
What does that mean？？？thanks

AttributeError: 'rrt_demo' object has no attribute 'Parent'

Hi,
When I run the rrt3D.py, it occured a mistake:
Original exception was: Traceback (most recent call last): File "/home/xia/mydemo01_ws/src/PathPlanning-master/Sampling_based_Planning/rrt_3D/utils3D.py", line 410, in <module> A.run() File "/home/xia/mydemo01_ws/src/PathPlanning-master/Sampling_based_Planning/rrt_3D/utils3D.py", line 403, in run visualization(self) File "/home/xia/mydemo01_ws/src/PathPlanning-master/Sampling_based_Planning/rrt_3D/../../Sampling_based_Planning/rrt_3D/plot_util3D.py", line 96, in visualization for i in initparams.Parent: AttributeError: 'rrt_demo' object has no attribute 'Parent'
What should I do next?

Some advice about license compliance

Hello,
such a nice repository benefits me a lot and so kind of you to make it open source!

Question
There’s some possible legal issues on the license of your repository when you combine numerous third-party packages.
For instance, matplotlib, scipy, numpy and random you imported are licensed with Python Software Foundation License (PSF), BSD License, BSD License and Python Software Foundation License, respectively.
However, the MIT license of your repository are less strict than above package licenses, which has violated the whole license compatibility in your repository and may bring legal and financial risks.

Advice
You can select another proper license for your repository, or write a custom license with license exception if some license terms couldn’t be summed up consistently.

Best wishes!

When I run rrt*, path will intersect with the circle obstacles.