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)