def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius
graph = Roadmap()
obstacles = scene_obstacles
robot_width, robot_height = robot_dim[0], robot_dim[1]
# Here we have just specified the start coordinate of the vehicle.
graph.addVertex((3, 5))
return graph
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius
graph = Roadmap()
directions = [0, 1, 2, 3, 4, 5, 6, 7]
angle = [0, pi / 4, pi / 2, 3 * pi / 4, pi, 5 * pi / 4, 2 * pi]
direction_name = ['N', 'NW', 'W', 'SW', 'S', 'SE', 'E', 'NE']
action = [-1, 0, 1]
# l = 1 # Length
# forward = [[0, l], #0: go N
# [-l/sqrt(2), l/sqrt(2)] #1: go NW
# [0, -l], #2: go W
# [-l/sqrt(2), -l/sqrt(2)] #3 go SW
# [0, -l], #4: go S
# [l/sqrt(2), -l/sqrt(2)] #5: go SE
# [l, 0] #6: go E
# [l/sqrt(2), l/sqrt(2)]] #7 go NE
action_name = ['R', 'F', 'L']
cost = [1, 1, 1] # corresponding cost values
graph = Roadmap()
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.
for i in range(0, 4):
xo = 1 + i * 5
for j in range(0, 5):
yo = 1 + j * 5
xon = xo * cos(math.radians(0.1)) - yo * sin(math.radians(0.1))
yon = yo * cos(math.radians(0.1)) + xo * sin(math.radians(0.1))
# print('x0', xon, yon)
theta = [45, 135, 225, 315]
graph.addVertex((xon, yon))
# theta = [90]
for k in range(len(theta)):
r = 5
x1 = xo + r * math.cos(math.radians(theta[k]))
y1 = yo + r * math.sin(math.radians(theta[k]))
x1n = x1 * cos(math.radians(0.1)) - y1 * sin(math.radians(0.1))
y1n = y1 * cos(math.radians(0.1)) + x1 * sin(math.radians(0.1))
# print('x1', x1n,y1n,theta)
# print('y1',y1n)
graph.addVertex((x1n, y1n))
return graph
def k_nearest_neighbors(graph: Roadmap, q, K=10):
"""
Returns the K-nearest roadmap vertices for a given configuration q.
You may also want to return the corresponding distances
"""
# dist = [0]*graph.getNrVertices()-1
dist = []
for i, vertex in enumerate(graph.getVertices()):
if (q != vertex.getConfiguration()):
dist.append(distance(q, vertex.getConfiguration()))
# nearest_neighbors = sorted(list(zip(dist, graph.getVertices())))[:K]
threshold = np.unique([i for i in dist if i > 1 * robot_radius])[K - 1]
nearest_neighbors = []
for vertex in graph.getVertices():
d = distance(q, vertex.getConfiguration())
if (1 * robot_radius < d <= threshold
and q != vertex.getConfiguration()):
nearest_neighbors.append((d, vertex))
return nearest_neighbors
def nearest_neighbors(graph: Roadmap, 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
"""
nearest_n = []
for vertex in graph.getVertices():
dist = distance(q, vertex.getConfiguration())
if (2 * robot_radius < dist <= max_dist):
nearest_n.append((dist, vertex))
return nearest_n
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius, edge_path
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.
graph = Roadmap() # the road map
xx = np.arange(x_limit[0], x_limit[1], 1.5)
random.shuffle(xx)
yy = np.arange(y_limit[0], y_limit[1], 1.5)
random.shuffle(yy)
x = []
y = []
for i in range(len(xx)):
for j in range(len(yy)):
x.append(xx[i] + randint(0, 1))
y.append(yy[j] + randint(0, 1))
print(len(x))
for i in range(len(x)): # looping through each vertex
x_rand = x[i]
y_rand = y[i]
vertex_in_obstacle = False
for j in obstacles:
if (j.x_min - robot_radius < x_rand < j.x_max + robot_radius) and (
j.y_min - robot_radius < y_rand < j.y_max + robot_radius):
vertex_in_obstacle = True
break
if vertex_in_obstacle == False:
graph.addVertex((x_rand, y_rand))
vertices = graph.getVertices()
print('Vertices legnth is', len(vertices))
edge_path.append(vertices[0]) # Appending the start to edge path
ep = 0
while ep < len(edge_path):
parent_vert = edge_path[ep]
parent_vert.connetedComponentNr = 1
k_nearest_agents = k_nearest_neighbors(graph, parent_vert, 5)
collision(graph, obstacles, k_nearest_agents, parent_vert, 0)
ep += 1
graph.saveRoadmap("prm_roadmap.txt")
return graph
def build_roadmap(q_range, robot_dim, scene_obstacles):
global obstacles, robot_width, robot_height, robot_radius, VerList
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()
# uncomment this to export the roadmap to a file
graph.saveRoadmap("prm_roadmap.txt")
count = 0
points = []
VerList = []
for i in range(0, 20):
xo = 1 + i * 5
for j in range(0, 20):
yo = 1 + j * 5
theta = [pi / 4, 3 * pi / 4, 5 * pi / 4, 7 * pi / 4]
graph.addVertex((xo, yo))
for k in range(len(theta)):
r = 5
x1 = xo + r * math.cos(theta[k])
y1 = yo + r * math.sin(theta[k])
graph.addVertex((x1, y1))
VerList = graph.getVertices()
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()
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 = []
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, N
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()
global max_dist
N = 5000 ## Number of points considered
even_spacing_parameter = 3 ## used for removing clustering of the uniformly sampled points
max_dist = 15 ## Max distance considered for obtaining neareset neighbors
'''Sampling of Vertices begins'''
## Sampling N points in x and y
xc = np.random.uniform(low=x_limit[0], high=x_limit[1], size=N)
yc = np.random.uniform(low=y_limit[0], high=y_limit[1], size=N)
### Adding all collision free Vertices to a list
vertices = []
for i in range(N):
if collision([xc[i], yc[i]]) == False:
vertices.append([xc[i], yc[i]])
### Removing clustering of points
for v1 in vertices:
for v2 in vertices:
if distance(
v1, v2
) <= even_spacing_parameter and v1 != v2 and v2 in vertices:
vertices.remove(v2)
print('Number of vertices in graph:', len(vertices))
## Adding above vertices to graph
for v in vertices:
graph.addVertex(v)
'''Sampling of Vertices completed'''
''' Obtaining and connecting edges'''
cl_set = OrderedSet()
count = 0
for item in graph.getVertices():
dist1_list = []
dist1_list = nearest_neighbors(graph, item)
cl_set.add(item)
for nbr in dist1_list:
if interpolate(nbr[0], item, 4) == False and nbr[
0] not in cl_set: ## interpolate removes collision-edges | checking with cl_set helps avoid triangulation
if graph.computeConnectedComponents(item, nbr[0]) == True:
continue
graph.addEdge(item, nbr[0], nbr[1])
count = count + 1
print('No of edges: ', count)
###
# 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.
print('r: ', robot_radius)
secene_x = q_range[0]
secene_y = q_range[1]
# samples = [[random.uniform(secene_x[0],secene_x[1]), random.uniform(secene_y[0], secene_y[1])] for _ in range(900)]
# samples = list(zip( list(np.linspace(x_limit[0], x_limit[1], n_sample))[1:-1], list(np.linspace(y_limit[0], y_limit[1], n_sample))[1:-1] ))
# print('samples: ', samples)
n_sample_iters = 34
samples = []
for i in range(n_sample_iters):
for j in range(n_sample_iters):
x_low = secene_x[0] + (secene_x[1] -
secene_x[0]) * i / n_sample_iters
x_high = x_low + (secene_x[1] - secene_x[0]) / n_sample_iters
y_low = secene_y[0] + (secene_y[1] -
secene_y[0]) * j / n_sample_iters
y_high = y_low + (secene_y[1] - secene_y[0]) / n_sample_iters
for k in range(0, 1):
x = random.uniform(x_low, x_high)
y = random.uniform(y_low, y_high)
# while(test_in_obs([x,y], obstacles, robot_radius)):
# x = random.uniform(x_low, x_high)
# y = random.uniform(y_low, y_high)
samples.append([x, y])
valid_samples = [
x for x in samples if not test_in_obs(x, obstacles, robot_radius)
]
# invalid_samples = [x for x in samples if test_in_obs(x, obstacles, robot_radius)]
# for i in range(len(invalid_samples)):
# for j in range(len(invalid_samples)):
# mid_x = 0.5*(invalid_samples[i][0] + invalid_samples[j][0])
# mid_y = 0.5*(invalid_samples[i][1] + invalid_samples[j][1])
# if(i!=j and (not test_in_obs([mid_x, mid_y], obstacles, robot_radius))):
# valid_samples.append([mid_x, mid_y])
print("valid_samples: ", len(valid_samples))
# the roadmap
graph = Roadmap()
for i, sample_p in enumerate(
valid_samples): #add all the valid sample vertex
graph.addVertex(sample_p)
for i_vertex in graph.getVertices():
# n_neighbors = nearest_neighbors(graph, i_vertex.getConfiguration(), max_dist=10)
k_neighbors = k_nearest_neighbors(graph,
i_vertex.getConfiguration(),
K=6)
# neighbors = list(set(n_neighbors).union(set(k_neighbors)))
for nbor in k_neighbors:
inters = interpolate(i_vertex.getConfiguration(),
nbor[1].getConfiguration(), 0.5)
if (not test_inters(inters, obstacles, robot_radius)):
i_vertex.addEdge(nbor[1].id, nbor[0])
nbor[1].addEdge(i_vertex.getId(), nbor[0])
graph.addEdge(i_vertex, nbor[1], nbor[0])
print('len graph: ', len(graph.vertices))
# 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()
# 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, 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
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 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()
# Generating the sample points
samples = pointGenerator(x_limit[0], y_limit[0], 46, 0.9999999)
# Eliminating the samples which aren't in the C-Space
reject = []
accept_x = []
accept_y = []
for p in samples:
for j in range(len(obstacles)):
if ((obstacles[j].x_min - robot_radius) <= p[0] <=
(obstacles[j].x_max + robot_radius)
and (obstacles[j].y_min - robot_radius) <= p[1] <=
(obstacles[j].y_max + robot_radius)):
reject.append(p)
reject = np.asarray(reject)
for a in samples[:, 0]:
if a not in reject[:, 0]:
accept_x.append(a)
for b in samples[:, 1]:
if b not in reject[:, 1]:
accept_y.append(b)
accept = np.zeros((len(accept_x), 2))
accept[:, 0] = accept_x
accept[:, 1] = accept_y
for k in range(len(accept_x)):
graph.addVertex(accept[k])
# Connecting vertices
# Nearest neighbors
allVertices = graph.getVertices()[:]
for val1 in allVertices:
neighbors, neighbor_dist, neighbor_id = nearest_neighbors(
graph, val1.q, val1.id, 6)
for val2 in allVertices:
if val2.id in neighbor_id:
if interpolate(val1.q, val2.q, 5, obstacles, robot_radius):
if val2.getConnectedNr() == -1 and val2.getEdge(
val1.id) == None:
graph.addEdge(val1, val2, distance(val1.q, val2.q))
val2.connectedComponentNr = val1.getId()
else:
parents = []
temp = val1
while temp.connectedComponentNr != -1:
idd = temp.connectedComponentNr
parents.append(idd)
for a in allVertices:
if a.id == idd:
temp = a
parents.append(temp.id)
children = []
for c in parents:
for a in allVertices:
if a.id == c:
temp = a
for h in temp.getEdges():
children.append(h.src_id)
children.append(h.dest_id)
total_val1 = parents + children
parents1 = []
temp1 = val2
while temp1.connectedComponentNr != -1:
idd1 = temp1.connectedComponentNr
parents1.append(idd1)
for a in allVertices:
if a.id == idd1:
temp1 = a
parents1.append(temp1.id)
children1 = []
for c in parents1:
for a in allVertices:
if a.id == c:
temp2 = a
for h in temp2.getEdges():
children1.append(h.src_id)
children1.append(h.dest_id)
total_val2 = parents1 + children1
compare = list(set(total_val1) & set(total_val2))
if len(compare) == 0 and val2.getEdge(
val1.getId()) == None:
graph.addEdge(val1, val2, distance(val1.q, val2.q))
val2.connectedComponentNr = val1.id
# if h.id == c.id:
# temp1 = h
# cvertices = temp1.getEdges()
# for k in cvertices:
# tx = k.src_id
# children.append(tx)
# ty = k.dest_id
# children.append(ty)
# if len(children) == 0 and val2.getEdge(val1.id) == None:
# graph.addEdge(val1, val2, distance(val1.q, val2.q))
# val2.connectedComponentNr = val1.getId()
"""allVertices = graph.getVertices()[:]
visited = [False]*len(allVertices)
start = allVertices[0]
queue = []
queue.append(start)
visited[start.id] = True
while queue:
a = queue.pop(0)
for i in allVertices:
if visited[i.id] == False:
if distance(a.q, i.q) < 3:
queue.append(i)
visited[i.id] = True
if interpolate(a.q, i.q, 8, obstacles, robot_radius):
graph.addEdge(a, i, distance(a.q, i.q))
#visited[i.id] = True"""
# 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, N_samples
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
candidate_stack = np.zeros((N_samples, 2))
candidate_stack = perturb(N_samples, x_limit, y_limit) #sampled number
graph = Roadmap() # the roadmap
candidate_rem = []
for pos, val in enumerate(candidate_stack):
for obj in obstacles:
xs = []
ys = []
for p in obj.points:
xs.append(p[0])
ys.append(p[1])
x_min = min(xs) - robot_radius
x_max = max(xs) + robot_radius
y_min = min(ys) - robot_radius
y_max = max(ys) + robot_radius
if (val[0] > x_min and val[0] < x_max and val[1] > y_min
and val[1] < y_max):
candidate_rem.append(candidate_stack[pos])
candidate_rem = np.asarray(candidate_rem)
cand_stack_x = ExtractX(candidate_stack)
cand_stack_y = ExtractY(candidate_stack)
cand_rem_x = ExtractX(candidate_rem)
cand_rem_y = ExtractY(candidate_rem)
differencesX = []
for list in cand_stack_x:
if list not in cand_rem_x:
differencesX.append(list)
differencesY = []
for list in cand_stack_y:
if list not in cand_rem_y:
differencesY.append(list)
cand_final = []
h = []
for i in range(len(differencesX)):
cand_final.append([differencesX[i], differencesY[i]])
for k in range(len(cand_final)):
graph.addVertex((cand_final[k][0], cand_final[k][1]))
for vertex in graph.vertices:
for child in graph.vertices:
c = 0
if vertex != child:
#distance=math.sqrt((vertice.q[0]-poss_neighbor.q[0])**2 + (vertice.q[1]-poss_neighbor.q[1])**2)
dist_2 = distance(vertex.q, child.q)
if dist_2 <= 10:
h = interpolate(vertex.q, child.q, 5)
for ho in h:
for o in range(len(obstacles)):
if ((obstacles[o].x_min - robot_radius <= ho[0] <=
obstacles[o].x_max + robot_radius) and
(obstacles[o].y_min - robot_radius <= ho[1] <=
obstacles[o].y_max + robot_radius)):
c = 1
if c == 0:
#graph.addEdge(vertice,poss_neighbor,distance)
if child.connectedComponentNr == -1 and child.getEdge(
vertex.id) == None:
#vertice.addEdge(poss_neighbor.id,distance)
graph.addEdge(vertex, child, dist_2)
child.connectedComponentNr = vertex.id
else:
path_parent = find(vertex, graph.vertices)
path_child = find(child, graph.vertices)
path_length = len(
set(path_child) & set(path_parent))
if path_length == 0 and child.getEdge(
vertex.id) == None:
graph.addEdge(vertex, child, dist_2)
child.connectedComponentNr = vertex.id
#print(Scene.default_start.get(loadProblem))
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.
sensitivity = 0.3 # 0 means no movement, 1 means max distance is init_dist - To add noise in the sample space
# create regularly spaced neurons
x = np.linspace(x_limit[0], x_limit[1], 40)
y = np.linspace(y_limit[0], y_limit[1], 40)
coords = np.stack(np.meshgrid(x, y), -1).reshape(
-1, 2) # Generating a meshgrid of x and y samples
# compute spacing
init_dist = np.min((x[1] - x[0], y[1] - y[0]))
min_dist = init_dist * (1 - sensitivity)
# perturb points
perturbation = (init_dist - min_dist) / 2
noise = np.random.uniform(
low=-perturbation, high=perturbation, size=(
len(coords),
2)) # Creating a linspace array of noise based on the perturbation
coords += noise # Adding noise to the points in ssample space
samples_x = coords[:, 0]
samples_y = coords[:, 1]
# the roadmap
graph = Roadmap()
# This loop will add all the vertices in the obstacle free configuration space
for i in range(len(samples_x)):
c = 0
for j in range(len(obstacles)):
if ((obstacles[j].x_min - robot_radius) <= samples_x[i] <=
(obstacles[j].x_max + robot_radius)) and (
(obstacles[j].y_min - robot_radius) <= samples_y[i] <=
(obstacles[j].y_max + robot_radius)):
c = 1
if c == 0:
samples = [samples_x[i], samples_y[i]]
graph.addVertex(samples)
# This loop is used to add the edges between the vertices that are within a certain distance from the vertex
for vertice in graph.vertices:
for poss_neighbor in graph.vertices:
if vertice != poss_neighbor:
distance = math.sqrt((vertice.q[0] - poss_neighbor.q[0])**2 +
(vertice.q[1] - poss_neighbor.q[1])**2)
if distance < 10:
if interpolate(
vertice, poss_neighbor, 5, obstacles, distance,
robot_radius
) == True: # Calling the incremental sampling function to avoid the obstacles
if poss_neighbor.connectedComponentNr == -1 and poss_neighbor.getEdge(
vertice.id
) == None: # Connecting configurations if they are not previously connected
graph.addEdge(vertice, poss_neighbor, distance)
poss_neighbor.connectedComponentNr = vertice.id
else: #If they are already connected, checking parents
x = path_find(vertice, graph.vertices)
y = path_find(poss_neighbor, graph.vertices)
z = len(
list(set(x) & set(y))
) # Checking for common parents between two configurations using set intersection
if z == 0 and poss_neighbor.getEdge(
vertice.id) == None:
graph.addEdge(vertice, poss_neighbor, distance)
poss_neighbor.connectedComponentNr = vertice.id
# 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.
graph = Roadmap()
n = 1100 # Number of samples
min_xy = [x_limit[0] + 2, y_limit[0] + 2] #Limit
max_xy = [x_limit[1] - 2, y_limit[1] - 2]
######### Sampling method : Random sampling########################
# array = np.random.uniform(low=min_xy, high=max_xy, size=(n, 2))
############## Sampling method: Unifrom sampling with gaussian noise addition#############
x = np.linspace(x_limit[0] + 3, x_limit[1] - 3,
35) # vertices in one line will be 35
y = np.linspace(y_limit[0] + 3, y_limit[1] - 3, 35) #
x_n = np.random.uniform(-0.8, 0.8,
100) # -0.8 and 0.8 will be min max limit
y_n = np.random.uniform(-0.8, 0.8, 100)
array = []
for i in range(len(x)):
for j in range(len(y)):
point = (x[i] + np.random.choice(x_n),
y[j] + np.random.choice(y_n))
array.append(point)
#### Sampling method: Kernnel based sampling ############333
# array = np.random.uniform(low=min_xy, high=max_xy, size=(1, 2))
# initial_array = np.random.uniform(low=min_xy, high=max_xy, size=(1, 2))
# kernelx=2
# kernely=2
#
# l=1.75
# l=1.75
# for i in np.arange(x_limit[0] +2.5, x_limit[1]-l,3):
# endx=i+kernelx
# lengthx=[i,endx]
# for j in np.arange(y_limit[0]+l,y_limit[1]-l,3):
# endy=j+kernely
# lengthy=[j,endy]
#
# """ This is the second way to do the problem"""
# mu, sigma = 0, 1 # mean and standard deviation
# s = (np.random.normal(mu, 0.9, 1))
# s1 = (np.random.normal(mu, 0.9, 1))
# # print(s)
# arr=[(int)(i+s),(int)(j+s1)]
#
# # arr=[i,j]
# """ ______________________________________________________"""
#
# """ This is the third way to do the problem Uncomment to use"""
# # arr = (np.random.uniform(lengthx, lengthy, size=(10, 2)))
# """ ______________________________________________________"""
# initial_array= np.vstack([initial_array, arr])
#
# endy=0
# endx=0
# # array=initial_array
#
#
#
# cond12=0
# for i in range(len(initial_array)):
#
# for j in range(len(initial_array)):
#
# if initial_array[i][0] != initial_array[j][0] and initial_array[i][1] != initial_array[j][1]:
# dista = distance([initial_array[i][0],initial_array[i][1]], [initial_array[j][0],initial_array[j][1]])
# if(dista>0.3):
# pass
# else:
# # print(dista)
# cond12=1 # Do not add in this case
#
# if (cond12!=1):
# x1=[initial_array[i][0],initial_array[i][1]]
# array= np.vstack([array, x1])
# # print(dista)
# cond12=0
#
coll_list = []
for point in range(
len(array)
): #Loop for checking whether the vertices are pn the obstacle or not and elimination of such vertices
obstacle_check = 0
for i in range(len(obstacles)):
obstacle_i = obstacles[i].points
xmin = obstacle_i[0][0] - robot_radius
xmax = obstacle_i[2][0] + robot_radius
ymin = obstacle_i[2][1] - robot_radius
ymax = obstacle_i[0][1] + robot_radius
if xmin < array[point][0] < xmax and ymin < array[point][1] < ymax:
obstacle_check = 1
if obstacle_check != 1:
collision_free = graph.addVertex(
array[point]) #Adding vertex in roadmap
coll_list.append(collision_free)
# print(coll_list[a].q)
list = [
] #For adding edges to the roadmap according to distance and to eliminate loopy path
visited = [False for i in range(len(coll_list))
] #Initially visited will be false as no vertices are visted
for pts in coll_list:
list.append(pts)
visited[
pts.
id] = True #After visiting the vertices make the visited of that point becomes True.
while list:
new_parent = list.pop(0)
for child in coll_list:
if visited[
child.
id] == False and child.id != pts.id: #If its not visited and it is not the same point find distance.
dis = distance(new_parent.q, child.q)
if dis < 5: #Condition for adding edges
list.append(child)
visited[child.id] = True
# check_point = np.linspace(new_parent.q,child.q,10)
# check = 0
# check_list = []
# for point in (check_point):
# for p in range(len(obstacles)):
# obstacle_i = obstacles[p].points
# xmin = obstacle_i[0][0] - robot_radius
# xmax = obstacle_i[2][0] + robot_radius
# ymin = obstacle_i[2][1] - robot_radius
# ymax = obstacle_i[0][1] + robot_radius
#
# if xmin < point[0] < xmax and ymin < point[1] < ymax:
# check = 1
#
# if check != 1:
graph.addEdge(new_parent, child, dis, path=[])
# the roadmap
# 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
