def get_valid_start_goal(dense_G, obstacles, shallow_G_currP): start_n = random.choice(list(dense_G.nodes())) goal_n = random.choice(list(dense_G.nodes())) # start_n = '341' # goal_n = '665' start = helper.state_to_numpy(dense_G.node[start_n]['state']) goal = helper.state_to_numpy(dense_G.node[goal_n]['state']) while (not helper.valid_start_goal(start, goal, obstacles)): start_n = random.choice(list(dense_G.nodes())) goal_n = random.choice(list(dense_G.nodes())) # start_n = '341' # goal_n = '665' start = helper.state_to_numpy(dense_G.node[start_n]['state']) goal = helper.state_to_numpy(dense_G.node[goal_n]['state']) shallow_G_currP.add_node('o' + start_n, state=dense_G.node[start_n]['state']) shallow_G_currP.add_node('o' + goal_n, state=dense_G.node[goal_n]['state']) # print("added node", 'o'+start_n, 'o'+goal_n) shallow_G_currP = helper.connect_knn_for_one_node(shallow_G_currP, K_nn, 'o' + start_n) shallow_G_currP = helper.connect_knn_for_one_node(shallow_G_currP, K_nn, 'o' + goal_n) return start_n, start, goal_n, goal, shallow_G_currP
def astar(G, start_v, goal_v, occ_g, inc, h_weight=1): # print(start_v) # print(goal_v) queue = [] heapq.heappush(queue, (0, 0, start_v)) nodes = dict() nodes[start_v] = (0, []) # start_time = time.time() count = 0 while len(queue): # curr_time = time.time() # if curr_time - start_time > 100: # return [], None heu, dis, cur = heapq.heappop(queue) if dis > nodes[cur][0]: continue if cur == goal_v: addv = goal_v plan = [] while addv != []: plan.append(addv) addv = nodes[addv][1] # print(" count = ", count) # print(" dis = ", dis) return np.array(plan[::-1]), dis next_cur = get_successors(G, cur) for v in next_cur: # dis_v = dis + compute_distance_id(G, cur, v) # print(v) dis_v = dis + G[cur][v]['weight'] # print(dis_v) # break if (v not in nodes) or nodes[v][0] > dis_v: count += 1 cost_v = dis_v + h_weight * get_heuristic(G, v, goal_v) node1_pos = helper.state_to_numpy(G.nodes[v]['state']) node2_pos = helper.state_to_numpy(G.nodes[cur]['state']) lines = [] colors = [] lines.append([node1_pos, node2_pos]) if not helper.is_edge_free(node1_pos, node2_pos, occ_g, inc = inc): colors.append((1,0,0,0.3)) lc = mc.LineCollection(lines, colors=colors, linewidths=1) continue colors.append((0,1,0,0.3)) heapq.heappush(queue, (cost_v, dis_v, v)) nodes[v] = (dis_v, cur) # print(" count = ", count) return [], None
def main(): INC = 0.03 dense_G = nx.read_graphml("graphs/dense_graph.graphml") sparse_G = nx.read_graphml("graphs/shallow_graph.graphml") occ_grid, row, col = helper.get_random_occ_grid() print(row, col) start_n, goal_n = helper.get_valid_start_goal(dense_G, occ_grid, row, col, inc=INC) start_pos = helper.state_to_numpy(dense_G.nodes[start_n]['state']) goal_pos = helper.state_to_numpy(dense_G.nodes[goal_n]['state']) path_nodes, dis = astar.astar(dense_G, start_n, goal_n, occ_grid, row, col, inc=INC) bottleneck_nodes, curr_path_nodes = get_bottleneck_nodes(dense_G, sparse_G, path_nodes, occ_grid, dis, row, col, inc=INC) print("bottleneck_nodes = ", bottleneck_nodes) points_x = [] points_y = [] print("path_nodes = ", path_nodes, bottleneck_nodes) for node in path_nodes: s = helper.state_to_numpy(dense_G.nodes[node]['state']) points_x.append(s[0]) points_y.append(s[1]) visualize_nodes(occ_grid, np.array(list(zip(points_x, points_y))), start_pos, goal_pos) points_x = [] points_y = [] print("path_nodes = ", path_nodes, bottleneck_nodes) for node in bottleneck_nodes: s = helper.state_to_numpy(sparse_G.nodes[node]['state']) points_x.append(s[0]) points_y.append(s[1]) visualize_nodes(occ_grid, np.array(list(zip(points_x, points_y))), start_pos, goal_pos)
def remove_invalid_edges(G, arm): to_remove = [] for i, node in enumerate(G.nodes()): config = helper.state_to_numpy(G.node[node]['state']) # print("config = ", config) arm.set_base_location(config[:2]) arm.set_configuration(config[2:]) if(arm.in_collision()): to_remove.append(node) for node in to_remove: G.remove_node(node) print("Removed "+`len(to_remove)`+" invalid nodes") # raw_input("press enter") to_remove = [] for i,edge in enumerate(G.edges()): if(i%1000==0): print("no of edges checked = ", i) u,v = edge # print("i, u, v = ", i, u, v) node1_pos = helper.state_to_numpy(G.node[u]['state']) node2_pos = helper.state_to_numpy(G.node[v]['state']) diff = node2_pos - node1_pos step = diff/EDGE_DISCRETIZATION for i in range(EDGE_DISCRETIZATION+1): nodepos = node1_pos + step*i arm.set_base_location(nodepos[:2]) arm.set_configuration(nodepos[2:]) if(arm.in_collision()): to_remove.append((u,v)) break for edge in to_remove: u, v = edge G.remove_edge(u, v) print("Removed "+`len(to_remove)`+" invalid edges") # raw_input("press enter") return G
def get_path_nodes(shallow_G_currP, dense_G, start_n, goal_n, obstacles, curr_occ_grid): temp_path_nodes = nx.dijkstra_path(dense_G, start_n, goal_n) lmbda_ = [1, 1.5, 2, 5, 10] no_nodes = [0, 0, 0, 0, 0] THRESHOLD = 0.1 s_path_nodes = ['o' + node for node in temp_path_nodes] for node in temp_path_nodes: shallow_G_currP.add_node('o' + node, state=dense_G.node[node]['state']) i = 0 curr_nodes = [] for lmbda in lmbda_: shallow_G_currP = helper.connect_within_thresh(shallow_G_currP, lmbda, THRESHOLD, s_path_nodes) shallow_G_currP = helper.remove_invalid_edges(shallow_G_currP, obstacles) curr_nodes = nx.dijkstra_path(shallow_G_currP, 'o' + start_n, 'o' + goal_n) curr_node_posns = [ helper.state_to_numpy(shallow_G_currP.node[node]['state']) for node in curr_nodes ] # print("curr_nodes = ", curr_nodes) for node in temp_path_nodes: if ('o' + node in curr_nodes): no_nodes[i] += 1 i += 1 # visualize_nodes(curr_occ_grid, curr_node_posns) fig1 = plt.figure(figsize=(10, 6), dpi=80) ax1 = fig1.add_subplot(111, aspect='equal') hfont = {'fontname': 'Helvetica'} plt.plot(lmbda_, no_nodes, linewidth=2) plt.ylim(0, max(no_nodes) + 5) plt.xlim(0, max(lmbda_) + 5) plt.xlabel("Lambda", **hfont) plt.ylabel("No of common nodes", **hfont) plt.savefig("lmbda_" + ` random.randint(0, 100) ` + ".jpg") plt.show() curr_nodes = [node.strip('o') for node in curr_nodes] return curr_nodes
def compute_distance_id(G, u, v): start_config = helper.state_to_numpy(G.nodes[u]['state']) end_config = helper.state_to_numpy(G.nodes[v]['state']) return compute_distance(start_config, end_config)
def get_path_nodes(shallow_G_currP, dense_G, start_n, goal_n, obstacles): temp_path_nodes = nx.dijkstra_path(dense_G, start_n, goal_n) path_nodes = [] prev = curr = 'o' + start_n prev_posn = curr_posn = helper.state_to_numpy( shallow_G_currP.node[curr]['state']) prev_temp = curr_temp = start_n prev_temp_posn = curr_temp_posn = helper.state_to_numpy( dense_G.node[curr_temp]['state']) i = 0 while (i < len(temp_path_nodes) - 1): i += 1 curr_temp = temp_path_nodes[i] curr_temp_posn = helper.state_to_numpy( dense_G.node[curr_temp]['state']) curr = helper.find_closest_node(shallow_G_currP, curr_temp_posn) curr_posn = helper.state_to_numpy(shallow_G_currP.node[curr]['state']) if (curr == prev): continue if (helper.satisfy_condition(curr_posn, curr_temp_posn, obstacles)): if (helper.satisfy_condition(curr_posn, prev_posn, obstacles)): prev_temp = curr_temp prev_temp_posn = curr_temp_posn prev = curr prev_posn = curr_posn continue elif (helper.satisfy_condition(prev_posn, curr_temp_posn, obstacles)): path_nodes.append(curr_temp) prev_temp = curr_temp prev = 'o' + curr_temp shallow_G_currP.add_node( prev, state=dense_G.node[curr_temp]['state']) prev_temp_posn = prev_posn = curr_temp_posn continue else: path_nodes.append(prev_temp) path_nodes.append(curr_temp) shallow_G_currP.add_node( 'o' + prev_temp, state=dense_G.node[prev_temp]['state']) shallow_G_currP.add_node( 'o' + curr_temp, state=dense_G.node[curr_temp]['state']) prev_temp = curr_temp prev = 'o' + curr_temp prev_temp_posn = prev_posn = curr_temp_posn continue else: if (helper.satisfy_condition(prev_posn, curr_temp_posn, obstacles)): path_nodes.append(curr_temp) prev_temp = curr_temp prev = 'o' + curr_temp shallow_G_currP.add_node( prev, state=dense_G.node[curr_temp]['state']) prev_temp_posn = prev_posn = curr_temp_posn continue else: path_nodes.append(prev_temp) path_nodes.append(curr_temp) shallow_G_currP.add_node( 'o' + prev_temp, state=dense_G.node[prev_temp]['state']) shallow_G_currP.add_node( 'o' + curr_temp, state=dense_G.node[curr_temp]['state']) prev_temp = curr_temp prev = 'o' + curr_temp prev_temp_posn = prev_posn = curr_temp_posn continue assert (len(path_nodes) > 0) return path_nodes
def astar2(G, edges, start_v, goal_v, occ_g, inc, h_weight=1): # print(start_v) # print(goal_v) num_nodes = 200 queue = [] heapq.heappush(queue, (0, 0, start_v)) nodes = dict() nodes[start_v] = (0, []) start_time = time.time() count = 0 while len(queue): curr_time = time.time() if curr_time - start_time > 100: return [], None heu, dis, cur = heapq.heappop(queue) if dis > nodes[cur][0]: continue if cur == goal_v: addv = goal_v plan = [] while addv != []: plan.append(addv) addv = nodes[addv][1] return np.array(plan[::-1]), dis, G, edges next_cur = get_successors(G, cur) for v in next_cur: # dis_v = dis + compute_distance_id(G, cur, v) dis_v = dis + G[cur][v]['weight'] if (v not in nodes) or nodes[v][0] > dis_v: count += 1 cost_v = dis_v + h_weight * get_heuristic(G, v, goal_v) node1_pos = helper.state_to_numpy(G.nodes[v]['state']) node2_pos = helper.state_to_numpy(G.nodes[cur]['state']) if int(v) and int(cur) < num_nodes: if not (v, cur) in edges.keys(): if not helper.is_edge_free( node1_pos, node2_pos, occ_g, inc=inc): # G.remove_edge(cur, v) edges[(cur, v)] = -1 edges[(v, cur)] = -1 continue else: edges[(v, cur)] = 1 edges[(cur, v)] = 1 if edges[(v, cur)] == -1: continue # lines = [] # colors = [] # lines.append([node1_pos, node2_pos]) # if not helper.is_edge_free(node1_pos, node2_pos, occ_g, inc = inc): # # colors.append((1,0,0,0.3)) # # lc = mc.LineCollection(lines, colors=colors, linewidths=1) # continue # colors.append((0,1,0,0.3)) heapq.heappush(queue, (cost_v, dis_v, v)) nodes[v] = (dis_v, cur) # print(" count = ", count) return [], None, G, edges