def nearest_neighbors(graph, q, max_dist=10.0):
"""
Returns all the nearest roadmap vertices for a given configuration q that lie within max_dist units
You may also want to return the corresponding distances
"""
RoadmapVertex.getConfiguration(q)
return None
def find_path(q_start, q_goal, graph):
path = []
goalx = q_goal[0]
startx = q_start[0]
goaly = q_goal[1]
starty = q_start[1]
goal = (goalx, goaly)
start = (startx, starty)
UpdatedVerList = graph.getVertices()
# A* algorithm:
closed_set = OrderedSet()
open_set = PriorityQueue(order=min, f=lambda v: v.f)
h = distance(startVer, goalVer)
g = 0
f = g + h
next = startVer
open_set.put(next, Value(f=f, g=g))
parent = {}
check = []
while len(open_set) > 0:
next, value = open_set.pop()
closed_set.add(next)
nextId = next.id
gn = value.g
#Print the path after it reaches the goal and traceback to the start point
if nextId == goalId:
while not nextId == startId:
nextId = parent[nextId]
ee = RoadmapVertex.getConfiguration(UpdatedVerList[nextId])
path.insert(0, ee)
break
else:
neighbour = RoadmapVertex.getEdges(next)
check.append(nextId)
for ji in range(len(neighbour)):
piId = neighbour[ji].id
if piId not in check:
parent.update({piId: nextId})
g = distance(next, UpdatedVerList[piId]) + gn
h = distance(UpdatedVerList[piId], goalVer)
f = g + h
if UpdatedVerList[piId] not in closed_set:
open_set.put(UpdatedVerList[piId], Value(f=f, g=g))
else:
pass
else:
pass
return path
def find_path(q_start, q_goal, graph):
path = []
trial = []
# print('goal',q_goal)
# print('start',q_start)
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use the PriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
grids = graph.getVertices() # nodes/ vertices
print(len(grids))
# Path
f = 0
g = 0
h = 0
for q in range(0, 1):
h = sqrt((grids[0].q[0] - grids[1].q[0])**2 +
(grids[0].q[1] - grids[1].q[1])**2)
f = g + h
open_set.put(grids[0], Value(f=f, g=g))
while len(open_set) > 0:
parent, val = open_set.pop()
closed_set.add(parent)
new = (RoadmapVertex.getConfiguration(parent))
new1 = new[0]
new2 = new[1]
trial = [new1, new2]
if parent == grids[1]:
break
else:
for child in grids:
sssss = RoadmapVertex.getEdge(parent, child.id)
if sssss is not None:
if child not in closed_set:
g_val = distance(child, parent) + val.g
if child not in open_set or open_set.get(
child).g > g_val:
h_val = distance(parent, grids[1])
# print('The H value is:',h_val)
f_val = g_val + h_val
open_set.put(child, Value(f=f_val, g=g_val))
# print('The trial is:', trial)
path.append(trial)
return path
def find_path(q_start, q_goal, graph):
path = []
x1 = q_start[0]
y1 = q_start[1]
x2 = q_goal[0]
y2 = q_goal[1]
start_n = graph.addVertex([x1, y1]) #Adding start and end goal
end_n = graph.addVertex([x2, y2])
all = Roadmap.getVertices(graph)
number = Roadmap.getNrVertices(graph)
parent = ["" for i in range(len(all))]
visited = [False] * len(all)
distances = []
distanceg = []
for i in all:
if i.id != start_n.id:
distance_s = distance(
i.q, start_n.q) #For finding the neighbour for the start point
distances.append(distance_s)
min_index_s = np.argmin(distances)
min_s = distances[min_index_s]
for i in all:
if i.id != end_n.id:
distance_g = distance(i.q, end_n.q)
distanceg.append(distance_g)
min_index_g = np.argmin(distanceg) #Index of nearest neighbour
min_g = distanceg[min_index_g] #Distance of nearest neighbour
near_s = all[min_index_s]
graph.addEdge(start_n, near_s,
min_s) #Connecting start point to the nearest neighbour
near_g = all[min_index_g]
graph.addEdge(end_n, near_g,
min_g) #Connecting end point to the nearest neighbour
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use the PriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
g = 0
h = distance(start_n.q, end_n.q)
f = g + h
open_set.put(start_n, Value(f=f, g=g))
while len(open_set) != 0:
node, value = open_set.pop()
visited[node.id] = True
closed_set.add(node)
# g = value.g
p_cost = value.g
if node == end_n:
last = end_n
for i in range(len(closed_set)):
if last != start_n:
prev_parent = parent[last.id]
path.append(prev_parent.q)
last = prev_parent
break
else:
children = RoadmapVertex.getEdges(node)
for child in children:
if not visited[child.id]:
c_vert = all[child.id]
g = sqrt((node.q[0] - c_vert.q[0])**2 +
(node.q[1] - c_vert.q[1])**2) + p_cost
h = sqrt((end_n.q[0] - c_vert.q[0])**2 +
(end_n.q[1] - c_vert.q[1])**2)
f = g + h
if c_vert not in open_set or c_vert not in closed_set:
open_set.put(c_vert, Value(f=f, g=g))
elif open_set.has[
c_vert]: #For backtracking of the nodes to plot the path
c_val = open_set.get(c_vert)
if c_val.g > g:
open_set.remove(c_vert)
open_set.put(c_vert, Value(f=f, g=g))
parent[child.id] = node
path = path[::-1] #We have to reverse the path due to backtracking
return path
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius
obstacles = scene_obstacles # setting the global obstacle variable
x_limit = q_range[0] # the range of x-positions for the robot
y_limit = q_range[1] # the range of y-positions for the robot
theta_limit = q_range[
2] # the range of orientations for the robot (advanced extension)
robot_width, robot_height = robot_dim[0], robot_dim[
1] # the dimensions of the robot, represented as an oriented bounding box
robot_radius = max(robot_width, robot_height) / 2.
# the roadmap
graph = Roadmap()
# scene1= Scene
# qop = scene1.default_query()
graph.addVertex((14.378, -4.328)) # Adding the start as the first vertex.
graph.addVertex((-44.184, -26.821)) # Adding the goal as the last vertex.
"""
The below for loop is used to create samples (vertices) where the coordinate x will be spaced equally
of length 3 and y will be spaced equally of length 2.5 throughout the entire plot. Both the x and y coordinate
will be having a random noise ranging from -0.5 to 0.5.
All these samples are checked first whether they are on the collision free space or not, if they are then
they get added as vertices.
"""
for i in range(0, 33):
x = -48 + 3 * i + (random.uniform(-0.5, 0.5))
for j1 in range(0, 40):
y = -48 + 2.5 * j1 + (random.uniform(-0.5, 0.5))
coll = collision(x, y)
if coll is True:
graph.addVertex((x, y))
Vertices = graph.getVertices(
) # A Vertices list is created with all the vertices being currently placed.
max_dist = 5 # A maximum distance value is set to connect the vertices between them with a edge.
"""
The for loop below will add edge between the vertices which are spaced at a distance less than the max_dist
variable. Once the edge is added, the interpolate function will check if the edge is passing through the C
obstacle space or not. If it is, then the edge gets removed.
Once the edge gets removed, for each parent we shall create a list of children and to remove the edges among
them to reduce the number of unnecessary edges.
"""
for i in Vertices:
list_vertex = []
for j in Vertices:
dist = distance(i, j)
if dist < max_dist and dist != 0:
graph.addEdge(i, j, dist)
some = interpolate(i, j, 0.5)
if some is False:
graph.removeEdge(i, j)
list_vertex.append(j)
if len(list_vertex) >= 2:
for rem in range(0, len(list_vertex)):
for rem1 in range(0, len(list_vertex)):
if rem != rem1:
ss = RoadmapVertex.getEdge(list_vertex[rem],
list_vertex[rem1].id)
if ss is not None:
graph.removeEdge(list_vertex[rem],
list_vertex[rem1])
# uncomment this to export the roadmap to a file
# graph.saveRoadmap("prm_roadmap.txt")
return graph
def find_path(q_start, q_goal, graph):
"""The goal and the start values are initialized for our condition in the first step for performing A* star algorithm"""
path = []
goal0 = q_goal[0]
goal1 = q_goal[1]
start0 = q_start[0]
start1 = q_start[1]
goal = (goal0, goal1)
start = (start0, start1)
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use the PriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
grids = graph.getVertices() # nodes/ vertices
"""The parent list and the cost for the parent list are created for the A* star path likewise in the third assignment"""
parent_list = [' '] * len(grids)
node_cost = [1000] * len(grids)
# Path
f = 0
g = 0
h = 0
"""The h value is calculated using the euclidean distance calculation method"""
for q in range(0, 1):
h = sqrt((grids[0].q[0] - grids[1].q[0])**2 +
(grids[0].q[1] - grids[1].q[1])**2)
f = g + h
"""Checking whether the collision happens while appending the path and if found any are removed
Making sure that start and goal configurations are collision-free. Otherwise returning None
as mentioned in the comment provided by Dr.Karamouzas"""
goal_coll = collision(goal0, goal1)
start_coll = collision(start0, start1)
if goal_coll and start_coll is True:
graph.addVertex((goal0, goal1))
graph.addVertex((start0, start1))
else:
print('Cannot produce path')
return None
open_set.put(grids[0], Value(f=f, g=g))
while len(open_set) > 0:
parent, val = open_set.pop() #parent is the current node here
children = []
xlo = RoadmapVertex.getEdges(parent)
path = []
"""The children list is appended finding the IDs between the two nodes at any instant.
They represent the destination points between the two points and the values are finally updated."""
for child1 in xlo:
cg = child1.getDestination()
cg_object = grids[cg]
if cg_object not in closed_set:
children.append(cg_object)
if parent == grids[1]:
closed_set.add(parent)
current_node = parent
while current_node != grids[0]:
parent_node = parent_list[
current_node.
id] #Updating the current node with respect to the ID in the parent list that is created
path.append(parent_node.q
) #Finally updating the correct nodes to the path
current_node = parent_node
return path
closed_set.add(parent)
for child in children:
"""The cost calculation is done by using the euclidean distance calculated g, from the start to the current node
and h, from the current node to the end node. The resulting f value which is the sum of both g and h are taken."""
g_val = distance(child, parent) + val.g
h_val = distance(parent, grids[1])
f_val = g_val + h_val
cost_child = f_val
child_new = g_val
if child not in open_set or closed_set:
open_set.put(child, Value(f=cost_child, g=child_new))
if child in open_set:
cost_need = open_set.get(child)
cost_f = cost_need.f
if cost_child < cost_f:
open_set.put(child, Value(f=cost_child, g=child_new))
"""Backtracking the parent list with respect to the cost value from the goal node to form the optimal path"""
if node_cost[child.id] > cost_child:
node_cost[child.id] = cost_child
parent_list[child.id] = parent
return path
def find_path(q_start, q_goal, graph):
"""
In the following code, we are checking the goal and start coordinates which are generated from random or
default buttons. We then add these vertices and check their nearest neighbors to add edges between them.
Once the edges are added, they are checked using the interpolate function to see if any edge is passing through
the obstacles or not.
Also, if the start or goal is falling on the obstacle space when a random query is generated, we shall not
generate path.
"""
path = []
goal0 = q_goal[0]
goal1 = q_goal[1]
start0 = q_start[0]
start1 = q_start[1]
goal = (goal0, goal1)
start = (start0, start1)
start_vett = graph.addVertex(start)
goal_vert = graph.addVertex(goal)
start_id = RoadmapVertex.getId(start_vett)
goal_id = RoadmapVertex.getId(goal_vert)
grids1 = graph.getVertices() # nodes/ vertices
for grid in grids1:
if distance(grid, start_vett) < 5:
graph.addEdge(grid, start_vett, distance(grid, start_vett))
some = interpolate(grid, start_vett, 0.5)
if some is False:
graph.removeEdge(grid, start_vett)
if distance(grid, goal_vert) < 5:
graph.addEdge(grid, goal_vert, distance(grid, goal_vert))
some = interpolate(grid, goal_vert, 0.5)
if some is False:
graph.removeEdge(grid, goal_vert)
grids = graph.getVertices()
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
# Use the PriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
"""The parent list and the cost for the parent list are created for the A* star path likewise in the third assignment"""
parent_list = [' '] * len(grids)
node_cost = [1000] * len(grids)
# Path
f = 0
g = 0
h = 0
"""The h value is calculated using the euclidean distance calculation method"""
for q in range(0, 1):
h = sqrt((grids[start_id].q[0] - grids[goal_id].q[0])**2 +
(grids[start_id].q[1] - grids[goal_id].q[1])**2)
f = g + h
"""
Checking whether the collision happens while appending the path and if found any are removed
Making sure that start and goal configurations are collision-free. Otherwise returning None
as mentioned in the comment provided by Dr.Karamouzas
"""
goal_coll = collision(goal0, goal1)
start_coll = collision(start0, start1)
if goal_coll and start_coll is True:
graph.addVertex((goal0, goal1))
graph.addVertex((start0, start1))
else:
print('Cannot produce path')
return None
open_set.put(grids[start_id], Value(f=f, g=g))
while len(open_set) > 0:
parent, val = open_set.pop() #parent is the current node here
children = []
xlo = RoadmapVertex.getEdges(parent)
path = []
"""
The children list is appended finding the IDs between the two nodes at any instant.
They represent the destination points between the two points and the values are finally updated.
"""
for child1 in xlo:
cg = child1.getDestination()
cg_object = grids[cg]
if cg_object not in closed_set:
children.append(cg_object)
if parent == grids[goal_id]:
closed_set.add(parent)
current_node = parent
while current_node != grids[start_id]:
parent_node = parent_list[
current_node.
id] #Updating the current node with respect to the ID in the parent list that is created
path.append(parent_node.q
) #Finally updating the correct nodes to the path
current_node = parent_node
return path
closed_set.add(parent)
for child in children:
"""
The cost calculation is done by using the euclidean distance calculated g, from the start to the current node
and h, from the current node to the end node. The resulting f value which is the sum of both g and h are taken.
"""
g_val = distance(child, parent) + val.g
h_val = distance(parent, grids[goal_id])
f_val = g_val + h_val
cost_child = f_val
child_new = g_val
if child not in open_set or closed_set:
open_set.put(child, Value(f=cost_child, g=child_new))
if child in open_set:
cost_need = open_set.get(child)
cost_f = cost_need.f
if cost_child < cost_f:
open_set.put(child, Value(f=cost_child, g=child_new))
"""
Backtracking the parent list with respect to the cost value from the goal node to form the optimal path
"""
if node_cost[child.id] > cost_child:
node_cost[child.id] = cost_child
parent_list[child.id] = parent
return path
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius
obstacles = scene_obstacles # setting the global obstacle variable
x_limit = q_range[0] # the range of x-positions for the robot
y_limit = q_range[1] # the range of y-positions for the robot
theta_limit = q_range[
2] # the range of orientations for the robot (advanced extension)
robot_width, robot_height = robot_dim[0], robot_dim[
1] # the dimensions of the robot, represented as an oriented bounding box
robot_radius = max(robot_width, robot_height) / 2.
A = np.zeros((1000, 2))
# the roadmap
graph = Roadmap()
list_app = []
vertices_app = []
scene1 = Scene
# qop = scene1.default_query()
goof = graph.addVertex((14.378, -4.328))
goof1 = graph.addVertex((-44.184, -26.821))
for i in range(0, 33):
# x = int(np.random.uniform(-50,50))
# y = int(np.random.uniform(-50,50))
x = -44 + 3 * i + (random.uniform(-0.5, 1))
for j1 in range(0, 33):
y = -44 + 3 * j1 + (random.uniform(-0.5, 1))
coll = collision(x, y)
if coll is True:
graph.addVertex((x, y))
Vertices = graph.getVertices()
max_dist = 5
for i in Vertices:
list_vertex = []
for j in Vertices:
dist = distance(i, j)
# print('in loop dist', dist)
if dist < max_dist and dist != 0:
edge_check = graph.addEdge(i, j, dist)
some = interpolate(i, j, 0.5)
# print('some',some)
if some is False:
graph.removeEdge(i, j)
list_a_x = i.id
list_a_y = j.id
list_a = (list_a_x, list_a_y)
list_app.append(list_a)
list_b = (list_a_y, list_a_x)
if list_b not in list_app:
graph.removeEdge(i, j)
list_vertex.append(j)
if len(list_vertex) >= 2:
for rem in range(0, len(list_vertex)):
for rem1 in range(0, len(list_vertex)):
if rem != rem1:
ss = RoadmapVertex.getEdge(list_vertex[rem],
list_vertex[rem1].id)
if ss is not None:
graph.removeEdge(list_vertex[rem],
list_vertex[rem1])
#print('build',RoadmapVertex.getConfiguration(goof1))
# uncomment this to export the roadmap to a file
# graph.saveRoadmap("prm_roadmap.txt")
return graph
def find_path(q_start, q_goal, graph):
path = []
trial = []
goal0 = q_goal[0]
goal1 = q_goal[1]
start0 = q_start[0]
start1 = q_start[1]
goal = (goal0, goal1)
start = (start0, start1)
# print('The goal is:', goal)
# print('The start is:', start)
# goof = graph.addVertex(goal)
# goof1 = graph.addVertex(start)
# Use the OrderedSet for your closed list
closed_set = OrderedSet()
goal_coll = collision(goal0, goal1)
start_coll = collision(start0, start1)
if goal_coll and start_coll is True:
aa = graph.addVertex((goal0, goal1))
sas = graph.addVertex((start0, start1))
# print(RoadmapVertex.getConfiguration(sas))
# print(ss)
else:
return None
# Use the PriorityQueue for the open list
open_set = PriorityQueue(order=min, f=lambda v: v.f)
grids = graph.getVertices() # nodes/ vertices
# print(grids)
# print(len(grids))
val_app = []
# Path
f = 0
g = 0
h = 0
for q in range(0, 1):
# h = sqrt((start0 - goal0) ** 2 + (start1 - goal1) ** 2)
h = sqrt((grids[0].q[0] - grids[1].q[0])**2 +
(grids[0].q[1] - grids[1].q[1])**2)
f = g + h
# open_set.put(sas, Value(f=f, g=g))
open_set.put(grids[0], Value(f=f, g=g))
while len(open_set) > 0:
parent, val = open_set.pop()
closed_set.add(parent)
val_app.append(val.f)
# print('f', val_app)
# print('inside',len(open_set))
new = (RoadmapVertex.getConfiguration(parent))
new1 = new[0]
new2 = new[1]
trial = [new1, new2]
# if parent == aa:
if parent == grids[1]:
break
else:
for child in grids:
sssss = RoadmapVertex.getEdge(parent, child.id)
if sssss is not None:
if child not in closed_set:
g_val = distance(child, parent) + val.g
# g_val = distance(child, parent) + val.g
if child not in open_set or open_set.get(
child).g > g_val:
# h_val = distance(parent, aa)
h_val = distance(parent, grids[1])
# print('The g value of child is:',g_val)
f_val = g_val + h_val
# print('The f value of child is:', f_val)
open_set.put(child, Value(f=f_val, g=g_val))
if val.g > g_val:
print('more')
# print('The trial is:', trial)
path.append(trial)
# for alpha in grids:
# pare = RoadmapVertex.getEdge(parent,alpha.id)
# if pare is not None:
# print('pare', RoadmapVertex.getConfiguration(alpha))
# parent = [[[' ' for row in range(len(grids[0]))] for col in range(len(grids))],
# [[' ' for row in range(len(grids[0]))] for col in range(len(grids))],
# [[' ' for row in range(len(grids[0]))] for col in range(len(grids))],
# [[' ' for row in range(len(grids[0]))] for col in range(len(grids))]]
# print(parent)
for i in range(len(val_app)):
for j in range(len(val_app)):
if i != j:
if val_app[i] > val_app[j]:
# np.remove(val_app[i])
print('gg')
return path
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius
obstacles = scene_obstacles # setting the global obstacle variable
x_limit = q_range[0] # the range of x-positions for the robot
y_limit = q_range[1] # the range of y-positions for the robot
theta_limit = q_range[
2] # the range of orientations for the robot (advanced extension)
robot_width, robot_height = robot_dim[0], robot_dim[
1] # the dimensions of the robot, represented as an oriented bounding box
robot_radius = max(robot_width, robot_height) / 2.
A = np.zeros((1000, 2))
# the roadmap
graph = Roadmap()
list_app = []
list_app = []
vertices_app = []
goof = graph.addVertex((14.378, -4.328))
goof1 = graph.addVertex((-44.184, -26.821))
#graph.addVertex((14.378,-4.328))
for i in range(0, 33):
# x = int(np.random.uniform(-50,50))
# y = int(np.random.uniform(-50,50))
x = -45 + 3 * i + (random.uniform(-0.5, 0.5))
for j1 in range(0, 33):
y = -45 + 3 * j1 + (random.uniform(-0.5, 0.5))
A[i][0] = x
A[i][1] = y
for j in range(0, 26):
xs = []
ys = []
for point in obstacles[j].points:
count = 0
xs.append(point[0])
ys.append(point[1])
x_min = min(xs) - 2
x_max = max(xs) + 2
y_min = min(ys) - 2
y_max = max(ys) + 2
if x_min <= x <= x_max and y_min <= y <= y_max:
count = 1
break
if count != 1:
graph.addVertex((x, y))
# for qq in range(len(A)):
Vertices = graph.getVertices()
max_dist = 5
for i in Vertices:
list_vertex = []
for j in Vertices:
dist = distance(i, j)
# print('in loop dist', dist)
if dist < max_dist and dist != 0:
edge_check = graph.addEdge(i, j, dist)
some = interpolate(i, j, 0.5)
# print('some',some)
if some is False:
graph.removeEdge(i, j)
list_a_x = i.id
list_a_y = j.id
list_a = (list_a_x, list_a_y)
list_app.append(list_a)
list_b = (list_a_y, list_a_x)
if list_b not in list_app:
graph.removeEdge(i, j)
list_vertex.append(j)
if len(list_vertex) >= 2:
for rem in range(0, len(list_vertex)):
for rem1 in range(0, len(list_vertex)):
if rem != rem1:
ss = RoadmapVertex.getEdge(list_vertex[rem],
list_vertex[rem1].id)
if ss is not None:
graph.removeEdge(list_vertex[rem],
list_vertex[rem1])
# p_neighbour = []
# c_neighbour = []
#
# for x in range(len(Vertices)):
# p_neighbour.clear()
# for z in range(len(Vertices)):
# if not x == z:
# parent = RoadmapVertex.getEdge(Vertices[x], Vertices[z].id)
# if not parent is None:
# p_neighbour.append(Vertices[z])
# c_neighbour.clear()
# for u in range(len(p_neighbour)):
# for h in range(len(Vertices)):
# if not u == h:
# children = RoadmapVertex.getEdge(p_neighbour[u], Vertices[h].id)
# if not children is None:
# c_neighbour.append(Vertices[h])
#
# for r in range(len(p_neighbour)):
# for m in range(len(c_neighbour)):
# if p_neighbour[r] == c_neighbour[m]:
# graph.removeEdge(Vertices[x], Vertices[z])
# break
aa = RoadmapVertex.getEdges(goof1)
#print('build',RoadmapVertex.getConfiguration(goof1))
# uncomment this to export the roadmap to a file
# graph.saveRoadmap("prm_roadmap.txt")
return graph