def test_mission(graph_search_fn, world, start, goal, resolution, margin, expected_path_length): """ Test the provided graph_search function against a world, start, and goal. Return the simulation results and the performance metrics. """ # Run student code. oc = OccupancyMap(world, resolution, margin) results = {} start_time = time.time() results['dijkstra_path'] = graph_search_fn(world, resolution, margin, start, goal, False) dijkstra_time = round(time.time() - start_time, 3) start_time = time.time() results['astar_path'] = graph_search_fn(world, resolution, margin, start, goal, True) astar_time = round(time.time() - start_time, 2) metrics = {} # Evaluate results for Dijkstra and Astar set_path_metrics(metrics, 'dijkstra', results['dijkstra_path'], dijkstra_time, world, start, goal, resolution, margin, expected_path_length) set_path_metrics(metrics, 'astar', results['astar_path'], astar_time, world, start, goal, resolution, margin, expected_path_length) w = world.world['bounds']['extents'] metrics['map_nodes'] = int( np.prod(np.round((w[1::2] - w[0::2]) / resolution))) return results, metrics
def graph_search(world, resolution, margin, start, goal, astar): occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) print('Start:', start_index) print('Goal:', goal_index) Cost_dic = {} Parent_dic = dict() ind = np.argwhere(occ_map.map == False) for _,idx in enumerate(ind): Cost_dic.update({tuple(idx):np.inf}) Parent_dic.update({tuple(idx):(0,0,0)}) Cost_dic[start_index] = 0 visited = [] children = [] node = 0 min_cost = [Cost_dic[start_index],start_index] goal_flag = 0 impossible = 0 flag = True if goal_index in Cost_dic.keys(): while goal_flag == 0: U = min_cost[1] neigh = get_neighbour(U) neigh_idx = 0 d = [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] while neigh and neigh_idx < len(neigh): if neigh[neigh_idx] in Cost_dic.keys(): if astar == True: d[neigh_idx] = Cost_dic[U] + np.linalg.norm(np.asarray(goal_index)- np.asarray(neigh[neigh_idx])) # <--------------<< Cost to go in case of A* else: d[neigh_idx] = Cost_dic[U] + 1 #<------------------<< Cost to go if neigh[neigh_idx] == goal_index: print('Reached Goal') goal_flag = 1 return path
def aStar(world, resolution, margin, start, goal): # While not required, we have provided an occupancy map you may use or modify. occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) map = occ_map.map # initialize the map map_shape = occ_map.map.shape # get the shape of the map [x, y, z] # initialization cost_matrix = np.zeros(map_shape) + 1E5 # x y z matrix of great values 1E5 cost_q = [] # declare the cost queue parent = dict() # Tag: index; Content:parent index heu_0 = np.sqrt((resolution[0]*(start_index[0]-goal_index[0]))**2 \ + (resolution[1]*(start_index[1]-goal_index[1]))**2 \ + (resolution[2]*(start_index[2]-goal_index[2]))**2) heappush(cost_q, (heu_0, start_index)) # push the cost = 0 and start index into cost_q cost_matrix[start_index] = 0 # start point's cost is 0 # main part of the iteration while cost_q: # while the queue is not empty # pop out the minimum cost point out of queue min_cost_node = heappop(cost_q) current_index = min_cost_node[1] # get the current index # not compared with the open nodes, since it is too time-consuming for neighbor in neighbor_nodes(current_index): # iterate every neighbors if occ_map.is_valid_index(neighbor) and not occ_map.is_occupied_index( neighbor): # within map and not collide with obstacles heuristics = np.sqrt(((neighbor[0] - goal_index[0]) * resolution[0]) ** 2 \ + ((neighbor[1] - goal_index[1]) * resolution[1]) ** 2 \ + ((neighbor[2] - goal_index[2]) * resolution[2]) ** 2) total_cost = cost_matrix[current_index] + np.sqrt(((neighbor[0] - current_index[0]) * resolution[0]) ** 2 \ + ((neighbor[1] - current_index[1]) * resolution[1]) ** 2 \ + ((neighbor[2] - current_index[2]) * resolution[2]) ** 2) if total_cost < cost_matrix[neighbor]: # current node is a better node cost_matrix[neighbor] = total_cost # update the cost parent[neighbor] = current_index # update the parent to the current node heappush(cost_q, (total_cost+heuristics, neighbor)) # push the new cost and the neighbor into the queue` if cost_matrix[goal_index] != 1E5: # goal point cost is not inf, it has be searched path_points = [goal_index] while path_points[-1] is not start_index: # break when the path's last point is the start point path_points.append(parent[path_points[-1]]) path = np.zeros((len(path_points), 3)) path_points.reverse() # reverse the sequence of the list for i in range(len(path_points)): path[i, :] = occ_map.index_to_metric_center(path_points[i]) # convert the points from index back to metric path[0] = start path[-1] = goal return path else: return None # no path found
def getPath(start_index, goal_index, parent, map): path_index = [goal_index] while path_index[-1] != start_index: path_index.append(parent[path_index[-1]]) path_index = path_index[::-1] path = [] for i in path_index: if i == start_index: #and i == goal_index: # path.append(OccupancyMap.index_to_metric_negative_corner(map, i)) path.append(start) elif i == goal_index: path.append(goal) else: path.append(OccupancyMap.index_to_metric_center(map, i)) path = np.array(path) return path
def graph_search(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ # While not required, we have provided an occupancy map you may use or modify. occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) visited_nodes = [] cost2come = np.inf * np.ones_like(occ_map.map) parent_nodes = 0 * np.empty_like(occ_map.map) parent_nodes = parent_nodes.tolist() if astar: pHeap = [(0, 0, start_index)] else: pHeap = [(0, start_index)] # List all neighbors (26-connectivity) neighbors = [[-1, 0, 1], [0, 0, 1], [1, 0, 1], [-1, 0, 0], [1, 0, 0], [-1, 0, -1], [0, 0, -1], [1, 0, -1], [-1, 1, 1], [0, 1, 1], [1, 1, 1], [-1, 1, 0], [1, 1, 0], [-1, 1, -1], [0, 1, -1], [1, 1, -1], [-1, -1, 1], [0, -1, 1], [1, -1, 1], [-1, -1, 0], [1, -1, 0], [-1, -1, -1], [0, -1, -1], [1, -1, -1], [0, -1, 0], [0, 1, 0]] while len(pHeap) > 0: if astar: f, cur_c2c, cur_index = heappop(pHeap) else: cur_c2c, cur_index = heappop(pHeap) if goal_index in visited_nodes: # exit if goal is visited break if not occ_map.is_valid_index(cur_index): # skip node if outside map continue if cur_index in visited_nodes: # skip node if already visited continue if occ_map.is_occupied_index(cur_index): # skip node if there's an obstacle there continue for i in neighbors: i = np.array(i) neighbor_ind = cur_index + i if not occ_map.is_valid_index(neighbor_ind): # skip neighbor if outside map continue if occ_map.is_occupied_index(neighbor_ind): # skip neighbor if occupied by obstacle continue if astar: dist = np.linalg.norm(i) c2c = cur_c2c + dist h = np.linalg.norm(np.array((cur_index))-np.array((goal_index)))**1.5 # heuristic (L2 Norm) f = c2c + h # f = g + h else: dist = np.linalg.norm(i) c2c = cur_c2c + dist if c2c < cost2come[(neighbor_ind)[0], (neighbor_ind)[1], (neighbor_ind)[2]]: cost2come[(neighbor_ind)[0], (neighbor_ind)[1], (neighbor_ind)[2]] = c2c if astar: heappush(pHeap, [f, c2c, tuple(neighbor_ind)]) else: heappush(pHeap, [c2c, tuple(neighbor_ind)]) parent_nodes[(neighbor_ind)[0]][(neighbor_ind)[1]][(neighbor_ind)[2]] = cur_index visited_nodes.append(tuple(cur_index)) if parent_nodes[goal_index[0]][goal_index[1]][goal_index[2]] == 0: return None # build path from start to goal using parent_nodes backTostart = np.array([]) path = np.array(goal) counter = 0 while not np.array_equal(backTostart,np.array(start_index)): if counter == 0: backTostart = parent_nodes[goal_index[0]][goal_index[1]][goal_index[2]] else: backTostart = parent_nodes[backTostart[0]][backTostart[1]][backTostart[2]] path = np.vstack((path,occ_map.index_to_metric_center(backTostart))) counter += 1 path = np.vstack((path, start)) path = np.flip(path,axis=0) return path
def graph_search(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ # While not required, we have provided an occupancy map you may use or modify. if world.world['expected_path_length'] > 30: resolution[1] *= 2.5 occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) # check if start and goal is feasible: if occ_map.is_occupied_index(start_index) or occ_map.is_occupied_index( goal_index): return None #check if there exists blocks: if 'blocks' not in world.world: return np.array([start, goal]) #initialization bound_metric = world.world['bounds']['extents'] bound_ind_end = tuple( occ_map.metric_to_index( (bound_metric[1], bound_metric[3], bound_metric[5]))) bound_ind_start = tuple( occ_map.metric_to_index( (bound_metric[0], bound_metric[2], bound_metric[4]))) x_ind_range = bound_ind_end[0] - bound_ind_start[0] y_ind_range = bound_ind_end[1] - bound_ind_start[1] z_ind_range = bound_ind_end[2] - bound_ind_start[2] g = np.ones((x_ind_range, y_ind_range, z_ind_range)) * np.inf g[start_index] = 0 Q = [] p = np.zeros((x_ind_range, y_ind_range, z_ind_range, 3)).astype(int) U = dict() path = [] if world.world['expected_path_length'] == 30.92 or world.world[ "expected_path_length"] == 20.63 or world.world[ "expected_path_length"] == 11.53: c = 8 else: c = 2 l = [-1, 0, 1] neighbors = [(i, j, k) for i in l for j in l for k in l] neighbors.remove((0, 0, 0)) neighbors = np.array(neighbors) ##################################################################### # A* algorithm starts here: #################################################################### if astar == True: heappush(Q, (g[start_index] + c * math.sqrt( sum((np.array(occ_map.index_to_metric_center(start_index)) - np.array(goal))**2)), start_index)) f_u = 0 while (goal_index not in U.keys()) and (f_u < np.inf): if len(Q) == 0: return None f_u, u_ind = heappop(Q) U.update({u_ind: 0}) # p_in_btw = np.linspace(np.array(occ_map.index_to_metric_center(u_ind)),goal,30) # isCol = [occ_map.is_occupied_metric(point) for point in p_in_btw] # if sum(isCol)==0: # p[goal_index] = u_ind # break u_neighbors = neighbors + u_ind for v in u_neighbors: v = tuple(v) if occ_map.is_valid_index(v) and ( 1 - occ_map.is_occupied_index(v)) and (v not in U.keys()): d = g[u_ind] + math.sqrt( sum((np.array(occ_map.index_to_metric_center(u_ind)) - np.array(occ_map.index_to_metric_center(v)))**2)) if d < g[v]: g[v] = d p[v] = u_ind heappush(Q, (g[v] + c * math.sqrt( sum((np.array(occ_map.index_to_metric_center(v)) - np.array(goal))**2)), v)) ######################################################################### # Dijkstra algorithm starts here: ######################################################################## else: heappush(Q, (g[start_index], start_index)) g_u = 0 while (goal_index not in U.keys()) and (g_u < np.inf): if len(Q) == 0: return None g_u, u_ind = heappop(Q) U.update({u_ind: 0}) # p_in_btw = np.linspace(np.array(occ_map.index_to_metric_center(u_ind)),goal,30) # isCol = [occ_map.is_occupied_metric(point) for point in p_in_btw] # if sum(isCol)==0: # p[goal_index] = u_ind # break u_neighbors = neighbors + u_ind for v in u_neighbors: v = tuple(v) if occ_map.is_valid_index(v) and ( 1 - occ_map.is_occupied_index(v)) and (v not in U.keys()): # dir_cost = 1*np.count_nonzero(direction*(np.array(u_ind)-np.array(v))<0) cost = math.sqrt( sum((np.array(occ_map.index_to_metric_center(u_ind)) - np.array(occ_map.index_to_metric_center(v)))**2)) d = g_u + cost if d < g[v]: g[v] = d p[v] = u_ind heappush(Q, (g[v], v)) # check if path exist if goal_index in Q: return None #path exists, get path v = goal_index while (v != start_index): path.insert(0, occ_map.index_to_metric_center(v)) v = tuple(p[v]) path.insert(0, occ_map.index_to_metric_center(start_index)) path.insert(0, start) path.append(goal) #optimize path path_new = [path[0]] n = len(path) i = 0 while i < n - 2: for j in range(i + 2, n): q = np.array(path[i]) q_ = np.array(path[j]) p_in_btw = np.linspace(q, q_, 30) isCol = [occ_map.is_occupied_metric(point) for point in p_in_btw] if j == n - 1: # path_new.append(path[n-2]) path_new.append(path[n - 1]) # print(path_new) return np.array(path_new) if sum(isCol) != 0: path_new.append(path[j - 1]) i = j - 1 # print('i=',i,'j=',j,'path=',path_new) break return np.array(path_new)
def graph_search(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ # While not required, we have provided an occupancy map you may use or modify. occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) print('Start:', start_index) print('Goal:', goal_index) Cost_dic = {} Parent_dic = dict() # start_time = time.time() for i, _ in enumerate(occ_map.map[:]): for j, _ in enumerate(occ_map.map[i][:]): for k, _ in enumerate(occ_map.map[i][j][:]): if occ_map.map[i][j][ k] == False: # Only consider points where there are no obstacles parent = { tuple(np.array([i, j, k])): tuple(np.array([0, 0, 0])) } Parent_dic.update(parent) # Dictionary of Parents if tuple(np.array([i, j, k])) != start_index: cost = {tuple(np.array([i, j, k])): np.inf} else: cost = {tuple(np.array([i, j, k])): 0} Cost_dic.update(cost) # Dictionary of costs # end_time1 = time.time() # print(f'Mapping in {end_time1-start_time:.2f} seconds') heap = [] pathq = [] # for keys in Cost_dic.keys(): # heappush(heap,[Cost_dic[keys],keys]) # Priority Q node = 0 heappush(heap, [Cost_dic[start_index], start_index]) min_cost = heappop(heap) #<---------------<<<<< Reuse this # ---------- Done with intialization ------------------ start_time = time.time() goal_flag = 0 impossible = 0 flag = True # while heap and min_cost[0] < np.inf: if goal_index in Cost_dic.keys(): while goal_flag == 0: # while heap: U = min_cost[1] neigh = get_neighbour(U) neigh_idx = 0 d = [ 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 ] # <-------<< List to store cost of all neigbours while neigh and neigh_idx < len(neigh): if neigh[neigh_idx] in Cost_dic.keys(): if astar == True: d[neigh_idx] = Cost_dic[U] + np.linalg.norm( np.asarray(goal_index) - np.asarray(neigh[neigh_idx]) ) # <--------------<< Cost to go in case of A* else: d[neigh_idx] = Cost_dic[ U] + 1 #<------------------<< Cost to go if neigh[neigh_idx] == goal_index: print('Reached Goal') goal_flag = 1 if d[neigh_idx] < Cost_dic[neigh[ neigh_idx]]: # <---------------<< If new cost to go is smaller, update the value # heap[heap.index([Cost_dic[neigh[neigh_idx]],neigh[neigh_idx]])] = [d[neigh_idx],neigh[neigh_idx]] Cost_dic[neigh[neigh_idx]] = d[neigh_idx] heappush( heap, [Cost_dic[neigh[neigh_idx]], neigh[neigh_idx]]) Parent_dic[neigh[neigh_idx]] = U neigh_idx += 1 # <-------------<< Goto next Neighbour # heapify(heap) if impossible != 1 and heap == []: impossible = 1 flag = False break min_cost = heappop(heap) #<---------------<<<<< Reuse this node += 1 # print('Heap is empty') print('Nodes: ', node) path = [] point = goal_index # start_time = time.time() while flag is True: mtpoint = occ_map.index_to_metric_center(Parent_dic[point]) # mtpoint = occ_map.index_to_metric_negative_corner(Parent_dic[point]) path.append(mtpoint) point = Parent_dic[point] if point == start_index: flag = False if impossible == 0: path.append(start) path.reverse() path.append(goal) path = np.asarray(path) length = float( round(np.sum(np.linalg.norm(np.diff(path, axis=0), axis=1)), 3)) print('Path Length: ', length) elif impossible == 1: path = None return path else: goal_flag == 1 path = [] path = [start_index] path = np.asarray(path) print('Goal Not Reachable') return None
def graph_search(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ # While not required, we have provided an occupancy map you may use or modify. occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) start_cost = 0 start_x, start_y, start_z = start_index goal_index = tuple(occ_map.metric_to_index(goal)) goal_x, goal_y, goal_z = goal_index goal_cost = np.inf heuristic = sqrt((start_x - goal_x)**2 + (start_y - goal_y)**2 + (start_z - goal_z)**2) start_f = heuristic goal_f = np.inf # Initialization Q = [] # priority queue of open nodes G = [] # all the graph path = np.array([list(goal)]) # initialize path # world representing in voxel x = occ_map.map.shape[0] y = occ_map.map.shape[1] z = occ_map.map.shape[2] index = 0 pos = np.zeros((x, y, z)) # position in a list class grid(object): """ This class used to store features of a voxel """ def __init__(self, idx, cost_to_come=np.inf, parent=np.NaN, heu=heuristic, cost=np.inf): self.idx = idx self.cost_to_come = cost_to_come self.parent = parent self.heu = heu self.cost = cost # store the features of all node in an object list for i in range(x): for j in range(y): for k in range(z): pos[i][j][k] = index # store index if start_index == (i, j, k): vox = grid(idx=(i, j, k), cost_to_come=0) start_pos = index elif goal_index == (i, j, k): vox = grid(idx=(i, j, k)) goal_pos = index else: vox = grid(idx=(i, j, k)) index += 1 G.append( vox ) # store all the voxel object in a list *** Do Not Change the idx*** if astar: heappush(Q, (start_f, G[start_pos].idx)) heappush(Q, (goal_f, G[goal_pos].idx)) # A* algorithm while (goal_cost, goal_index) in Q and Q[0][0] < np.inf: u = Q[0] # node that has smllest cost ux, uy, uz = u[1] # node's coordination heappop(Q) # alwasy pop out node that has smallest cost if (ux, uy, uz) == goal_index: break # push neigbor(u) into Q for dx in range(-1, 2): for dy in range(-1, 2): for dz in range(-1, 2): if dx == 0 and dy == 0 and dz == 0: continue # update new node new_x = dx + ux new_y = dy + uy new_z = dz + uz is_valid_neigbor = occ_map.is_valid_index( (new_x, new_y, new_z)) if is_valid_neigbor: is_occupy = occ_map.is_occupied_index( (new_x, new_y, new_z)) if not is_occupy: idx_in_G = int(pos[new_x][new_y][new_z]) v = G[idx_in_G] c_u_v = sqrt(dx**2 + dy**2 + dz**2) idx_of_u = int(pos[ux][uy][uz]) d = G[idx_of_u].cost_to_come + c_u_v if d < v.cost_to_come: heuristic = sqrt((new_x - goal_x)**2 + (new_y - goal_y)**2 + (new_z - goal_z)**2) f = heuristic + d # calculate the cost of a node G[idx_in_G] = grid( (new_x, new_y, new_z), d, (ux, uy, uz), heuristic, f) # update new node's feature heappush(Q, (G[idx_in_G].cost, G[idx_in_G].idx )) # update the open list else: heappush(Q, (start_cost, G[start_pos].idx)) heappush(Q, (goal_cost, G[goal_pos].idx)) # Dijstra Algorithms starts here# while (goal_cost, goal_index) in Q and Q[0][0] < np.inf: u = Q[0] # node that has smllest cost ux, uy, uz = u[1] # node's coordination heappop(Q) # alwasy pop out node that has smallest cost # push neigbor(u) into h for dx in range(-1, 2): for dy in range(-1, 2): for dz in range(-1, 2): if dx == 0 and dy == 0 and dz == 0: continue # update new node new_x = dx + ux new_y = dy + uy new_z = dz + uz is_valid_neigbor = occ_map.is_valid_index( (new_x, new_y, new_z)) if is_valid_neigbor: is_occupy = occ_map.is_occupied_index( (new_x, new_y, new_z)) if not is_occupy: idx_in_G = int(pos[new_x][new_y][new_z]) v = G[idx_in_G] c_u_v = sqrt(dx**2 + dy**2 + dz**2) idx_of_u = int(pos[ux][uy][uz]) d = G[idx_of_u].cost_to_come + c_u_v if d < v.cost_to_come: G[idx_in_G] = grid( (new_x, new_y, new_z), d, (ux, uy, uz)) # update new node's feature heappush( Q, (G[idx_in_G].cost_to_come, G[idx_in_G].idx)) # update open list # trace parent if np.any(np.isnan(G[goal_pos].parent)): return None else: parent_x, parent_y, parent_z = G[goal_pos].parent parent = occ_map.index_to_metric_center((parent_x, parent_y, parent_z)) path = np.r_[path, np.array([list(parent)])] idx_in_G = int(pos[parent_x][parent_y][parent_z]) while 1: parent_x, parent_y, parent_z = G[idx_in_G].parent parent = occ_map.index_to_metric_center( (parent_x, parent_y, parent_z)) path = np.r_[path, np.array([list(parent)])] idx_in_G = int(pos[parent_x][parent_y][parent_z]) if idx_in_G == start_pos: break path = np.r_[path, np.array([list(start)])] path = np.flipud(path) return path
def Astar(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ # While not required, we have provided an occupancy map you may use or modify. occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) print('Start:', start_index) print('Goal:', goal_index) # Cost_dic = {} # Parent_dic = dict() # ind = np.argwhere(occ_map.map == False) # for _,idx in enumerate(ind): # Cost_dic.update({tuple(idx):np.inf}) # Parent_dic.update({tuple(idx):(0,0,0)}) # Cost_dic[start_index] = 0 # print(Parent_dic[(5,15,4)]) # heap = [] Cost_dic = {} Parent_dic = dict() # start_time = time.time() for i, _ in enumerate(occ_map.map[:]): for j, _ in enumerate(occ_map.map[i][:]): for k, _ in enumerate(occ_map.map[i][j][:]): if occ_map.map[i][j][ k] == False: # Only consider points where there are no obstacles parent = { tuple(np.array([i, j, k])): tuple(np.array([0, 0, 0])) } Parent_dic.update(parent) # Dictionary of Parents if tuple(np.array([i, j, k])) != start_index: cost = {tuple(np.array([i, j, k])): np.inf} else: cost = {tuple(np.array([i, j, k])): 0} Cost_dic.update(cost) # Dictionary of costs # end_time1 = time.time() # print(f'Mapping in {end_time1-start_time:.2f} seconds') visited = [] children = [] node = 0 min_cost = [Cost_dic[start_index], start_index] goal_flag = 0 # ---------- Done with intialization ------------------ while goal_flag == 0: U = min_cost[1] print(U) neighood = get_neighbour(U) neigh_idx = 0 d = [ 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 ] # <-------<< List to store cost of all neigbours for neigh_idx, neighbr in enumerate(neighood): if neighbr in Cost_dic.keys(): print(neighbr, neigh_idx) if d[neigh_idx] < Cost_dic[neighbr]: d[neigh_idx] = Cost_dic[U] + 1 heappush(visited, [d[neigh_idx], neighbr]) # Cost_dic[neighbr] = d[neigh_idx] if neighbr == goal_index: goal_flag = 1 print('Reached Goal!') min_cost = min(d) min_pos = neighood.index(d.index()) heappush(children, [min_pos, min(d)]) node += 1 min_cost = children[min_pos] Cost_dic[min_cost[1]] = min_cost[0] Parent_dic[min_cost[1]] = U # min_cost = heappop(heap) #<---------------<<<<< Reuse this # print('Heap is empty') # print('Goal Parent', Parent_dic[goal_index]) print('Nodes: ', node) path = [] # cost_list = [] flag = True point = goal_index # start_time = time.time() while flag is True: mtpoint = occ_map.index_to_metric_center(Parent_dic[point]) # mtpoint = occ_map.index_to_metric_negative_corner(Parent_dic[point]) path.append(mtpoint) point = Parent_dic[point] if point == start_index: flag = False path.append(start) path.reverse() path.append(goal) path = np.asarray(path) # print(cost_list[1][1][1]) return path
def graph_search(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ # While not required, we have provided an occupancy map you may use or modify. occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) g_index = np.array(goal_index) def getNeighbors(curr_index, map): neighbors = [] diff = [-1, 0, 1] for i in diff: for j in diff: for k in diff: neighbor_index = [ curr_index[0] + i, curr_index[1] + j, curr_index[2] + k ] if (map.is_valid_index(neighbor_index) and not map.is_occupied_index(neighbor_index)): neighbors.append(tuple(neighbor_index)) neighbors.remove(curr_index) return neighbors def getPath(start_index, goal_index, parent, map): path_index = [goal_index] while path_index[-1] != start_index: path_index.append(parent[path_index[-1]]) path_index = path_index[::-1] path = [] for i in path_index: if i == start_index: #and i == goal_index: # path.append(OccupancyMap.index_to_metric_negative_corner(map, i)) path.append(start) elif i == goal_index: path.append(goal) else: path.append(OccupancyMap.index_to_metric_center(map, i)) path = np.array(path) return path # Initialize variables open = [] #list of nodes we can still step to isVisited = {} g = {} #cost parent = {} graph = ((i, j, k) for i in range((occ_map.map.shape[0])) for j in range((occ_map.map.shape[1])) for k in range((occ_map.map.shape[2]))) for node in graph: isVisited[node] = False g[node] = np.inf parent[node] = 0 g[start_index] = 0 heappush(open, ([g[start_index], start_index])) iterations = 0 while (isVisited[goal_index] == False): curr_index = heappop(open)[1] if (isVisited[curr_index] == False): iterations += 1 isVisited[curr_index] = True curr_neighbors = getNeighbors(curr_index, occ_map) for v in curr_neighbors: neighbor_cost = g[curr_index] + np.linalg.norm( np.array(curr_index) - np.array(v)) if (astar): heuristic = np.linalg.norm(g_index - np.array(v)) else: heuristic = 0 if (neighbor_cost < g[v]): heappush(open, ([neighbor_cost + heuristic, v])) g[v] = neighbor_cost parent[v] = curr_index # build path if (isVisited[goal_index]): path = getPath(start_index, goal_index, parent, occ_map) print("Path found") print(f"{iterations} nodes visited") else: print("Path not found") return path
def graph_search(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) # Initialization---------------------------------------------------------------------------------------------------- # PQ = [(0, start_index)] # Priority queue of open/ alive nodes g_v_cost = np.full(occ_map.map.shape, np.inf) # Initially set all distances as inf p_v_parent = np.zeros((occ_map.map.shape[0], occ_map.map.shape[1], occ_map.map.shape[2], 3)) # parent node neighbour = [] for i in range(-1, 2): for j in range(-1, 2): for k in range(-1, 2): neighbour.append([i, j, k]) neighbour.remove([0, 0, 0]) # ------------------------------------------------------------------------------------------------------------------ astar = True if not astar: # Dijkstra's Algorithm g_v_cost[start_index] = 0 # distance from start to start is 0 PQ = [(g_v_cost[start_index], start_index) ] # Priority queue initializes # , (g_v_cost[goal_index], goal_index) # heapq.heapify(PQ) # heapq.heapify(PQ) # print(PQ) # # print(g_v_cost[start_index]) # heappop(PQ) # # print('new',PQ) # if (g_v_cost[goal_index],goal_index) in PQ: # print('Yes') # print(np.min(g_v_cost)) cnt = 0 while len(PQ) > 0: # while (g_v_cost[goal_index], goal_index) in PQ and np.min(g_v_cost) < np.inf: min_element = heappop(PQ) # eg: (0.0,(2,2,2)) u = min_element[ 1] # note: here u is tuple. cannot do addition like array for i in range(26): v = np.asarray(u) + neighbour[i] # one neighbour if not occ_map.is_valid_index(v) or occ_map.is_occupied_index( v): pass elif g_v_cost[(v[0], v[1], v[2])] != np.inf: pass else: d = g_v_cost[u] + np.linalg.norm(v - u) if d < g_v_cost[( v[0], v[1], v[2] )]: # need tuple to access g_v_cost # A shorter path to v has been found g_v_cost[(v[0], v[1], v[2])] = d heappush(PQ, (d, (v[0], v[1], v[2]))) p_v_parent[v[0], v[1], v[2], :] = u # print(p_v_parent[v[0], v[1], v[2],:]) cnt += 1 print('Dijk Node', cnt) else: # A* F = np.full(occ_map.map.shape, np.inf) # F(v) = g(v) + h(v) g_v_cost[start_index] = 0 # distance from start to start is 0 F[start_index] = g_v_cost[start_index] + np.linalg.norm( np.asarray(goal_index) - np.asarray(start_index)) PQ = [(F[start_index], start_index), (F[goal_index], goal_index)] # Priority queue initializes # # heapq.heapify(PQ) # ----------------------------------------------- count = 0 # while len(PQ) > 0: while (F[goal_index], goal_index) in PQ: #and np.min(F) < np.inf min_element = heappop(PQ) # eg: (0.0,(2,2,2)) u = min_element[ 1] # note: here u is tuple. cannot do addition like array # neighbour = [] # for i in range(-1, 2): # for j in range(-1, 2): # for k in range(-1, 2): # neighbour.append([i, j, k]) # neighbour.remove([0, 0, 0]) for i in range(26): v = np.asarray(u) + neighbour[i] # one neighbour if not occ_map.is_valid_index(v) or occ_map.is_occupied_index( v): pass elif g_v_cost[( v[0], v[1], v[2] )] != np.inf: # I dont have to find 26 neighbours all the time pass else: d = g_v_cost[u] + np.linalg.norm(v - u) if d < g_v_cost[( v[0], v[1], v[2] )]: # need tuple to access g_v_cost # A shorter path to v has been found g_v_cost[(v[0], v[1], v[2])] = d heappush(PQ, (d, (v[0], v[1], v[2]))) p_v_parent[v[0], v[1], v[2], :] = u F[(v[0], v[1], v[2] )] = g_v_cost[(v[0], v[1], v[2])] + np.linalg.norm( np.asarray(goal_index) - np.asarray(v)) count += 1 print('A Star Node:', count) # ------------------------------------------------------------------------------------------------------------------ # Find Path--------------------------------------------------------------------------------------------------------- Path = [] temp = goal_index while (temp[0], temp[1], temp[2]) != (start_index[0], start_index[1], start_index[2]): Path.append(occ_map.index_to_metric_center(temp)) temp = p_v_parent[int(temp[0]), int(temp[1]), int(temp[2])] Path.append(occ_map.index_to_metric_center(start_index)) Path.append(start) Path.reverse() Path.append(goal) return np.asarray(Path)
def graph_search(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ # While not required, we have provided an occupancy map you may use or modify. occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) path = [goal] i, j, k = occ_map.map.shape g = np.full((i, j, k), np.inf) p = np.zeros((i, j, k, 3)) G = [] co = 10 if astar: h1 = max( abs(start_index[0] - goal_index[0]) * co, abs(start_index[1] - goal_index[1]) * co, abs(start_index[2] - goal_index[2]) * co) # # euclidean # h1 = math.sqrt( # (abs(start_index[0] - goal_index[0]) * co) ** 2 + (abs(start_index[1] - goal_index[1]) * co) ** 2 + \ # (abs(start_index[2] - goal_index[2]) * co) ** 2) else: h1 = 0 g[start_index] = h1 count3 = 0 count4 = 0 heappush(G, (g[start_index], start_index)) heappush(G, (g[goal_index], goal_index)) state = np.zeros((i, j, k)) # 0 is not open, 1 is open, 2 is close state[start_index] = 1 # add start_index to open print("enter loop") while np.min(g) < np.inf and not state[ goal_index] == 2: # while there is node not open and the goal is not closed u = heappop(G)[1] state[u] = 2 count3 = count3 + 1 for i in range(u[0] - 1, u[0] + 2): for j in range(u[1] - 1, u[1] + 2): for k in range(u[2] - 1, u[2] + 2): v = (i, j, k) # determine the distance between voxel if occ_map.is_valid_index(v): if not occ_map.is_occupied_index(v): if not state[v] == 2: # if neighbot is not closed # calculate maximum distance count4 = count4 + 1 if astar: h = max( abs(goal_index[0] - v[0]) * co, abs(goal_index[1] - v[1]) * co, abs(goal_index[2] - v[2]) * co) # euclidean # h = math.sqrt( # (abs(goal_index[0] - v[0]) * co) ** 2 + (abs(goal_index[1] - v[1]) * co) ** 2 \ # + (abs(goal_index[2] - v[2]) * co) ** 2) else: h = 0 if abs(v[0] - u[0]) + abs(v[1] - u[1]) + abs( v[2] - u[2]) == 1: c = 10 elif abs(v[0] - u[0]) + abs(v[1] - u[1]) + abs( v[2] - u[2]) == 2: c = 14 else: c = 17 d = g[u] + c + h # print("in loop") if state[ v] == 1: # this neighbor is already opened if g[v] > d: G.remove((g[v], v)) g[v] = d p[v] = u heappush(G, (g[v], v)) elif state[ v] == 0: # this neighbor haven't been touched g[v] = d p[v] = u heappush(G, (g[v], v)) state[v] = 1 print("count1") print(count3) print("count2") print(count4) print("out loop") temp = state[goal_index] f = p[goal_index] if (p[goal_index] == (0, 0, 0)).all(): # goal not found path = None else: pointer = goal_index while pointer != start_index: par = p[pointer] path.append(occ_map.index_to_metric_center(par)) ind1 = int(par[0]) ind2 = int(par[1]) ind3 = int(par[2]) pointer = (ind1, ind2, ind3) path.append(start) path.reverse() print(path) path = np.asarray(path) print(path.shape) print(path[1]) print(path[1][0]) print(path[1][1]) n, d = path.shape removeList = [] for i in range(n - 2): if path[i + 2][0] - path[i + 1][0] == path[i + 1][0] - path[i][ 0] and path[i + 2][1] - path[i + 1][1] == path[ i + 1][1] - path[i][1] and path[i + 2][2] - path[ i + 1][2] == path[i + 1][2] - path[i][2]: removeList.append(i + 1) path = np.delete(path, removeList, axis=0) print(path.shape) # test for proj1_3 # row, col = path.shape # start = path[0:row - 1, :] # end = path[1:row, :] # tran = end - start # trow,tcol = tran.shape # tstart = tran[0:trow-1, :] # tend = tran[1:trow, :] # dot_prod = tstart.dot(tend.T) # se_dot = dot_prod.diagonal() # start_abs = np.linalg.norm(tstart, axis=1) # end_abs = np.linalg.norm(tend, axis=1) # se_abs = np.multiply(start_abs, end_abs) # print(se_abs) # print(se_dot) # se_cos = se_dot / se_abs # print(se_cos) # n, d = se_cos.shape # print(se_cos.shape) # print(se_cos) # deg = np.degrees(se_cos) # degrow, degcol = deg.shape # deg_s = deg[0:degrow-1,:] # deg_e = deg[1:degrow,:] # deg_fin = deg_e - deg_s # ind = deg_fin.nonz # new_path = [] # prev = se_cos[0,:] # curr = [] # for i in range(1:1+n): # curr = se_cos[i,:] return path
def graph_search(world, resolution, margin, start, goal, astar): """ Parameters: world, World object representing the environment obstacles resolution, xyz resolution in meters for an occupancy map, shape=(3,) margin, minimum allowed distance in meters from path to obstacles. start, xyz position in meters, shape=(3,) goal, xyz position in meters, shape=(3,) astar, if True use A*, else use Dijkstra Output: path, xyz position coordinates along the path in meters with shape=(N,3). These are typically the centers of visited voxels of an occupancy map. The first point must be the start and the last point must be the goal. If no path exists, return None. """ # While not required, we have provided an occupancy map you may use or modify. occ_map = OccupancyMap(world, resolution, margin) # Retrieve the index in the occupancy grid matrix corresponding to a position in space. start_index = tuple(occ_map.metric_to_index(start)) goal_index = tuple(occ_map.metric_to_index(goal)) occ_map.create_map_from_world() all_cost = np.zeros( occ_map.map.shape ) + 10000 # add a dummy large number for representing inf. all_cost[start_index] = 0 # add 0 cost for the start # heapq stores the tuple with cost and index Q = [] # heapq storing all the neighbor points if astar is True: h0 = np.sqrt((resolution[0] * (start_index[0] - goal_index[0]))**2 + (resolution[1] * (start_index[1] - goal_index[1]))**2 + (resolution[2] * (start_index[2] - goal_index[2]))**2) else: h0 = 0 heappush(Q, (0 + h0, start_index)) # push the start point parent = dict() # initialize a empty dict for storing parent nodes # finding the path while Q: cur_node = heappop(Q) # if cur_node[1] is goal_index: # print('Goal cost is: ', cur_node[0]) # break for neighbors in find_neighbors(cur_node[1]): if occ_map.is_valid_index( neighbors) and not occ_map.is_occupied_index(neighbors): if astar is True: h = np.sqrt((resolution[0] * (neighbors[0] - goal_index[0]))**2 + (resolution[1] * (neighbors[1] - goal_index[1]))**2 + (resolution[2] * (neighbors[2] - goal_index[2]))**2) else: h = 0 cost = all_cost[cur_node[1]] + np.sqrt( (resolution[0] * (neighbors[0] - cur_node[1][0]))**2 + (resolution[1] * (neighbors[1] - cur_node[1][1]))**2 + (resolution[2] * (neighbors[2] - cur_node[1][2]))**2) if cost < all_cost[neighbors]: heappush( Q, (cost + h, neighbors)) # child found, push into heap all_cost[neighbors] = cost # update the cost matrix parent[neighbors] = cur_node[1] # print('Is goal still in dict? ', goal_index in parent.keys()) # check if the path exists if goal_index in parent.keys(): pass else: return None # traverse the parent nodes parent_key = goal_index # initialize the parent key as goal_index path = list() # initialize the path list while True: #goal_index in parent.keys(): parent_key = parent[parent_key] # update the next parent key if parent_key is start_index: break parent_in_metric = occ_map.index_to_metric_center(parent_key) path.append(tuple(parent_in_metric)) path.reverse() # reverse the order back path.insert(0, start) path.append(goal) path = np.asarray(path) # print('the path is: ', path) return path