# 2D vs 3D Path Planning with Python

## Path Planning

Path planning or motion planning is a computational technique to find a geometrical path from the current location of a vehicle. It means to define a sequence of moves that can lead the vehicle collision-free from the source to the destination. Path planning is essential for many fields such as computer animation, computer games, robotics, and many others.

### Path Planning Algorithms

There are various path planning algorithms. These path planning algorithms are available for both 2D and 3D techniques. However, path planning is mainly done for 2D systems, but 3D approaches have also gained importance in the past few years. Therefore, much work is going on to achieve stable 3D algorithms for path planning.

### 2D Path Planning Algorithms

Some of the famous 2D path planning algorithms and approaches are the following:

- Dynamic-Window Approach
- A* Algorithm
- D* Algorithm
- D* Lite Algorithm
- Potential Field Algorithm
- Grid-Based Coverage Path Planning
- Quintic Polynomials Planning
- Linear Quadratic Regulator Based Path Planning
- State Lattice Planning
- Biased Polar Sampling
- Lane Sampling

### 3D Path Planning Algorithms

Similarly, there are various 3D path planning algorithms. Following are the categories of 3D path planning algorithms:

- Sampling-Based Algorithms
- Rapidly Exploring Random Trees (RRT)
- Dynamic Domain RRT (DDRRT)
- RRT*
- Artificial Potential Field
- 3D Voronoi
- Rapidly Exploring Random Graph (RRG)
- Probabilistic Roadmap (PRM)
- k-PRM
- s-PRM (PRM*)

- Node Based Optimal Algorithms
- Dijkstra’s Algorithm
- Lifelong Planning A* (LPA*)
- Theta*
- Lazy Theta*

- Mathematic Model-Based Algorithms
- Natural Language Processing (NLP)
- Fitness based
- Mixed-Integer Linear Programming (MILP)
- Broadcast Incremental Power (BIP)

- Bioinspired Algorithms
- Neural Network
- Genetic Algorithm (GA)
- Memetic Algorithm (MA)
- Particle Swarm Optimization (PSO)
- Ant Colony Optimization (ACO)
- Shuffled Frog Leaping Algorithm (SFLA)

- Multifusion Based Algorithms

## A* Algorithm for Path Planning in Python

A* is the advanced form of the Breadth-First Search (BFS) algorithm. It is an artificial intelligence algorithm to find optimal paths and graph traversals. It uses the heuristic path cost, the starting point, and the endpoint cost. Following is the sample code of using the A* algorithm to find the path between nodes:

import numpy as np import heapq import matplotlib.pyplot as plt from matplotlib.pyplot import figure grid_1 = np.array([ [0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1], [1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1], [0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]) source_p = (0,0) dest_p = (0,19) def heu(i, j): return np.sqrt((j[0] - i[0]) ** 2 + ([j1] - i[1]) ** 2) def astar(array, source_p, dest_p): neigh_1 = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] close_arr = set() came_from = {} g_score = {source_p:0} f_score = {source_p:heu(source_p, dest_p)} oheap = [] heapq.heappush(oheap, (f_score[source_p], source_p)) while oheap: current = heapq.heappop(oheap)[1] if current == dest_p: data = [] while curr in came_from: data.append(curr) curr = came_from[curr] return data close_arr.add(curr) for i, j in neigh_1: neigh = curr[0] + i, curr[1] + j tentative_g_score = g_score[curr] + heu(curr, neigh) if 0 <= neigh[0] < array.shape[0]: if 0 <= neigh1] < array.shape[1]: if array[neigh[0]][neigh[1]] == 1: continue else: continue else: continue if neigh in close_arr and tentative_g_score >= g_score.get(neigh, 0): continue if tentative_g_score < g_score.get(neigh, 0) or neigh not in [i[1]for i in oheap]: came_from[neigh] = curr g_score[neigh] = tentative_g_score f_score[neigh] = tentative_g_score + heu(neigh, dest_p) heapq.heappush(oheap, (f_score[neigh], neigh)) return False path = astar(grid_1, source_p, dest_p) path = path + [source_p] path = path[::-1] print(path) x_coords = [] y_coords = [] for i in (range(0,len(path))): x = path[i][0] y = path[i][1] x_coords.append(x) y_coords.append(y) fig, ax = plt.subplots(figsize=(20,20)) ax.imshow(grid_1, cmap=plt.cm.Dark2) ax.scatter(source_p[1],source_p[0], marker = "*", color = "yellow", s = 200) ax.scatter(dest_p[1],dest_p[0], marker = "*", color = "red", s = 200) ax.plot(y_coords,x_coords, color = "black") plt.show()

Here is a 20*20 NumPy array where “0” means the allowed and “1” means not allowed. The heuristic uses straight-line distance using the Pythagoras theorem to find the distance between two paths. The “close_arr” contains positions that the program does not have to consider again, “came_from” holds parent positions, and the “oheap” includes the places that the program has to consider to find the shortest path. The while loop checks for available positions and extracts the positions of neighbors having the lowest F score. The “heap.heappop” returns the smallest from the heap. If it is the goal, it returns. Otherwise, it adds the position to the closed list and finds possible neighbors for G scores. If the neighbor is outside the grid or is in the closet list having the greater G score, the program ignores the neighbor and continues. If the G score is low or the neighbor is not on the open list, the program updates it in the open list. The “astar” function takes the grid, start position, and destination to find the shortest path.

## Dijkstra’s Algorithm for Path Planning in Python

Dijkstra’s algorithm helps find the shortest possible path with already known weights of the edges. It is a dynamic programming algorithm that uses the BFS method. The concept is to fill the 3D weighted graph first and then search the whole graph for the minimum cost path. Following is the sample code for Dijkstra’s shortest path variants for 6,18, and 26 connected 3D image volumes or 4 and 8 2D connected images.

import dijkstra3d import numpy as np field_1 = np.ones((512, 512, 512), dtype=np.int32) start = (0,0,0) dest = (511, 511, 511) path = dijkstra3d.dijkstra(field, start, dest, connectivity=26) path = dijkstra3d.dijkstra(field, start, dest, bidirectional=True) path = dijkstra3d.dijkstra(field, start, dest, compass=True) parents = dijkstra3d.parental_field(field_1, start=(0,0,0), connectivity=6) path = dijkstra3d.path_from_parents(parents, dest=(511, 511, 511)) print(path.shape) dist_field = dijkstra3d.euclidean_distance_field(field_1, start=(0,0,0), anisotropy=(4,4,40)) dist_field = dijkstra3d.euclidean_distance_field( field_1, start=[ (0,0,0), (10, 40, 232) ], anisotropy=(4,4,40) ) dist_field = dijkstra3d.euclidean_distance_field(field_1, start=(0,0,0), anisotropy=(4,4,40), free_space_radius=300) dist_field, max_loc = dijkstra3d.euclidean_distance_field(field_1, start=(0,0,0), return_max_location=True) dist_field = dijkstra3d.distance_field(field_1, start=(0,0,0)) dist_field = dijkstra3d.distance_field(field_1, start=[ (0,0,0), (52, 55, 23) ]) dist_field, max_loc = dijkstra3d.distance_field(field_!, start=(0,0,0), return_max_location=True) graph = np.zeros(field_1.shape, dtype=np.uint32) graph += 0xffffffff graph[5,5,5] = graph[5,5,5] & 0xfffffffe path = dijkstra.dijkstra(..., voxel_graph=graph)

The path here is the list of x,y, and z coordinates. The program uses the distance from the target as a heuristic. The “parents” contains parent voxels, and “path_from_parents” helps compute a path from the source to the target. The “field_1” and start vortex help calculate the Euclidean distance from the start to all vertices. The program computes the distance from a source to all finite voxels. In this program, the vertices are voxels, and the edges are nearest neighbors. Similarly, the users can customize the code for required solutions and visualizations.

**Other useful articles:**

- OOP in Python
- Python v2 vs Python v3
- Variables, Data Types, and Syntaxes in Python
- Operators, Booleans, and Tuples
- Loops and Statements in Python
- Python Functions and Modules
- Regular Expressions in Python
- Python Interfaces
- JSON Data and Python
- Pip and its Uses in Python
- File Handling in Python
- Searching and Sorting Algorithms in Python
- System Programming (Pipes &Threads etc.)
- Database Programming in Python
- Debugging with Assertion in Python
- Sockets in Python
- InterOp in Python
- Exception Handling in Python
- Environments in Python
- Foundation of Data Science
- Reinforcement Learning
- Python for AI
- Applied Text Mining in Python
- Python Iterations using Libraries
- NumPy vs SciPy
- Python Array Indexing and Slicing
- PyGame
- PyTorch
- Python & Libraries
- Python with MySQL
- Python with MongoDB
- Path Planning Algorithm in Python
- Image Processing with Python
- Python and Machine Learning
- Numerical Computation with Python
- Web Automation in Python
- Network Fundamentals and Socket Programming
- Basic Artificial Neural Networks in Python
- Distributed Computing with Python
- Python Role in Big Data
- Python and Data Science
- Internet Data Handling with Python
- 2D vs 3D Path Planning with Python