def build_graph(boundary: List[Location], stat_obstacles: List[Obstacle],
params) -> nx.Graph:
"""Builds a space-filling graph within flight boundaries and removes nodes in obstacles.
Args:
boundary: the gps locations of the polygon defining the flight boundary
stat_obstacles: the locations and sizes of the stationary obstacles
params: algorithm parameters dictionary
Returns:
graph: a graph filling the space, whose nodes are Locations
"""
# space filling graph
min_boundary = Location(*(min(coord) for coord in zip(*boundary)))
max_boundary = Location(*(max(coord) for coord in zip(*boundary)))
graph_size = Location.granularity_diff(max_boundary, min_boundary,
params["granularity"])
graph = xgrid_graph(*graph_size)
# removes nodes in obstacles
graph_origin = min_boundary
for obs in stat_obstacles:
to_remove = set()
for node in graph:
loc = Location.from_grid(node, graph_origin, params["granularity"])
if loc in obs:
to_remove.add(node)
for node in to_remove:
graph.remove_node(node)
# TODO remove nodes outside boundary
return graph, graph_origin
def build(boundary: List[Location], stat_obstacles: List[Obstacle],
granularity: float) -> nx.Graph:
"""Builds a space-filling graph within flight boundaries and removes nodes in obstacles.
Args:
boundary: the gps locations of the polygon defining the flight boundary
stat_obstacles: the locations and sizes of the stationary obstacles
granularity: the distance between graph nodes for (lat, lon)
Returns:
graph: a graph filling the space, whose nodes are Locations
"""
# space filling graph
min_boundary = Location(*(min(coord) for coord in zip(*boundary)))
max_boundary = Location(*(max(coord) for coord in zip(*boundary)))
graph_size = Location.granularity_diff(max_boundary, min_boundary,
granularity)
graph = xgrid_graph(*graph_size)
# TODO remove nodes in obstacles
graph_origin = min_boundary
for obs in stat_obstacles:
to_remove = set()
for node in graph:
loc = Location.from_grid(node, graph_origin, granularity)
if loc in obs:
to_remove.add(node)
for node in to_remove:
graph.remove_node(node)
# TODO return mapping from location to node, node to location?
return graph, graph_origin
def plan(waypoints: List[Location], graph: nx.Graph, origin,
granularity) -> np.ndarray:
"""Builds a path to the next waypoint(s)
Args:
graph: inside flight boundaries and outside obstacles
waypoints: location waypoints to fly
Returns:
path: a list of locations corresponding to nodes in the graph path
"""
# waypoints to nodes
nodes = [point.to_grid(origin, granularity) for point in waypoints]
# nodes to graph path
paths = [
nx.bidirectional_dijkstra(graph, source, target, weight='weight')[1]
for source, target in zip(nodes[0:-1], nodes[1:])
]
path = []
for p in paths:
path += p[:-1]
path += [paths[-1][-1]]
path_coords = [
list(Location.from_grid(node, origin, granularity)) for node in path
]
rn_path = np.array(path_coords).T
return path, rn_path
def quantize_graph_path(path: list, origin, params) -> np.ndarray:
"""Quantizes a given graph path -> a path in Rn. Guaranteed that endpoints are included
Args:
path: Contains a list of vertex names in order of the path.
rn_path: A numpy array of points in r3 of each vertex in path, where the columns are each point.
params: the algorithm parameters
"""
path_coords = [
list(Location.from_grid(node, origin, params["granularity"]))
for node in path
]
rn_path = np.array(path_coords).T
if rn_path.shape[1] <= 1:
raise ValueError('A path must have at least two points')
dist_mat = linalg.norm(rn_path[:, :-1] - rn_path[:, 1:], axis=0)
total_norm = sum(dist_mat)
quantization_distance = params["quantization_distance"]
if total_norm <= quantization_distance:
raise ValueError(
'A path\'s total distance must be larger than the quantization distance'
)
curr_path_len = 0 # ∆(dist) between current quantization path and last node explored
full_path = np.zeros(
(rn_path.shape[0], int(ceil(total_norm / quantization_distance))))
curr_idx = 0
for v_idx, edge_len in enumerate(dist_mat):
curr_vertex, next_vertex = rn_path[:, v_idx], rn_path[:, v_idx + 1]
conv_comb = curr_path_len / edge_len
n_iter = int(ceil((edge_len - curr_path_len) / quantization_distance))
for _ in range(n_iter):
assert conv_comb <= 1
full_path[:, curr_idx] = (
1 - conv_comb) * curr_vertex + conv_comb * next_vertex
conv_comb += quantization_distance / edge_len
curr_idx += 1
curr_path_len += n_iter * quantization_distance - edge_len
assert curr_path_len >= 0.0
return full_path
"optim_cooling_schedule": [1.0005, 1.0001],
"optim_fast_cooling_schedule": [1.1, 1.3],
"optim_init_constraint_hardness": [1, 2],
"optim_init_spring_hardness": [.1, .2, .4],
"optim_max_time_increase": [10], # what does this do?
"optim_init_momentum": [.99],
"optim_momentum_change": [.1, .2],
"optim_scale": [1.0]}
results_grid = []
score_grid = []
params_grid = [dict(zip(params_values, v)) for v in product(*params_values.values())]
# Set problem variables
((x_min, y_min), (x_max, y_max)) = problem.area
boundary = [Location(x_min, y_min), Location(x_min, y_max),
Location(x_max, y_min), Location(x_max, y_max)]
((wx_min, wy_min), (wx_max, wy_max)) = problem.waypoints
waypoints = [Location(wx_min, wy_min), Location(wx_max, wy_max)]
for pn, params in enumerate(params_grid):
print("trying params {} of {}".format(pn, len(params_grid)))
envs = environments.generate(ENVIRONMENT_COUNT, ENVIRONMENT_ID)
flight_paths = []
for i, env in enumerate(envs):
print("env {}".format(i))
stat_obstacles = []
for obs in env:
cx, cy, r = obs
stat_obstacles.append(Obstacle(Location(cx, cy), r))
for node in graph:
loc = Location.from_grid(node, graph_origin, granularity)
if loc in obs:
to_remove.add(node)
for node in to_remove:
graph.remove_node(node)
# TODO return mapping from location to node, node to location?
return graph, graph_origin
# test if run as main
if __name__ == "__main__":
boundary = [
Location(-50.0, -50.0),
Location(-50.0, 150.0),
Location(150.0, -50.0),
Location(150.0, 150.0)
]
stat_obstacles = [
Obstacle(Location(0.0, 0.0), 20),
Obstacle(Location(30.0, 30.0), 10)
]
granularity = 20.0
initial_graph, _ = build(boundary, stat_obstacles, granularity)
plt.figure()
pos = nx.get_node_attributes(initial_graph, 'pos')
nx.draw(initial_graph, pos, with_labels=True, font_weight='bold')
plt.show()
from nav.graph import initial_graph
from nav.graph import graph_path
from nav.utility.classes import Location, Obstacle
import networkx as nx
import matplotlib.pyplot as plt
boundary = [
Location(-50.0, -50.0),
Location(-50.0, 150.0),
Location(150.0, -50.0),
Location(150.0, 150.0)
]
stat_obstacles = [
Obstacle(Location(15.0, -15.0), 10),
Obstacle(Location(30.0, 40.0), 20),
Obstacle(Location(80.0, 70.0), 15)
]
granularity = 7.0
waypoints = [Location(0.0, 0.0), Location(100.0, 100.0)]
initial_graph, origin = initial_graph.build(boundary, stat_obstacles,
granularity)
path, rn_path = graph_path.plan(waypoints, initial_graph, origin, granularity)
print(path)
plt.figure()
pos = nx.get_node_attributes(initial_graph, 'pos')
nx.draw(initial_graph, pos, nodelist=path, font_weight='bold')
quantization_distance = 1.5
quantized_path = graph_path.quantize(path, rn_path, quantization_distance)
from nav.graph import initial as initial_graph
from nav.graph import path as graph_path
from nav.utility.classes import Location, Obstacle
import networkx as nx
import matplotlib.pyplot as plt
boundary = [Location(-50.0, -50.0), Location(-50.0, 150.0),
Location(150.0, -50.0), Location(150.0, 150.0)]
obstacles = [
((15.0, -15.0), 10),
((30.0, 40.0), 20),
((80.0, 70.0), 15),
]
stat_obstacles = [Obstacle(Location(*o[0]), o[1]) for o in obstacles]
# stat_obstacles = [Obstacle(Location(15.0, -15.0), 10),
# Obstacle(Location(30.0, 40.0), 20),
# Obstacle(Location(80.0, 70.0), 15)]
granularity = 7.0
waypoints = [Location(0.0, 0.0), Location(100.0, 100.0)]
initial_graph, origin = initial_graph.build(boundary, stat_obstacles, granularity)
path, rn_path = graph_path.plan(waypoints, initial_graph, origin, granularity)
# print(path)
pos = nx.get_node_attributes(initial_graph, 'pos')
nx.draw(initial_graph, pos, nodelist=path, font_weight='bold')
quantization_distance = 1.5
quantized_path = graph_path.quantize(path, rn_path, quantization_distance)
# print(quantized_path.shape)
# print(quantized_path)