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
