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
# get the vertices
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()
nr = 800 # used for the amount of samples
while graph.getNrVertices() < nr:
for i in range(nr):
addVertice = True
q_new = [
np.random.randint(x_limit[0], x_limit[1]),
np.random.randint(y_limit[0], y_limit[1])
]
for vertice in graph.getVertices():
if distance(vertice.getConfiguration(), q_new) < 2:
addVertice = False
if addVertice and not collision(q_new):
graph.addVertex(q_new)
# get the edges
stepsize = 1 / 15 # used for checking the interpolate
path = []
Vertices = graph.getVertices()
for v in Vertices:
neighbor, neighbor_d = nearest_neighbors(graph, v.q, max_dist=2.2)
if len(neighbor) < 4:
neighbor, neighbor_d = k_nearest_neighbors(graph,
v.getConfiguration(),
K=4)
for neigh in neighbor:
if not interpolate(v.getConfiguration(), neigh.getConfiguration(),
stepsize):
graph.addEdge(v, neigh, neighbor_d[neighbor.index(neigh)],
path)
#uncomment this to export the roadmap to a file
graph.saveRoadmap("prm_roadmap.txt")
return graph
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()
# Generate Vertice
n = 375 # Sampling numbers
while graph.getNrVertices() < n:
for a in range(n):
# Genernate a vertice
q_current = [np.random.randint(x_limit[0],x_limit[1]), np.random.randint(y_limit[0],y_limit[1])]
append = True
# Determine whether current vertice is valuable
for vertice in graph.getVertices():
if distance(vertice.getConfiguration(),q_current) < 3:
append = False
if not collision(q_current) and append:
graph.addVertex(q_current)
# Find all valuable path for the robot off line
path = []
Vertices = graph.getVertices() # Get vertices from " build_roadmap"
# Find nearest neighbors for current vertice
for v in Vertices:
q_neighbors, q_distance = k_nearest_neighbors(graph,v.getConfiguration())
for i in range(len(q_neighbors)):
graph.addEdge(v,q_neighbors[i],q_distance[i],path)
#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 = []
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, nei_list
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()
#checking for optimum samples which will reside in the C-Space
for i in range(0,10000):
x = random.uniform(-48, 48)
y = random.uniform(-48, 48)
q = (x, y)
c1 = 0
vertex_class = graph.getVertices()
vertex_no = graph.getNrVertices()
for u in range(0, vertex_no):
if distance(q, vertex_class[u].getConfiguration()) < 2: #minimum distance between 2 configurations = 2
c1 = 50
break
if c1 == 50:
pass
elif collision(q):
pass
else:
graph.addVertex(q)
vertex_class = graph.getVertices()
vertex_no = graph.getNrVertices()
#************ The below code checks for the redundant edges ******************
# It checks edges connecting to each neighbor and returns a list of children.
# For each of the children, it generates another set of children which check
# for the vertex_id of the parent vertex and adds an edge if it is not in the list
for h in range(0, vertex_no):
config = vertex_class[h]
config_p = vertex_class[h].getConfiguration()
config_id = vertex_class[h].getId()
nei, dist_list = nearest_neighbors(graph, config_p, 8)
for r in range(0, len(nei)):
edge_id = []
edge = []
n_id = nei[r].getId()
edge = nei[r].getEdges() #checks edges connecting to each neighbor
for l in range(0, len(edge)):
edge_id.append(edge[l].getId())
em = []
for o in range(0, len(edge_id)):
em.append(vertex_class[edge_id[o]])
edge_cid = []
for k in range(0, len(em)):
edge_1 = em[k].getEdges()
for u in range(0, len(edge_1)):
edge_cid.append(edge_1[u].getId())
em1 = []
for o in range(0, len(edge_cid)):
em1.append(vertex_class[edge_cid[o]])
edge_cid1 = []
for k in range(0, len(em1)):
edge_11 = em1[k].getEdges()
for u in range(0, len(edge_11)):
edge_cid1.append(edge_11[u].getId())
em11 = []
for o in range(0, len(edge_cid1)):
em11.append(vertex_class[edge_cid1[o]])
edge_cid11 = []
for k in range(0, len(em11)):
edge_111 = em11[k].getEdges()
for u in range(0, len(edge_111)):
edge_cid11.append(edge_111[u].getId())
em111 = []
for o in range(0, len(edge_cid11)):
em111.append(vertex_class[edge_cid11[o]])
edge_cid111 = []
for k in range(0, len(em111)):
edge_1111 = em111[k].getEdges()
for u in range(0, len(edge_1111)):
edge_cid111.append(edge_1111[u].getId())
if config_id in edge_id:
pass
elif config_id in edge_cid:
pass
elif config_id in edge_cid1:
pass
elif config_id in edge_cid11:
pass
elif config_id in edge_cid111:
pass
else:
il = interpolate(config_p, nei[r].getConfiguration(), 8)
dist = dist_list[r]
count = 0
for j in range(0, len(il)):
if collision(il[j]) == True:
count = 50
else:
pass
if count == 0:
graph.addEdge(config, nei[r],dist, path=[])
return graph
