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 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 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
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()
#### Sampling ####
p = 0.02
x_pos = np.arange(x_limit[0]+3, x_limit[1]-3, p)
y_pos = np.arange(y_limit[0]+3, y_limit[1]-3, p)
samples = []
vertex = [0, 0]
samples.append(vertex)
first = samples.pop()
check = collision(first)
if not check:
graph.addVertex(first)
k = 1
l = 1
while k <= 60:
if l == 500:
break
vertex = [np.random.choice(x_pos), np.random.choice(y_pos)]
samples.append(vertex)
trial = samples.pop()
check1 = collision(trial)
if not check1:
trial_put = graph.addVertex(trial)
k += 1
l += 1
vertices = graph.getVertices()
for point in vertices:
if not point == trial_put:
d = distance(point, trial_put)
if d < 0.2:
graph.removeVertex(trial_put.id)
k -= 1
######## Map Search ##########
vertices = graph.getVertices()
visited = OrderedSet()
stacked = OrderedSet()
stacked.add(vertices[0])
while len(stacked) != 0:
point = stacked.pop(len(stacked) - 1)
visited.add(point)
current_position_x, current_position_y = point.q
tot_neighbor = 0
for v in vertices:
neighbor_position_x, neighbor_position_y = v.q
if point == v:
continue
neighbor_distance = distance(point, v)
if neighbor_distance <= 0.8 and v not in stacked and v not in visited:
stacked.add(v)
graph.addEdge(point, v, neighbor_distance)
tot_neighbor = tot_neighbor + 1
for m in range(len(obstacles)):
step = 15
for x in range(1, step):
z = step - x
check_x = (x * neighbor_position_x + z * current_position_x) / (x + z)
check_y = (x * neighbor_position_y + z * current_position_y) / (x + z)
y = [check_x, check_y]
if collision(y) == True:
if v in stacked:
stacked.remove(v)
graph.removeEdge(point, v)
tot_neighbor = tot_neighbor - 1
if tot_neighbor > 5:
break
# 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.
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