├── Main.py ├── README.md ├── a_star_3D.py ├── grid_3D_safe_zone.py ├── lazy_theta_star_3D.py └── theta_star_3D.py /Main.py: -------------------------------------------------------------------------------- 1 | # Main file 2 | 3 | import numpy as np 4 | import time 5 | import matplotlib.pyplot as plt 6 | from grid_3D_safe_zone import grid_3D_safe_zone 7 | from a_star_3D import a_star_3D 8 | from theta_star_3D import theta_star_3D 9 | 10 | 11 | ############################################################################################################## 12 | # Grid and path parameters 13 | 14 | # Grid size and resolution 15 | sizeE = [80, 80, 20] 16 | d_grid = 1 17 | 18 | # Starting point 19 | x0 = 5 20 | y0 = 7 21 | z0 = 10 22 | 23 | # Arrival point 24 | xend = 68 25 | yend = 66 26 | zend = 6 27 | 28 | # Number of points with low elevation around start and end point area 29 | n_low = 3 30 | 31 | # A* or Theta* cost weights 32 | kg = 1 33 | kh = 1.25 34 | ke = np.sqrt((xend-x0)**2+(yend-y0)**2+(zend-z0)**2) 35 | 36 | 37 | ############################################################################################################## 38 | # Map definition 39 | 40 | # Average flight altitude 41 | h = max(z0, zend) 42 | 43 | # Points coordinates in [y,x,z] format 44 | P0 = np.array([y0, x0, z0]) 45 | Pend = np.array([yend, xend, zend]) 46 | 47 | # Generate map 48 | [E, E_safe, E3d, E3d_safe] = grid_3D_safe_zone(sizeE, d_grid, h, P0, Pend, n_low) 49 | 50 | 51 | ############################################################################################################## 52 | # Path generation 53 | 54 | # Store gains in vector 55 | K = [kg, kh, ke] 56 | 57 | # Start measuring execution time 58 | start_time = time.time() 59 | 60 | # Generate path (chose one) 61 | # [path, n_points] = a_star_3D(K, E3d_safe, x0, y0, z0, xend, yend, zend, sizeE) 62 | [path, n_points] = theta_star_3D(K, E3d_safe, x0, y0, z0, xend, yend, zend, sizeE) 63 | 64 | # Stop measuring and print execution time 65 | print(" %s seconds" % (time.time() - start_time)) 66 | 67 | 68 | ############################################################################################################## 69 | # Figure 70 | 71 | X = np.arange(1, sizeE[0]-1, d_grid) 72 | Y = np.arange(1, sizeE[1]-1, d_grid) 73 | X, Y = np.meshgrid(X, Y) 74 | 75 | fig = plt.figure() 76 | ax = fig.gca(projection='3d') 77 | ax.plot_surface(X, Y, E[1:-1][:, 1:-1]) 78 | ax.plot(path[:][:, 1], path[:][:, 0], path[:][:, 2], 'kx-') 79 | ax.plot([x0], [y0], [z0], 'go') 80 | ax.plot([xend], [yend], [zend], 'ro') 81 | ax.set_xlabel('x') 82 | ax.set_ylabel('y') 83 | ax.set_zlabel('z') 84 | ax.auto_scale_xyz([0, sizeE[1]], [0, sizeE[0]], [0, sizeE[2]]) 85 | plt.show() 86 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Some Python tools for path planning on a 3D grid map, including 3D A star 2 | 3 | Main_3D.py: main file for defining the map and the path properties, to call the grid and path generation functions and to plot the results 4 | 5 | Grid_3D_safe_zone.py: randomly generates a 3D cluttered environment (obstacles connected to the groud), which is represented both as a 2D matrix and as 3D occupancy map. For both maps, a safety buffer zone is created around and above obstacles 6 | 7 | a_star_3D.py: 3D A* path planning algorithm 8 | theta_star_3D.py: 3D Theta* path planning algorithm 9 | lazy_theta_star_3D.py: 3D Lazy Theta* path planning algorithm 10 | -------------------------------------------------------------------------------- /a_star_3D.py: -------------------------------------------------------------------------------- 1 | # 3D A star algorithm 2 | 3 | import numpy as np 4 | 5 | 6 | def a_star_3D(K, E3d_safe, x0_old, y0_old, z0_old, xend_old, yend_old, zend_old, sizeE): 7 | 8 | 9 | ############################################################################################################## 10 | # Cost weights 11 | 12 | kg = K[0] 13 | kh = K[1] 14 | ke = K[2] 15 | 16 | 17 | ############################################################################################################## 18 | # Algorithm 19 | 20 | # If start and end points are not integer, they are rounded for the path generation. 21 | # Use of floor and ceil so they are different even if very close 22 | x0 = np.int(np.floor(x0_old)) 23 | y0 = np.int(np.floor(y0_old)) 24 | z0 = np.int(np.floor(z0_old)) 25 | xend = np.int(np.ceil(xend_old)) 26 | yend = np.int(np.ceil(yend_old)) 27 | zend = np.int(np.ceil(zend_old)) 28 | 29 | 30 | # Initialize 31 | 32 | # Size of environment matrix 33 | y_size = sizeE[0] 34 | x_size = sizeE[1] 35 | z_size = sizeE[2] 36 | 37 | 38 | # Node from which the good neighbour is reached 39 | came_fromx = np.zeros((y_size, x_size, z_size)) 40 | came_fromy = np.zeros((y_size, x_size, z_size)) 41 | came_fromz = np.zeros((y_size, x_size, z_size)) 42 | came_fromx[y0, x0, z0] = x0 43 | came_fromy[y0, x0, z0] = y0 44 | came_fromz[y0, x0, z0] = z0 45 | 46 | # Nodes already evaluated 47 | closed_list = np.array([]) 48 | 49 | # Nodes to be evaluated 50 | open_list = np.array([[y0, x0, z0]]) 51 | 52 | # Cost of moving from start point to the node 53 | G = float("inf") * np.ones((y_size, x_size, z_size)) 54 | G[y0, x0, z0] = 0 55 | 56 | # Initial total cost 57 | F = float("inf") * np.ones((y_size, x_size, z_size)) 58 | F[y0, x0, z0] = np.sqrt((xend-x0)**2+(yend-y0)**2+(zend-z0)**2) 59 | 60 | # Initialize 61 | f_open = np.array([[F[y0, x0, z0]]]) 62 | exit_path = 0 63 | 64 | 65 | # While open is not empty and we have not reached last point 66 | while len(open_list) > 0 and exit_path == 0: 67 | 68 | # Find the index location in open for the node with minimum f 69 | i_f_open_min = np.argmin(f_open) 70 | 71 | # Location of node with minimum f in open list 72 | ycurrent = open_list[i_f_open_min, 0] 73 | xcurrent = open_list[i_f_open_min, 1] 74 | zcurrent = open_list[i_f_open_min, 2] 75 | 76 | # Check if the arrival node is reached 77 | if xcurrent == xend and ycurrent == yend and zcurrent == zend: 78 | 79 | # Arrival node reached, exit and generate path 80 | exit_path = 1 81 | 82 | else: 83 | 84 | # Add the node to closed list 85 | if closed_list.shape[0] == 0: 86 | closed_list = np.array([[ycurrent, xcurrent, zcurrent]]) 87 | else: 88 | closed_list = np.vstack((closed_list, np.array([ycurrent, xcurrent, zcurrent]))) 89 | 90 | # Remove the node from open list 91 | open_list = np.delete(open_list, i_f_open_min, 0) 92 | f_open = np.delete(f_open, i_f_open_min, 0) 93 | 94 | # Check neighbour nodes 95 | for i in range(-1, 2): 96 | for j in range(-1, 2): 97 | for k in range(-1, 2): 98 | 99 | # If the neighbour node is within the grid 100 | if 0 <= xcurrent + i < x_size and 0 <= ycurrent + j < y_size and 0 <= zcurrent + k < z_size: 101 | 102 | # If the neighbour node does not belong to open and to closed lists 103 | neigh = np.array([ycurrent + j, xcurrent + i, zcurrent + k]) 104 | 105 | sum_open = np.sum(neigh == open_list, 1) 106 | sum_closed = np.sum(neigh == closed_list, 1) 107 | 108 | if len(sum_open) > 0: 109 | max_sum_open = max(sum_open) 110 | else: 111 | max_sum_open = 0 112 | 113 | if len(sum_closed) > 0: 114 | max_sum_closed = max(sum_closed) 115 | else: 116 | max_sum_closed = 0 117 | 118 | if max_sum_open < 3 and max_sum_closed < 3: 119 | 120 | # Add the neighbour node to open list 121 | open_list = np.vstack((open_list, neigh)) 122 | 123 | # Evaluate the distance from start to the current node + from the current node to the neighbour node 124 | g_try = G[ycurrent, xcurrent, zcurrent] + np.sqrt(i**2 + j**2 + k**2) 125 | 126 | if g_try < G[ycurrent + j, xcurrent + i, zcurrent + k]: 127 | 128 | # We are on the good path, save information 129 | 130 | # Record from which node the good neighbour is reached 131 | came_fromy[ycurrent + j, xcurrent + i, zcurrent + k] = ycurrent 132 | came_fromx[ycurrent + j, xcurrent + i, zcurrent + k] = xcurrent 133 | came_fromz[ycurrent + j, xcurrent + i, zcurrent + k] = zcurrent 134 | 135 | # Evaluate the cost function 136 | G[ycurrent + j, xcurrent + i, zcurrent + k] = g_try 137 | H = np.sqrt((xend - (xcurrent + i))**2 + (yend - (ycurrent + j))**2 + (zend - (zcurrent + k))**2) 138 | F[ycurrent + j, xcurrent + i, zcurrent + k] = kg * G[ycurrent + j, xcurrent + i, zcurrent + k] + kh * H + ke * E3d_safe[ycurrent + j, xcurrent + i, zcurrent + k] 139 | 140 | f_open = np.vstack((f_open, [F[ycurrent + j, xcurrent + i, zcurrent + k]])) 141 | 142 | 143 | 144 | # Reconstruct path backwards knowing from which node the good neighbour is reached 145 | 146 | # First element is the arrival point 147 | path_backwards = [ycurrent, xcurrent, zcurrent] 148 | 149 | # Initialize 150 | i = 1 151 | 152 | # While the start point is not reached 153 | while (xcurrent != x0) or (ycurrent != y0) or (zcurrent != z0): 154 | 155 | next_point = np.array([came_fromy[ycurrent, xcurrent, zcurrent], came_fromx[ycurrent, xcurrent, zcurrent], came_fromz[ycurrent, xcurrent, zcurrent]]) 156 | path_backwards = np.vstack((path_backwards, next_point)) 157 | 158 | ycurrent = np.int(path_backwards[i, 0]) 159 | xcurrent = np.int(path_backwards[i, 1]) 160 | zcurrent = np.int(path_backwards[i, 2]) 161 | 162 | i = i + 1 163 | 164 | 165 | # Number of waypoints 166 | n_points = path_backwards.shape[0] 167 | 168 | # Reverse path sequence 169 | path = np.flipud(path_backwards) 170 | 171 | # Reassign the original start and end points 172 | path[0, :] = [y0_old, x0_old, z0_old] 173 | path[-1, :] = [yend_old, xend_old, zend_old] 174 | 175 | return [path, n_points] 176 | -------------------------------------------------------------------------------- /grid_3D_safe_zone.py: -------------------------------------------------------------------------------- 1 | # 3D grid map generator 2 | 3 | import numpy as np 4 | from numpy.random import default_rng 5 | 6 | 7 | def grid_3D_safe_zone(sizeE, d_grid, h, P0, Pend, n_low): 8 | 9 | 10 | # Grid size 11 | y_size = sizeE[0] 12 | x_size = sizeE[1] 13 | z_size = sizeE[2] 14 | 15 | # Vertical grid vector 16 | z_grid = np.linspace(1, z_size, z_size//d_grid) 17 | 18 | 19 | ############################################################################################################## 20 | # Random grid generation, discrete height in big blocks 21 | 22 | mean_E = 0 23 | sigma = 1 24 | k_sigma = 2.25 # edit this to change obstacles density 25 | rng = default_rng() 26 | E = rng.normal(mean_E, sigma, (y_size, x_size)) 27 | sigma_obstacle = k_sigma * sigma 28 | E = np.uint8(E > sigma_obstacle) 29 | 30 | 31 | # Assign random altitude to blocks around points 32 | 33 | # Minimum obstacle altitude 34 | hh_min = 3 35 | 36 | # Initialize temporary matrix for evaluation 37 | EE = E.copy() 38 | 39 | for i in range(x_size): 40 | for j in range(y_size): 41 | 42 | # Block boundaries with random dimension (max -2:+2) 43 | k = np.arange(i - 1 - round(rng.beta(0.5, 0.5)), i + 1 + round(rng.beta(0.5, 0.5)) + 1) 44 | l = np.arange(j - 1 - round(rng.beta(0.5, 0.5)), j + 1 + round(rng.beta(0.5, 0.5)) + 1) 45 | 46 | # If block boundaries within the grid and if the node point value is high, 47 | if min(k) >= 0 and min(l) >= 0 and max(k) < x_size and max(l) < y_size and EE[j, i] == 1: 48 | 49 | # Assign random value to block 50 | hh = round(rng.normal(0.75*h, 0.5*h)) 51 | 52 | # Give a minimum value to the altitude and limit maximum altitude 53 | if hh < hh_min: 54 | hh = hh_min 55 | elif hh > z_size: 56 | hh = z_size 57 | 58 | E[np.ix_(l, k)] = hh * np.ones((len(l), len(k))) 59 | 60 | 61 | # Assign low elevation to and around start and end points 62 | E[np.ix_(np.arange(P0[0] - n_low, P0[0] + n_low + 1), np.arange(P0[1] - n_low, P0[1] + n_low + 1))] = 0 63 | E[np.ix_(np.arange(Pend[0] - n_low, Pend[0] + n_low + 1), np.arange(Pend[1] - n_low, Pend[1] + n_low + 1))] = 0 64 | 65 | 66 | ############################################################################################################## 67 | # Create 3D grid 68 | 69 | # Initialize 70 | E3d = np.zeros((y_size, x_size, z_size)) 71 | 72 | # Create grid occupancy index (0=free, 1=occupied) 73 | for i in range(z_size): 74 | E3d[np.ix_(np.arange(0, y_size), np.arange(0, x_size), [i])] = np.atleast_3d(E[0:y_size][:, 0:x_size] >= z_grid[i]) 75 | 76 | 77 | ############################################################################################################## 78 | # Create safe zone near high elevation elements 79 | 80 | # Initialize 81 | E_safe = E.copy() 82 | 83 | for i in range(x_size): 84 | for j in range(y_size): 85 | 86 | # Check neighbour nodes 87 | k = np.arange(i - 1, i + 2) 88 | l = np.arange(j - 1, j + 2) 89 | 90 | # Limit neighbours within the grid 91 | if min(k) < 0: 92 | k = np.arange(i, i + 2) 93 | elif max(k) >= x_size: 94 | k = np.arange(i - 1, i + 1) 95 | if min(l) < 0: 96 | l = np.arange(j, j + 2) 97 | elif max(l) >= y_size: 98 | l = np.arange(j - 1, j + 1) 99 | 100 | # Evaluation matrix 101 | E_eval = E[l][:, k] 102 | 103 | # If we are in a free point and nearby there is an obstacle 104 | if E[j, i] == 0 and E_eval.max() > 0: 105 | # Assign the maximum value of the neighbour nodes 106 | E_safe[j, i] = E_eval.max() 107 | 108 | 109 | # If point is elevated, add one safe step in altitude 110 | if 0 < E_safe[j, i] < (z_size - 1): 111 | E_safe[j, i] = E_safe[j, i] + 1 112 | 113 | 114 | ############################################################################################################## 115 | # Create grid occupancy index (0=free, 0.5=safe, 1=occupied) 116 | 117 | # Initialize 118 | E3d_safe = E3d.copy() 119 | 120 | for i in range(x_size): 121 | for j in range(y_size): 122 | for k in range(z_size): 123 | 124 | # Check neighbour nodes 125 | l = np.arange(i - 1, i + 2) 126 | m = np.arange(j - 1, j + 2) 127 | n = np.arange(k - 1, k + 2) 128 | 129 | # Limit neighbours within the grid 130 | if min(l) < 0: 131 | l = np.arange(i, i + 2) 132 | elif max(l) >= x_size: 133 | l = np.arange(i - 1, i + 1) 134 | if min(m) < 0: 135 | m = np.arange(j, j + 2) 136 | elif max(m) >= y_size: 137 | m = np.arange(j - 1, j + 1) 138 | if min(n) < 0: 139 | n = np.arange(k, k + 2) 140 | elif max(n) >= z_size: 141 | n = np.arange(k - 1, k + 1) 142 | 143 | # Evaluation matrix 144 | E_eval = E3d[m][:, l][:, :, n] 145 | 146 | # If we are in a free point and nearby there is an obstacle 147 | if E3d[j, i, k] == 0 and E_eval.max() == 1: 148 | # Assign safe value 149 | E3d_safe[j, i, k] = 0.5 150 | 151 | 152 | ############################################################################################################## 153 | # Create safe zone near borders 154 | 155 | E[np.ix_([0, -1], np.arange(0, y_size))] = z_size 156 | E[np.ix_(np.arange(0, x_size), [0, -1])] = z_size 157 | 158 | E_safe[np.ix_([0, -1], np.arange(0, y_size))] = z_size 159 | E_safe[np.ix_(np.arange(0, x_size), [0, -1])] = z_size 160 | 161 | E3d[np.ix_([0, -1], np.arange(0, y_size), np.arange(0, z_size))] = 1 162 | E3d[np.ix_(np.arange(0, x_size), [0, -1], np.arange(0, z_size))] = 1 163 | E3d[np.ix_(np.arange(0, x_size), np.arange(0, y_size), [0, -1])] = 1 164 | 165 | E3d_safe[np.ix_([0, -1], np.arange(0, y_size), np.arange(0, z_size))] = 1 166 | E3d_safe[np.ix_(np.arange(0, x_size), [0, -1], np.arange(0, z_size))] = 1 167 | E3d_safe[np.ix_(np.arange(0, x_size), np.arange(0, y_size), [0, -1])] = 1 168 | 169 | 170 | return [E, E_safe, E3d, E3d_safe] 171 | -------------------------------------------------------------------------------- /lazy_theta_star_3D.py: -------------------------------------------------------------------------------- 1 | # 3D A star algorithm 2 | 3 | import numpy as np 4 | from line_sight_partial_3D import line_sight_partial_3D 5 | 6 | 7 | def lazy_theta_star_3D(K, E3d_safe, x0_old, y0_old, z0_old, xend_old, yend_old, zend_old, sizeE): 8 | 9 | 10 | ############################################################################################################## 11 | # Cost weights 12 | 13 | kg = K[0] 14 | kh = K[1] 15 | ke = K[2] 16 | 17 | 18 | ############################################################################################################## 19 | # Algorithm 20 | 21 | # If start and end points are not integer, they are rounded for the path generation. 22 | # Use of floor and ceil so they are different even if very close 23 | x0 = np.int(np.floor(x0_old)) 24 | y0 = np.int(np.floor(y0_old)) 25 | z0 = np.int(np.floor(z0_old)) 26 | xend = np.int(np.ceil(xend_old)) 27 | yend = np.int(np.ceil(yend_old)) 28 | zend = np.int(np.ceil(zend_old)) 29 | 30 | 31 | # Initialize 32 | 33 | # Size of environment matrix 34 | y_size = sizeE[0] 35 | x_size = sizeE[1] 36 | z_size = sizeE[2] 37 | 38 | 39 | # Node from which the good neighbour is reached 40 | came_fromx = np.zeros((y_size, x_size, z_size)) 41 | came_fromy = np.zeros((y_size, x_size, z_size)) 42 | came_fromz = np.zeros((y_size, x_size, z_size)) 43 | came_fromx[y0, x0, z0] = x0 44 | came_fromy[y0, x0, z0] = y0 45 | came_fromz[y0, x0, z0] = z0 46 | 47 | # Nodes already evaluated 48 | closed_list = np.array([]) 49 | 50 | # Nodes to be evaluated 51 | open_list = np.array([[y0, x0, z0]]) 52 | 53 | # Cost of moving from start point to the node 54 | G = float("inf") * np.ones((y_size, x_size, z_size)) 55 | G[y0, x0, z0] = 0 56 | 57 | # Initial total cost 58 | F = float("inf") * np.ones((y_size, x_size, z_size)) 59 | F[y0, x0, z0] = np.sqrt((xend-x0)**2+(yend-y0)**2+(zend-z0)**2) 60 | 61 | # Initialize 62 | f_open = np.array([[F[y0, x0, z0]]]) 63 | exit_path = 0 64 | 65 | 66 | # While open is not empty and we have not reached last point 67 | while len(open_list) > 0 and exit_path == 0: 68 | 69 | # Find node with minimum f in open 70 | 71 | # Find the index location in open for the node with minimum f 72 | i_f_open_min = np.argmin(f_open) 73 | 74 | # Location of node with minimum f in open list 75 | ycurrent = open_list[i_f_open_min, 0] 76 | xcurrent = open_list[i_f_open_min, 1] 77 | zcurrent = open_list[i_f_open_min, 2] 78 | 79 | # Define line of sight evaluation points 80 | xb = [came_fromx[ycurrent, xcurrent, zcurrent], xcurrent] 81 | yb = [came_fromy[ycurrent, xcurrent, zcurrent], ycurrent] 82 | zb = [came_fromz[ycurrent, xcurrent, zcurrent], zcurrent] 83 | 84 | # Line of sight check between coming point(parent) and neighbour 85 | sight = line_sight_partial_3D(E3d_safe, xb, yb, zb, sizeE) 86 | 87 | 88 | # If there is no line of sight 89 | if sight == 0 or sight == 0.5: 90 | 91 | # Initialize minimum g value 92 | g_min = float("inf") 93 | 94 | # Check neighbour nodes 95 | for i in range(-1, 2): 96 | for j in range(-1, 2): 97 | for k in range(-1, 2): 98 | 99 | 100 | # If the neighbour node is within the grid 101 | if 0 <= xcurrent + i < x_size and 0 <= ycurrent + j < y_size and 0 <= zcurrent + k < z_size: 102 | 103 | # If the neighbour belongs to closed lists 104 | neigh = np.array([ycurrent + j, xcurrent + i, zcurrent + k]) 105 | 106 | sum_closed = np.sum(neigh == closed_list, 1) 107 | if len(sum_closed) > 0: 108 | max_sum_closed = max(sum_closed) 109 | else: 110 | max_sum_closed = 0 111 | 112 | if max_sum_closed == 3: 113 | 114 | # Add the neighbour node to open list 115 | open_list = np.vstack((open_list, neigh)) 116 | 117 | 118 | # Evaluate the distance from start to neighbour node + from the neighbour node to the current node 119 | g_try = G[ycurrent + j, xcurrent + i, zcurrent + k] + np.sqrt(i ** 2 + j ** 2 + k ** 2) 120 | 121 | # If this distance is smallest, save it and assign neighbour as parent node 122 | if g_try < g_min: 123 | 124 | g_min = g_try 125 | G[ycurrent, xcurrent, zcurrent] = g_try 126 | 127 | # Record from which node the good neighbour is reached 128 | came_fromy[ycurrent, xcurrent, zcurrent] = ycurrent + j 129 | came_fromx[ycurrent, xcurrent, zcurrent] = xcurrent + i 130 | came_fromz[ycurrent, xcurrent, zcurrent] = zcurrent + k 131 | 132 | f_open = np.vstack((f_open, [F[ycurrent + j, xcurrent + i, zcurrent + k]])) 133 | 134 | 135 | 136 | # Check if the arrival node is reached 137 | if xcurrent == xend and ycurrent == yend and zcurrent == zend: 138 | 139 | # Arrival node reached, exit and generate path 140 | exit_path = 1 141 | 142 | else: 143 | 144 | # Add the node to closed list 145 | if closed_list.shape[0] == 0: 146 | closed_list = np.array([[ycurrent, xcurrent, zcurrent]]) 147 | else: 148 | closed_list = np.vstack((closed_list, np.array([ycurrent, xcurrent, zcurrent]))) 149 | 150 | # Remove the node from open list 151 | open_list = np.delete(open_list, i_f_open_min, 0) 152 | f_open = np.delete(f_open, i_f_open_min, 0) 153 | 154 | 155 | # Check neighbour nodes 156 | for i in range(-1, 2): 157 | for j in range(-1, 2): 158 | for k in range(-1, 2): 159 | 160 | # If the neighbour node is within the grid 161 | if 0 <= xcurrent + i < x_size and 0 <= ycurrent + j < y_size and 0 <= zcurrent + k < z_size: 162 | 163 | # If the neighbour node does not belong to open and to closed lists 164 | neigh = np.array([ycurrent + j, xcurrent + i, zcurrent + k]) 165 | 166 | sum_open = np.sum(neigh == open_list, 1) 167 | sum_closed = np.sum(neigh == closed_list, 1) 168 | 169 | if len(sum_open) > 0: 170 | max_sum_open = max(sum_open) 171 | else: 172 | max_sum_open = 0 173 | 174 | if len(sum_closed) > 0: 175 | max_sum_closed = max(sum_closed) 176 | else: 177 | max_sum_closed = 0 178 | 179 | if max_sum_open < 3 and max_sum_closed < 3: 180 | 181 | # Add the neighbour node to open list 182 | open_list = np.vstack((open_list, neigh)) 183 | 184 | # Evaluate the distance from start to coming point(parent)+ from the coming point(parent) to the neighbour of the current node 185 | yg = np.int(came_fromy[ycurrent, xcurrent, zcurrent]) 186 | xg = np.int(came_fromx[ycurrent, xcurrent, zcurrent]) 187 | zg = np.int(came_fromz[ycurrent, xcurrent, zcurrent]) 188 | g_try = G[yg, xg, zg] + np.sqrt((came_fromy[ycurrent, xcurrent, zcurrent] - (ycurrent + j)) ** 2 + (came_fromx[ycurrent, xcurrent, zcurrent] - (xcurrent + i)) ** 2 + (came_fromz[ycurrent, xcurrent, zcurrent] - (zcurrent + k)) ** 2) 189 | 190 | # If this distance is smaller than the neighbour distance 191 | if g_try < G[ycurrent + j, xcurrent + i, zcurrent + k]: 192 | # We are on the good path, save information 193 | 194 | # Record from which node the good neighbour is reached 195 | came_fromy[ycurrent + j, xcurrent + i, zcurrent + k] = came_fromy[ycurrent, xcurrent, zcurrent] 196 | came_fromx[ycurrent + j, xcurrent + i, zcurrent + k] = came_fromx[ycurrent, xcurrent, zcurrent] 197 | came_fromz[ycurrent + j, xcurrent + i, zcurrent + k] = came_fromz[ycurrent, xcurrent, zcurrent] 198 | 199 | # Evaluate the cost function 200 | G[ycurrent + j, xcurrent + i, zcurrent + k] = g_try 201 | H = np.sqrt((xend - (xcurrent + i)) ** 2 + (yend - (ycurrent + j)) ** 2 + (zend - (zcurrent + k)) ** 2) 202 | F[ycurrent + j, xcurrent + i, zcurrent + k] = kg * G[ycurrent + j, xcurrent + i, zcurrent + k] + kh * H + ke * E3d_safe[ycurrent + j, xcurrent + i, zcurrent + k] 203 | 204 | 205 | f_open = np.vstack((f_open, [F[ycurrent + j, xcurrent + i, zcurrent + k]])) 206 | 207 | 208 | 209 | # Reconstruct path backwards knowing from which node the good neighbour is reached 210 | 211 | # First element is the arrival point 212 | path_backwards = [ycurrent, xcurrent, zcurrent] 213 | 214 | # Initialize 215 | i = 1 216 | 217 | # While the start point is not reached 218 | while (xcurrent != x0) or (ycurrent != y0) or (zcurrent != z0): 219 | 220 | next_point = np.array([came_fromy[ycurrent, xcurrent, zcurrent], came_fromx[ycurrent, xcurrent, zcurrent], came_fromz[ycurrent, xcurrent, zcurrent]]) 221 | path_backwards = np.vstack((path_backwards, next_point)) 222 | 223 | ycurrent = np.int(path_backwards[i, 0]) 224 | xcurrent = np.int(path_backwards[i, 1]) 225 | zcurrent = np.int(path_backwards[i, 2]) 226 | 227 | i = i + 1 228 | 229 | 230 | # Number of waypoints 231 | n_points = path_backwards.shape[0] 232 | 233 | # Reverse path sequence 234 | path = np.flipud(path_backwards) 235 | 236 | # Reassign the original start and end points 237 | path[0, :] = [y0_old, x0_old, z0_old] 238 | path[-1, :] = [yend_old, xend_old, zend_old] 239 | 240 | 241 | return [path, n_points] 242 | -------------------------------------------------------------------------------- /theta_star_3D.py: -------------------------------------------------------------------------------- 1 | # 3D A star algorithm 2 | 3 | import numpy as np 4 | from line_sight_partial_3D import line_sight_partial_3D 5 | 6 | 7 | def theta_star_3D(K, E3d_safe, x0_old, y0_old, z0_old, xend_old, yend_old, zend_old, sizeE): 8 | 9 | 10 | ############################################################################################################## 11 | # Cost weights 12 | 13 | kg = K[0] 14 | kh = K[1] 15 | ke = K[2] 16 | 17 | 18 | ############################################################################################################## 19 | # Algorithm 20 | 21 | # If start and end points are not integer, they are rounded for the path generation. 22 | # Use of floor and ceil so they are different even if very close 23 | x0 = np.int(np.floor(x0_old)) 24 | y0 = np.int(np.floor(y0_old)) 25 | z0 = np.int(np.floor(z0_old)) 26 | xend = np.int(np.ceil(xend_old)) 27 | yend = np.int(np.ceil(yend_old)) 28 | zend = np.int(np.ceil(zend_old)) 29 | 30 | 31 | # Initialize 32 | 33 | # Size of environment matrix 34 | y_size = sizeE[0] 35 | x_size = sizeE[1] 36 | z_size = sizeE[2] 37 | 38 | 39 | # Node from which the good neighbour is reached 40 | came_fromx = np.zeros((y_size, x_size, z_size)) 41 | came_fromy = np.zeros((y_size, x_size, z_size)) 42 | came_fromz = np.zeros((y_size, x_size, z_size)) 43 | came_fromx[y0, x0, z0] = x0 44 | came_fromy[y0, x0, z0] = y0 45 | came_fromz[y0, x0, z0] = z0 46 | 47 | # Nodes already evaluated 48 | closed_list = np.array([]) 49 | 50 | # Nodes to be evaluated 51 | open_list = np.array([[y0, x0, z0]]) 52 | 53 | # Cost of moving from start point to the node 54 | G = float("inf") * np.ones((y_size, x_size, z_size)) 55 | G[y0, x0, z0] = 0 56 | 57 | # Initial total cost 58 | F = float("inf") * np.ones((y_size, x_size, z_size)) 59 | F[y0, x0, z0] = np.sqrt((xend-x0)**2+(yend-y0)**2+(zend-z0)**2) 60 | 61 | # Initialize 62 | f_open = np.array([[F[y0, x0, z0]]]) 63 | exit_path = 0 64 | 65 | 66 | # While open is not empty and we have not reached last point 67 | while len(open_list) > 0 and exit_path == 0: 68 | 69 | # Find node with minimum f in open 70 | 71 | # Find the index location in open for the node with minimum f 72 | i_f_open_min = np.argmin(f_open) 73 | 74 | # Location of node with minimum f in open list 75 | ycurrent = open_list[i_f_open_min, 0] 76 | xcurrent = open_list[i_f_open_min, 1] 77 | zcurrent = open_list[i_f_open_min, 2] 78 | 79 | # Check if the arrival node is reached 80 | if xcurrent == xend and ycurrent == yend and zcurrent == zend: 81 | 82 | # Arrival node reached, exit and generate path 83 | exit_path = 1 84 | 85 | else: 86 | 87 | # Add the node to closed list 88 | if closed_list.shape[0] == 0: 89 | closed_list = np.array([[ycurrent, xcurrent, zcurrent]]) 90 | else: 91 | closed_list = np.vstack((closed_list, np.array([ycurrent, xcurrent, zcurrent]))) 92 | 93 | # Remove the node from open list 94 | open_list = np.delete(open_list, i_f_open_min, 0) 95 | f_open = np.delete(f_open, i_f_open_min, 0) 96 | 97 | # Check neighbour nodes 98 | for i in range(-1, 2): 99 | for j in range(-1, 2): 100 | for k in range(-1, 2): 101 | 102 | # If the neighbour node is within the grid 103 | if 0 <= xcurrent + i < x_size and 0 <= ycurrent + j < y_size and 0 <= zcurrent + k < z_size: 104 | 105 | # If the neighbour node does not belong to open and to closed lists 106 | neigh = np.array([ycurrent + j, xcurrent + i, zcurrent + k]) 107 | 108 | sum_open = np.sum(neigh == open_list, 1) 109 | sum_closed = np.sum(neigh == closed_list, 1) 110 | 111 | if len(sum_open) > 0: 112 | max_sum_open = max(sum_open) 113 | else: 114 | max_sum_open = 0 115 | 116 | if len(sum_closed) > 0: 117 | max_sum_closed = max(sum_closed) 118 | else: 119 | max_sum_closed = 0 120 | 121 | if max_sum_open < 3 and max_sum_closed < 3: 122 | 123 | # Add the neighbour node to open list 124 | open_list = np.vstack((open_list, neigh)) 125 | 126 | 127 | # Define line of sight evaluation points 128 | xb = [came_fromx[ycurrent, xcurrent, zcurrent], xcurrent + i] 129 | yb = [came_fromy[ycurrent, xcurrent, zcurrent], ycurrent + j] 130 | zb = [came_fromz[ycurrent, xcurrent, zcurrent], zcurrent + k] 131 | 132 | 133 | # Line of sight check between coming point(parent) and neighbour 134 | sight = line_sight_partial_3D(E3d_safe, xb, yb, zb, sizeE) 135 | 136 | if sight == 1: 137 | 138 | # Evaluate the distance from start to coming point(parent)+ from the coming point(parent) to the neighbour of the current node 139 | yg = np.int(came_fromy[ycurrent, xcurrent, zcurrent]) 140 | xg = np.int(came_fromx[ycurrent, xcurrent, zcurrent]) 141 | zg = np.int(came_fromz[ycurrent, xcurrent, zcurrent]) 142 | g_try = G[yg, xg, zg] + np.sqrt((came_fromy[ycurrent, xcurrent, zcurrent] - (ycurrent+j))**2 + (came_fromx[ycurrent, xcurrent, zcurrent] - (xcurrent+i))**2 + (came_fromz[ycurrent, xcurrent, zcurrent] - (zcurrent+k))**2) 143 | 144 | # If this distance is smaller than the neighbour distance 145 | if g_try < G[ycurrent + j, xcurrent + i, zcurrent + k]: 146 | 147 | # We are on the good path, save information 148 | 149 | # Record from which node the good neighbour is reached 150 | came_fromy[ycurrent + j, xcurrent + i, zcurrent + k] = came_fromy[ycurrent, xcurrent, zcurrent] 151 | came_fromx[ycurrent + j, xcurrent + i, zcurrent + k] = came_fromx[ycurrent, xcurrent, zcurrent] 152 | came_fromz[ycurrent + j, xcurrent + i, zcurrent + k] = came_fromz[ycurrent, xcurrent, zcurrent] 153 | 154 | # Evaluate the cost function 155 | G[ycurrent + j, xcurrent + i, zcurrent + k] = g_try 156 | H = np.sqrt((xend - (xcurrent + i))**2 + (yend - (ycurrent + j))**2 + (zend - (zcurrent + k))**2) 157 | F[ycurrent + j, xcurrent + i, zcurrent + k] = kg * G[ycurrent + j, xcurrent + i, zcurrent + k] + kh * H + ke * E3d_safe[ycurrent + j, xcurrent + i, zcurrent + k] 158 | 159 | 160 | else: 161 | 162 | # Evaluate the distance from start to the current node + from the current node to the neighbour node 163 | g_try = G[ycurrent, xcurrent, zcurrent] + np.sqrt(i**2 + j**2 + k**2) 164 | 165 | if g_try < G[ycurrent + j, xcurrent + i, zcurrent + k]: 166 | 167 | # We are on the good path, save information 168 | 169 | # Record from which node the good neighbour is reached 170 | came_fromy[ycurrent + j, xcurrent + i, zcurrent + k] = ycurrent 171 | came_fromx[ycurrent + j, xcurrent + i, zcurrent + k] = xcurrent 172 | came_fromz[ycurrent + j, xcurrent + i, zcurrent + k] = zcurrent 173 | 174 | # Evaluate the cost function 175 | G[ycurrent + j, xcurrent + i, zcurrent + k] = g_try 176 | H = np.sqrt((xend - (xcurrent + i))**2 + (yend - (ycurrent + j))**2 + (zend - (zcurrent + k))**2) 177 | F[ycurrent + j, xcurrent + i, zcurrent + k] = kg * G[ycurrent + j, xcurrent + i, zcurrent + k] + kh * H + ke * E3d_safe[ycurrent + j, xcurrent + i, zcurrent + k] 178 | 179 | 180 | f_open = np.vstack((f_open, [F[ycurrent + j, xcurrent + i, zcurrent + k]])) 181 | 182 | 183 | 184 | # Reconstruct path backwards knowing from which node the good neighbour is reached 185 | 186 | # First element is the arrival point 187 | path_backwards = [ycurrent, xcurrent, zcurrent] 188 | 189 | # Initialize 190 | i = 1 191 | 192 | # While the start point is not reached 193 | while (xcurrent != x0) or (ycurrent != y0) or (zcurrent != z0): 194 | 195 | next_point = np.array([came_fromy[ycurrent, xcurrent, zcurrent], came_fromx[ycurrent, xcurrent, zcurrent], came_fromz[ycurrent, xcurrent, zcurrent]]) 196 | path_backwards = np.vstack((path_backwards, next_point)) 197 | 198 | ycurrent = np.int(path_backwards[i, 0]) 199 | xcurrent = np.int(path_backwards[i, 1]) 200 | zcurrent = np.int(path_backwards[i, 2]) 201 | 202 | i = i + 1 203 | 204 | 205 | # Number of waypoints 206 | n_points = path_backwards.shape[0] 207 | 208 | # Reverse path sequence 209 | path = np.flipud(path_backwards) 210 | 211 | # Reassign the original start and end points 212 | path[0, :] = [y0_old, x0_old, z0_old] 213 | path[-1, :] = [yend_old, xend_old, zend_old] 214 | 215 | 216 | return [path, n_points] 217 | --------------------------------------------------------------------------------