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: