def calculate_edge_to_vertex_conflicts(self, robot_radius, nn):
print("calculate_edge_to_vertex_conflicts")
for edge in self.edges:
sure_s = nn.neighbors_in_radius(edge.src.point,
KER.FT(2) * robot_radius)
sure_d = nn.neighbors_in_radius(edge.dest.point,
KER.FT(2) * robot_radius)
sure_p = set(
[self.points_to_nodes[point] for point in sure_s + sure_d])
for node in sure_p:
node.edge_conflicts.append(edge)
edge.node_conflicts.append(node)
x1 = nn.neighbors_in_radius(
edge.src.point,
KER.FT(2) * robot_radius + Config.connection_radius)
x2 = nn.neighbors_in_radius(
edge.dest.point,
KER.FT(2) * robot_radius + Config.connection_radius)
points = set([self.points_to_nodes[point] for point in x1 + x2])
for node in points - sure_p:
if KER.squared_distance(
point_d_to_point_2(node.point), edge.segment
) < KER.FT(4) * robot_radius * robot_radius:
node.edge_conflicts.append(edge)
edge.node_conflicts.append(node)
def generate_milestones(cd):
res = []
if Config.sample_method == "eps_net":
i = 0
half_points_diff = (Config.edge_len / Config.balls_per_dim) / 2
l1_cords = [
Config.delta + (2 * i - 1) * half_points_diff
for i in range(1, Config.balls_per_dim + 1)
]
l2_cords = [
Config.delta + (2 * i) * half_points_diff
for i in range(Config.balls_per_dim + 1)
]
l1 = cords_to_2d_points(l1_cords, l1_cords)
l2 = cords_to_2d_points(l2_cords, l2_cords)
all_points = l1 + l2
for point in all_points:
if cd.is_point_valid(point):
res.append(PrmNode(point, "v" + str(i)))
i += 1
return res
if Config.sample_method == "grid":
i = 0
points_diff = (Config.edge_len / (Config.grid_points_per_dim - 1))
cords = [
Config.delta + i * points_diff
for i in range(Config.grid_points_per_dim)
]
points = cords_to_2d_points(cords, cords)
for point in points:
if cd.is_point_valid(point):
res.append(PrmNode(point, "v" + str(i)))
i += 1
return res
if Config.sample_method == "random":
i = 0
points = [
SS.Point_d(2, [
KER.FT(random.uniform(Config.delta, 1 - Config.delta)),
KER.FT(random.uniform(Config.delta, 1 - Config.delta))
]) for _ in range(Config.num_of_sample_points)
]
for point in points:
if cd.is_point_valid(point):
res.append(PrmNode(point, "v" + str(i)))
i += 1
return res
raise ValueError("Invalid configuration")
def calculate_vertices_conflicts(self, nn, robot_radius):
print("calculate_vertices_conflicts")
for point, node in self.points_to_nodes.items():
nearest = nn.neighbors_in_radius(point, KER.FT(2) * robot_radius)
for neighbor in nearest: # first point is self and no need for edge from v to itself
neighbor_node = self.points_to_nodes[neighbor]
if neighbor_node == node:
continue
node.node_conflicts.append(neighbor_node)
neighbor_node.node_conflicts.append(node)
def reset(self):
self.out_file_name = "optimal_length_1_" + (
"multi_" if self.is_multi_robot else "single_") + str(
self.eps) + "_" + str(
self.delta) + "_" + self.sample_method + ".ymal"
print("out_file_name:", self.out_file_name)
alpha = self.eps / sqrt(1 + self.eps**2)
y = self.eps / (2 * (2 + self.eps))
self.edge_len = 1 - 2 * self.delta
if self.is_multi_robot:
# These may change as we modify bounds in the paper
self.ball_radius = y * self.delta
# self.connection_radius = KER.FT(self.delta)*(KER.FT(1+self.eps)/KER.FT(2+self.eps))
self.connection_radius = KER.FT(self.delta) * (
KER.FT(1 + self.eps) / KER.FT(2 + self.eps)) * KER.FT(1.0001)
# self.connection_radius = KER.FT(self.delta*1.001)
else:
self.ball_radius = alpha * self.delta
self.connection_radius = KER.FT(
2 * (alpha + sqrt(1 - alpha**2)) * self.delta) * KER.FT(1.0001)
unrounded_balls_per_dim = self.edge_len / (2 * self.ball_radius)
print("unrounded number of balls:",
unrounded_balls_per_dim**2 + (unrounded_balls_per_dim + 1)**2)
self.balls_per_dim = ceil(unrounded_balls_per_dim)
self.num_of_sample_points = self.balls_per_dim**2 + (
self.balls_per_dim + 1)**2
print("real number of balls:", self.num_of_sample_points)
self.grid_points_per_dim = ceil(sqrt(self.num_of_sample_points))
print("real number of grid points:", self.grid_points_per_dim**2)
def calculate_edge_to_edge_conflicts(
self, robot_radius, nn
): # Dror: this can be improved by looking at edges of "near" vertices
print("calculate_edge_to_edge_conflicts")
i = 0
for edge1 in self.edges:
if i % 1000 == 0:
gc.collect()
print(i)
i += 1
sure_s = nn.neighbors_in_radius(edge1.src.point,
KER.FT(2) * robot_radius)
sure_d = nn.neighbors_in_radius(edge1.dest.point,
KER.FT(2) * robot_radius)
sure_p = set(
[self.points_to_nodes[point] for point in sure_s + sure_d])
sure_e = set()
for node in sure_p:
sure_e.update(
list(node.in_connections.values()) +
list(node.out_connections.values()))
for edge2 in sure_e:
if edge1 == edge2 or edge2 in edge1.edge_conflicts:
continue
edge1.edge_conflicts.add(edge2)
edge2.edge_conflicts.add(edge1)
maybe_s = nn.neighbors_in_radius(
edge1.src.point,
KER.FT(2) * robot_radius +
KER.FT(2) * Config.connection_radius)
maybe_d = nn.neighbors_in_radius(
edge1.dest.point,
KER.FT(2) * robot_radius +
KER.FT(2) * Config.connection_radius)
maybe_p = set(
[self.points_to_nodes[point] for point in maybe_s + maybe_d])
maybe_e = set()
for node in maybe_p:
maybe_e.update(
list(node.in_connections.values()) +
list(node.out_connections.values()))
for edge2 in maybe_e - sure_e:
if edge1 == edge2 or edge2 in edge1.edge_conflicts:
continue
else:
if KER.squared_distance(
edge1.segment, edge2.segment
) < KER.FT(4) * robot_radius * robot_radius:
edge1.edge_conflicts.add(edge2)
edge2.edge_conflicts.add(edge1)
def get_neighbors(points):
connections_list = [
list(p.out_connections.keys()) + list(p.in_connections.keys())
for p in points
]
res = []
for i in range(len(points)):
is_good = True
for next_point in connections_list[i]:
for j in range(len(points)):
if i == j:
continue
if KER.squared_distance(
next_point.point_2, points[j].point_2
) < KER.FT(4) * robot_radius * robot_radius:
is_good = False
break
if not is_good:
continue
seg = points[i].in_connections[
next_point] if next_point in points[
i].in_connections else points[i].out_connections[
next_point]
for j in range(len(points)):
if i == j:
continue
if KER.squared_distance(
seg.segment, points[j].point_2
) < KER.FT(4) * robot_radius * robot_radius:
is_good = False
break
if not is_good:
continue
res.append((tuple([
points[k] if k != i else next_point
for k in range(len(points))
]), seg))
return res
def generate_path(path, starts, obstacles, destinations, in_radius):
start_t = time.time()
radius = KER.FT(in_radius)
cd = Collision_detection.Collision_detector(obstacles, radius)
milestones = []
for (i, start_p) in enumerate(starts):
milestones.append(
PrmNode(SS.Point_d(2, [start_p.x(), start_p.y()]),
"start" + str(i)))
for (i, destination_p) in enumerate(destinations):
milestones.append(
PrmNode(SS.Point_d(
2, [destination_p.x(), destination_p.y()]), "goal" + str(i)))
milestones += generate_milestones(cd)
nn = NeighborsFinder([milestone.point for milestone in milestones])
g = make_graph(cd, milestones, nn)
print("vertices amount:", len(g.points_to_nodes))
print("edges amount:", len(g.edges))
if Config.run_a_star:
a_star_res, d_path = g.a_star(
radius,
[SS.Point_d(2, [start_p.x(), start_p.y()])
for start_p in starts], [
SS.Point_d(2, [destination_p.x(),
destination_p.y()])
for destination_p in destinations
])
print("a_star_res:", a_star_res)
if Config.create_yaml:
g.calculate_vertices_conflicts(nn, radius)
g.calculate_edge_to_vertex_conflicts(radius, nn)
g.calculate_edge_to_edge_conflicts(radius, nn)
with open(Config.out_file_name, "w") as f:
f.write("vertices:\n")
for vertex in g.points_to_nodes.values():
f.write(" - name: " + vertex.name + "\n")
f.write(" pos: [" + str(vertex.point[0].to_double()) +
", " + str(vertex.point[1].to_double()) + "]\n")
f.write(" vertexConflicts: " +
str([node.name
for node in vertex.node_conflicts]) + "\n")
f.write(" edgeConflicts: " +
str([e.name for e in vertex.edge_conflicts]) + "\n")
f.write("edges:\n")
for edge in g.edges:
f.write(" - name: " + edge.name + "\n")
f.write(" from: " + edge.src.name + "\n")
f.write(" to: " + edge.dest.name + "\n")
f.write(" edgeConflicts: " +
str([e.name for e in edge.edge_conflicts]) + "\n")
f.write(" vertexConflicts: " +
str([node.name
for node in edge.node_conflicts]) + "\n")
f.flush()
took = time.time() - start_t
print("GOODYYY", took)
if Config.run_a_star:
for i in d_path:
path.append(i)
return a_star_res, took
return 0
def cords_to_2d_points(cords_x, cords_y):
res = [
SS.Point_d(2, [KER.FT(x), KER.FT(y)]) for x in cords_x for y in cords_y
]
return res
def neighbors_in_radius(self, query, r):
epsilon = Ker.FT(0)
a = SS.Fuzzy_sphere(query, r, epsilon)
res = []
self.tree.search(a, res)
return res